1150 files changed, 95895 insertions, 39768 deletions

diff --git a/Documentation/ABI/testing/sysfs-driver-wacom b/Documentation/ABI/testing/sysfs-driver-wacom
index 56c54558c8a4..8d55a83d6921 100644
--- a/Documentation/ABI/testing/sysfs-driver-wacom
+++ b/Documentation/ABI/testing/sysfs-driver-wacom
@@ -23,9 +23,10 @@ Contact:	linux-input@vger.kernel.org
 Description:
 		Attribute group for control of the status LEDs and the OLEDs.
 		This attribute group is only available for Intuos 4 M, L,
-		and XL (with LEDs and OLEDs) and Cintiq 21UX2 and Cintiq 24HD
-		(LEDs only). Therefore its presence implicitly signifies the
-		presence of said LEDs and OLEDs on the tablet device.
+		and XL (with LEDs and OLEDs), Intuos 5 (LEDs only), and Cintiq
+		21UX2 and Cintiq 24HD (LEDs only). Therefore its presence
+		implicitly signifies the presence of said LEDs and OLEDs on the
+		tablet device.
 
 What:		/sys/bus/usb/devices/<busnum>-<devnum>:<cfg>.<intf>/wacom_led/status0_luminance
 Date:		August 2011
@@ -48,10 +49,10 @@ What:		/sys/bus/usb/devices/<busnum>-<devnum>:<cfg>.<intf>/wacom_led/status_led0
 Date:		August 2011
 Contact:	linux-input@vger.kernel.org
 Description:
-		Writing to this file sets which one of the four (for Intuos 4)
-		or of the right four (for Cintiq 21UX2 and Cintiq 24HD) status
-		LEDs is active (0..3). The other three LEDs on the same side are
-		always inactive.
+		Writing to this file sets which one of the four (for Intuos 4
+		and Intuos 5) or of the right four (for Cintiq 21UX2 and Cintiq
+		24HD) status LEDs is active (0..3). The other three LEDs on the
+		same side are always inactive.
 
 What:		/sys/bus/usb/devices/<busnum>-<devnum>:<cfg>.<intf>/wacom_led/status_led1_select
 Date:		September 2011
diff --git a/Documentation/DocBook/media/Makefile b/Documentation/DocBook/media/Makefile
index 6628b4b9cac4..362520992ced 100644
--- a/Documentation/DocBook/media/Makefile
+++ b/Documentation/DocBook/media/Makefile
@@ -70,6 +70,8 @@ IOCTLS = \
 	VIDIOC_SUBDEV_ENUM_MBUS_CODE \
 	VIDIOC_SUBDEV_ENUM_FRAME_SIZE \
 	VIDIOC_SUBDEV_ENUM_FRAME_INTERVAL \
+	VIDIOC_SUBDEV_G_SELECTION \
+	VIDIOC_SUBDEV_S_SELECTION \
 
 TYPES = \
 	$(shell perl -ne 'print "$$1 " if /^typedef\s+[^\s]+\s+([^\s]+)\;/' $(srctree)/include/linux/videodev2.h) \
@@ -193,7 +195,7 @@ DVB_DOCUMENTED = \
 #
 
 install_media_images = \
-	$(Q)cp $(OBJIMGFILES) $(MEDIA_OBJ_DIR)/media_api
+	$(Q)cp $(OBJIMGFILES) $(MEDIA_SRC_DIR)/v4l/*.svg $(MEDIA_OBJ_DIR)/media_api
 
 $(MEDIA_OBJ_DIR)/%: $(MEDIA_SRC_DIR)/%.b64
 	$(Q)base64 -d $< >$@
diff --git a/Documentation/DocBook/media/dvb/dvbproperty.xml b/Documentation/DocBook/media/dvb/dvbproperty.xml
index c7a4ca517859..e633c097a8d1 100644
--- a/Documentation/DocBook/media/dvb/dvbproperty.xml
+++ b/Documentation/DocBook/media/dvb/dvbproperty.xml
@@ -531,6 +531,139 @@ typedef enum fe_delivery_system {
 				here are referring to what can be found in the TMCC-structure -
 				independent of the mode.</para>
 		</section>
+		<section id="DTV-ATSCMH-FIC-VER">
+			<title><constant>DTV_ATSCMH_FIC_VER</constant></title>
+			<para>Version number of the FIC (Fast Information Channel) signaling data.</para>
+			<para>FIC is used for relaying information to allow rapid service acquisition by the receiver.</para>
+			<para>Possible values: 0, 1, 2, 3, ..., 30, 31</para>
+		</section>
+		<section id="DTV-ATSCMH-PARADE-ID">
+			<title><constant>DTV_ATSCMH_PARADE_ID</constant></title>
+			<para>Parade identification number</para>
+			<para>A parade is a collection of up to eight MH groups, conveying one or two ensembles.</para>
+			<para>Possible values: 0, 1, 2, 3, ..., 126, 127</para>
+		</section>
+		<section id="DTV-ATSCMH-NOG">
+			<title><constant>DTV_ATSCMH_NOG</constant></title>
+			<para>Number of MH groups per MH subframe for a designated parade.</para>
+			<para>Possible values: 1, 2, 3, 4, 5, 6, 7, 8</para>
+		</section>
+		<section id="DTV-ATSCMH-TNOG">
+			<title><constant>DTV_ATSCMH_TNOG</constant></title>
+			<para>Total number of MH groups including all MH groups belonging to all MH parades in one MH subframe.</para>
+			<para>Possible values: 0, 1, 2, 3, ..., 30, 31</para>
+		</section>
+		<section id="DTV-ATSCMH-SGN">
+			<title><constant>DTV_ATSCMH_SGN</constant></title>
+			<para>Start group number.</para>
+			<para>Possible values: 0, 1, 2, 3, ..., 14, 15</para>
+		</section>
+		<section id="DTV-ATSCMH-PRC">
+			<title><constant>DTV_ATSCMH_PRC</constant></title>
+			<para>Parade repetition cycle.</para>
+			<para>Possible values: 1, 2, 3, 4, 5, 6, 7, 8</para>
+		</section>
+		<section id="DTV-ATSCMH-RS-FRAME-MODE">
+			<title><constant>DTV_ATSCMH_RS_FRAME_MODE</constant></title>
+			<para>RS frame mode.</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_rs_frame_mode {
+	ATSCMH_RSFRAME_PRI_ONLY  = 0,
+	ATSCMH_RSFRAME_PRI_SEC   = 1,
+} atscmh_rs_frame_mode_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-RS-FRAME-ENSEMBLE">
+			<title><constant>DTV_ATSCMH_RS_FRAME_ENSEMBLE</constant></title>
+			<para>RS frame ensemble.</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_rs_frame_ensemble {
+	ATSCMH_RSFRAME_ENS_PRI   = 0,
+	ATSCMH_RSFRAME_ENS_SEC   = 1,
+} atscmh_rs_frame_ensemble_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-RS-CODE-MODE-PRI">
+			<title><constant>DTV_ATSCMH_RS_CODE_MODE_PRI</constant></title>
+			<para>RS code mode (primary).</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_rs_code_mode {
+	ATSCMH_RSCODE_211_187    = 0,
+	ATSCMH_RSCODE_223_187    = 1,
+	ATSCMH_RSCODE_235_187    = 2,
+} atscmh_rs_code_mode_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-RS-CODE-MODE-SEC">
+			<title><constant>DTV_ATSCMH_RS_CODE_MODE_SEC</constant></title>
+			<para>RS code mode (secondary).</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_rs_code_mode {
+	ATSCMH_RSCODE_211_187    = 0,
+	ATSCMH_RSCODE_223_187    = 1,
+	ATSCMH_RSCODE_235_187    = 2,
+} atscmh_rs_code_mode_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-SCCC-BLOCK-MODE">
+			<title><constant>DTV_ATSCMH_SCCC_BLOCK_MODE</constant></title>
+			<para>Series Concatenated Convolutional Code Block Mode.</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_sccc_block_mode {
+	ATSCMH_SCCC_BLK_SEP      = 0,
+	ATSCMH_SCCC_BLK_COMB     = 1,
+} atscmh_sccc_block_mode_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-SCCC-CODE-MODE-A">
+			<title><constant>DTV_ATSCMH_SCCC_CODE_MODE_A</constant></title>
+			<para>Series Concatenated Convolutional Code Rate.</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_sccc_code_mode {
+	ATSCMH_SCCC_CODE_HLF     = 0,
+	ATSCMH_SCCC_CODE_QTR     = 1,
+} atscmh_sccc_code_mode_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-SCCC-CODE-MODE-B">
+			<title><constant>DTV_ATSCMH_SCCC_CODE_MODE_B</constant></title>
+			<para>Series Concatenated Convolutional Code Rate.</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_sccc_code_mode {
+	ATSCMH_SCCC_CODE_HLF     = 0,
+	ATSCMH_SCCC_CODE_QTR     = 1,
+} atscmh_sccc_code_mode_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-SCCC-CODE-MODE-C">
+			<title><constant>DTV_ATSCMH_SCCC_CODE_MODE_C</constant></title>
+			<para>Series Concatenated Convolutional Code Rate.</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_sccc_code_mode {
+	ATSCMH_SCCC_CODE_HLF     = 0,
+	ATSCMH_SCCC_CODE_QTR     = 1,
+} atscmh_sccc_code_mode_t;
+</programlisting>
+		</section>
+		<section id="DTV-ATSCMH-SCCC-CODE-MODE-D">
+			<title><constant>DTV_ATSCMH_SCCC_CODE_MODE_D</constant></title>
+			<para>Series Concatenated Convolutional Code Rate.</para>
+			<para>Possible values are:</para>
+<programlisting>
+typedef enum atscmh_sccc_code_mode {
+	ATSCMH_SCCC_CODE_HLF     = 0,
+	ATSCMH_SCCC_CODE_QTR     = 1,
+} atscmh_sccc_code_mode_t;
+</programlisting>
+		</section>
 	</section>
 	<section id="DTV-API-VERSION">
 	<title><constant>DTV_API_VERSION</constant></title>
@@ -774,6 +907,33 @@ typedef enum fe_hierarchy {
 				<listitem><para><link linkend="DTV-BANDWIDTH-HZ"><constant>DTV_BANDWIDTH_HZ</constant></link></para></listitem>
 			</itemizedlist>
 		</section>
+		<section id="atscmh-params">
+			<title>ATSC-MH delivery system</title>
+			<para>The following parameters are valid for ATSC-MH:</para>
+			<itemizedlist mark='opencircle'>
+				<listitem><para><link linkend="DTV-API-VERSION"><constant>DTV_API_VERSION</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-DELIVERY-SYSTEM"><constant>DTV_DELIVERY_SYSTEM</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-TUNE"><constant>DTV_TUNE</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-CLEAR"><constant>DTV_CLEAR</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-FREQUENCY"><constant>DTV_FREQUENCY</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-BANDWIDTH-HZ"><constant>DTV_BANDWIDTH_HZ</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-FIC-VER"><constant>DTV_ATSCMH_FIC_VER</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-PARADE-ID"><constant>DTV_ATSCMH_PARADE_ID</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-NOG"><constant>DTV_ATSCMH_NOG</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-TNOG"><constant>DTV_ATSCMH_TNOG</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-SGN"><constant>DTV_ATSCMH_SGN</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-PRC"><constant>DTV_ATSCMH_PRC</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-RS-FRAME-MODE"><constant>DTV_ATSCMH_RS_FRAME_MODE</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-RS-FRAME-ENSEMBLE"><constant>DTV_ATSCMH_RS_FRAME_ENSEMBLE</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-CODE-MODE-PRI"><constant>DTV_ATSCMH_CODE_MODE_PRI</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-CODE-MODE-SEC"><constant>DTV_ATSCMH_CODE_MODE_SEC</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-SCCC-BLOCK-MODE"><constant>DTV_ATSCMH_SCCC_BLOCK_MODE</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-SCCC-CODE_MODE-A"><constant>DTV_ATSCMH_SCCC_CODE_MODE_A</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-SCCC-CODE_MODE-B"><constant>DTV_ATSCMH_SCCC_CODE_MODE_B</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-SCCC-CODE_MODE-C"><constant>DTV_ATSCMH_SCCC_CODE_MODE_C</constant></link></para></listitem>
+				<listitem><para><link linkend="DTV-ATSCMH-SCCC-CODE_MODE-D"><constant>DTV_ATSCMH_SCCC_CODE_MODE_D</constant></link></para></listitem>
+			</itemizedlist>
+		</section>
 	</section>
 	<section id="frontend-property-cable-systems">
 	<title>Properties used on cable delivery systems</title>
diff --git a/Documentation/DocBook/media/v4l/biblio.xml b/Documentation/DocBook/media/v4l/biblio.xml
index 7dc65c592a87..7c49facecd25 100644
--- a/Documentation/DocBook/media/v4l/biblio.xml
+++ b/Documentation/DocBook/media/v4l/biblio.xml
@@ -197,4 +197,33 @@ in the frequency range from 87,5 to 108,0 MHz</title>
       <title>NTSC-4: United States RBDS Standard</title>
     </biblioentry>
 
+    <biblioentry id="iso12232">
+      <abbrev>ISO&nbsp;12232:2006</abbrev>
+      <authorgroup>
+	<corpauthor>International Organization for Standardization
+(<ulink url="http://www.iso.org">http://www.iso.org</ulink>)</corpauthor>
+      </authorgroup>
+      <title>Photography &mdash; Digital still cameras &mdash; Determination
+      of exposure index, ISO speed ratings, standard output sensitivity, and
+      recommended exposure index</title>
+    </biblioentry>
+
+    <biblioentry id="cea861">
+      <abbrev>CEA-861-E</abbrev>
+      <authorgroup>
+	<corpauthor>Consumer Electronics Association
+(<ulink url="http://www.ce.org">http://www.ce.org</ulink>)</corpauthor>
+      </authorgroup>
+      <title>A DTV Profile for Uncompressed High Speed Digital Interfaces</title>
+    </biblioentry>
+
+    <biblioentry id="vesadmt">
+      <abbrev>VESA&nbsp;DMT</abbrev>
+      <authorgroup>
+	<corpauthor>Video Electronics Standards Association
+(<ulink url="http://www.vesa.org">http://www.vesa.org</ulink>)</corpauthor>
+      </authorgroup>
+      <title>VESA and Industry Standards and Guidelines for Computer Display Monitor Timing (DMT)</title>
+    </biblioentry>
+
   </bibliography>
diff --git a/Documentation/DocBook/media/v4l/common.xml b/Documentation/DocBook/media/v4l/common.xml
index c79278acfb0e..4101aeb56540 100644
--- a/Documentation/DocBook/media/v4l/common.xml
+++ b/Documentation/DocBook/media/v4l/common.xml
@@ -724,41 +724,49 @@ if (-1 == ioctl (fd, &VIDIOC-S-STD;, &amp;std_id)) {
 }
       </programlisting>
     </example>
+  </section>
   <section id="dv-timings">
 	<title>Digital Video (DV) Timings</title>
 	<para>
-	The video standards discussed so far has been dealing with Analog TV and the
+	The video standards discussed so far have been dealing with Analog TV and the
 corresponding video timings. Today there are many more different hardware interfaces
 such as High Definition TV interfaces (HDMI), VGA, DVI connectors etc., that carry
 video signals and there is a need to extend the API to select the video timings
 for these interfaces. Since it is not possible to extend the &v4l2-std-id; due to
-the limited bits available, a new set of IOCTLs is added to set/get video timings at
+the limited bits available, a new set of IOCTLs was added to set/get video timings at
 the input and output: </para><itemizedlist>
 	<listitem>
-	<para>DV Presets: Digital Video (DV) presets. These are IDs representing a
+	<para>DV Timings: This will allow applications to define detailed
+video timings for the interface. This includes parameters such as width, height,
+polarities, frontporch, backporch etc. The <filename>linux/v4l2-dv-timings.h</filename>
+header can be used to get the timings of the formats in the <xref linkend="cea861" /> and
+<xref linkend="vesadmt" /> standards.
+	</para>
+	</listitem>
+	<listitem>
+	<para>DV Presets: Digital Video (DV) presets (<emphasis role="bold">deprecated</emphasis>).
+	These are IDs representing a
 video timing at the input/output. Presets are pre-defined timings implemented
 by the hardware according to video standards. A __u32 data type is used to represent
 a preset unlike the bit mask that is used in &v4l2-std-id; allowing future extensions
-to support as many different presets as needed.</para>
-	</listitem>
-	<listitem>
-	<para>Custom DV Timings: This will allow applications to define more detailed
-custom video timings for the interface. This includes parameters such as width, height,
-polarities, frontporch, backporch etc.
-	</para>
+to support as many different presets as needed. This API is deprecated in favor of the DV Timings
+API.</para>
 	</listitem>
 	</itemizedlist>
+	<para>To enumerate and query the attributes of the DV timings supported by a device,
+	applications use the &VIDIOC-ENUM-DV-TIMINGS; and &VIDIOC-DV-TIMINGS-CAP; ioctls.
+	To set DV timings for the device, applications use the
+&VIDIOC-S-DV-TIMINGS; ioctl and to get current DV timings they use the
+&VIDIOC-G-DV-TIMINGS; ioctl. To detect the DV timings as seen by the video receiver applications
+use the &VIDIOC-QUERY-DV-TIMINGS; ioctl.</para>
 	<para>To enumerate and query the attributes of DV presets supported by a device,
 applications use the &VIDIOC-ENUM-DV-PRESETS; ioctl. To get the current DV preset,
 applications use the &VIDIOC-G-DV-PRESET; ioctl and to set a preset they use the
-&VIDIOC-S-DV-PRESET; ioctl.</para>
-	<para>To set custom DV timings for the device, applications use the
-&VIDIOC-S-DV-TIMINGS; ioctl and to get current custom DV timings they use the
-&VIDIOC-G-DV-TIMINGS; ioctl.</para>
+&VIDIOC-S-DV-PRESET; ioctl. To detect the preset as seen by the video receiver applications
+use the &VIDIOC-QUERY-DV-PRESET; ioctl.</para>
 	<para>Applications can make use of the <xref linkend="input-capabilities" /> and
 <xref linkend="output-capabilities"/> flags to decide what ioctls are available to set the
 video timings for the device.</para>
-	</section>
   </section>
 
   &sub-controls;
diff --git a/Documentation/DocBook/media/v4l/compat.xml b/Documentation/DocBook/media/v4l/compat.xml
index bce97c50391b..ea42ef824948 100644
--- a/Documentation/DocBook/media/v4l/compat.xml
+++ b/Documentation/DocBook/media/v4l/compat.xml
@@ -2407,6 +2407,54 @@ details.</para>
 	  <para>Added <link linkend="jpeg-controls">JPEG compression control
 	  class</link>.</para>
         </listitem>
+        <listitem>
+	  <para>Extended the DV Timings API:
+	  &VIDIOC-ENUM-DV-TIMINGS;, &VIDIOC-QUERY-DV-TIMINGS; and
+	  &VIDIOC-DV-TIMINGS-CAP;.</para>
+        </listitem>
+      </orderedlist>
+    </section>
+
+    <section>
+      <title>V4L2 in Linux 3.5</title>
+      <orderedlist>
+        <listitem>
+	  <para>Added integer menus, the new type will be
+	  V4L2_CTRL_TYPE_INTEGER_MENU.</para>
+        </listitem>
+        <listitem>
+	  <para>Added selection API for V4L2 subdev interface:
+	  &VIDIOC-SUBDEV-G-SELECTION; and
+	  &VIDIOC-SUBDEV-S-SELECTION;.</para>
+        </listitem>
+        <listitem>
+	  <para> Added <constant>V4L2_COLORFX_ANTIQUE</constant>,
+	  <constant>V4L2_COLORFX_ART_FREEZE</constant>,
+	  <constant>V4L2_COLORFX_AQUA</constant>,
+	  <constant>V4L2_COLORFX_SILHOUETTE</constant>,
+	  <constant>V4L2_COLORFX_SOLARIZATION</constant>,
+	  <constant>V4L2_COLORFX_VIVID</constant> and
+	  <constant>V4L2_COLORFX_ARBITRARY_CBCR</constant> menu items
+	  to the <constant>V4L2_CID_COLORFX</constant> control.</para>
+        </listitem>
+        <listitem>
+	  <para> Added <constant>V4L2_CID_COLORFX_CBCR</constant> control.</para>
+        </listitem>
+        <listitem>
+	  <para> Added camera controls <constant>V4L2_CID_AUTO_EXPOSURE_BIAS</constant>,
+	  <constant>V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE</constant>,
+	  <constant>V4L2_CID_IMAGE_STABILIZATION</constant>,
+	  <constant>V4L2_CID_ISO_SENSITIVITY</constant>,
+	  <constant>V4L2_CID_ISO_SENSITIVITY_AUTO</constant>,
+	  <constant>V4L2_CID_EXPOSURE_METERING</constant>,
+	  <constant>V4L2_CID_SCENE_MODE</constant>,
+	  <constant>V4L2_CID_3A_LOCK</constant>,
+	  <constant>V4L2_CID_AUTO_FOCUS_START</constant>,
+	  <constant>V4L2_CID_AUTO_FOCUS_STOP</constant>,
+	  <constant>V4L2_CID_AUTO_FOCUS_STATUS</constant> and
+	  <constant>V4L2_CID_AUTO_FOCUS_RANGE</constant>.
+	  </para>
+        </listitem>
       </orderedlist>
     </section>
 
@@ -2508,6 +2556,10 @@ and may change in the future.</para>
 ioctls.</para>
         </listitem>
         <listitem>
+	  <para>&VIDIOC-DECODER-CMD; and &VIDIOC-TRY-DECODER-CMD;
+ioctls.</para>
+        </listitem>
+        <listitem>
 	  <para>&VIDIOC-DBG-G-REGISTER; and &VIDIOC-DBG-S-REGISTER;
 ioctls.</para>
         </listitem>
@@ -2515,6 +2567,10 @@ ioctls.</para>
 	  <para>&VIDIOC-DBG-G-CHIP-IDENT; ioctl.</para>
         </listitem>
         <listitem>
+	  <para>&VIDIOC-ENUM-DV-TIMINGS;, &VIDIOC-QUERY-DV-TIMINGS; and
+	  &VIDIOC-DV-TIMINGS-CAP; ioctls.</para>
+        </listitem>
+        <listitem>
 	  <para>Flash API. <xref linkend="flash-controls" /></para>
         </listitem>
         <listitem>
@@ -2523,6 +2579,14 @@ ioctls.</para>
         <listitem>
 	  <para>Selection API. <xref linkend="selection-api" /></para>
         </listitem>
+        <listitem>
+	  <para>Sub-device selection API: &VIDIOC-SUBDEV-G-SELECTION;
+	  and &VIDIOC-SUBDEV-S-SELECTION; ioctls.</para>
+        </listitem>
+        <listitem>
+	  <para><link linkend="v4l2-auto-focus-area"><constant>
+	  V4L2_CID_AUTO_FOCUS_AREA</constant></link> control.</para>
+        </listitem>
       </itemizedlist>
     </section>
 
@@ -2538,6 +2602,17 @@ interfaces and should not be implemented in new drivers.</para>
 <constant>VIDIOC_S_MPEGCOMP</constant> ioctls. Use Extended Controls,
 <xref linkend="extended-controls" />.</para>
         </listitem>
+        <listitem>
+	  <para>&VIDIOC-G-DV-PRESET;, &VIDIOC-S-DV-PRESET;, &VIDIOC-ENUM-DV-PRESETS; and
+	  &VIDIOC-QUERY-DV-PRESET; ioctls. Use the DV Timings API (<xref linkend="dv-timings" />).</para>
+        </listitem>
+        <listitem>
+	  <para><constant>VIDIOC_SUBDEV_G_CROP</constant> and
+	  <constant>VIDIOC_SUBDEV_S_CROP</constant> ioctls. Use
+	  <constant>VIDIOC_SUBDEV_G_SELECTION</constant> and
+	  <constant>VIDIOC_SUBDEV_S_SELECTION</constant>, <xref
+	  linkend="vidioc-subdev-g-selection" />.</para>
+        </listitem>
       </itemizedlist>
     </section>
   </section>
diff --git a/Documentation/DocBook/media/v4l/controls.xml b/Documentation/DocBook/media/v4l/controls.xml
index dd03cf4a6539..676bc46f9c52 100644
--- a/Documentation/DocBook/media/v4l/controls.xml
+++ b/Documentation/DocBook/media/v4l/controls.xml
@@ -285,18 +285,92 @@ minimum value disables backlight compensation.</entry>
 	  <row id="v4l2-colorfx">
 	    <entry><constant>V4L2_CID_COLORFX</constant></entry>
 	    <entry>enum</entry>
-	    <entry>Selects a color effect. Possible values for
-<constant>enum v4l2_colorfx</constant> are:
-<constant>V4L2_COLORFX_NONE</constant> (0),
-<constant>V4L2_COLORFX_BW</constant> (1),
-<constant>V4L2_COLORFX_SEPIA</constant> (2),
-<constant>V4L2_COLORFX_NEGATIVE</constant> (3),
-<constant>V4L2_COLORFX_EMBOSS</constant> (4),
-<constant>V4L2_COLORFX_SKETCH</constant> (5),
-<constant>V4L2_COLORFX_SKY_BLUE</constant> (6),
-<constant>V4L2_COLORFX_GRASS_GREEN</constant> (7),
-<constant>V4L2_COLORFX_SKIN_WHITEN</constant> (8) and
-<constant>V4L2_COLORFX_VIVID</constant> (9).</entry>
+	    <entry>Selects a color effect. The following values are defined:
+	    </entry>
+	  </row><row>
+	  <entry></entry>
+	  <entry></entry>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_COLORFX_NONE</constant>&nbsp;</entry>
+		  <entry>Color effect is disabled.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_ANTIQUE</constant>&nbsp;</entry>
+		  <entry>An aging (old photo) effect.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_ART_FREEZE</constant>&nbsp;</entry>
+		  <entry>Frost color effect.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_AQUA</constant>&nbsp;</entry>
+		  <entry>Water color, cool tone.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_BW</constant>&nbsp;</entry>
+		  <entry>Black and white.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_EMBOSS</constant>&nbsp;</entry>
+		  <entry>Emboss, the highlights and shadows replace light/dark boundaries
+		  and low contrast areas are set to a gray background.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_GRASS_GREEN</constant>&nbsp;</entry>
+		  <entry>Grass green.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_NEGATIVE</constant>&nbsp;</entry>
+		  <entry>Negative.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_SEPIA</constant>&nbsp;</entry>
+		  <entry>Sepia tone.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_SKETCH</constant>&nbsp;</entry>
+		  <entry>Sketch.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_SKIN_WHITEN</constant>&nbsp;</entry>
+		  <entry>Skin whiten.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_SKY_BLUE</constant>&nbsp;</entry>
+		  <entry>Sky blue.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_SOLARIZATION</constant>&nbsp;</entry>
+		  <entry>Solarization, the image is partially reversed in tone,
+		  only color values above or below a certain threshold are inverted.
+		  </entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_SILHOUETTE</constant>&nbsp;</entry>
+		  <entry>Silhouette (outline).</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_VIVID</constant>&nbsp;</entry>
+		  <entry>Vivid colors.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_COLORFX_SET_CBCR</constant>&nbsp;</entry>
+		  <entry>The Cb and Cr chroma components are replaced by fixed
+		  coefficients determined by <constant>V4L2_CID_COLORFX_CBCR</constant>
+		  control.</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CID_COLORFX_CBCR</constant></entry>
+	    <entry>integer</entry>
+	    <entry>Determines the Cb and Cr coefficients for <constant>V4L2_COLORFX_SET_CBCR</constant>
+	    color effect. Bits [7:0] of the supplied 32 bit value are interpreted as
+	    Cr component, bits [15:8] as Cb component and bits [31:16] must be zero.
+	  </entry>
 	  </row>
 	  <row>
 	    <entry><constant>V4L2_CID_ROTATE</constant></entry>
@@ -2775,6 +2849,51 @@ remain constant.</entry>
 	  <row><entry></entry></row>
 
 	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_EXPOSURE_BIAS</constant>&nbsp;</entry>
+	    <entry>integer menu</entry>
+	  </row><row><entry spanname="descr"> Determines the automatic
+exposure compensation, it is effective only when <constant>V4L2_CID_EXPOSURE_AUTO</constant>
+control is set to <constant>AUTO</constant>, <constant>SHUTTER_PRIORITY </constant>
+or <constant>APERTURE_PRIORITY</constant>.
+It is expressed in terms of EV, drivers should interpret the values as 0.001 EV
+units, where the value 1000 stands for +1 EV.
+<para>Increasing the exposure compensation value is equivalent to decreasing
+the exposure value (EV) and will increase the amount of light at the image
+sensor. The camera performs the exposure compensation by adjusting absolute
+exposure time and/or aperture.</para></entry>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row id="v4l2-exposure-metering">
+	    <entry spanname="id"><constant>V4L2_CID_EXPOSURE_METERING</constant>&nbsp;</entry>
+	    <entry>enum&nbsp;v4l2_exposure_metering</entry>
+	  </row><row><entry spanname="descr">Determines how the camera measures
+the amount of light available for the frame exposure. Possible values are:</entry>
+	  </row>
+	  <row>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_EXPOSURE_METERING_AVERAGE</constant>&nbsp;</entry>
+		  <entry>Use the light information coming from the entire frame
+and average giving no weighting to any particular portion of the metered area.
+		  </entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_EXPOSURE_METERING_CENTER_WEIGHTED</constant>&nbsp;</entry>
+		  <entry>Average the light information coming from the entire frame
+giving priority to the center of the metered area.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_EXPOSURE_METERING_SPOT</constant>&nbsp;</entry>
+		  <entry>Measure only very small area at the center of the frame.</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row>
 	    <entry spanname="id"><constant>V4L2_CID_PAN_RELATIVE</constant>&nbsp;</entry>
 	    <entry>integer</entry>
 	  </row><row><entry spanname="descr">This control turns the
@@ -2857,13 +2976,107 @@ negative values towards infinity. This is a write-only control.</entry>
 	  <row>
 	    <entry spanname="id"><constant>V4L2_CID_FOCUS_AUTO</constant>&nbsp;</entry>
 	    <entry>boolean</entry>
-	  </row><row><entry spanname="descr">Enables automatic focus
-adjustments. The effect of manual focus adjustments while this feature
+	  </row><row><entry spanname="descr">Enables continuous automatic
+focus adjustments. The effect of manual focus adjustments while this feature
 is enabled is undefined, drivers should ignore such requests.</entry>
 	  </row>
 	  <row><entry></entry></row>
 
 	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_AUTO_FOCUS_START</constant>&nbsp;</entry>
+	    <entry>button</entry>
+	  </row><row><entry spanname="descr">Starts single auto focus process.
+The effect of setting this control when <constant>V4L2_CID_FOCUS_AUTO</constant>
+is set to <constant>TRUE</constant> (1) is undefined, drivers should ignore
+such requests.</entry>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_AUTO_FOCUS_STOP</constant>&nbsp;</entry>
+	    <entry>button</entry>
+	  </row><row><entry spanname="descr">Aborts automatic focusing
+started with <constant>V4L2_CID_AUTO_FOCUS_START</constant> control. It is
+effective only when the continuous autofocus is disabled, that is when
+<constant>V4L2_CID_FOCUS_AUTO</constant> control is set to <constant>FALSE
+</constant> (0).</entry>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row id="v4l2-auto-focus-status">
+	    <entry spanname="id">
+	      <constant>V4L2_CID_AUTO_FOCUS_STATUS</constant>&nbsp;</entry>
+	    <entry>bitmask</entry>
+	  </row>
+	  <row><entry spanname="descr">The automatic focus status. This is a read-only
+	  control.</entry>
+	  </row>
+	  <row>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_STATUS_IDLE</constant>&nbsp;</entry>
+		  <entry>Automatic focus is not active.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_STATUS_BUSY</constant>&nbsp;</entry>
+		  <entry>Automatic focusing is in progress.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_STATUS_REACHED</constant>&nbsp;</entry>
+		  <entry>Focus has been reached.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_STATUS_FAILED</constant>&nbsp;</entry>
+		  <entry>Automatic focus has failed, the driver will not
+		  transition from this state until another action is
+		  performed by an application.</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row><entry spanname="descr">
+Setting <constant>V4L2_LOCK_FOCUS</constant> lock bit of the <constant>V4L2_CID_3A_LOCK
+</constant> control may stop updates of the <constant>V4L2_CID_AUTO_FOCUS_STATUS</constant>
+control value.</entry>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row id="v4l2-auto-focus-range">
+	    <entry spanname="id">
+	      <constant>V4L2_CID_AUTO_FOCUS_RANGE</constant>&nbsp;</entry>
+	    <entry>enum&nbsp;v4l2_auto_focus_range</entry>
+	  </row>
+	  <row><entry spanname="descr">Determines auto focus distance range
+for which lens may be adjusted. </entry>
+	  </row>
+	  <row>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_RANGE_AUTO</constant>&nbsp;</entry>
+		  <entry>The camera automatically selects the focus range.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_RANGE_NORMAL</constant>&nbsp;</entry>
+		  <entry>Normal distance range, limited for best automatic focus
+performance.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_RANGE_MACRO</constant>&nbsp;</entry>
+		  <entry>Macro (close-up) auto focus. The camera will
+use its minimum possible distance for auto focus.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_AUTO_FOCUS_RANGE_INFINITY</constant>&nbsp;</entry>
+		  <entry>The lens is set to focus on an object at infinite distance.</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row>
 	    <entry spanname="id"><constant>V4L2_CID_ZOOM_ABSOLUTE</constant>&nbsp;</entry>
 	    <entry>integer</entry>
 	  </row><row><entry spanname="descr">Specify the objective lens
@@ -2932,6 +3145,295 @@ camera sensor on or off, or specify its strength. Such band-stop filters can
 be used, for example, to filter out the fluorescent light component.</entry>
 	  </row>
 	  <row><entry></entry></row>
+
+	  <row id="v4l2-auto-n-preset-white-balance">
+	    <entry spanname="id"><constant>V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE</constant>&nbsp;</entry>
+	    <entry>enum&nbsp;v4l2_auto_n_preset_white_balance</entry>
+	  </row><row><entry spanname="descr">Sets white balance to automatic,
+manual or a preset. The presets determine color temperature of the light as
+a hint to the camera for white balance adjustments resulting in most accurate
+color representation. The following white balance presets are listed in order
+of increasing color temperature.</entry>
+	  </row>
+	  <row>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_MANUAL</constant>&nbsp;</entry>
+		  <entry>Manual white balance.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_AUTO</constant>&nbsp;</entry>
+		  <entry>Automatic white balance adjustments.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_INCANDESCENT</constant>&nbsp;</entry>
+		  <entry>White balance setting for incandescent (tungsten) lighting.
+It generally cools down the colors and corresponds approximately to 2500...3500 K
+color temperature range.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_FLUORESCENT</constant>&nbsp;</entry>
+		  <entry>White balance preset for fluorescent lighting.
+It corresponds approximately to 4000...5000 K color temperature.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_FLUORESCENT_H</constant>&nbsp;</entry>
+		  <entry>With this setting the camera will compensate for
+fluorescent H lighting.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_HORIZON</constant>&nbsp;</entry>
+		  <entry>White balance setting for horizon daylight.
+It corresponds approximately to 5000 K color temperature.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_DAYLIGHT</constant>&nbsp;</entry>
+		  <entry>White balance preset for daylight (with clear sky).
+It corresponds approximately to 5000...6500 K color temperature.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_FLASH</constant>&nbsp;</entry>
+		  <entry>With this setting the camera will compensate for the flash
+light. It slightly warms up the colors and corresponds roughly to 5000...5500 K
+color temperature.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_CLOUDY</constant>&nbsp;</entry>
+		  <entry>White balance preset for moderately overcast sky.
+This option corresponds approximately to 6500...8000 K color temperature
+range.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_WHITE_BALANCE_SHADE</constant>&nbsp;</entry>
+		  <entry>White balance preset for shade or heavily overcast
+sky. It corresponds approximately to 9000...10000 K color temperature.
+</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row id="v4l2-wide-dynamic-range">
+	    <entry spanname="id"><constant>V4L2_CID_WIDE_DYNAMIC_RANGE</constant></entry>
+	    <entry>boolean</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">Enables or disables the camera's wide dynamic
+range feature. This feature allows to obtain clear images in situations where
+intensity of the illumination varies significantly throughout the scene, i.e.
+there are simultaneously very dark and very bright areas. It is most commonly
+realized in cameras by combining two subsequent frames with different exposure
+times. <footnote id="ctypeconv"><para> This control may be changed to a menu
+control in the future, if more options are required.</para></footnote></entry>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row id="v4l2-image-stabilization">
+	    <entry spanname="id"><constant>V4L2_CID_IMAGE_STABILIZATION</constant></entry>
+	    <entry>boolean</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">Enables or disables image stabilization.
+	      <footnoteref linkend="ctypeconv"/></entry>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_ISO_SENSITIVITY</constant>&nbsp;</entry>
+	    <entry>integer menu</entry>
+	  </row><row><entry spanname="descr">Determines ISO equivalent of an
+image sensor indicating the sensor's sensitivity to light. The numbers are
+expressed in arithmetic scale, as per <xref linkend="iso12232" /> standard,
+where doubling the sensor sensitivity is represented by doubling the numerical
+ISO value. Applications should interpret the values as standard ISO values
+multiplied by 1000, e.g. control value 800 stands for ISO 0.8. Drivers will
+usually support only a subset of standard ISO values. The effect of setting
+this control while the <constant>V4L2_CID_ISO_SENSITIVITY_AUTO</constant>
+control is set to a value other than <constant>V4L2_CID_ISO_SENSITIVITY_MANUAL
+</constant> is undefined, drivers should ignore such requests.</entry>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row id="v4l2-iso-sensitivity-auto-type">
+	    <entry spanname="id"><constant>V4L2_CID_ISO_SENSITIVITY_AUTO</constant>&nbsp;</entry>
+	    <entry>enum&nbsp;v4l2_iso_sensitivity_type</entry>
+	  </row><row><entry spanname="descr">Enables or disables automatic ISO
+sensitivity adjustments.</entry>
+	  </row>
+	  <row>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_CID_ISO_SENSITIVITY_MANUAL</constant>&nbsp;</entry>
+		  <entry>Manual ISO sensitivity.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_CID_ISO_SENSITIVITY_AUTO</constant>&nbsp;</entry>
+		  <entry>Automatic ISO sensitivity adjustments.</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row id="v4l2-scene-mode">
+	    <entry spanname="id"><constant>V4L2_CID_SCENE_MODE</constant>&nbsp;</entry>
+	    <entry>enum&nbsp;v4l2_scene_mode</entry>
+	  </row><row><entry spanname="descr">This control allows to select
+scene programs as the camera automatic modes optimized for common shooting
+scenes. Within these modes the camera determines best exposure, aperture,
+focusing, light metering, white balance and equivalent sensitivity. The
+controls of those parameters are influenced by the scene mode control.
+An exact behavior in each mode is subject to the camera specification.
+
+<para>When the scene mode feature is not used, this control should be set to
+<constant>V4L2_SCENE_MODE_NONE</constant> to make sure the other possibly
+related controls are accessible. The following scene programs are defined:
+</para>
+</entry>
+	  </row>
+	  <row>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_NONE</constant>&nbsp;</entry>
+		  <entry>The scene mode feature is disabled.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_BACKLIGHT</constant>&nbsp;</entry>
+		  <entry>Backlight. Compensates for dark shadows when light is
+		  coming from behind a subject, also by automatically turning
+		  on the flash.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_BEACH_SNOW</constant>&nbsp;</entry>
+		  <entry>Beach and snow. This mode compensates for all-white or
+bright scenes, which tend to look gray and low contrast, when camera's automatic
+exposure is based on an average scene brightness. To compensate, this mode
+automatically slightly overexposes the frames. The white balance may also be
+adjusted to compensate for the fact that reflected snow looks bluish rather
+than white.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_CANDLELIGHT</constant>&nbsp;</entry>
+		  <entry>Candle light. The camera generally raises the ISO
+sensitivity and lowers the shutter speed. This mode compensates for relatively
+close subject in the scene. The flash is disabled in order to preserve the
+ambiance of the light.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_DAWN_DUSK</constant>&nbsp;</entry>
+		  <entry>Dawn and dusk. Preserves the colors seen in low
+natural light before dusk and after down. The camera may turn off the flash,
+and automatically focus at infinity. It will usually boost saturation and
+lower the shutter speed.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_FALL_COLORS</constant>&nbsp;</entry>
+		  <entry>Fall colors. Increases saturation and adjusts white
+balance for color enhancement. Pictures of autumn leaves get saturated reds
+and yellows.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_FIREWORKS</constant>&nbsp;</entry>
+		  <entry>Fireworks. Long exposure times are used to capture
+the expanding burst of light from a firework. The camera may invoke image
+stabilization.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_LANDSCAPE</constant>&nbsp;</entry>
+		  <entry>Landscape. The camera may choose a small aperture to
+provide deep depth of field and long exposure duration to help capture detail
+in dim light conditions. The focus is fixed at infinity. Suitable for distant
+and wide scenery.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_NIGHT</constant>&nbsp;</entry>
+		  <entry>Night, also known as Night Landscape. Designed for low
+light conditions, it preserves detail in the dark areas without blowing out bright
+objects. The camera generally sets itself to a medium-to-high ISO sensitivity,
+with a relatively long exposure time, and turns flash off. As such, there will be
+increased image noise and the possibility of blurred image.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_PARTY_INDOOR</constant>&nbsp;</entry>
+		  <entry>Party and indoor. Designed to capture indoor scenes
+that are lit by indoor background lighting as well as the flash. The camera
+usually increases ISO sensitivity, and adjusts exposure for the low light
+conditions.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_PORTRAIT</constant>&nbsp;</entry>
+		  <entry>Portrait. The camera adjusts the aperture so that the
+depth of field is reduced, which helps to isolate the subject against a smooth
+background. Most cameras recognize the presence of faces in the scene and focus
+on them. The color hue is adjusted to enhance skin tones. The intensity of the
+flash is often reduced.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_SPORTS</constant>&nbsp;</entry>
+		  <entry>Sports. Significantly increases ISO and uses a fast
+shutter speed to freeze motion of rapidly-moving subjects. Increased image
+noise may be seen in this mode.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_SUNSET</constant>&nbsp;</entry>
+		  <entry>Sunset. Preserves deep hues seen in sunsets and
+sunrises. It bumps up the saturation.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_SCENE_MODE_TEXT</constant>&nbsp;</entry>
+		  <entry>Text. It applies extra contrast and sharpness, it is
+typically a black-and-white mode optimized for readability. Automatic focus
+may be switched to close-up mode and this setting may also involve some
+lens-distortion correction.</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row><entry></entry></row>
+
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_3A_LOCK</constant></entry>
+	    <entry>bitmask</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">This control locks or unlocks the automatic
+focus, exposure and white balance. The automatic adjustments can be paused
+independently by setting the corresponding lock bit to 1. The camera then retains
+the settings until the lock bit is cleared. The following lock bits are defined:
+</entry>
+	  </row>
+	  <row>
+	    <entrytbl spanname="descr" cols="2">
+	      <tbody valign="top">
+		<row>
+		  <entry><constant>V4L2_LOCK_EXPOSURE</constant></entry>
+		  <entry>Automatic exposure adjustments lock.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_LOCK_WHITE_BALANCE</constant></entry>
+		  <entry>Automatic white balance adjustments lock.</entry>
+		</row>
+		<row>
+		  <entry><constant>V4L2_LOCK_FOCUS</constant></entry>
+		  <entry>Automatic focus lock.</entry>
+		</row>
+	      </tbody>
+	    </entrytbl>
+	  </row>
+	  <row><entry spanname="descr">
+When a given algorithm is not enabled, drivers should ignore requests
+to lock it and should return no error. An example might be an application
+setting bit <constant>V4L2_LOCK_WHITE_BALANCE</constant> when the
+<constant>V4L2_CID_AUTO_WHITE_BALANCE</constant> control is set to
+<constant>FALSE</constant>. The value of this control may be changed
+by exposure, white balance or focus controls.</entry>
+	  </row>
+	  <row><entry></entry></row>
+
 	</tbody>
       </tgroup>
     </table>
@@ -3476,7 +3978,7 @@ interface and may change in the future.</para>
 	    <entry spanname="id"><constant>V4L2_CID_JPEG_CHROMA_SUBSAMPLING</constant></entry>
 	    <entry>menu</entry>
 	  </row>
-	  <row id="jpeg-chroma-subsampling-control">
+	  <row id="v4l2-jpeg-chroma-subsampling">
 	    <entry spanname="descr">The chroma subsampling factors describe how
 	    each component of an input image is sampled, in respect to maximum
 	    sample rate in each spatial dimension. See <xref linkend="itu-t81"/>,
@@ -3486,7 +3988,7 @@ interface and may change in the future.</para>
 	    from RGB to Y'CbCr color space.
 	    </entry>
 	  </row>
-	  <row>
+	  <row id = "v4l2-jpeg-chroma-subsampling">
 	    <entrytbl spanname="descr" cols="2">
 	      <tbody valign="top">
 		<row>
@@ -3538,12 +4040,12 @@ interface and may change in the future.</para>
 	    </entry>
 	  </row>
 	  <row id="jpeg-quality-control">
-	    <entry spanname="id"><constant>V4L2_CID_JPEG_COMPRESION_QUALITY</constant></entry>
+	    <entry spanname="id"><constant>V4L2_CID_JPEG_COMPRESSION_QUALITY</constant></entry>
 	    <entry>integer</entry>
 	  </row>
 	  <row>
 	    <entry spanname="descr">
-	      <constant>V4L2_CID_JPEG_COMPRESION_QUALITY</constant> control
+	      <constant>V4L2_CID_JPEG_COMPRESSION_QUALITY</constant> control
 	      determines trade-off between image quality and size.
 	      It provides simpler method for applications to control image quality,
 	      without a need for direct reconfiguration of luminance and chrominance
@@ -3551,7 +4053,7 @@ interface and may change in the future.</para>
 
 	      In cases where a driver uses quantization tables configured directly
 	      by an application, using interfaces defined elsewhere, <constant>
-	      V4L2_CID_JPEG_COMPRESION_QUALITY</constant> control should be set
+	      V4L2_CID_JPEG_COMPRESSION_QUALITY</constant> control should be set
 	      by driver to 0.
 
 	      <para>The value range of this control is driver-specific. Only
@@ -3599,4 +4101,172 @@ interface and may change in the future.</para>
       to <xref linkend="itu-t81"/>, <xref linkend="jfif"/>,
       <xref linkend="w3c-jpeg-jfif"/>.</para>
     </section>
+
+    <section id="image-source-controls">
+      <title>Image Source Control Reference</title>
+
+      <note>
+	<title>Experimental</title>
+
+	<para>This is an <link
+	linkend="experimental">experimental</link> interface and may
+	change in the future.</para>
+      </note>
+
+      <para>
+	The Image Source control class is intended for low-level
+	control of image source devices such as image sensors. The
+	devices feature an analogue to digital converter and a bus
+	transmitter to transmit the image data out of the device.
+      </para>
+
+      <table pgwide="1" frame="none" id="image-source-control-id">
+      <title>Image Source Control IDs</title>
+
+      <tgroup cols="4">
+	<colspec colname="c1" colwidth="1*" />
+	<colspec colname="c2" colwidth="6*" />
+	<colspec colname="c3" colwidth="2*" />
+	<colspec colname="c4" colwidth="6*" />
+	<spanspec namest="c1" nameend="c2" spanname="id" />
+	<spanspec namest="c2" nameend="c4" spanname="descr" />
+	<thead>
+	  <row>
+	    <entry spanname="id" align="left">ID</entry>
+	    <entry align="left">Type</entry>
+	  </row><row rowsep="1"><entry spanname="descr" align="left">Description</entry>
+	  </row>
+	</thead>
+	<tbody valign="top">
+	  <row><entry></entry></row>
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_IMAGE_SOURCE_CLASS</constant></entry>
+	    <entry>class</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">The IMAGE_SOURCE class descriptor.</entry>
+	  </row>
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_VBLANK</constant></entry>
+	    <entry>integer</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">Vertical blanking. The idle period
+	    after every frame during which no image data is produced.
+	    The unit of vertical blanking is a line. Every line has
+	    length of the image width plus horizontal blanking at the
+	    pixel rate defined by
+	    <constant>V4L2_CID_PIXEL_RATE</constant> control in the
+	    same sub-device.</entry>
+	  </row>
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_HBLANK</constant></entry>
+	    <entry>integer</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">Horizontal blanking. The idle
+	    period after every line of image data during which no
+	    image data is produced. The unit of horizontal blanking is
+	    pixels.</entry>
+	  </row>
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_ANALOGUE_GAIN</constant></entry>
+	    <entry>integer</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">Analogue gain is gain affecting
+	    all colour components in the pixel matrix. The gain
+	    operation is performed in the analogue domain before A/D
+	    conversion.
+	    </entry>
+	  </row>
+	  <row><entry></entry></row>
+	</tbody>
+      </tgroup>
+      </table>
+
+    </section>
+
+    <section id="image-process-controls">
+      <title>Image Process Control Reference</title>
+
+      <note>
+	<title>Experimental</title>
+
+	<para>This is an <link
+	linkend="experimental">experimental</link> interface and may
+	change in the future.</para>
+      </note>
+
+      <para>
+	The Image Source control class is intended for low-level control of
+	image processing functions. Unlike
+	<constant>V4L2_CID_IMAGE_SOURCE_CLASS</constant>, the controls in
+	this class affect processing the image, and do not control capturing
+	of it.
+      </para>
+
+      <table pgwide="1" frame="none" id="image-process-control-id">
+      <title>Image Source Control IDs</title>
+
+      <tgroup cols="4">
+	<colspec colname="c1" colwidth="1*" />
+	<colspec colname="c2" colwidth="6*" />
+	<colspec colname="c3" colwidth="2*" />
+	<colspec colname="c4" colwidth="6*" />
+	<spanspec namest="c1" nameend="c2" spanname="id" />
+	<spanspec namest="c2" nameend="c4" spanname="descr" />
+	<thead>
+	  <row>
+	    <entry spanname="id" align="left">ID</entry>
+	    <entry align="left">Type</entry>
+	  </row><row rowsep="1"><entry spanname="descr" align="left">Description</entry>
+	  </row>
+	</thead>
+	<tbody valign="top">
+	  <row><entry></entry></row>
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_IMAGE_PROC_CLASS</constant></entry>
+	    <entry>class</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">The IMAGE_PROC class descriptor.</entry>
+	  </row>
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_LINK_FREQ</constant></entry>
+	    <entry>integer menu</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">Data bus frequency. Together with the
+	    media bus pixel code, bus type (clock cycles per sample), the
+	    data bus frequency defines the pixel rate
+	    (<constant>V4L2_CID_PIXEL_RATE</constant>) in the
+	    pixel array (or possibly elsewhere, if the device is not an
+	    image sensor). The frame rate can be calculated from the pixel
+	    clock, image width and height and horizontal and vertical
+	    blanking. While the pixel rate control may be defined elsewhere
+	    than in the subdev containing the pixel array, the frame rate
+	    cannot be obtained from that information. This is because only
+	    on the pixel array it can be assumed that the vertical and
+	    horizontal blanking information is exact: no other blanking is
+	    allowed in the pixel array. The selection of frame rate is
+	    performed by selecting the desired horizontal and vertical
+	    blanking. The unit of this control is Hz. </entry>
+	  </row>
+	  <row>
+	    <entry spanname="id"><constant>V4L2_CID_PIXEL_RATE</constant></entry>
+	    <entry>64-bit integer</entry>
+	  </row>
+	  <row>
+	    <entry spanname="descr">Pixel rate in the source pads of
+	    the subdev. This control is read-only and its unit is
+	    pixels / second.
+	    </entry>
+	  </row>
+	  <row><entry></entry></row>
+	</tbody>
+      </tgroup>
+      </table>
+
+    </section>
 </section>
diff --git a/Documentation/DocBook/media/v4l/dev-subdev.xml b/Documentation/DocBook/media/v4l/dev-subdev.xml
index 0916a7343a16..4afcbbec5eda 100644
--- a/Documentation/DocBook/media/v4l/dev-subdev.xml
+++ b/Documentation/DocBook/media/v4l/dev-subdev.xml
@@ -76,11 +76,12 @@
     <wordasword>format</wordasword> means the combination of media bus data
     format, frame width and frame height.</para></note>
 
-    <para>Image formats are typically negotiated on video capture and output
-    devices using the <link linkend="crop">cropping and scaling</link> ioctls.
-    The driver is responsible for configuring every block in the video pipeline
-    according to the requested format at the pipeline input and/or
-    output.</para>
+    <para>Image formats are typically negotiated on video capture and
+    output devices using the format and <link
+    linkend="vidioc-subdev-g-selection">selection</link> ioctls. The
+    driver is responsible for configuring every block in the video
+    pipeline according to the requested format at the pipeline input
+    and/or output.</para>
 
     <para>For complex devices, such as often found in embedded systems,
     identical image sizes at the output of a pipeline can be achieved using
@@ -276,11 +277,11 @@
     </section>
 
     <section>
-      <title>Cropping and scaling</title>
+      <title>Selections: cropping, scaling and composition</title>
 
       <para>Many sub-devices support cropping frames on their input or output
       pads (or possible even on both). Cropping is used to select the area of
-      interest in an image, typically on a video sensor or video decoder. It can
+      interest in an image, typically on an image sensor or a video decoder. It can
       also be used as part of digital zoom implementations to select the area of
       the image that will be scaled up.</para>
 
@@ -288,26 +289,179 @@
       &v4l2-rect; by the coordinates of the top left corner and the rectangle
       size. Both the coordinates and sizes are expressed in pixels.</para>
 
-      <para>The crop rectangle is retrieved and set using the
-      &VIDIOC-SUBDEV-G-CROP; and &VIDIOC-SUBDEV-S-CROP; ioctls. Like for pad
-      formats, drivers store try and active crop rectangles. The format
-      negotiation mechanism applies to crop settings as well.</para>
-
-      <para>On input pads, cropping is applied relatively to the current pad
-      format. The pad format represents the image size as received by the
-      sub-device from the previous block in the pipeline, and the crop rectangle
-      represents the sub-image that will be transmitted further inside the
-      sub-device for processing. The crop rectangle be entirely containted
-      inside the input image size.</para>
-
-      <para>Input crop rectangle are reset to their default value when the input
-      image format is modified. Drivers should use the input image size as the
-      crop rectangle default value, but hardware requirements may prevent this.
-      </para>
+      <para>As for pad formats, drivers store try and active
+      rectangles for the selection targets of ACTUAL type <xref
+      linkend="v4l2-subdev-selection-targets">.</xref></para>
+
+      <para>On sink pads, cropping is applied relative to the
+      current pad format. The pad format represents the image size as
+      received by the sub-device from the previous block in the
+      pipeline, and the crop rectangle represents the sub-image that
+      will be transmitted further inside the sub-device for
+      processing.</para>
+
+      <para>The scaling operation changes the size of the image by
+      scaling it to new dimensions. The scaling ratio isn't specified
+      explicitly, but is implied from the original and scaled image
+      sizes. Both sizes are represented by &v4l2-rect;.</para>
+
+      <para>Scaling support is optional. When supported by a subdev,
+      the crop rectangle on the subdev's sink pad is scaled to the
+      size configured using the &VIDIOC-SUBDEV-S-SELECTION; IOCTL
+      using <constant>V4L2_SUBDEV_SEL_COMPOSE_ACTUAL</constant>
+      selection target on the same pad. If the subdev supports scaling
+      but not composing, the top and left values are not used and must
+      always be set to zero.</para>
+
+      <para>On source pads, cropping is similar to sink pads, with the
+      exception that the source size from which the cropping is
+      performed, is the COMPOSE rectangle on the sink pad. In both
+      sink and source pads, the crop rectangle must be entirely
+      contained inside the source image size for the crop
+      operation.</para>
+
+      <para>The drivers should always use the closest possible
+      rectangle the user requests on all selection targets, unless
+      specifically told otherwise.
+      <constant>V4L2_SUBDEV_SEL_FLAG_SIZE_GE</constant> and
+      <constant>V4L2_SUBDEV_SEL_FLAG_SIZE_LE</constant> flags may be
+      used to round the image size either up or down. <xref
+      linkend="v4l2-subdev-selection-flags"></xref></para>
+    </section>
+
+    <section>
+      <title>Types of selection targets</title>
+
+      <section>
+	<title>ACTUAL targets</title>
+
+	<para>ACTUAL targets reflect the actual hardware configuration
+	at any point of time. There is a BOUNDS target
+	corresponding to every ACTUAL.</para>
+      </section>
+
+      <section>
+	<title>BOUNDS targets</title>
+
+	<para>BOUNDS targets is the smallest rectangle that contains
+	all valid ACTUAL rectangles. It may not be possible to set the
+	ACTUAL rectangle as large as the BOUNDS rectangle, however.
+	This may be because e.g. a sensor's pixel array is not
+	rectangular but cross-shaped or round. The maximum size may
+	also be smaller than the BOUNDS rectangle.</para>
+      </section>
 
-      <para>Cropping behaviour on output pads is not defined.</para>
+    </section>
+
+    <section>
+      <title>Order of configuration and format propagation</title>
+
+      <para>Inside subdevs, the order of image processing steps will
+      always be from the sink pad towards the source pad. This is also
+      reflected in the order in which the configuration must be
+      performed by the user: the changes made will be propagated to
+      any subsequent stages. If this behaviour is not desired, the
+      user must set
+      <constant>V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG</constant> flag. This
+      flag causes no propagation of the changes are allowed in any
+      circumstances. This may also cause the accessed rectangle to be
+      adjusted by the driver, depending on the properties of the
+      underlying hardware.</para>
+
+      <para>The coordinates to a step always refer to the actual size
+      of the previous step. The exception to this rule is the source
+      compose rectangle, which refers to the sink compose bounds
+      rectangle --- if it is supported by the hardware.</para>
+
+      <orderedlist>
+	<listitem>Sink pad format. The user configures the sink pad
+	format. This format defines the parameters of the image the
+	entity receives through the pad for further processing.</listitem>
+
+	<listitem>Sink pad actual crop selection. The sink pad crop
+	defines the crop performed to the sink pad format.</listitem>
+
+	<listitem>Sink pad actual compose selection. The size of the
+	sink pad compose rectangle defines the scaling ratio compared
+	to the size of the sink pad crop rectangle. The location of
+	the compose rectangle specifies the location of the actual
+	sink compose rectangle in the sink compose bounds
+	rectangle.</listitem>
+
+	<listitem>Source pad actual crop selection. Crop on the source
+	pad defines crop performed to the image in the sink compose
+	bounds rectangle.</listitem>
+
+	<listitem>Source pad format. The source pad format defines the
+	output pixel format of the subdev, as well as the other
+	parameters with the exception of the image width and height.
+	Width and height are defined by the size of the source pad
+	actual crop selection.</listitem>
+      </orderedlist>
+
+      <para>Accessing any of the above rectangles not supported by the
+      subdev will return <constant>EINVAL</constant>. Any rectangle
+      referring to a previous unsupported rectangle coordinates will
+      instead refer to the previous supported rectangle. For example,
+      if sink crop is not supported, the compose selection will refer
+      to the sink pad format dimensions instead.</para>
+
+      <figure id="subdev-image-processing-crop">
+	<title>Image processing in subdevs: simple crop example</title>
+	<mediaobject>
+	  <imageobject>
+	    <imagedata fileref="subdev-image-processing-crop.svg"
+	    format="SVG" scale="200" />
+	  </imageobject>
+	</mediaobject>
+      </figure>
+
+      <para>In the above example, the subdev supports cropping on its
+      sink pad. To configure it, the user sets the media bus format on
+      the subdev's sink pad. Now the actual crop rectangle can be set
+      on the sink pad --- the location and size of this rectangle
+      reflect the location and size of a rectangle to be cropped from
+      the sink format. The size of the sink crop rectangle will also
+      be the size of the format of the subdev's source pad.</para>
+
+      <figure id="subdev-image-processing-scaling-multi-source">
+	<title>Image processing in subdevs: scaling with multiple sources</title>
+	<mediaobject>
+	  <imageobject>
+	    <imagedata fileref="subdev-image-processing-scaling-multi-source.svg"
+	    format="SVG" scale="200" />
+	  </imageobject>
+	</mediaobject>
+      </figure>
+
+      <para>In this example, the subdev is capable of first cropping,
+      then scaling and finally cropping for two source pads
+      individually from the resulting scaled image. The location of
+      the scaled image in the cropped image is ignored in sink compose
+      target. Both of the locations of the source crop rectangles
+      refer to the sink scaling rectangle, independently cropping an
+      area at location specified by the source crop rectangle from
+      it.</para>
+
+      <figure id="subdev-image-processing-full">
+	<title>Image processing in subdevs: scaling and composition
+	with multiple sinks and sources</title>
+	<mediaobject>
+	  <imageobject>
+	    <imagedata fileref="subdev-image-processing-full.svg"
+	    format="SVG" scale="200" />
+	  </imageobject>
+	</mediaobject>
+      </figure>
+
+      <para>The subdev driver supports two sink pads and two source
+      pads. The images from both of the sink pads are individually
+      cropped, then scaled and further composed on the composition
+      bounds rectangle. From that, two independent streams are cropped
+      and sent out of the subdev from the source pads.</para>
 
     </section>
+
   </section>
 
   &sub-subdev-formats;
diff --git a/Documentation/DocBook/media/v4l/io.xml b/Documentation/DocBook/media/v4l/io.xml
index b815929b5bba..fd6aca2922b6 100644
--- a/Documentation/DocBook/media/v4l/io.xml
+++ b/Documentation/DocBook/media/v4l/io.xml
@@ -543,12 +543,13 @@ and can range from zero to the number of buffers allocated
 with the &VIDIOC-REQBUFS; ioctl (&v4l2-requestbuffers; <structfield>count</structfield>) minus one.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry></entry>
 	    <entry>Type of the buffer, same as &v4l2-format;
 <structfield>type</structfield> or &v4l2-requestbuffers;
-<structfield>type</structfield>, set by the application.</entry>
+<structfield>type</structfield>, set by the application. See <xref
+linkend="v4l2-buf-type" /></entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
@@ -568,7 +569,7 @@ refers to an input stream, applications when an output stream.</entry>
 linkend="buffer-flags" />.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-field;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>field</structfield></entry>
 	    <entry></entry>
 	    <entry>Indicates the field order of the image in the
@@ -630,11 +631,12 @@ bandwidth. These devices identify by not enumerating any video
 standards, see <xref linkend="standard" />.</para></entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-memory;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>memory</structfield></entry>
 	    <entry></entry>
 	    <entry>This field must be set by applications and/or drivers
-in accordance with the selected I/O method.</entry>
+in accordance with the selected I/O method. See <xref linkend="v4l2-memory"
+	    /></entry>
 	  </row>
 	  <row>
 	    <entry>union</entry>
diff --git a/Documentation/DocBook/media/v4l/pixfmt-srggb10.xml b/Documentation/DocBook/media/v4l/pixfmt-srggb10.xml
index 7b274092e60c..c1c62a9acc2a 100644
--- a/Documentation/DocBook/media/v4l/pixfmt-srggb10.xml
+++ b/Documentation/DocBook/media/v4l/pixfmt-srggb10.xml
@@ -1,4 +1,4 @@
-    <refentry>
+    <refentry id="pixfmt-srggb10">
       <refmeta>
 	<refentrytitle>V4L2_PIX_FMT_SRGGB10 ('RG10'),
 	 V4L2_PIX_FMT_SGRBG10 ('BA10'),
diff --git a/Documentation/DocBook/media/v4l/pixfmt-srggb10dpcm8.xml b/Documentation/DocBook/media/v4l/pixfmt-srggb10dpcm8.xml
new file mode 100644
index 000000000000..8eace3e2e7d4
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/pixfmt-srggb10dpcm8.xml
@@ -0,0 +1,29 @@
+    <refentry id="pixfmt-srggb10dpcm8">
+      <refmeta>
+	<refentrytitle>
+	 V4L2_PIX_FMT_SBGGR10DPCM8 ('bBA8'),
+	 V4L2_PIX_FMT_SGBRG10DPCM8 ('bGA8'),
+	 V4L2_PIX_FMT_SGRBG10DPCM8 ('BD10'),
+	 V4L2_PIX_FMT_SRGGB10DPCM8 ('bRA8'),
+	 </refentrytitle>
+	&manvol;
+      </refmeta>
+      <refnamediv>
+	<refname id="V4L2-PIX-FMT-SBGGR10DPCM8"><constant>V4L2_PIX_FMT_SBGGR10DPCM8</constant></refname>
+	<refname id="V4L2-PIX-FMT-SGBRG10DPCM8"><constant>V4L2_PIX_FMT_SGBRG10DPCM8</constant></refname>
+	<refname id="V4L2-PIX-FMT-SGRBG10DPCM8"><constant>V4L2_PIX_FMT_SGRBG10DPCM8</constant></refname>
+	<refname id="V4L2-PIX-FMT-SRGGB10DPCM8"><constant>V4L2_PIX_FMT_SRGGB10DPCM8</constant></refname>
+	<refpurpose>10-bit Bayer formats compressed to 8 bits</refpurpose>
+      </refnamediv>
+      <refsect1>
+	<title>Description</title>
+
+	<para>The following four pixel formats are raw sRGB / Bayer formats
+	with 10 bits per colour compressed to 8 bits each, using DPCM
+	compression. DPCM, differential pulse-code modulation, is lossy.
+	Each colour component consumes 8 bits of memory. In other respects
+	this format is similar to <xref
+	linkend="pixfmt-srggb10">.</xref></para>
+
+      </refsect1>
+    </refentry>
diff --git a/Documentation/DocBook/media/v4l/pixfmt.xml b/Documentation/DocBook/media/v4l/pixfmt.xml
index 31eaae2469f9..f5ac15ed0549 100644
--- a/Documentation/DocBook/media/v4l/pixfmt.xml
+++ b/Documentation/DocBook/media/v4l/pixfmt.xml
@@ -673,6 +673,7 @@ access the palette, this must be done with ioctls of the Linux framebuffer API.<
     &sub-srggb8;
     &sub-sbggr16;
     &sub-srggb10;
+    &sub-srggb10dpcm8;
     &sub-srggb12;
   </section>
 
@@ -876,11 +877,6 @@ kernel sources in the file <filename>Documentation/video4linux/cx2341x/README.hm
 	    <entry>'S561'</entry>
 	    <entry>Compressed GBRG Bayer format used by the gspca driver.</entry>
 	  </row>
-	  <row id="V4L2-PIX-FMT-SGRBG10DPCM8">
-	    <entry><constant>V4L2_PIX_FMT_SGRBG10DPCM8</constant></entry>
-	    <entry>'DB10'</entry>
-	    <entry>10 bit raw Bayer DPCM compressed to 8 bits.</entry>
-	  </row>
 	  <row id="V4L2-PIX-FMT-PAC207">
 	    <entry><constant>V4L2_PIX_FMT_PAC207</constant></entry>
 	    <entry>'P207'</entry>
diff --git a/Documentation/DocBook/media/v4l/subdev-image-processing-crop.dia b/Documentation/DocBook/media/v4l/subdev-image-processing-crop.dia
new file mode 100644
index 000000000000..e32ba5362e1d
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/subdev-image-processing-crop.dia
@@ -0,0 +1,614 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<dia:diagram xmlns:dia="http://www.lysator.liu.se/~alla/dia/">
+  <dia:diagramdata>
+    <dia:attribute name="background">
+      <dia:color val="#ffffff"/>
+    </dia:attribute>
+    <dia:attribute name="pagebreak">
+      <dia:color val="#000099"/>
+    </dia:attribute>
+    <dia:attribute name="paper">
+      <dia:composite type="paper">
+        <dia:attribute name="name">
+          <dia:string>#A4#</dia:string>
+        </dia:attribute>
+        <dia:attribute name="tmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="bmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="lmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="rmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="is_portrait">
+          <dia:boolean val="false"/>
+        </dia:attribute>
+        <dia:attribute name="scaling">
+          <dia:real val="0.49000000953674316"/>
+        </dia:attribute>
+        <dia:attribute name="fitto">
+          <dia:boolean val="false"/>
+        </dia:attribute>
+      </dia:composite>
+    </dia:attribute>
+    <dia:attribute name="grid">
+      <dia:composite type="grid">
+        <dia:attribute name="width_x">
+          <dia:real val="1"/>
+        </dia:attribute>
+        <dia:attribute name="width_y">
+          <dia:real val="1"/>
+        </dia:attribute>
+        <dia:attribute name="visible_x">
+          <dia:int val="1"/>
+        </dia:attribute>
+        <dia:attribute name="visible_y">
+          <dia:int val="1"/>
+        </dia:attribute>
+        <dia:composite type="color"/>
+      </dia:composite>
+    </dia:attribute>
+    <dia:attribute name="color">
+      <dia:color val="#d8e5e5"/>
+    </dia:attribute>
+    <dia:attribute name="guides">
+      <dia:composite type="guides">
+        <dia:attribute name="hguides"/>
+        <dia:attribute name="vguides"/>
+      </dia:composite>
+    </dia:attribute>
+  </dia:diagramdata>
+  <dia:layer name="Background" visible="true" active="true">
+    <dia:object type="Standard - Box" version="0" id="O0">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-0.4,6.5"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-0.45,6.45;23.1387,16.2"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="-0.4,6.5"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="23.48871579904775"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="9.6500000000000004"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O1">
+      <dia:attribute name="obj_pos">
+        <dia:point val="0.225,9.45"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="0.175,9.4;8.225,14.7"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="0.225,9.45"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="7.9499999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="5.1999999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a52a2a"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O2">
+      <dia:attribute name="obj_pos">
+        <dia:point val="3.175,10.55"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3.125,10.5;7.925,14.45"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="3.175,10.55"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="4.6999999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.8499999999999979"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#0000ff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O3">
+      <dia:attribute name="obj_pos">
+        <dia:point val="3.725,11.3875"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3.725,10.7925;6.6025,13.14"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink
+crop
+selection#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="3.725,11.3875"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#0000ff"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O4">
+      <dia:attribute name="obj_pos">
+        <dia:point val="1.475,7.9"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="1.475,7.305;1.475,8.0525"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>##</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="1.475,7.9"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O5">
+      <dia:attribute name="obj_pos">
+        <dia:point val="0.426918,7.89569"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="0.426918,7.30069;3.90942,8.84819"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink media
+bus format#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="0.426918,7.89569"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#a52a2a"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O6">
+      <dia:attribute name="obj_pos">
+        <dia:point val="17.4887,7.75"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="17.4887,7.155;21.8112,8.7025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#source media
+bus format#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="17.4887,7.75"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#8b6914"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O7">
+      <dia:attribute name="obj_pos">
+        <dia:point val="17.5244,9.5417"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="17.4744,9.4917;22.2387,13.35"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="17.5244,9.5417"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="4.6643157990477508"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.758300000000002"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#8b6914"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O8">
+      <dia:attribute name="obj_pos">
+        <dia:point val="17.5244,13.3"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3.12132,13.2463;17.5781,14.4537"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="17.5244,13.3"/>
+        <dia:point val="3.175,14.4"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O7" connection="5"/>
+        <dia:connection handle="1" to="O2" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O9">
+      <dia:attribute name="obj_pos">
+        <dia:point val="17.5244,9.5417"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3.12162,9.48832;17.5778,10.6034"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="17.5244,9.5417"/>
+        <dia:point val="3.175,10.55"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O7" connection="0"/>
+        <dia:connection handle="1" to="O2" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O10">
+      <dia:attribute name="obj_pos">
+        <dia:point val="22.1887,13.3"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="7.82132,13.2463;22.2424,14.4537"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="22.1887,13.3"/>
+        <dia:point val="7.875,14.4"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O7" connection="7"/>
+        <dia:connection handle="1" to="O2" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O11">
+      <dia:attribute name="obj_pos">
+        <dia:point val="22.1887,9.5417"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="7.82161,9.48831;22.2421,10.6034"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="22.1887,9.5417"/>
+        <dia:point val="7.875,10.55"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O7" connection="2"/>
+        <dia:connection handle="1" to="O2" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O12">
+      <dia:attribute name="obj_pos">
+        <dia:point val="23.23,10.5742"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="23.18,10.5242;24.13,11.4742"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="23.23,10.5742"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O13">
+      <dia:attribute name="obj_pos">
+        <dia:point val="24.08,10.9992"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="24.03,10.6388;32.4953,11.3624"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="24.08,10.9992"/>
+        <dia:point val="32.3835,11.0007"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O12" connection="3"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O14">
+      <dia:attribute name="obj_pos">
+        <dia:point val="25.3454,10.49"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="25.3454,9.895;29.9904,10.6425"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 1 (source)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="25.3454,10.49"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O15">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-1.44491,11.6506"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-1.49491,11.6006;-0.54491,12.5506"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="-1.44491,11.6506"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O16">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-9.61991,12.09"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-9.67,11.7149;-1.33311,12.4385"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="-9.61991,12.09"/>
+        <dia:point val="-1.44491,12.0756"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O15" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O17">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-7.39291,11.49"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-7.39291,10.895;-3.58791,11.6425"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 0 (sink)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="-7.39291,11.49"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+  </dia:layer>
+</dia:diagram>
diff --git a/Documentation/DocBook/media/v4l/subdev-image-processing-crop.svg b/Documentation/DocBook/media/v4l/subdev-image-processing-crop.svg
new file mode 100644
index 000000000000..18b0f5de9ed2
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/subdev-image-processing-crop.svg
@@ -0,0 +1,63 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.0//EN" "http://www.w3.org/TR/2001/PR-SVG-20010719/DTD/svg10.dtd">
+<svg width="43cm" height="10cm" viewBox="-194 128 844 196" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
+  <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x="-8" y="130" width="469.774" height="193"/>
+  <g>
+    <rect style="fill: #ffffff" x="4.5" y="189" width="159" height="104"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a52a2a" x="4.5" y="189" width="159" height="104"/>
+  </g>
+  <g>
+    <rect style="fill: #ffffff" x="63.5" y="211" width="94" height="77"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" x="63.5" y="211" width="94" height="77"/>
+  </g>
+  <text style="fill: #0000ff;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="74.5" y="227.75">
+    <tspan x="74.5" y="227.75">sink</tspan>
+    <tspan x="74.5" y="243.75">crop</tspan>
+    <tspan x="74.5" y="259.75">selection</tspan>
+  </text>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="29.5" y="158">
+    <tspan x="29.5" y="158"></tspan>
+  </text>
+  <text style="fill: #a52a2a;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="8.53836" y="157.914">
+    <tspan x="8.53836" y="157.914">sink media</tspan>
+    <tspan x="8.53836" y="173.914">bus format</tspan>
+  </text>
+  <text style="fill: #8b6914;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="349.774" y="155">
+    <tspan x="349.774" y="155">source media</tspan>
+    <tspan x="349.774" y="171">bus format</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="350.488" y="190.834" width="93.2863" height="75.166"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #8b6914" x="350.488" y="190.834" width="93.2863" height="75.166"/>
+  </g>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="350.488" y1="266" x2="63.5" y2="288"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="350.488" y1="190.834" x2="63.5" y2="211"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="443.774" y1="266" x2="157.5" y2="288"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="443.774" y1="190.834" x2="157.5" y2="211"/>
+  <g>
+    <ellipse style="fill: #ffffff" cx="473.1" cy="219.984" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="473.1" cy="219.984" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="473.1" cy="219.984" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="481.6" y1="219.984" x2="637.934" y2="220.012"/>
+    <polygon style="fill: #000000" points="645.434,220.014 635.433,225.012 637.934,220.012 635.435,215.012 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="645.434,220.014 635.433,225.012 637.934,220.012 635.435,215.012 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="506.908" y="209.8">
+    <tspan x="506.908" y="209.8">pad 1 (source)</tspan>
+  </text>
+  <g>
+    <ellipse style="fill: #ffffff" cx="-20.3982" cy="241.512" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-20.3982" cy="241.512" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-20.3982" cy="241.512" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="-192.398" y1="241.8" x2="-38.6343" y2="241.529"/>
+    <polygon style="fill: #000000" points="-31.1343,241.516 -41.1254,246.534 -38.6343,241.529 -41.1431,236.534 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="-31.1343,241.516 -41.1254,246.534 -38.6343,241.529 -41.1431,236.534 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="-147.858" y="229.8">
+    <tspan x="-147.858" y="229.8">pad 0 (sink)</tspan>
+  </text>
+</svg>
diff --git a/Documentation/DocBook/media/v4l/subdev-image-processing-full.dia b/Documentation/DocBook/media/v4l/subdev-image-processing-full.dia
new file mode 100644
index 000000000000..a0d782927840
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/subdev-image-processing-full.dia
@@ -0,0 +1,1588 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<dia:diagram xmlns:dia="http://www.lysator.liu.se/~alla/dia/">
+  <dia:diagramdata>
+    <dia:attribute name="background">
+      <dia:color val="#ffffff"/>
+    </dia:attribute>
+    <dia:attribute name="pagebreak">
+      <dia:color val="#000099"/>
+    </dia:attribute>
+    <dia:attribute name="paper">
+      <dia:composite type="paper">
+        <dia:attribute name="name">
+          <dia:string>#A4#</dia:string>
+        </dia:attribute>
+        <dia:attribute name="tmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="bmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="lmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="rmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="is_portrait">
+          <dia:boolean val="false"/>
+        </dia:attribute>
+        <dia:attribute name="scaling">
+          <dia:real val="0.49000000953674316"/>
+        </dia:attribute>
+        <dia:attribute name="fitto">
+          <dia:boolean val="false"/>
+        </dia:attribute>
+      </dia:composite>
+    </dia:attribute>
+    <dia:attribute name="grid">
+      <dia:composite type="grid">
+        <dia:attribute name="width_x">
+          <dia:real val="1"/>
+        </dia:attribute>
+        <dia:attribute name="width_y">
+          <dia:real val="1"/>
+        </dia:attribute>
+        <dia:attribute name="visible_x">
+          <dia:int val="1"/>
+        </dia:attribute>
+        <dia:attribute name="visible_y">
+          <dia:int val="1"/>
+        </dia:attribute>
+        <dia:composite type="color"/>
+      </dia:composite>
+    </dia:attribute>
+    <dia:attribute name="color">
+      <dia:color val="#d8e5e5"/>
+    </dia:attribute>
+    <dia:attribute name="guides">
+      <dia:composite type="guides">
+        <dia:attribute name="hguides"/>
+        <dia:attribute name="vguides"/>
+      </dia:composite>
+    </dia:attribute>
+  </dia:diagramdata>
+  <dia:layer name="Background" visible="true" active="true">
+    <dia:object type="Standard - Box" version="0" id="O0">
+      <dia:attribute name="obj_pos">
+        <dia:point val="15.945,6.45"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="15.895,6.4;26.4,18.95"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="15.945,6.45"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="10.404999999254942"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="12.449999999999992"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#ff765a"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O1">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-0.1,3.65"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-0.15,3.6;40.25,20.85"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="-0.1,3.65"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="40.300000000000004"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="17.149999999999999"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O2">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-1.05,7.9106"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-1.1,7.8606;-0.15,8.8106"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="-1.05,7.9106"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O3">
+      <dia:attribute name="obj_pos">
+        <dia:point val="40.3366,9.8342"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="40.2866,9.7842;41.2366,10.7342"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="40.3366,9.8342"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O4">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-9.225,8.35"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-9.27509,7.97487;-0.938197,8.69848"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="-9.225,8.35"/>
+        <dia:point val="-1.05,8.3356"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O2" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O5">
+      <dia:attribute name="obj_pos">
+        <dia:point val="41.1866,10.2592"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="41.1366,9.89879;49.6019,10.6224"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="41.1866,10.2592"/>
+        <dia:point val="49.4901,10.2607"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O3" connection="3"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O6">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-6.998,7.75"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-6.998,7.155;-3.193,7.9025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 0 (sink)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="-6.998,7.75"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O7">
+      <dia:attribute name="obj_pos">
+        <dia:point val="42.452,9.75"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="42.452,9.155;47.097,9.9025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 2 (source)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="42.452,9.75"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O8">
+      <dia:attribute name="obj_pos">
+        <dia:point val="0.275,6"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="0.225,5.95;8.275,11.25"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="0.275,6"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="7.9499999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="5.1999999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a52a2a"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O9">
+      <dia:attribute name="obj_pos">
+        <dia:point val="3.125,6.8"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3.075,6.75;7.875,10.7"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="3.125,6.8"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="4.6999999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.8499999999999979"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#0000ff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O10">
+      <dia:attribute name="obj_pos">
+        <dia:point val="1.525,4.45"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="1.525,3.855;1.525,4.6025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>##</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="1.525,4.45"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O11">
+      <dia:attribute name="obj_pos">
+        <dia:point val="0.476918,4.44569"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="0.476918,3.85069;3.95942,5.39819"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink media
+bus format#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="0.476918,4.44569"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#a52a2a"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O12">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.6822,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="16.6322,9.23251;24.9922,17.9564"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="16.6822,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="8.2600228398861297"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="8.6238900617957164"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#00ff00"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O13">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.6822,17.9064"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3.05732,10.5823;16.7499,17.9741"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="16.6822,17.9064"/>
+        <dia:point val="3.125,10.65"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O12" connection="5"/>
+        <dia:connection handle="1" to="O9" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O14">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.6822,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3.06681,6.74181;16.7404,9.3407"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="16.6822,9.28251"/>
+        <dia:point val="3.125,6.8"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O12" connection="0"/>
+        <dia:connection handle="1" to="O9" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O15">
+      <dia:attribute name="obj_pos">
+        <dia:point val="24.9422,17.9064"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="7.75945,10.5845;25.0077,17.9719"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="24.9422,17.9064"/>
+        <dia:point val="7.825,10.65"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O12" connection="7"/>
+        <dia:connection handle="1" to="O9" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O16">
+      <dia:attribute name="obj_pos">
+        <dia:point val="24.9422,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="7.76834,6.74334;24.9989,9.33917"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="24.9422,9.28251"/>
+        <dia:point val="7.825,6.8"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O12" connection="2"/>
+        <dia:connection handle="1" to="O9" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O17">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.7352,7.47209"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="16.7352,6.87709;22.5602,8.42459"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink compose
+selection (scaling)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="16.7352,7.47209"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#00ff00"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O18">
+      <dia:attribute name="obj_pos">
+        <dia:point val="20.4661,9.72825"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="20.4161,9.67825;25.5254,13.3509"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="20.4661,9.72825"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="5.009308462554376"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.5726155970598077"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O19">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.475,5.2564"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="34.475,4.6614;38.7975,6.2089"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#source media
+bus format#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="34.475,5.2564"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#8b6914"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O20">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.4244,8.6917"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="34.3744,8.6417;39.4837,12.3143"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="34.4244,8.6917"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="5.009308462554376"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.5726155970598077"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#8b6914"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O21">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.4244,12.2643"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="20.4125,12.2107;34.478,13.3545"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.4244,12.2643"/>
+        <dia:point val="20.4661,13.3009"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O20" connection="5"/>
+        <dia:connection handle="1" to="O18" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O22">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.4244,8.6917"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="20.4125,8.63813;34.478,9.78182"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.4244,8.6917"/>
+        <dia:point val="20.4661,9.72825"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O20" connection="0"/>
+        <dia:connection handle="1" to="O18" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O23">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.4337,12.2643"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="25.4218,12.2107;39.4873,13.3545"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="39.4337,12.2643"/>
+        <dia:point val="25.4754,13.3009"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O20" connection="7"/>
+        <dia:connection handle="1" to="O18" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O24">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.4337,8.6917"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="25.4218,8.63813;39.4873,9.78182"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="39.4337,8.6917"/>
+        <dia:point val="25.4754,9.72825"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O20" connection="2"/>
+        <dia:connection handle="1" to="O18" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O25">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.25,5.15"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="16.25,4.555;21.68,6.1025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink compose
+bounds selection#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="16.25,5.15"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#ff765a"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O26">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-1.02991,16.6506"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-1.07991,16.6006;-0.12991,17.5506"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="-1.02991,16.6506"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O27">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-9.20491,17.09"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-9.255,16.7149;-0.918107,17.4385"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="-9.20491,17.09"/>
+        <dia:point val="-1.02991,17.0756"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O26" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O28">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-6.95,16.45"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-6.95,15.855;-3.145,16.6025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 1 (sink)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="-6.95,16.45"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O29">
+      <dia:attribute name="obj_pos">
+        <dia:point val="0.390412,14.64"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="0.340412,14.59;6.045,18.8"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="0.390412,14.64"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="5.604587512785236"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="4.1099999999999994"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a52a2a"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O30">
+      <dia:attribute name="obj_pos">
+        <dia:point val="2.645,15.74"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="2.595,15.69;5.6,18.3"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="2.645,15.74"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="2.904999999254942"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="2.5100000000000016"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#0000ff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O31">
+      <dia:attribute name="obj_pos">
+        <dia:point val="1.595,12.99"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="1.595,12.395;1.595,13.1425"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>##</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="1.595,12.99"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O32">
+      <dia:attribute name="obj_pos">
+        <dia:point val="17.945,12.595"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="2.58596,12.536;18.004,15.799"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="17.945,12.595"/>
+        <dia:point val="2.645,15.74"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O36" connection="0"/>
+        <dia:connection handle="1" to="O30" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O33">
+      <dia:attribute name="obj_pos">
+        <dia:point val="17.945,15.8"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="2.58772,15.7427;18.0023,18.3073"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="17.945,15.8"/>
+        <dia:point val="2.645,18.25"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O36" connection="5"/>
+        <dia:connection handle="1" to="O30" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O34">
+      <dia:attribute name="obj_pos">
+        <dia:point val="21.7,15.8"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="5.49307,15.7431;21.7569,18.3069"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="21.7,15.8"/>
+        <dia:point val="5.55,18.25"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O36" connection="7"/>
+        <dia:connection handle="1" to="O30" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O35">
+      <dia:attribute name="obj_pos">
+        <dia:point val="21.7,12.595"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="5.49136,12.5364;21.7586,15.7986"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="21.7,12.595"/>
+        <dia:point val="5.55,15.74"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O36" connection="2"/>
+        <dia:connection handle="1" to="O30" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O36">
+      <dia:attribute name="obj_pos">
+        <dia:point val="17.945,12.595"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="17.895,12.545;21.75,15.85"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="17.945,12.595"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="3.7549999992549452"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.2049999992549427"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#00ff00"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O37">
+      <dia:attribute name="obj_pos">
+        <dia:point val="22.1631,14.2233"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="22.1131,14.1733;25.45,16.7"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="22.1631,14.2233"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="3.2369000000000021"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="2.4267000000000003"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O38">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.6714,16.2367"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="34.6214,16.1867;37.9,18.75"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="34.6714,16.2367"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="3.178600000000003"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="2.4632999999999967"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#8b6914"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O39">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.6714,18.7"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="22.1057,16.5926;34.7288,18.7574"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.6714,18.7"/>
+        <dia:point val="22.1631,16.65"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O38" connection="5"/>
+        <dia:connection handle="1" to="O37" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O40">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.6714,16.2367"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="22.1058,14.166;34.7287,16.294"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.6714,16.2367"/>
+        <dia:point val="22.1631,14.2233"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O38" connection="0"/>
+        <dia:connection handle="1" to="O37" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O41">
+      <dia:attribute name="obj_pos">
+        <dia:point val="37.85,18.7"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="25.3425,16.5925;37.9075,18.7575"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="37.85,18.7"/>
+        <dia:point val="25.4,16.65"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O38" connection="7"/>
+        <dia:connection handle="1" to="O37" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O42">
+      <dia:attribute name="obj_pos">
+        <dia:point val="37.85,16.2367"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="25.3427,14.166;37.9073,16.294"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="37.85,16.2367"/>
+        <dia:point val="25.4,14.2233"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O38" connection="2"/>
+        <dia:connection handle="1" to="O37" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O43">
+      <dia:attribute name="obj_pos">
+        <dia:point val="40.347,16.7742"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="40.297,16.7242;41.247,17.6742"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="40.347,16.7742"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O44">
+      <dia:attribute name="obj_pos">
+        <dia:point val="41.197,17.1992"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="41.147,16.8388;49.6123,17.5624"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="41.197,17.1992"/>
+        <dia:point val="49.5005,17.2007"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O43" connection="3"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O45">
+      <dia:attribute name="obj_pos">
+        <dia:point val="42.4624,16.69"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="42.4624,16.095;47.1074,16.8425"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 3 (source)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="42.4624,16.69"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O46">
+      <dia:attribute name="obj_pos">
+        <dia:point val="9.85,4.55"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="9.85,3.955;12.7275,6.3025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink
+crop
+selection#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="9.85,4.55"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#0000ff"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O47">
+      <dia:attribute name="obj_pos">
+        <dia:point val="27.65,4.75"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="27.65,4.155;30.5275,6.5025"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#source
+crop
+selection#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="27.65,4.75"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#a020f0"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O48">
+      <dia:attribute name="obj_pos">
+        <dia:point val="10.55,6.6"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="7.7135,6.39438;10.6035,7.11605"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="10.55,6.6"/>
+        <dia:point val="7.825,6.8"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#0000ff"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O9" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O49">
+      <dia:attribute name="obj_pos">
+        <dia:point val="10.45,6.55"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="5.48029,6.48236;10.5176,15.8387"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="10.45,6.55"/>
+        <dia:point val="5.55,15.74"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#0000ff"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O30" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O50">
+      <dia:attribute name="obj_pos">
+        <dia:point val="27.5246,6.66071"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="25.406,6.59136;27.594,9.82122"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="27.5246,6.66071"/>
+        <dia:point val="25.4754,9.72825"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O18" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O51">
+      <dia:attribute name="obj_pos">
+        <dia:point val="27.5036,6.68935"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="25.2161,6.62775;27.5652,14.331"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="27.5036,6.68935"/>
+        <dia:point val="25.4,14.2233"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O37" connection="2"/>
+      </dia:connections>
+    </dia:object>
+  </dia:layer>
+</dia:diagram>
diff --git a/Documentation/DocBook/media/v4l/subdev-image-processing-full.svg b/Documentation/DocBook/media/v4l/subdev-image-processing-full.svg
new file mode 100644
index 000000000000..3322cf4c0093
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/subdev-image-processing-full.svg
@@ -0,0 +1,163 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.0//EN" "http://www.w3.org/TR/2001/PR-SVG-20010719/DTD/svg10.dtd">
+<svg width="59cm" height="18cm" viewBox="-186 71 1178 346" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
+  <g>
+    <rect style="fill: #ffffff" x="318.9" y="129" width="208.1" height="249"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #ff765a" x="318.9" y="129" width="208.1" height="249"/>
+  </g>
+  <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x="-2" y="73" width="806" height="343"/>
+  <g>
+    <ellipse style="fill: #ffffff" cx="-12.5" cy="166.712" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-12.5" cy="166.712" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-12.5" cy="166.712" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <ellipse style="fill: #ffffff" cx="815.232" cy="205.184" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="815.232" cy="205.184" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="815.232" cy="205.184" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="-184.5" y1="167" x2="-30.7361" y2="166.729"/>
+    <polygon style="fill: #000000" points="-23.2361,166.716 -33.2272,171.734 -30.7361,166.729 -33.2449,161.734 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="-23.2361,166.716 -33.2272,171.734 -30.7361,166.729 -33.2449,161.734 "/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="823.732" y1="205.184" x2="980.066" y2="205.212"/>
+    <polygon style="fill: #000000" points="987.566,205.214 977.565,210.212 980.066,205.212 977.567,200.212 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="987.566,205.214 977.565,210.212 980.066,205.212 977.567,200.212 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="-139.96" y="155">
+    <tspan x="-139.96" y="155">pad 0 (sink)</tspan>
+  </text>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="849.04" y="195">
+    <tspan x="849.04" y="195">pad 2 (source)</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="5.5" y="120" width="159" height="104"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a52a2a" x="5.5" y="120" width="159" height="104"/>
+  </g>
+  <g>
+    <rect style="fill: #ffffff" x="62.5" y="136" width="94" height="77"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" x="62.5" y="136" width="94" height="77"/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="30.5" y="89">
+    <tspan x="30.5" y="89"></tspan>
+  </text>
+  <text style="fill: #a52a2a;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="9.53836" y="88.9138">
+    <tspan x="9.53836" y="88.9138">sink media</tspan>
+    <tspan x="9.53836" y="104.914">bus format</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="333.644" y="185.65" width="165.2" height="172.478"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #00ff00" x="333.644" y="185.65" width="165.2" height="172.478"/>
+  </g>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="333.644" y1="358.128" x2="62.5" y2="213"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="333.644" y1="185.65" x2="62.5" y2="136"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="498.844" y1="358.128" x2="156.5" y2="213"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="498.844" y1="185.65" x2="156.5" y2="136"/>
+  <text style="fill: #00ff00;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="334.704" y="149.442">
+    <tspan x="334.704" y="149.442">sink compose</tspan>
+    <tspan x="334.704" y="165.442">selection (scaling)</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="409.322" y="194.565" width="100.186" height="71.4523"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x="409.322" y="194.565" width="100.186" height="71.4523"/>
+  </g>
+  <text style="fill: #8b6914;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="689.5" y="105.128">
+    <tspan x="689.5" y="105.128">source media</tspan>
+    <tspan x="689.5" y="121.128">bus format</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="688.488" y="173.834" width="100.186" height="71.4523"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #8b6914" x="688.488" y="173.834" width="100.186" height="71.4523"/>
+  </g>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="688.488" y1="245.286" x2="409.322" y2="266.018"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="688.488" y1="173.834" x2="409.322" y2="194.565"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="788.674" y1="245.286" x2="509.508" y2="266.018"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="788.674" y1="173.834" x2="509.508" y2="194.565"/>
+  <text style="fill: #ff765a;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="325" y="103">
+    <tspan x="325" y="103">sink compose</tspan>
+    <tspan x="325" y="119">bounds selection</tspan>
+  </text>
+  <g>
+    <ellipse style="fill: #ffffff" cx="-12.0982" cy="341.512" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-12.0982" cy="341.512" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-12.0982" cy="341.512" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="-184.098" y1="341.8" x2="-30.3343" y2="341.529"/>
+    <polygon style="fill: #000000" points="-22.8343,341.516 -32.8254,346.534 -30.3343,341.529 -32.8431,336.534 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="-22.8343,341.516 -32.8254,346.534 -30.3343,341.529 -32.8431,336.534 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="-139" y="329">
+    <tspan x="-139" y="329">pad 1 (sink)</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="7.80824" y="292.8" width="112.092" height="82.2"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a52a2a" x="7.80824" y="292.8" width="112.092" height="82.2"/>
+  </g>
+  <g>
+    <rect style="fill: #ffffff" x="52.9" y="314.8" width="58.1" height="50.2"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" x="52.9" y="314.8" width="58.1" height="50.2"/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="31.9" y="259.8">
+    <tspan x="31.9" y="259.8"></tspan>
+  </text>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="358.9" y1="251.9" x2="52.9" y2="314.8"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="358.9" y1="316" x2="52.9" y2="365"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="434" y1="316" x2="111" y2="365"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="434" y1="251.9" x2="111" y2="314.8"/>
+  <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #00ff00" x="358.9" y="251.9" width="75.1" height="64.1"/>
+  <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x="443.262" y="284.466" width="64.738" height="48.534"/>
+  <g>
+    <rect style="fill: #ffffff" x="693.428" y="324.734" width="63.572" height="49.266"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #8b6914" x="693.428" y="324.734" width="63.572" height="49.266"/>
+  </g>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="693.428" y1="374" x2="443.262" y2="333"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="693.428" y1="324.734" x2="443.262" y2="284.466"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="757" y1="374" x2="508" y2="333"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="757" y1="324.734" x2="508" y2="284.466"/>
+  <g>
+    <ellipse style="fill: #ffffff" cx="815.44" cy="343.984" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="815.44" cy="343.984" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="815.44" cy="343.984" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="823.94" y1="343.984" x2="980.274" y2="344.012"/>
+    <polygon style="fill: #000000" points="987.774,344.014 977.773,349.012 980.274,344.012 977.775,339.012 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="987.774,344.014 977.773,349.012 980.274,344.012 977.775,339.012 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="849.248" y="333.8">
+    <tspan x="849.248" y="333.8">pad 3 (source)</tspan>
+  </text>
+  <text style="fill: #0000ff;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="197" y="91">
+    <tspan x="197" y="91">sink</tspan>
+    <tspan x="197" y="107">crop</tspan>
+    <tspan x="197" y="123">selection</tspan>
+  </text>
+  <text style="fill: #a020f0;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="553" y="95">
+    <tspan x="553" y="95">source</tspan>
+    <tspan x="553" y="111">crop</tspan>
+    <tspan x="553" y="127">selection</tspan>
+  </text>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" x1="211" y1="132" x2="166.21" y2="135.287"/>
+    <polygon style="fill: #0000ff" points="158.73,135.836 168.337,130.118 166.21,135.287 169.069,140.091 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" points="158.73,135.836 168.337,130.118 166.21,135.287 169.069,140.091 "/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" x1="209" y1="131" x2="115.581" y2="306.209"/>
+    <polygon style="fill: #0000ff" points="112.052,312.827 112.345,301.65 115.581,306.209 121.169,306.355 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" points="112.052,312.827 112.345,301.65 115.581,306.209 121.169,306.355 "/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x1="550.492" y1="133.214" x2="514.916" y2="186.469"/>
+    <polygon style="fill: #a020f0" points="510.75,192.706 512.147,181.613 514.916,186.469 520.463,187.168 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" points="510.75,192.706 512.147,181.613 514.916,186.469 520.463,187.168 "/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x1="550.072" y1="133.787" x2="510.618" y2="275.089"/>
+    <polygon style="fill: #a020f0" points="508.601,282.312 506.475,271.336 510.618,275.089 516.106,274.025 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" points="508.601,282.312 506.475,271.336 510.618,275.089 516.106,274.025 "/>
+  </g>
+</svg>
diff --git a/Documentation/DocBook/media/v4l/subdev-image-processing-scaling-multi-source.dia b/Documentation/DocBook/media/v4l/subdev-image-processing-scaling-multi-source.dia
new file mode 100644
index 000000000000..0cd50a7bda80
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/subdev-image-processing-scaling-multi-source.dia
@@ -0,0 +1,1152 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<dia:diagram xmlns:dia="http://www.lysator.liu.se/~alla/dia/">
+  <dia:diagramdata>
+    <dia:attribute name="background">
+      <dia:color val="#ffffff"/>
+    </dia:attribute>
+    <dia:attribute name="pagebreak">
+      <dia:color val="#000099"/>
+    </dia:attribute>
+    <dia:attribute name="paper">
+      <dia:composite type="paper">
+        <dia:attribute name="name">
+          <dia:string>#A4#</dia:string>
+        </dia:attribute>
+        <dia:attribute name="tmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="bmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="lmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="rmargin">
+          <dia:real val="2.8222000598907471"/>
+        </dia:attribute>
+        <dia:attribute name="is_portrait">
+          <dia:boolean val="false"/>
+        </dia:attribute>
+        <dia:attribute name="scaling">
+          <dia:real val="0.49000000953674316"/>
+        </dia:attribute>
+        <dia:attribute name="fitto">
+          <dia:boolean val="false"/>
+        </dia:attribute>
+      </dia:composite>
+    </dia:attribute>
+    <dia:attribute name="grid">
+      <dia:composite type="grid">
+        <dia:attribute name="width_x">
+          <dia:real val="1"/>
+        </dia:attribute>
+        <dia:attribute name="width_y">
+          <dia:real val="1"/>
+        </dia:attribute>
+        <dia:attribute name="visible_x">
+          <dia:int val="1"/>
+        </dia:attribute>
+        <dia:attribute name="visible_y">
+          <dia:int val="1"/>
+        </dia:attribute>
+        <dia:composite type="color"/>
+      </dia:composite>
+    </dia:attribute>
+    <dia:attribute name="color">
+      <dia:color val="#d8e5e5"/>
+    </dia:attribute>
+    <dia:attribute name="guides">
+      <dia:composite type="guides">
+        <dia:attribute name="hguides"/>
+        <dia:attribute name="vguides"/>
+      </dia:composite>
+    </dia:attribute>
+  </dia:diagramdata>
+  <dia:layer name="Background" visible="true" active="true">
+    <dia:object type="Standard - Box" version="0" id="O0">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-0.4,6.5"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-0.45,6.45;39.95,22.9"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="-0.4,6.5"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="40.299999999999997"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="16.349999999999998"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O1">
+      <dia:attribute name="obj_pos">
+        <dia:point val="0.225,9.45"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="0.175,9.4;8.225,14.7"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="0.225,9.45"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="7.9499999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="5.1999999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a52a2a"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O2">
+      <dia:attribute name="obj_pos">
+        <dia:point val="2.475,10.2"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="2.425,10.15;7.225,14.1"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="2.475,10.2"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="4.6999999999999975"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.8499999999999979"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#0000ff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O3">
+      <dia:attribute name="obj_pos">
+        <dia:point val="3,11.2"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="3,10.605;5.8775,12.9525"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink
+crop
+selection#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="3,11.2"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#0000ff"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O4">
+      <dia:attribute name="obj_pos">
+        <dia:point val="1.475,7.9"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="1.475,7.305;1.475,8.0525"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>##</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="1.475,7.9"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O5">
+      <dia:attribute name="obj_pos">
+        <dia:point val="0.426918,7.89569"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="0.426918,7.30069;3.90942,8.84819"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink media
+bus format#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="0.426918,7.89569"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#a52a2a"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O6">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.6822,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="16.6322,9.23251;24.9922,17.9564"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="16.6822,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="8.2600228398861297"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="8.6238900617957164"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#00ff00"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O7">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.6822,17.9064"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="2.41365,13.9886;16.7436,17.9678"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="16.6822,17.9064"/>
+        <dia:point val="2.475,14.05"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O6" connection="5"/>
+        <dia:connection handle="1" to="O2" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O8">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.6822,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="2.42188,9.22939;16.7353,10.2531"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="16.6822,9.28251"/>
+        <dia:point val="2.475,10.2"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O6" connection="0"/>
+        <dia:connection handle="1" to="O2" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O9">
+      <dia:attribute name="obj_pos">
+        <dia:point val="24.9422,17.9064"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="7.11553,13.9905;25.0017,17.9659"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="24.9422,17.9064"/>
+        <dia:point val="7.175,14.05"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O6" connection="7"/>
+        <dia:connection handle="1" to="O2" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O10">
+      <dia:attribute name="obj_pos">
+        <dia:point val="24.9422,9.28251"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="7.12249,9.23;24.9947,10.2525"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="24.9422,9.28251"/>
+        <dia:point val="7.175,10.2"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O6" connection="2"/>
+        <dia:connection handle="1" to="O2" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O11">
+      <dia:attribute name="obj_pos">
+        <dia:point val="16.7352,7.47209"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="16.7352,6.87709;22.5602,8.42459"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#sink compose
+selection (scaling)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="16.7352,7.47209"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#00ff00"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O12">
+      <dia:attribute name="obj_pos">
+        <dia:point val="19.1161,9.97825"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="19.0661,9.92825;24.1754,13.6009"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="19.1161,9.97825"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="5.009308462554376"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.5726155970598077"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O13">
+      <dia:attribute name="obj_pos">
+        <dia:point val="27.1661,7.47209"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="27.1661,6.87709;30.0436,9.22459"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#source
+crop
+selection#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="27.1661,7.47209"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#a020f0"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O14">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.575,7.8564"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="34.575,7.2614;38.8975,8.8089"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#source media
+bus format#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="34.575,7.8564"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#8b6914"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O15">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.5244,11.2917"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="34.4744,11.2417;39.5837,14.9143"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="34.5244,11.2917"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="5.009308462554376"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.5726155970598077"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#8b6914"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O16">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.5244,14.8643"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="19.062,13.4968;34.5785,14.9184"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.5244,14.8643"/>
+        <dia:point val="19.1161,13.5509"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O15" connection="5"/>
+        <dia:connection handle="1" to="O12" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O17">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.5244,11.2917"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="19.062,9.92418;34.5785,11.3458"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.5244,11.2917"/>
+        <dia:point val="19.1161,9.97825"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O15" connection="0"/>
+        <dia:connection handle="1" to="O12" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O18">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.5337,14.8643"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="24.0713,13.4968;39.5878,14.9184"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="39.5337,14.8643"/>
+        <dia:point val="24.1254,13.5509"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O15" connection="7"/>
+        <dia:connection handle="1" to="O12" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O19">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.5337,11.2917"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="24.0713,9.92418;39.5878,11.3458"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="39.5337,11.2917"/>
+        <dia:point val="24.1254,9.97825"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O15" connection="2"/>
+        <dia:connection handle="1" to="O12" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O20">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.98,12.0742"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="39.93,12.0242;40.88,12.9742"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="39.98,12.0742"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O21">
+      <dia:attribute name="obj_pos">
+        <dia:point val="40.83,12.4992"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="40.78,12.1388;49.2453,12.8624"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="40.83,12.4992"/>
+        <dia:point val="49.1335,12.5007"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O20" connection="3"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O22">
+      <dia:attribute name="obj_pos">
+        <dia:point val="42.0954,11.99"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="42.0954,11.395;46.7404,12.1425"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 1 (source)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="42.0954,11.99"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O23">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-1.44491,11.6506"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-1.49491,11.6006;-0.54491,12.5506"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="-1.44491,11.6506"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O24">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-9.61991,12.09"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-9.67,11.7149;-1.33311,12.4385"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="-9.61991,12.09"/>
+        <dia:point val="-1.44491,12.0756"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O23" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O25">
+      <dia:attribute name="obj_pos">
+        <dia:point val="-7.39291,11.49"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="-7.39291,10.895;-3.58791,11.6425"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 0 (sink)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="-7.39291,11.49"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O26">
+      <dia:attribute name="obj_pos">
+        <dia:point val="19.4911,13.8333"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="19.4411,13.7833;24.5504,17.4559"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="19.4911,13.8333"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="5.009308462554376"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.5726155970598077"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Box" version="0" id="O27">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.4994,17.2967"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="34.4494,17.2467;39.5587,20.9193"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="34.4994,17.2967"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="5.009308462554376"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="3.5726155970598077"/>
+      </dia:attribute>
+      <dia:attribute name="border_width">
+        <dia:real val="0.10000000149011612"/>
+      </dia:attribute>
+      <dia:attribute name="border_color">
+        <dia:color val="#8b6914"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O28">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.4994,20.8693"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="19.4311,17.3459;34.5594,20.9293"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.4994,20.8693"/>
+        <dia:point val="19.4911,17.4059"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O27" connection="5"/>
+        <dia:connection handle="1" to="O26" connection="5"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O29">
+      <dia:attribute name="obj_pos">
+        <dia:point val="34.4994,17.2967"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="19.4311,13.7733;34.5594,17.3567"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="34.4994,17.2967"/>
+        <dia:point val="19.4911,13.8333"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O27" connection="0"/>
+        <dia:connection handle="1" to="O26" connection="0"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O30">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.5087,20.8693"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="24.4404,17.3459;39.5687,20.9293"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="39.5087,20.8693"/>
+        <dia:point val="24.5004,17.4059"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O27" connection="7"/>
+        <dia:connection handle="1" to="O26" connection="7"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O31">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.5087,17.2967"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="24.4404,13.7733;39.5687,17.3567"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="39.5087,17.2967"/>
+        <dia:point val="24.5004,13.8333"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#e60505"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="4"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O27" connection="2"/>
+        <dia:connection handle="1" to="O26" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Geometric - Perfect Circle" version="1" id="O32">
+      <dia:attribute name="obj_pos">
+        <dia:point val="39.855,18.7792"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="39.805,18.7292;40.755,19.6792"/>
+      </dia:attribute>
+      <dia:attribute name="meta">
+        <dia:composite type="dict"/>
+      </dia:attribute>
+      <dia:attribute name="elem_corner">
+        <dia:point val="39.855,18.7792"/>
+      </dia:attribute>
+      <dia:attribute name="elem_width">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="elem_height">
+        <dia:real val="0.84999999999999787"/>
+      </dia:attribute>
+      <dia:attribute name="line_width">
+        <dia:real val="0.10000000000000001"/>
+      </dia:attribute>
+      <dia:attribute name="line_colour">
+        <dia:color val="#000000"/>
+      </dia:attribute>
+      <dia:attribute name="fill_colour">
+        <dia:color val="#ffffff"/>
+      </dia:attribute>
+      <dia:attribute name="show_background">
+        <dia:boolean val="true"/>
+      </dia:attribute>
+      <dia:attribute name="line_style">
+        <dia:enum val="0"/>
+        <dia:real val="1"/>
+      </dia:attribute>
+      <dia:attribute name="flip_horizontal">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="flip_vertical">
+        <dia:boolean val="false"/>
+      </dia:attribute>
+      <dia:attribute name="subscale">
+        <dia:real val="1"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O33">
+      <dia:attribute name="obj_pos">
+        <dia:point val="40.705,19.2042"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="40.655,18.8438;49.1203,19.5674"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="40.705,19.2042"/>
+        <dia:point val="49.0085,19.2057"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="0" to="O32" connection="3"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Text" version="1" id="O34">
+      <dia:attribute name="obj_pos">
+        <dia:point val="41.9704,18.695"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="41.9704,18.1;46.6154,18.8475"/>
+      </dia:attribute>
+      <dia:attribute name="text">
+        <dia:composite type="text">
+          <dia:attribute name="string">
+            <dia:string>#pad 2 (source)#</dia:string>
+          </dia:attribute>
+          <dia:attribute name="font">
+            <dia:font family="sans" style="0" name="Helvetica"/>
+          </dia:attribute>
+          <dia:attribute name="height">
+            <dia:real val="0.80000000000000004"/>
+          </dia:attribute>
+          <dia:attribute name="pos">
+            <dia:point val="41.9704,18.695"/>
+          </dia:attribute>
+          <dia:attribute name="color">
+            <dia:color val="#000000"/>
+          </dia:attribute>
+          <dia:attribute name="alignment">
+            <dia:enum val="0"/>
+          </dia:attribute>
+        </dia:composite>
+      </dia:attribute>
+      <dia:attribute name="valign">
+        <dia:enum val="3"/>
+      </dia:attribute>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O35">
+      <dia:attribute name="obj_pos">
+        <dia:point val="27.3,9.55"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="24.0146,9.49376;27.3562,10.255"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="27.3,9.55"/>
+        <dia:point val="24.1254,9.97825"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O12" connection="2"/>
+      </dia:connections>
+    </dia:object>
+    <dia:object type="Standard - Line" version="0" id="O36">
+      <dia:attribute name="obj_pos">
+        <dia:point val="27.3454,9.53624"/>
+      </dia:attribute>
+      <dia:attribute name="obj_bb">
+        <dia:rectangle val="24.4311,9.46695;27.4147,13.9265"/>
+      </dia:attribute>
+      <dia:attribute name="conn_endpoints">
+        <dia:point val="27.3454,9.53624"/>
+        <dia:point val="24.5004,13.8333"/>
+      </dia:attribute>
+      <dia:attribute name="numcp">
+        <dia:int val="1"/>
+      </dia:attribute>
+      <dia:attribute name="line_color">
+        <dia:color val="#a020f0"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow">
+        <dia:enum val="22"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_length">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:attribute name="end_arrow_width">
+        <dia:real val="0.5"/>
+      </dia:attribute>
+      <dia:connections>
+        <dia:connection handle="1" to="O26" connection="2"/>
+      </dia:connections>
+    </dia:object>
+  </dia:layer>
+</dia:diagram>
diff --git a/Documentation/DocBook/media/v4l/subdev-image-processing-scaling-multi-source.svg b/Documentation/DocBook/media/v4l/subdev-image-processing-scaling-multi-source.svg
new file mode 100644
index 000000000000..2340c0f8bc92
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/subdev-image-processing-scaling-multi-source.svg
@@ -0,0 +1,116 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.0//EN" "http://www.w3.org/TR/2001/PR-SVG-20010719/DTD/svg10.dtd">
+<svg width="59cm" height="17cm" viewBox="-194 128 1179 330" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
+  <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x="-8" y="130" width="806" height="327"/>
+  <g>
+    <rect style="fill: #ffffff" x="4.5" y="189" width="159" height="104"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a52a2a" x="4.5" y="189" width="159" height="104"/>
+  </g>
+  <g>
+    <rect style="fill: #ffffff" x="49.5" y="204" width="94" height="77"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #0000ff" x="49.5" y="204" width="94" height="77"/>
+  </g>
+  <text style="fill: #0000ff;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="60" y="224">
+    <tspan x="60" y="224">sink</tspan>
+    <tspan x="60" y="240">crop</tspan>
+    <tspan x="60" y="256">selection</tspan>
+  </text>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="29.5" y="158">
+    <tspan x="29.5" y="158"></tspan>
+  </text>
+  <text style="fill: #a52a2a;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="8.53836" y="157.914">
+    <tspan x="8.53836" y="157.914">sink media</tspan>
+    <tspan x="8.53836" y="173.914">bus format</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="333.644" y="185.65" width="165.2" height="172.478"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #00ff00" x="333.644" y="185.65" width="165.2" height="172.478"/>
+  </g>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="333.644" y1="358.128" x2="49.5" y2="281"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="333.644" y1="185.65" x2="49.5" y2="204"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="498.844" y1="358.128" x2="143.5" y2="281"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="498.844" y1="185.65" x2="143.5" y2="204"/>
+  <text style="fill: #00ff00;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="334.704" y="149.442">
+    <tspan x="334.704" y="149.442">sink compose</tspan>
+    <tspan x="334.704" y="165.442">selection (scaling)</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="382.322" y="199.565" width="100.186" height="71.4523"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x="382.322" y="199.565" width="100.186" height="71.4523"/>
+  </g>
+  <text style="fill: #a020f0;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="543.322" y="149.442">
+    <tspan x="543.322" y="149.442">source</tspan>
+    <tspan x="543.322" y="165.442">crop</tspan>
+    <tspan x="543.322" y="181.442">selection</tspan>
+  </text>
+  <text style="fill: #8b6914;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="691.5" y="157.128">
+    <tspan x="691.5" y="157.128">source media</tspan>
+    <tspan x="691.5" y="173.128">bus format</tspan>
+  </text>
+  <g>
+    <rect style="fill: #ffffff" x="690.488" y="225.834" width="100.186" height="71.4523"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #8b6914" x="690.488" y="225.834" width="100.186" height="71.4523"/>
+  </g>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="690.488" y1="297.286" x2="382.322" y2="271.018"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="690.488" y1="225.834" x2="382.322" y2="199.565"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="790.674" y1="297.286" x2="482.508" y2="271.018"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="790.674" y1="225.834" x2="482.508" y2="199.565"/>
+  <g>
+    <ellipse style="fill: #ffffff" cx="808.1" cy="249.984" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="808.1" cy="249.984" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="808.1" cy="249.984" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="816.6" y1="249.984" x2="972.934" y2="250.012"/>
+    <polygon style="fill: #000000" points="980.434,250.014 970.433,255.012 972.934,250.012 970.435,245.012 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="980.434,250.014 970.433,255.012 972.934,250.012 970.435,245.012 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="841.908" y="239.8">
+    <tspan x="841.908" y="239.8">pad 1 (source)</tspan>
+  </text>
+  <g>
+    <ellipse style="fill: #ffffff" cx="-20.3982" cy="241.512" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-20.3982" cy="241.512" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="-20.3982" cy="241.512" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="-192.398" y1="241.8" x2="-38.6343" y2="241.529"/>
+    <polygon style="fill: #000000" points="-31.1343,241.516 -41.1254,246.534 -38.6343,241.529 -41.1431,236.534 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="-31.1343,241.516 -41.1254,246.534 -38.6343,241.529 -41.1431,236.534 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="-147.858" y="229.8">
+    <tspan x="-147.858" y="229.8">pad 0 (sink)</tspan>
+  </text>
+  <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x="389.822" y="276.666" width="100.186" height="71.4523"/>
+  <g>
+    <rect style="fill: #ffffff" x="689.988" y="345.934" width="100.186" height="71.4523"/>
+    <rect style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #8b6914" x="689.988" y="345.934" width="100.186" height="71.4523"/>
+  </g>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="689.988" y1="417.386" x2="389.822" y2="348.118"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="689.988" y1="345.934" x2="389.822" y2="276.666"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="790.174" y1="417.386" x2="490.008" y2="348.118"/>
+  <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke-dasharray: 4; stroke: #e60505" x1="790.174" y1="345.934" x2="490.008" y2="276.666"/>
+  <g>
+    <ellipse style="fill: #ffffff" cx="805.6" cy="384.084" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="805.6" cy="384.084" rx="8.5" ry="8.5"/>
+    <ellipse style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" cx="805.6" cy="384.084" rx="8.5" ry="8.5"/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" x1="814.1" y1="384.084" x2="970.434" y2="384.112"/>
+    <polygon style="fill: #000000" points="977.934,384.114 967.933,389.112 970.434,384.112 967.935,379.112 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #000000" points="977.934,384.114 967.933,389.112 970.434,384.112 967.935,379.112 "/>
+  </g>
+  <text style="fill: #000000;text-anchor:start;font-size:12.8;font-family:sanserif;font-style:normal;font-weight:normal" x="839.408" y="373.9">
+    <tspan x="839.408" y="373.9">pad 2 (source)</tspan>
+  </text>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x1="546" y1="191" x2="492.157" y2="198.263"/>
+    <polygon style="fill: #a020f0" points="484.724,199.266 493.966,192.974 492.157,198.263 495.303,202.884 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" points="484.724,199.266 493.966,192.974 492.157,198.263 495.303,202.884 "/>
+  </g>
+  <g>
+    <line style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" x1="546.908" y1="190.725" x2="495.383" y2="268.548"/>
+    <polygon style="fill: #a020f0" points="491.242,274.802 492.594,263.703 495.383,268.548 500.932,269.224 "/>
+    <polygon style="fill: none; fill-opacity:0; stroke-width: 2; stroke: #a020f0" points="491.242,274.802 492.594,263.703 495.383,268.548 500.932,269.224 "/>
+  </g>
+</svg>
diff --git a/Documentation/DocBook/media/v4l/v4l2.xml b/Documentation/DocBook/media/v4l/v4l2.xml
index 8ae38876172e..015c561754b7 100644
--- a/Documentation/DocBook/media/v4l/v4l2.xml
+++ b/Documentation/DocBook/media/v4l/v4l2.xml
@@ -28,8 +28,8 @@ documentation.</contrib>
 	<firstname>Hans</firstname>
 	<surname>Verkuil</surname>
 	<contrib>Designed and documented the VIDIOC_LOG_STATUS ioctl,
-the extended control ioctls and major parts of the sliced VBI
-API.</contrib>
+the extended control ioctls, major parts of the sliced VBI API, the
+MPEG encoder and decoder APIs and the DV Timings API.</contrib>
 	<affiliation>
 	  <address>
 	    <email>hverkuil@xs4all.nl</email>
@@ -96,6 +96,17 @@ Remote Controller chapter.</contrib>
 	  </address>
 	</affiliation>
       </author>
+
+      <author>
+	<firstname>Sakari</firstname>
+	<surname>Ailus</surname>
+	<contrib>Subdev selections API.</contrib>
+	<affiliation>
+	  <address>
+	    <email>sakari.ailus@iki.fi</email>
+	  </address>
+	</affiliation>
+      </author>
     </authorgroup>
 
     <copyright>
@@ -112,6 +123,7 @@ Remote Controller chapter.</contrib>
       <year>2009</year>
       <year>2010</year>
       <year>2011</year>
+      <year>2012</year>
       <holder>Bill Dirks, Michael H. Schimek, Hans Verkuil, Martin
 Rubli, Andy Walls, Muralidharan Karicheri, Mauro Carvalho Chehab,
 	Pawel Osciak</holder>
@@ -128,6 +140,28 @@ structs, ioctls) must be noted in more detail in the history chapter
 applications. -->
 
       <revision>
+	<revnumber>3.5</revnumber>
+	<date>2012-05-07</date>
+	<authorinitials>sa, sn</authorinitials>
+	<revremark>Added V4L2_CTRL_TYPE_INTEGER_MENU and V4L2 subdev
+	    selections API. Improved the description of V4L2_CID_COLORFX
+	    control, added V4L2_CID_COLORFX_CBCR control.
+	    Added camera controls V4L2_CID_AUTO_EXPOSURE_BIAS,
+	    V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE, V4L2_CID_IMAGE_STABILIZATION,
+	    V4L2_CID_ISO_SENSITIVITY, V4L2_CID_ISO_SENSITIVITY_AUTO,
+	    V4L2_CID_EXPOSURE_METERING, V4L2_CID_SCENE_MODE,
+	    V4L2_CID_3A_LOCK, V4L2_CID_AUTO_FOCUS_START,
+	    V4L2_CID_AUTO_FOCUS_STOP, V4L2_CID_AUTO_FOCUS_STATUS
+	    and V4L2_CID_AUTO_FOCUS_RANGE.
+	</revremark>
+	<date>2012-05-01</date>
+	<authorinitials>hv</authorinitials>
+	<revremark>Added VIDIOC_ENUM_DV_TIMINGS, VIDIOC_QUERY_DV_TIMINGS and
+	VIDIOC_DV_TIMINGS_CAP.
+	</revremark>
+      </revision>
+
+      <revision>
 	<revnumber>3.4</revnumber>
 	<date>2012-01-25</date>
 	<authorinitials>sn</authorinitials>
@@ -433,7 +467,7 @@ and discussions on the V4L mailing list.</revremark>
 </partinfo>
 
 <title>Video for Linux Two API Specification</title>
- <subtitle>Revision 3.3</subtitle>
+ <subtitle>Revision 3.5</subtitle>
 
   <chapter id="common">
     &sub-common;
@@ -491,10 +525,12 @@ and discussions on the V4L mailing list.</revremark>
     &sub-dbg-g-register;
     &sub-decoder-cmd;
     &sub-dqevent;
+    &sub-dv-timings-cap;
     &sub-encoder-cmd;
     &sub-enumaudio;
     &sub-enumaudioout;
     &sub-enum-dv-presets;
+    &sub-enum-dv-timings;
     &sub-enum-fmt;
     &sub-enum-framesizes;
     &sub-enum-frameintervals;
@@ -529,6 +565,7 @@ and discussions on the V4L mailing list.</revremark>
     &sub-querycap;
     &sub-queryctrl;
     &sub-query-dv-preset;
+    &sub-query-dv-timings;
     &sub-querystd;
     &sub-prepare-buf;
     &sub-reqbufs;
@@ -540,6 +577,7 @@ and discussions on the V4L mailing list.</revremark>
     &sub-subdev-g-crop;
     &sub-subdev-g-fmt;
     &sub-subdev-g-frame-interval;
+    &sub-subdev-g-selection;
     &sub-subscribe-event;
     <!-- End of ioctls. -->
     &sub-mmap;
diff --git a/Documentation/DocBook/media/v4l/vidioc-create-bufs.xml b/Documentation/DocBook/media/v4l/vidioc-create-bufs.xml
index 73ae8a6cd004..765549ff8a71 100644
--- a/Documentation/DocBook/media/v4l/vidioc-create-bufs.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-create-bufs.xml
@@ -48,6 +48,12 @@
   <refsect1>
     <title>Description</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 ioctl is used to create buffers for <link linkend="mmap">memory
 mapped</link> or <link linkend="userp">user pointer</link>
 I/O. It can be used as an alternative or in addition to the
@@ -94,16 +100,18 @@ information.</para>
 	    <entry>The number of buffers requested or granted.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-memory;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>memory</structfield></entry>
 	    <entry>Applications set this field to
 <constant>V4L2_MEMORY_MMAP</constant> or
-<constant>V4L2_MEMORY_USERPTR</constant>.</entry>
+<constant>V4L2_MEMORY_USERPTR</constant>. See <xref linkend="v4l2-memory"
+/></entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-format;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>format</structfield></entry>
-	    <entry>Filled in by the application, preserved by the driver.</entry>
+	    <entry>Filled in by the application, preserved by the driver.
+	    See <xref linkend="v4l2-format" />.</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-cropcap.xml b/Documentation/DocBook/media/v4l/vidioc-cropcap.xml
index b4f2f255211e..f1bac2c6e978 100644
--- a/Documentation/DocBook/media/v4l/vidioc-cropcap.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-cropcap.xml
@@ -65,7 +65,7 @@ output.</para>
 	&cs-str;
 	<tbody valign="top">
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>Type of the data stream, set by the application.
 Only these types are valid here:
@@ -73,7 +73,7 @@ Only these types are valid here:
 <constant>V4L2_BUF_TYPE_VIDEO_OUTPUT</constant>,
 <constant>V4L2_BUF_TYPE_VIDEO_OVERLAY</constant>, and custom (driver
 defined) types with code <constant>V4L2_BUF_TYPE_PRIVATE</constant>
-and higher.</entry>
+and higher. See <xref linkend="v4l2-buf-type" />.</entry>
 	  </row>
 	  <row>
 	    <entry>struct <link linkend="v4l2-rect-crop">v4l2_rect</link></entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-dv-timings-cap.xml b/Documentation/DocBook/media/v4l/vidioc-dv-timings-cap.xml
new file mode 100644
index 000000000000..6673ce582050
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/vidioc-dv-timings-cap.xml
@@ -0,0 +1,211 @@
+<refentry id="vidioc-dv-timings-cap">
+  <refmeta>
+    <refentrytitle>ioctl VIDIOC_DV_TIMINGS_CAP</refentrytitle>
+    &manvol;
+  </refmeta>
+
+  <refnamediv>
+    <refname>VIDIOC_DV_TIMINGS_CAP</refname>
+    <refpurpose>The capabilities of the Digital Video receiver/transmitter</refpurpose>
+  </refnamediv>
+
+  <refsynopsisdiv>
+    <funcsynopsis>
+      <funcprototype>
+	<funcdef>int <function>ioctl</function></funcdef>
+	<paramdef>int <parameter>fd</parameter></paramdef>
+	<paramdef>int <parameter>request</parameter></paramdef>
+	<paramdef>struct v4l2_dv_timings_cap *<parameter>argp</parameter></paramdef>
+      </funcprototype>
+    </funcsynopsis>
+  </refsynopsisdiv>
+
+  <refsect1>
+    <title>Arguments</title>
+
+    <variablelist>
+      <varlistentry>
+	<term><parameter>fd</parameter></term>
+	<listitem>
+	  <para>&fd;</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>request</parameter></term>
+	<listitem>
+	  <para>VIDIOC_DV_TIMINGS_CAP</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>argp</parameter></term>
+	<listitem>
+	  <para></para>
+	</listitem>
+      </varlistentry>
+    </variablelist>
+  </refsect1>
+
+  <refsect1>
+    <title>Description</title>
+
+    <note>
+      <title>Experimental</title>
+      <para>This is an <link linkend="experimental"> experimental </link>
+      interface and may change in the future.</para>
+    </note>
+
+    <para>To query the available timings, applications initialize the
+<structfield>index</structfield> field and zero the reserved array of &v4l2-dv-timings-cap;
+and call the <constant>VIDIOC_DV_TIMINGS_CAP</constant> ioctl with a pointer to this
+structure. Drivers fill the rest of the structure or return an
+&EINVAL; when the index is out of bounds. To enumerate all supported DV timings,
+applications shall begin at index zero, incrementing by one until the
+driver returns <errorcode>EINVAL</errorcode>. Note that drivers may enumerate a
+different set of DV timings after switching the video input or
+output.</para>
+
+    <table pgwide="1" frame="none" id="v4l2-bt-timings-cap">
+      <title>struct <structname>v4l2_bt_timings_cap</structname></title>
+      <tgroup cols="3">
+	&cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>min_width</structfield></entry>
+	    <entry>Minimum width of the active video in pixels.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>max_width</structfield></entry>
+	    <entry>Maximum width of the active video in pixels.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>min_height</structfield></entry>
+	    <entry>Minimum height of the active video in lines.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>max_height</structfield></entry>
+	    <entry>Maximum height of the active video in lines.</entry>
+	  </row>
+	  <row>
+	    <entry>__u64</entry>
+	    <entry><structfield>min_pixelclock</structfield></entry>
+	    <entry>Minimum pixelclock frequency in Hz.</entry>
+	  </row>
+	  <row>
+	    <entry>__u64</entry>
+	    <entry><structfield>max_pixelclock</structfield></entry>
+	    <entry>Maximum pixelclock frequency in Hz.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>standards</structfield></entry>
+	    <entry>The video standard(s) supported by the hardware.
+	    See <xref linkend="dv-bt-standards"/> for a list of standards.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>capabilities</structfield></entry>
+	    <entry>Several flags giving more information about the capabilities.
+	    See <xref linkend="dv-bt-cap-capabilities"/> for a description of the flags.
+	    </entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>reserved</structfield>[16]</entry>
+	    <entry></entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+
+    <table pgwide="1" frame="none" id="v4l2-dv-timings-cap">
+      <title>struct <structname>v4l2_dv_timings_cap</structname></title>
+      <tgroup cols="4">
+	&cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>type</structfield></entry>
+	    <entry>Type of DV timings as listed in <xref linkend="dv-timing-types"/>.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>reserved</structfield>[3]</entry>
+	    <entry>Reserved for future extensions. Drivers must set the array to zero.</entry>
+	  </row>
+	  <row>
+	    <entry>union</entry>
+	    <entry><structfield></structfield></entry>
+	    <entry></entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry>&v4l2-bt-timings-cap;</entry>
+	    <entry><structfield>bt</structfield></entry>
+	    <entry>BT.656/1120 timings capabilities of the hardware.</entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry>__u32</entry>
+	    <entry><structfield>raw_data</structfield>[32]</entry>
+	    <entry></entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+
+    <table pgwide="1" frame="none" id="dv-bt-cap-capabilities">
+      <title>DV BT Timing capabilities</title>
+      <tgroup cols="2">
+	&cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>Flag</entry>
+	    <entry>Description</entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry></entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_CAP_INTERLACED</entry>
+	    <entry>Interlaced formats are supported.
+	    </entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_CAP_PROGRESSIVE</entry>
+	    <entry>Progressive formats are supported.
+	    </entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_CAP_REDUCED_BLANKING</entry>
+	    <entry>CVT/GTF specific: the timings can make use of reduced blanking (CVT)
+or the 'Secondary GTF' curve (GTF).
+	    </entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_CAP_CUSTOM</entry>
+	    <entry>Can support non-standard timings, i.e. timings not belonging to the
+standards set in the <structfield>standards</structfield> field.
+	    </entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+  </refsect1>
+
+  <refsect1>
+    &return-value;
+  </refsect1>
+</refentry>
+
+<!--
+Local Variables:
+mode: sgml
+sgml-parent-document: "v4l2.sgml"
+indent-tabs-mode: nil
+End:
+-->
diff --git a/Documentation/DocBook/media/v4l/vidioc-enum-dv-presets.xml b/Documentation/DocBook/media/v4l/vidioc-enum-dv-presets.xml
index 0be17c232d3a..509f0012d2a6 100644
--- a/Documentation/DocBook/media/v4l/vidioc-enum-dv-presets.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-enum-dv-presets.xml
@@ -48,6 +48,10 @@
   <refsect1>
     <title>Description</title>
 
+    <para>This ioctl is <emphasis role="bold">deprecated</emphasis>.
+    New drivers and applications should use &VIDIOC-ENUM-DV-TIMINGS; instead.
+    </para>
+
     <para>To query the attributes of a DV preset, applications initialize the
 <structfield>index</structfield> field and zero the reserved array of &v4l2-dv-enum-preset;
 and call the <constant>VIDIOC_ENUM_DV_PRESETS</constant> ioctl with a pointer to this
diff --git a/Documentation/DocBook/media/v4l/vidioc-enum-dv-timings.xml b/Documentation/DocBook/media/v4l/vidioc-enum-dv-timings.xml
new file mode 100644
index 000000000000..24c3bf4fd29a
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/vidioc-enum-dv-timings.xml
@@ -0,0 +1,119 @@
+<refentry id="vidioc-enum-dv-timings">
+  <refmeta>
+    <refentrytitle>ioctl VIDIOC_ENUM_DV_TIMINGS</refentrytitle>
+    &manvol;
+  </refmeta>
+
+  <refnamediv>
+    <refname>VIDIOC_ENUM_DV_TIMINGS</refname>
+    <refpurpose>Enumerate supported Digital Video timings</refpurpose>
+  </refnamediv>
+
+  <refsynopsisdiv>
+    <funcsynopsis>
+      <funcprototype>
+	<funcdef>int <function>ioctl</function></funcdef>
+	<paramdef>int <parameter>fd</parameter></paramdef>
+	<paramdef>int <parameter>request</parameter></paramdef>
+	<paramdef>struct v4l2_enum_dv_timings *<parameter>argp</parameter></paramdef>
+      </funcprototype>
+    </funcsynopsis>
+  </refsynopsisdiv>
+
+  <refsect1>
+    <title>Arguments</title>
+
+    <variablelist>
+      <varlistentry>
+	<term><parameter>fd</parameter></term>
+	<listitem>
+	  <para>&fd;</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>request</parameter></term>
+	<listitem>
+	  <para>VIDIOC_ENUM_DV_TIMINGS</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>argp</parameter></term>
+	<listitem>
+	  <para></para>
+	</listitem>
+      </varlistentry>
+    </variablelist>
+  </refsect1>
+
+  <refsect1>
+    <title>Description</title>
+
+    <note>
+      <title>Experimental</title>
+      <para>This is an <link linkend="experimental"> experimental </link>
+      interface and may change in the future.</para>
+    </note>
+
+    <para>While some DV receivers or transmitters support a wide range of timings, others
+support only a limited number of timings. With this ioctl applications can enumerate a list
+of known supported timings. Call &VIDIOC-DV-TIMINGS-CAP; to check if it also supports other
+standards or even custom timings that are not in this list.</para>
+
+    <para>To query the available timings, applications initialize the
+<structfield>index</structfield> field and zero the reserved array of &v4l2-enum-dv-timings;
+and call the <constant>VIDIOC_ENUM_DV_TIMINGS</constant> ioctl with a pointer to this
+structure. Drivers fill the rest of the structure or return an
+&EINVAL; when the index is out of bounds. To enumerate all supported DV timings,
+applications shall begin at index zero, incrementing by one until the
+driver returns <errorcode>EINVAL</errorcode>. Note that drivers may enumerate a
+different set of DV timings after switching the video input or
+output.</para>
+
+    <table pgwide="1" frame="none" id="v4l2-enum-dv-timings">
+      <title>struct <structname>v4l2_enum_dv_timings</structname></title>
+      <tgroup cols="3">
+	&cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>index</structfield></entry>
+	    <entry>Number of the DV timings, set by the
+application.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>reserved</structfield>[3]</entry>
+	    <entry>Reserved for future extensions. Drivers must set the array to zero.</entry>
+	  </row>
+	  <row>
+	    <entry>&v4l2-dv-timings;</entry>
+	    <entry><structfield>timings</structfield></entry>
+	    <entry>The timings.</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+  </refsect1>
+
+  <refsect1>
+    &return-value;
+
+    <variablelist>
+      <varlistentry>
+	<term><errorcode>EINVAL</errorcode></term>
+	<listitem>
+	  <para>The &v4l2-enum-dv-timings; <structfield>index</structfield>
+is out of bounds.</para>
+	</listitem>
+      </varlistentry>
+    </variablelist>
+  </refsect1>
+</refentry>
+
+<!--
+Local Variables:
+mode: sgml
+sgml-parent-document: "v4l2.sgml"
+indent-tabs-mode: nil
+End:
+-->
diff --git a/Documentation/DocBook/media/v4l/vidioc-enum-fmt.xml b/Documentation/DocBook/media/v4l/vidioc-enum-fmt.xml
index 347d142e7431..81ebe48317fe 100644
--- a/Documentation/DocBook/media/v4l/vidioc-enum-fmt.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-enum-fmt.xml
@@ -71,7 +71,7 @@ the application. This is in no way related to the <structfield>
 pixelformat</structfield> field.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>Type of the data stream, set by the application.
 Only these types are valid here:
@@ -81,7 +81,7 @@ Only these types are valid here:
 <constant>V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE</constant>,
 <constant>V4L2_BUF_TYPE_VIDEO_OVERLAY</constant>, and custom (driver
 defined) types with code <constant>V4L2_BUF_TYPE_PRIVATE</constant>
-and higher.</entry>
+and higher. See <xref linkend="v4l2-buf-type" />.</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-enuminput.xml b/Documentation/DocBook/media/v4l/vidioc-enuminput.xml
index 9b8efcd6e947..46d5a044a537 100644
--- a/Documentation/DocBook/media/v4l/vidioc-enuminput.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-enuminput.xml
@@ -285,7 +285,7 @@ input/output interface to linux-media@vger.kernel.org on 19 Oct 2009.
 	  <row>
 	    <entry><constant>V4L2_IN_CAP_CUSTOM_TIMINGS</constant></entry>
 	    <entry>0x00000002</entry>
-	    <entry>This input supports setting custom video timings by using VIDIOC_S_DV_TIMINGS.</entry>
+	    <entry>This input supports setting video timings by using VIDIOC_S_DV_TIMINGS.</entry>
 	  </row>
 	  <row>
 	    <entry><constant>V4L2_IN_CAP_STD</constant></entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml b/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml
index a64d5ef103fa..428020000ef0 100644
--- a/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml
@@ -170,7 +170,7 @@ input/output interface to linux-media@vger.kernel.org on 19 Oct 2009.
 	  <row>
 	    <entry><constant>V4L2_OUT_CAP_CUSTOM_TIMINGS</constant></entry>
 	    <entry>0x00000002</entry>
-	    <entry>This output supports setting custom video timings by using VIDIOC_S_DV_TIMINGS.</entry>
+	    <entry>This output supports setting video timings by using VIDIOC_S_DV_TIMINGS.</entry>
 	  </row>
 	  <row>
 	    <entry><constant>V4L2_OUT_CAP_STD</constant></entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-crop.xml b/Documentation/DocBook/media/v4l/vidioc-g-crop.xml
index 01a50640dce0..c4ff3b1887fb 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-crop.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-crop.xml
@@ -100,14 +100,14 @@ changed and <constant>VIDIOC_S_CROP</constant> returns the
 	&cs-str;
 	<tbody valign="top">
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>Type of the data stream, set by the application.
 Only these types are valid here: <constant>V4L2_BUF_TYPE_VIDEO_CAPTURE</constant>,
 <constant>V4L2_BUF_TYPE_VIDEO_OUTPUT</constant>,
 <constant>V4L2_BUF_TYPE_VIDEO_OVERLAY</constant>, and custom (driver
 defined) types with code <constant>V4L2_BUF_TYPE_PRIVATE</constant>
-and higher.</entry>
+and higher. See <xref linkend="v4l2-buf-type" />.</entry>
 	  </row>
 	  <row>
 	    <entry>&v4l2-rect;</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-dv-preset.xml b/Documentation/DocBook/media/v4l/vidioc-g-dv-preset.xml
index 7940c1149393..61be9fa3803a 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-dv-preset.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-dv-preset.xml
@@ -48,6 +48,12 @@
 
   <refsect1>
     <title>Description</title>
+
+    <para>These ioctls are <emphasis role="bold">deprecated</emphasis>.
+    New drivers and applications should use &VIDIOC-G-DV-TIMINGS; and &VIDIOC-S-DV-TIMINGS;
+    instead.
+    </para>
+
     <para>To query and select the current DV preset, applications
 use the <constant>VIDIOC_G_DV_PRESET</constant> and <constant>VIDIOC_S_DV_PRESET</constant>
 ioctls which take a pointer to a &v4l2-dv-preset; type as argument.
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-dv-timings.xml b/Documentation/DocBook/media/v4l/vidioc-g-dv-timings.xml
index 4a8648ae9a63..eda1a2991bbe 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-dv-timings.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-dv-timings.xml
@@ -7,7 +7,7 @@
   <refnamediv>
     <refname>VIDIOC_G_DV_TIMINGS</refname>
     <refname>VIDIOC_S_DV_TIMINGS</refname>
-    <refpurpose>Get or set custom DV timings for input or output</refpurpose>
+    <refpurpose>Get or set DV timings for input or output</refpurpose>
   </refnamediv>
 
   <refsynopsisdiv>
@@ -48,12 +48,15 @@
 
   <refsect1>
     <title>Description</title>
-    <para>To set custom DV timings for the input or output, applications use the
-<constant>VIDIOC_S_DV_TIMINGS</constant> ioctl and to get the current custom timings,
+    <para>To set DV timings for the input or output, applications use the
+<constant>VIDIOC_S_DV_TIMINGS</constant> ioctl and to get the current timings,
 applications use the <constant>VIDIOC_G_DV_TIMINGS</constant> ioctl. The detailed timing
 information is filled in using the structure &v4l2-dv-timings;. These ioctls take
 a pointer to the &v4l2-dv-timings; structure as argument. If the ioctl is not supported
 or the timing values are not correct, the driver returns &EINVAL;.</para>
+<para>The <filename>linux/v4l2-dv-timings.h</filename> header can be used to get the
+timings of the formats in the <xref linkend="cea861" /> and <xref linkend="vesadmt" />
+standards.</para>
   </refsect1>
 
   <refsect1>
@@ -83,12 +86,13 @@ or the timing values are not correct, the driver returns &EINVAL;.</para>
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>width</structfield></entry>
-	    <entry>Width of the active video in pixels</entry>
+	    <entry>Width of the active video in pixels.</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>height</structfield></entry>
-	    <entry>Height of the active video in lines</entry>
+	    <entry>Height of the active video frame in lines. So for interlaced formats the
+	    height of the active video in each field is <structfield>height</structfield>/2.</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
@@ -125,32 +129,52 @@ bit 0 (V4L2_DV_VSYNC_POS_POL) is for vertical sync polarity and bit 1 (V4L2_DV_H
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>vfrontporch</structfield></entry>
-	    <entry>Vertical front porch in lines</entry>
+	    <entry>Vertical front porch in lines. For interlaced formats this refers to the
+	    odd field (aka field 1).</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>vsync</structfield></entry>
-	    <entry>Vertical sync length in lines</entry>
+	    <entry>Vertical sync length in lines. For interlaced formats this refers to the
+	    odd field (aka field 1).</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>vbackporch</structfield></entry>
-	    <entry>Vertical back porch in lines</entry>
+	    <entry>Vertical back porch in lines. For interlaced formats this refers to the
+	    odd field (aka field 1).</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>il_vfrontporch</structfield></entry>
-	    <entry>Vertical front porch in lines for bottom field of interlaced field formats</entry>
+	    <entry>Vertical front porch in lines for the even field (aka field 2) of
+	    interlaced field formats.</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>il_vsync</structfield></entry>
-	    <entry>Vertical sync length in lines for bottom field of interlaced field formats</entry>
+	    <entry>Vertical sync length in lines for the even field (aka field 2) of
+	    interlaced field formats.</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
 	    <entry><structfield>il_vbackporch</structfield></entry>
-	    <entry>Vertical back porch in lines for bottom field of interlaced field formats</entry>
+	    <entry>Vertical back porch in lines for the even field (aka field 2) of
+	    interlaced field formats.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>standards</structfield></entry>
+	    <entry>The video standard(s) this format belongs to. This will be filled in by
+	    the driver. Applications must set this to 0. See <xref linkend="dv-bt-standards"/>
+	    for a list of standards.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>flags</structfield></entry>
+	    <entry>Several flags giving more information about the format.
+	    See <xref linkend="dv-bt-flags"/> for a description of the flags.
+	    </entry>
 	  </row>
 	</tbody>
       </tgroup>
@@ -211,6 +235,90 @@ bit 0 (V4L2_DV_VSYNC_POS_POL) is for vertical sync polarity and bit 1 (V4L2_DV_H
 	</tbody>
       </tgroup>
     </table>
+    <table pgwide="1" frame="none" id="dv-bt-standards">
+      <title>DV BT Timing standards</title>
+      <tgroup cols="2">
+	&cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>Timing standard</entry>
+	    <entry>Description</entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry></entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_STD_CEA861</entry>
+	    <entry>The timings follow the CEA-861 Digital TV Profile standard</entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_STD_DMT</entry>
+	    <entry>The timings follow the VESA Discrete Monitor Timings standard</entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_STD_CVT</entry>
+	    <entry>The timings follow the VESA Coordinated Video Timings standard</entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_BT_STD_GTF</entry>
+	    <entry>The timings follow the VESA Generalized Timings Formula standard</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+    <table pgwide="1" frame="none" id="dv-bt-flags">
+      <title>DV BT Timing flags</title>
+      <tgroup cols="2">
+	&cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>Flag</entry>
+	    <entry>Description</entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry></entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_FL_REDUCED_BLANKING</entry>
+	    <entry>CVT/GTF specific: the timings use reduced blanking (CVT) or the 'Secondary
+GTF' curve (GTF). In both cases the horizontal and/or vertical blanking
+intervals are reduced, allowing a higher resolution over the same
+bandwidth. This is a read-only flag, applications must not set this.
+	    </entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_FL_CAN_REDUCE_FPS</entry>
+	    <entry>CEA-861 specific: set for CEA-861 formats with a framerate that is a multiple
+of six. These formats can be optionally played at 1 / 1.001 speed to
+be compatible with 60 Hz based standards such as NTSC and PAL-M that use a framerate of
+29.97 frames per second. If the transmitter can't generate such frequencies, then the
+flag will also be cleared. This is a read-only flag, applications must not set this.
+	    </entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_FL_REDUCED_FPS</entry>
+	    <entry>CEA-861 specific: only valid for video transmitters, the flag is cleared
+by receivers. It is also only valid for formats with the V4L2_DV_FL_CAN_REDUCE_FPS flag
+set, for other formats the flag will be cleared by the driver.
+
+If the application sets this flag, then the pixelclock used to set up the transmitter is
+divided by 1.001 to make it compatible with NTSC framerates. If the transmitter
+can't generate such frequencies, then the flag will also be cleared.
+	    </entry>
+	  </row>
+	  <row>
+	    <entry>V4L2_DV_FL_HALF_LINE</entry>
+	    <entry>Specific to interlaced formats: if set, then field 1 (aka the odd field)
+is really one half-line longer and field 2 (aka the even field) is really one half-line
+shorter, so each field has exactly the same number of half-lines. Whether half-lines can be
+detected or used depends on the hardware.
+	    </entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
   </refsect1>
   <refsect1>
     &return-value;
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml b/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml
index b17a7aac6997..e3d5afcdafbb 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml
@@ -265,6 +265,32 @@ These controls are described in <xref
 These controls are described in <xref
 		linkend="flash-controls" />.</entry>
 	  </row>
+	  <row>
+	    <entry><constant>V4L2_CTRL_CLASS_JPEG</constant></entry>
+	    <entry>0x9d0000</entry>
+	    <entry>The class containing JPEG compression controls.
+These controls are described in <xref
+		linkend="jpeg-controls" />.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CTRL_CLASS_IMAGE_SOURCE</constant></entry>
+	    <entry>0x9e0000</entry> <entry>The class containing image
+	    source controls. These controls are described in <xref
+	    linkend="image-source-controls" />.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CTRL_CLASS_IMAGE_PROC</constant></entry>
+	    <entry>0x9f0000</entry> <entry>The class containing image
+	    processing controls. These controls are described in <xref
+	    linkend="image-process-controls" />.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CTRL_CLASS_JPEG</constant></entry>
+	    <entry>0x9d0000</entry>
+	    <entry>The class containing JPEG compression controls.
+These controls are described in <xref
+		linkend="jpeg-controls" />.</entry>
+	  </row>
 	</tbody>
       </tgroup>
     </table>
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-fmt.xml b/Documentation/DocBook/media/v4l/vidioc-g-fmt.xml
index 17fbda15137b..52acff193a6f 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-fmt.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-fmt.xml
@@ -116,7 +116,7 @@ this ioctl.</para>
 	<colspec colname="c4" />
 	<tbody valign="top">
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry></entry>
 	    <entry>Type of the data stream, see <xref
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-frequency.xml b/Documentation/DocBook/media/v4l/vidioc-g-frequency.xml
index 66e9a5257861..69c178a4d205 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-frequency.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-frequency.xml
@@ -95,14 +95,14 @@ the &v4l2-output; <structfield>modulator</structfield> field and the
 &v4l2-modulator; <structfield>index</structfield> field.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-tuner-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>The tuner type. This is the same value as in the
-&v4l2-tuner; <structfield>type</structfield> field. The type must be set
+&v4l2-tuner; <structfield>type</structfield> field. See The type must be set
 to <constant>V4L2_TUNER_RADIO</constant> for <filename>/dev/radioX</filename>
 device nodes, and to <constant>V4L2_TUNER_ANALOG_TV</constant>
 for all others. The field is not applicable to modulators, &ie; ignored
-by drivers.</entry>
+by drivers. See <xref linkend="v4l2-tuner-type" /></entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-parm.xml b/Documentation/DocBook/media/v4l/vidioc-g-parm.xml
index 19b1d85dd668..f83d2cdd1185 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-parm.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-parm.xml
@@ -75,11 +75,12 @@ devices.</para>
 	&cs-ustr;
 	<tbody valign="top">
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry></entry>
 	    <entry>The buffer (stream) type, same as &v4l2-format;
-<structfield>type</structfield>, set by the application.</entry>
+<structfield>type</structfield>, set by the application. See <xref
+	    linkend="v4l2-buf-type" /></entry>
 	  </row>
 	  <row>
 	    <entry>union</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-sliced-vbi-cap.xml b/Documentation/DocBook/media/v4l/vidioc-g-sliced-vbi-cap.xml
index 71741daaf725..bd015d1563ff 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-sliced-vbi-cap.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-sliced-vbi-cap.xml
@@ -148,7 +148,7 @@ using the &VIDIOC-S-FMT; ioctl as described in <xref
 <structfield>service_lines</structfield>[1][0] to zero.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>Type of the data stream, see <xref
 		  linkend="v4l2-buf-type" />. Should be
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-tuner.xml b/Documentation/DocBook/media/v4l/vidioc-g-tuner.xml
index 91ec2fb658f8..62a1aa200a36 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-tuner.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-tuner.xml
@@ -107,7 +107,7 @@ user.<!-- FIXME Video inputs already have a name, the purpose of this
 field is not quite clear.--></para></entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-tuner-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry spanname="hspan">Type of the tuner, see <xref
 		linkend="v4l2-tuner-type" />.</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-prepare-buf.xml b/Documentation/DocBook/media/v4l/vidioc-prepare-buf.xml
index 7bde698760e4..fa7ad7e33228 100644
--- a/Documentation/DocBook/media/v4l/vidioc-prepare-buf.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-prepare-buf.xml
@@ -48,6 +48,12 @@
   <refsect1>
     <title>Description</title>
 
+    <note>
+      <title>Experimental</title>
+      <para>This is an <link linkend="experimental"> experimental </link>
+      interface and may change in the future.</para>
+    </note>
+
     <para>Applications can optionally call the
 <constant>VIDIOC_PREPARE_BUF</constant> ioctl to pass ownership of the buffer
 to the driver before actually enqueuing it, using the
diff --git a/Documentation/DocBook/media/v4l/vidioc-query-dv-preset.xml b/Documentation/DocBook/media/v4l/vidioc-query-dv-preset.xml
index 23b17f604211..1bc8aeb3ff1f 100644
--- a/Documentation/DocBook/media/v4l/vidioc-query-dv-preset.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-query-dv-preset.xml
@@ -49,6 +49,10 @@ input</refpurpose>
   <refsect1>
     <title>Description</title>
 
+    <para>This ioctl is <emphasis role="bold">deprecated</emphasis>.
+    New drivers and applications should use &VIDIOC-QUERY-DV-TIMINGS; instead.
+    </para>
+
     <para>The hardware may be able to detect the current DV preset
 automatically, similar to sensing the video standard. To do so, applications
 call <constant> VIDIOC_QUERY_DV_PRESET</constant> with a pointer to a
diff --git a/Documentation/DocBook/media/v4l/vidioc-query-dv-timings.xml b/Documentation/DocBook/media/v4l/vidioc-query-dv-timings.xml
new file mode 100644
index 000000000000..44935a0ffcf0
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/vidioc-query-dv-timings.xml
@@ -0,0 +1,104 @@
+<refentry id="vidioc-query-dv-timings">
+  <refmeta>
+    <refentrytitle>ioctl VIDIOC_QUERY_DV_TIMINGS</refentrytitle>
+    &manvol;
+  </refmeta>
+
+  <refnamediv>
+    <refname>VIDIOC_QUERY_DV_TIMINGS</refname>
+    <refpurpose>Sense the DV preset received by the current
+input</refpurpose>
+  </refnamediv>
+
+  <refsynopsisdiv>
+    <funcsynopsis>
+      <funcprototype>
+	<funcdef>int <function>ioctl</function></funcdef>
+	<paramdef>int <parameter>fd</parameter></paramdef>
+	<paramdef>int <parameter>request</parameter></paramdef>
+	<paramdef>struct v4l2_dv_timings *<parameter>argp</parameter></paramdef>
+      </funcprototype>
+    </funcsynopsis>
+  </refsynopsisdiv>
+
+  <refsect1>
+    <title>Arguments</title>
+
+    <variablelist>
+	<varlistentry>
+	<term><parameter>fd</parameter></term>
+	<listitem>
+	  <para>&fd;</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>request</parameter></term>
+	<listitem>
+	  <para>VIDIOC_QUERY_DV_TIMINGS</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>argp</parameter></term>
+	<listitem>
+	  <para></para>
+	</listitem>
+      </varlistentry>
+    </variablelist>
+  </refsect1>
+
+  <refsect1>
+    <title>Description</title>
+
+    <note>
+      <title>Experimental</title>
+      <para>This is an <link linkend="experimental"> experimental </link>
+      interface and may change in the future.</para>
+    </note>
+
+    <para>The hardware may be able to detect the current DV timings
+automatically, similar to sensing the video standard. To do so, applications
+call <constant>VIDIOC_QUERY_DV_TIMINGS</constant> with a pointer to a
+&v4l2-dv-timings;. Once the hardware detects the timings, it will fill in the
+timings structure.
+
+If the timings could not be detected because there was no signal, then
+<errorcode>ENOLINK</errorcode> is returned. If a signal was detected, but
+it was unstable and the receiver could not lock to the signal, then
+<errorcode>ENOLCK</errorcode> is returned. If the receiver could lock to the signal,
+but the format is unsupported (e.g. because the pixelclock is out of range
+of the hardware capabilities), then the driver fills in whatever timings it
+could find and returns <errorcode>ERANGE</errorcode>. In that case the application
+can call &VIDIOC-DV-TIMINGS-CAP; to compare the found timings with the hardware's
+capabilities in order to give more precise feedback to the user.
+</para>
+  </refsect1>
+
+  <refsect1>
+    &return-value;
+
+    <variablelist>
+      <varlistentry>
+	<term><errorcode>ENOLINK</errorcode></term>
+	<listitem>
+	  <para>No timings could be detected because no signal was found.
+</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><errorcode>ENOLCK</errorcode></term>
+	<listitem>
+	  <para>The signal was unstable and the hardware could not lock on to it.
+</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><errorcode>ERANGE</errorcode></term>
+	<listitem>
+	  <para>Timings were found, but they are out of range of the hardware
+capabilities.
+</para>
+	</listitem>
+      </varlistentry>
+    </variablelist>
+  </refsect1>
+</refentry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-queryctrl.xml b/Documentation/DocBook/media/v4l/vidioc-queryctrl.xml
index 36660d311b51..e6645b996558 100644
--- a/Documentation/DocBook/media/v4l/vidioc-queryctrl.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-queryctrl.xml
@@ -127,7 +127,7 @@ the first control with a higher ID. Drivers which do not support this
 flag yet always return an &EINVAL;.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-ctrl-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>Type of control, see <xref
 		linkend="v4l2-ctrl-type" />.</entry>
@@ -215,11 +215,12 @@ the array to zero.</entry>
 
     <table pgwide="1" frame="none" id="v4l2-querymenu">
       <title>struct <structname>v4l2_querymenu</structname></title>
-      <tgroup cols="3">
+      <tgroup cols="4">
 	&cs-str;
 	<tbody valign="top">
 	  <row>
 	    <entry>__u32</entry>
+	    <entry></entry>
 	    <entry><structfield>id</structfield></entry>
 	    <entry>Identifies the control, set by the application
 from the respective &v4l2-queryctrl;
@@ -227,18 +228,38 @@ from the respective &v4l2-queryctrl;
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
+	    <entry></entry>
 	    <entry><structfield>index</structfield></entry>
 	    <entry>Index of the menu item, starting at zero, set by
 	    the application.</entry>
 	  </row>
 	  <row>
+	    <entry>union</entry>
+	    <entry></entry>
+	    <entry></entry>
+	    <entry></entry>
+	  </row>
+	  <row>
+	    <entry></entry>
 	    <entry>__u8</entry>
 	    <entry><structfield>name</structfield>[32]</entry>
 	    <entry>Name of the menu item, a NUL-terminated ASCII
-string. This information is intended for the user.</entry>
+string. This information is intended for the user. This field is valid
+for <constant>V4L2_CTRL_FLAG_MENU</constant> type controls.</entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry>__s64</entry>
+	    <entry><structfield>value</structfield></entry>
+	    <entry>
+              Value of the integer menu item. This field is valid for
+              <constant>V4L2_CTRL_FLAG_INTEGER_MENU</constant> type
+              controls.
+            </entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
+	    <entry></entry>
 	    <entry><structfield>reserved</structfield></entry>
 	    <entry>Reserved for future extensions. Drivers must set
 the array to zero.</entry>
@@ -292,6 +313,20 @@ the menu items can be enumerated with the
 <constant>VIDIOC_QUERYMENU</constant> ioctl.</entry>
 	  </row>
 	  <row>
+	    <entry><constant>V4L2_CTRL_TYPE_INTEGER_MENU</constant></entry>
+	    <entry>&ge; 0</entry>
+	    <entry>1</entry>
+	    <entry>N-1</entry>
+	    <entry>
+              The control has a menu of N choices. The values of the
+              menu items can be enumerated with the
+              <constant>VIDIOC_QUERYMENU</constant> ioctl. This is
+              similar to <constant>V4L2_CTRL_TYPE_MENU</constant>
+              except that instead of strings, the menu items are
+              signed 64-bit integers.
+            </entry>
+	  </row>
+	  <row>
 	    <entry><constant>V4L2_CTRL_TYPE_BITMASK</constant></entry>
 	    <entry>0</entry>
 	    <entry>n/a</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-reqbufs.xml b/Documentation/DocBook/media/v4l/vidioc-reqbufs.xml
index 7be4b1d29b90..d7c95057bc51 100644
--- a/Documentation/DocBook/media/v4l/vidioc-reqbufs.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-reqbufs.xml
@@ -92,18 +92,19 @@ streamoff.--></para>
 	    <entry>The number of buffers requested or granted.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-buf-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>Type of the stream or buffers, this is the same
 as the &v4l2-format; <structfield>type</structfield> field. See <xref
 		linkend="v4l2-buf-type" /> for valid values.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-memory;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>memory</structfield></entry>
 	    <entry>Applications set this field to
 <constant>V4L2_MEMORY_MMAP</constant> or
-<constant>V4L2_MEMORY_USERPTR</constant>.</entry>
+<constant>V4L2_MEMORY_USERPTR</constant>. See <xref linkend="v4l2-memory"
+/>.</entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-s-hw-freq-seek.xml b/Documentation/DocBook/media/v4l/vidioc-s-hw-freq-seek.xml
index 18b1a8266f7c..407dfceb71f0 100644
--- a/Documentation/DocBook/media/v4l/vidioc-s-hw-freq-seek.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-s-hw-freq-seek.xml
@@ -73,10 +73,11 @@ same value as in the &v4l2-input; <structfield>tuner</structfield>
 field and the &v4l2-tuner; <structfield>index</structfield> field.</entry>
 	  </row>
 	  <row>
-	    <entry>&v4l2-tuner-type;</entry>
+	    <entry>__u32</entry>
 	    <entry><structfield>type</structfield></entry>
 	    <entry>The tuner type. This is the same value as in the
-&v4l2-tuner; <structfield>type</structfield> field.</entry>
+&v4l2-tuner; <structfield>type</structfield> field. See <xref
+	    linkend="v4l2-tuner-type" /></entry>
 	  </row>
 	  <row>
 	    <entry>__u32</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-subdev-g-crop.xml b/Documentation/DocBook/media/v4l/vidioc-subdev-g-crop.xml
index 06197323a8cc..4cddd788c589 100644
--- a/Documentation/DocBook/media/v4l/vidioc-subdev-g-crop.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-subdev-g-crop.xml
@@ -58,9 +58,12 @@
     <title>Description</title>
 
     <note>
-      <title>Experimental</title>
-      <para>This is an <link linkend="experimental">experimental</link>
-      interface and may change in the future.</para>
+      <title>Obsolete</title>
+
+      <para>This is an <link linkend="obsolete">obsolete</link>
+      interface and may be removed in the future. It is superseded by
+      <link linkend="vidioc-subdev-g-selection">the selection
+      API</link>.</para>
     </note>
 
     <para>To retrieve the current crop rectangle applications set the
diff --git a/Documentation/DocBook/media/v4l/vidioc-subdev-g-selection.xml b/Documentation/DocBook/media/v4l/vidioc-subdev-g-selection.xml
new file mode 100644
index 000000000000..208e9f0da3f3
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/vidioc-subdev-g-selection.xml
@@ -0,0 +1,228 @@
+<refentry id="vidioc-subdev-g-selection">
+  <refmeta>
+    <refentrytitle>ioctl VIDIOC_SUBDEV_G_SELECTION, VIDIOC_SUBDEV_S_SELECTION</refentrytitle>
+    &manvol;
+  </refmeta>
+
+  <refnamediv>
+    <refname>VIDIOC_SUBDEV_G_SELECTION</refname>
+    <refname>VIDIOC_SUBDEV_S_SELECTION</refname>
+    <refpurpose>Get or set selection rectangles on a subdev pad</refpurpose>
+  </refnamediv>
+
+  <refsynopsisdiv>
+    <funcsynopsis>
+      <funcprototype>
+	<funcdef>int <function>ioctl</function></funcdef>
+	<paramdef>int <parameter>fd</parameter></paramdef>
+	<paramdef>int <parameter>request</parameter></paramdef>
+	<paramdef>struct v4l2_subdev_selection *<parameter>argp</parameter></paramdef>
+      </funcprototype>
+    </funcsynopsis>
+  </refsynopsisdiv>
+
+  <refsect1>
+    <title>Arguments</title>
+
+    <variablelist>
+      <varlistentry>
+	<term><parameter>fd</parameter></term>
+	<listitem>
+	  <para>&fd;</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>request</parameter></term>
+	<listitem>
+	  <para>VIDIOC_SUBDEV_G_SELECTION, VIDIOC_SUBDEV_S_SELECTION</para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><parameter>argp</parameter></term>
+	<listitem>
+	  <para></para>
+	</listitem>
+      </varlistentry>
+    </variablelist>
+  </refsect1>
+
+  <refsect1>
+    <title>Description</title>
+
+    <note>
+      <title>Experimental</title>
+      <para>This is an <link linkend="experimental">experimental</link>
+      interface and may change in the future.</para>
+    </note>
+
+    <para>The selections are used to configure various image
+    processing functionality performed by the subdevs which affect the
+    image size. This currently includes cropping, scaling and
+    composition.</para>
+
+    <para>The selection API replaces <link
+    linkend="vidioc-subdev-g-crop">the old subdev crop API</link>. All
+    the function of the crop API, and more, are supported by the
+    selections API.</para>
+
+    <para>See <xref linkend="subdev"></xref> for
+    more information on how each selection target affects the image
+    processing pipeline inside the subdevice.</para>
+
+    <section>
+      <title>Types of selection targets</title>
+
+      <para>There are two types of selection targets: actual and bounds.
+      The ACTUAL targets are the targets which configure the hardware.
+      The BOUNDS target will return a rectangle that contain all
+      possible ACTUAL rectangles.</para>
+    </section>
+
+    <section>
+      <title>Discovering supported features</title>
+
+      <para>To discover which targets are supported, the user can
+      perform <constant>VIDIOC_SUBDEV_G_SELECTION</constant> on them.
+      Any unsupported target will return
+      <constant>EINVAL</constant>.</para>
+    </section>
+
+    <table pgwide="1" frame="none" id="v4l2-subdev-selection-targets">
+      <title>V4L2 subdev selection targets</title>
+      <tgroup cols="3">
+        &cs-def;
+	<tbody valign="top">
+	  <row>
+	    <entry><constant>V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL</constant></entry>
+	    <entry>0x0000</entry>
+	    <entry>Actual crop. Defines the cropping
+	    performed by the processing step.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS</constant></entry>
+	    <entry>0x0002</entry>
+	    <entry>Bounds of the crop rectangle.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL</constant></entry>
+	    <entry>0x0100</entry>
+	    <entry>Actual compose rectangle. Used to configure scaling
+	    on sink pads and composition on source pads.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS</constant></entry>
+	    <entry>0x0102</entry>
+	    <entry>Bounds of the compose rectangle.</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+
+    <table pgwide="1" frame="none" id="v4l2-subdev-selection-flags">
+      <title>V4L2 subdev selection flags</title>
+      <tgroup cols="3">
+        &cs-def;
+	<tbody valign="top">
+	  <row>
+	    <entry><constant>V4L2_SUBDEV_SEL_FLAG_SIZE_GE</constant></entry>
+	    <entry>(1 &lt;&lt; 0)</entry> <entry>Suggest the driver it
+	    should choose greater or equal rectangle (in size) than
+	    was requested. Albeit the driver may choose a lesser size,
+	    it will only do so due to hardware limitations. Without
+	    this flag (and
+	    <constant>V4L2_SUBDEV_SEL_FLAG_SIZE_LE</constant>) the
+	    behaviour is to choose the closest possible
+	    rectangle.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_SUBDEV_SEL_FLAG_SIZE_LE</constant></entry>
+	    <entry>(1 &lt;&lt; 1)</entry> <entry>Suggest the driver it
+	    should choose lesser or equal rectangle (in size) than was
+	    requested. Albeit the driver may choose a greater size, it
+	    will only do so due to hardware limitations.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG</constant></entry>
+	    <entry>(1 &lt;&lt; 2)</entry>
+	    <entry>The configuration should not be propagated to any
+	    further processing steps. If this flag is not given, the
+	    configuration is propagated inside the subdevice to all
+	    further processing steps.</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+
+    <table pgwide="1" frame="none" id="v4l2-subdev-selection">
+      <title>struct <structname>v4l2_subdev_selection</structname></title>
+      <tgroup cols="3">
+        &cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>which</structfield></entry>
+	    <entry>Active or try selection, from
+	    &v4l2-subdev-format-whence;.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>pad</structfield></entry>
+	    <entry>Pad number as reported by the media framework.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>target</structfield></entry>
+	    <entry>Target selection rectangle. See
+	    <xref linkend="v4l2-subdev-selection-targets">.</xref>.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>flags</structfield></entry>
+	    <entry>Flags. See
+	    <xref linkend="v4l2-subdev-selection-flags">.</xref></entry>
+	  </row>
+	  <row>
+	    <entry>&v4l2-rect;</entry>
+	    <entry><structfield>rect</structfield></entry>
+	    <entry>Selection rectangle, in pixels.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>reserved</structfield>[8]</entry>
+	    <entry>Reserved for future extensions. Applications and drivers must
+	    set the array to zero.</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+
+  </refsect1>
+
+  <refsect1>
+    &return-value;
+
+    <variablelist>
+      <varlistentry>
+	<term><errorcode>EBUSY</errorcode></term>
+	<listitem>
+	  <para>The selection rectangle can't be changed because the
+	  pad is currently busy. This can be caused, for instance, by
+	  an active video stream on the pad. The ioctl must not be
+	  retried without performing another action to fix the problem
+	  first. Only returned by
+	  <constant>VIDIOC_SUBDEV_S_SELECTION</constant></para>
+	</listitem>
+      </varlistentry>
+      <varlistentry>
+	<term><errorcode>EINVAL</errorcode></term>
+	<listitem>
+	  <para>The &v4l2-subdev-selection;
+	  <structfield>pad</structfield> references a non-existing
+	  pad, the <structfield>which</structfield> field references a
+	  non-existing format, or the selection target is not
+	  supported on the given subdev pad.</para>
+	</listitem>
+      </varlistentry>
+    </variablelist>
+  </refsect1>
+</refentry>
diff --git a/Documentation/devicetree/bindings/input/spear-keyboard.txt b/Documentation/devicetree/bindings/input/spear-keyboard.txt
new file mode 100644
index 000000000000..4a846d26da23
--- /dev/null
+++ b/Documentation/devicetree/bindings/input/spear-keyboard.txt
@@ -0,0 +1,20 @@
+* SPEAr keyboard controller
+
+Required properties:
+- compatible: "st,spear300-kbd"
+
+Optional properties, in addition to those specified by the shared
+matrix-keyboard bindings:
+- autorepeat: bool: enables key autorepeat
+- st,mode: keyboard mode: 0 - 9x9, 1 - 6x6, 2 - 2x2
+
+Example:
+
+kbd@fc400000 {
+	compatible = "st,spear300-kbd";
+	reg = <0xfc400000 0x100>;
+	linux,keymap = < 0x00030012
+			 0x0102003a >;
+	autorepeat;
+	st,mode = <0>;
+};
diff --git a/Documentation/devicetree/bindings/input/touchscreen/lpc32xx-tsc.txt b/Documentation/devicetree/bindings/input/touchscreen/lpc32xx-tsc.txt
new file mode 100644
index 000000000000..41cbf4b7a670
--- /dev/null
+++ b/Documentation/devicetree/bindings/input/touchscreen/lpc32xx-tsc.txt
@@ -0,0 +1,16 @@
+* NXP LPC32xx SoC Touchscreen Controller (TSC)
+
+Required properties:
+- compatible: must be "nxp,lpc3220-tsc"
+- reg: physical base address of the controller and length of memory mapped
+  region.
+- interrupts: The TSC/ADC interrupt
+
+Example:
+
+	tsc@40048000 {
+		compatible = "nxp,lpc3220-tsc";
+		reg = <0x40048000 0x1000>;
+		interrupt-parent = <&mic>;
+		interrupts = <39 0>;
+	};
diff --git a/Documentation/devicetree/bindings/input/twl6040-vibra.txt b/Documentation/devicetree/bindings/input/twl6040-vibra.txt
new file mode 100644
index 000000000000..5b1918b818fb
--- /dev/null
+++ b/Documentation/devicetree/bindings/input/twl6040-vibra.txt
@@ -0,0 +1,37 @@
+Vibra driver for the twl6040 family
+
+The vibra driver is a child of the twl6040 MFD dirver.
+Documentation/devicetree/bindings/mfd/twl6040.txt
+
+Required properties:
+- compatible : Must be "ti,twl6040-vibra";
+- interrupts: 4, Vibra overcurrent interrupt
+- vddvibl-supply: Regulator supplying the left vibra motor
+- vddvibr-supply: Regulator supplying the right vibra motor
+- vibldrv_res: Board specific left driver resistance
+- vibrdrv_res: Board specific right driver resistance
+- viblmotor_res: Board specific left motor resistance
+- vibrmotor_res: Board specific right motor resistance
+
+Optional properties:
+- vddvibl_uV: If the vddvibl default voltage need to be changed
+- vddvibr_uV: If the vddvibr default voltage need to be changed
+
+Example:
+/*
+ * 8-channel high quality low-power audio codec
+ * http://www.ti.com/lit/ds/symlink/twl6040.pdf
+ */
+twl6040: twl6040@4b {
+	...
+	twl6040_vibra: twl6040@1 {
+		compatible = "ti,twl6040-vibra";
+		interrupts = <4>;
+		vddvibl-supply = <&vbat>;
+		vddvibr-supply = <&vbat>;
+		vibldrv_res = <8>;
+		vibrdrv_res = <3>;
+		viblmotor_res = <10>;
+		vibrmotor_res = <10>;
+	};
+};
diff --git a/Documentation/dvb/get_dvb_firmware b/Documentation/dvb/get_dvb_firmware
index d1d4a179a382..fbb241174486 100755
--- a/Documentation/dvb/get_dvb_firmware
+++ b/Documentation/dvb/get_dvb_firmware
@@ -28,7 +28,8 @@ use IO::Handle;
 		"opera1", "cx231xx", "cx18", "cx23885", "pvrusb2", "mpc718",
 		"af9015", "ngene", "az6027", "lme2510_lg", "lme2510c_s7395",
 		"lme2510c_s7395_old", "drxk", "drxk_terratec_h5",
-		"drxk_hauppauge_hvr930c", "tda10071", "it9135", "it9137");
+		"drxk_hauppauge_hvr930c", "tda10071", "it9135", "it9137",
+		"drxk_pctv");
 
 # Check args
 syntax() if (scalar(@ARGV) != 1);
@@ -730,6 +731,23 @@ sub tda10071 {
     "$fwfile";
 }
 
+sub drxk_pctv {
+    my $sourcefile = "PCTV_460e_reference.zip";
+    my $url = "ftp://ftp.pctvsystems.com/TV/driver/PCTV%2070e%2080e%20100e%20320e%20330e%20800e/";
+    my $hash = "4403de903bf2593464c8d74bbc200a57";
+    my $fwfile = "dvb-demod-drxk-pctv.fw";
+    my $tmpdir = tempdir(DIR => "/tmp", CLEANUP => 1);
+
+    checkstandard();
+
+    wgetfile($sourcefile, $url . $sourcefile);
+    verify($sourcefile, $hash);
+    unzip($sourcefile, $tmpdir);
+    extract("$tmpdir/PCTV\ 70e\ 80e\ 100e\ 320e\ 330e\ 800e/32\ bit/emOEM.sys", 0x72b80, 42692, $fwfile);
+
+    "$fwfile";
+}
+
 # ---------------------------------------------------------------
 # Utilities
 
diff --git a/Documentation/feature-removal-schedule.txt b/Documentation/feature-removal-schedule.txt
index 1e69a81e99d4..50d82ae09e2a 100644
--- a/Documentation/feature-removal-schedule.txt
+++ b/Documentation/feature-removal-schedule.txt
@@ -541,6 +541,18 @@ Who:	Kees Cook <keescook@chromium.org>
 
 ----------------------------
 
+What:	Removing the pn544 raw driver.
+When:	3.6
+Why:	With the introduction of the NFC HCI and SHDL kernel layers, pn544.c
+	is being replaced by pn544_hci.c which is accessible through the netlink
+	and socket NFC APIs. Moreover, pn544.c is outdated and does not seem to
+	work properly with the latest Android stacks.
+	Having 2 drivers for the same hardware is confusing and as such we
+	should only keep the one following the kernel NFC APIs.
+Who:	Samuel Ortiz <sameo@linux.intel.com>
+
+----------------------------
+
 What:	setitimer accepts user NULL pointer (value)
 When:	3.6
 Why:	setitimer is not returning -EFAULT if user pointer is NULL. This
@@ -549,6 +561,15 @@ Who:	Sasikantha Babu <sasikanth.v19@gmail.com>
 
 ----------------------------
 
+What:	remove bogus DV presets V4L2_DV_1080I29_97, V4L2_DV_1080I30 and
+	V4L2_DV_1080I25
+When:	3.6
+Why:	These HDTV formats do not exist and were added by a confused mind
+	(that was me, to be precise...)
+Who:	Hans Verkuil <hans.verkuil@cisco.com>
+
+----------------------------
+
 What:	V4L2_CID_HCENTER, V4L2_CID_VCENTER V4L2 controls
 When:	3.7
 Why:	The V4L2_CID_VCENTER, V4L2_CID_HCENTER controls have been deprecated
diff --git a/Documentation/kernel-parameters.txt b/Documentation/kernel-parameters.txt
index 5b6e58492229..b69cfdc12112 100644
--- a/Documentation/kernel-parameters.txt
+++ b/Documentation/kernel-parameters.txt
@@ -987,6 +987,20 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	i8k.restricted	[HW] Allow controlling fans only if SYS_ADMIN
 			capability is set.
 
+	i915.invert_brightness=
+			[DRM] Invert the sense of the variable that is used to
+			set the brightness of the panel backlight. Normally a
+			brightness value of 0 indicates backlight switched off,
+			and the maximum of the brightness value sets the backlight
+			to maximum brightness. If this parameter is set to 0
+			(default) and the machine requires it, or this parameter
+			is set to 1, a brightness value of 0 sets the backlight
+			to maximum brightness, and the maximum of the brightness
+			value switches the backlight off.
+			-1 -- never invert brightness
+			 0 -- machine default
+			 1 -- force brightness inversion
+
 	icn=		[HW,ISDN]
 			Format: <io>[,<membase>[,<icn_id>[,<icn_id2>]]]
 
diff --git a/Documentation/media-framework.txt b/Documentation/media-framework.txt
index 3a0f879533ce..802875413873 100644
--- a/Documentation/media-framework.txt
+++ b/Documentation/media-framework.txt
@@ -335,6 +335,9 @@ the media_entity pipe field.
 Calls to media_entity_pipeline_start() can be nested. The pipeline pointer must
 be identical for all nested calls to the function.
 
+media_entity_pipeline_start() may return an error. In that case, it will
+clean up any the changes it did by itself.
+
 When stopping the stream, drivers must notify the entities with
 
 	media_entity_pipeline_stop(struct media_entity *entity);
@@ -351,3 +354,19 @@ If other operations need to be disallowed on streaming entities (such as
 changing entities configuration parameters) drivers can explicitly check the
 media_entity stream_count field to find out if an entity is streaming. This
 operation must be done with the media_device graph_mutex held.
+
+
+Link validation
+---------------
+
+Link validation is performed by media_entity_pipeline_start() for any
+entity which has sink pads in the pipeline. The
+media_entity::link_validate() callback is used for that purpose. In
+link_validate() callback, entity driver should check that the properties of
+the source pad of the connected entity and its own sink pad match. It is up
+to the type of the entity (and in the end, the properties of the hardware)
+what matching actually means.
+
+Subsystems should facilitate link validation by providing subsystem specific
+helper functions to provide easy access for commonly needed information, and
+in the end provide a way to use driver-specific callbacks.
diff --git a/Documentation/nfc/nfc-hci.txt b/Documentation/nfc/nfc-hci.txt
index 216b7254fcc3..320f9336c781 100644
--- a/Documentation/nfc/nfc-hci.txt
+++ b/Documentation/nfc/nfc-hci.txt
@@ -22,9 +22,9 @@ response to arrive.
 HCI events can also be received from the host controller. They will be handled
 and a translation will be forwarded to NFC Core as needed.
 HCI uses 2 execution contexts:
-- one if for executing commands : nfc_hci_msg_tx_work(). Only one command
+- one for executing commands : nfc_hci_msg_tx_work(). Only one command
 can be executing at any given moment.
-- one if for dispatching received events and responses : nfc_hci_msg_rx_work()
+- one for dispatching received events and commands : nfc_hci_msg_rx_work().
 
 HCI Session initialization:
 ---------------------------
@@ -52,18 +52,42 @@ entry points:
 struct nfc_hci_ops {
 	int (*open)(struct nfc_hci_dev *hdev);
 	void (*close)(struct nfc_hci_dev *hdev);
+	int (*hci_ready) (struct nfc_hci_dev *hdev);
 	int (*xmit)(struct nfc_hci_dev *hdev, struct sk_buff *skb);
 	int (*start_poll)(struct nfc_hci_dev *hdev, u32 protocols);
 	int (*target_from_gate)(struct nfc_hci_dev *hdev, u8 gate,
 				struct nfc_target *target);
+	int (*complete_target_discovered) (struct nfc_hci_dev *hdev, u8 gate,
+					   struct nfc_target *target);
+	int (*data_exchange) (struct nfc_hci_dev *hdev,
+			      struct nfc_target *target,
+			      struct sk_buff *skb, struct sk_buff **res_skb);
+	int (*check_presence)(struct nfc_hci_dev *hdev,
+			      struct nfc_target *target);
 };
 
-open() and close() shall turn the hardware on and off. xmit() shall simply
-write a frame to the chip. start_poll() is an optional entrypoint that shall
-set the hardware in polling mode. This must be implemented only if the hardware
-uses proprietary gates or a mechanism slightly different from the HCI standard.
-target_from_gate() is another optional entrypoint to return the protocols
+- open() and close() shall turn the hardware on and off.
+- hci_ready() is an optional entry point that is called right after the hci
+session has been set up. The driver can use it to do additional initialization
+that must be performed using HCI commands.
+- xmit() shall simply write a frame to the chip.
+- start_poll() is an optional entrypoint that shall set the hardware in polling
+mode. This must be implemented only if the hardware uses proprietary gates or a
+mechanism slightly different from the HCI standard.
+- target_from_gate() is an optional entrypoint to return the nfc protocols
 corresponding to a proprietary gate.
+- complete_target_discovered() is an optional entry point to let the driver
+perform additional proprietary processing necessary to auto activate the
+discovered target.
+- data_exchange() must be implemented by the driver if proprietary HCI commands
+are required to send data to the tag. Some tag types will require custom
+commands, others can be written to using the standard HCI commands. The driver
+can check the tag type and either do proprietary processing, or return 1 to ask
+for standard processing.
+- check_presence() is an optional entry point that will be called regularly
+by the core to check that an activated tag is still in the field. If this is
+not implemented, the core will not be able to push tag_lost events to the user
+space
 
 On the rx path, the driver is responsible to push incoming HCP frames to HCI
 using nfc_hci_recv_frame(). HCI will take care of re-aggregation and handling
@@ -99,7 +123,8 @@ fast, cannot sleep. stores incoming frames into an shdlc rx queue
 handles shdlc rx & tx queues. Dispatches HCI cmd responses.
 
 - HCI Tx Cmd worker (MSGTXWQ)
-Serialize execution of HCI commands. Complete execution in case of resp timeout.
+Serializes execution of HCI commands. Completes execution in case of response
+timeout.
 
 - HCI Rx worker (MSGRXWQ)
 Dispatches incoming HCI commands or events.
@@ -133,11 +158,11 @@ able to complete the command with a timeout error if no response arrive.
 SMW context gets scheduled and invokes nfc_shdlc_sm_work(). This function
 handles shdlc framing in and out. It uses the driver xmit to send frames and
 receives incoming frames in an skb queue filled from the driver IRQ handler.
-SHDLC I(nformation) frames payload are HCP fragments. They are agregated to
+SHDLC I(nformation) frames payload are HCP fragments. They are aggregated to
 form complete HCI frames, which can be a response, command, or event.
 
 HCI Responses are dispatched immediately from this context to unblock
-waiting command execution. Reponse processing involves invoking the completion
+waiting command execution. Response processing involves invoking the completion
 callback that was provided by nfc_hci_msg_tx_work() when it sent the command.
 The completion callback will then wake the syscall context.
 
diff --git a/Documentation/trace/uprobetracer.txt b/Documentation/trace/uprobetracer.txt
new file mode 100644
index 000000000000..24ce6823a09e
--- /dev/null
+++ b/Documentation/trace/uprobetracer.txt
@@ -0,0 +1,113 @@
+		Uprobe-tracer: Uprobe-based Event Tracing
+		=========================================
+                 Documentation written by Srikar Dronamraju
+
+Overview
+--------
+Uprobe based trace events are similar to kprobe based trace events.
+To enable this feature, build your kernel with CONFIG_UPROBE_EVENT=y.
+
+Similar to the kprobe-event tracer, this doesn't need to be activated via
+current_tracer. Instead of that, add probe points via
+/sys/kernel/debug/tracing/uprobe_events, and enable it via
+/sys/kernel/debug/tracing/events/uprobes/<EVENT>/enabled.
+
+However unlike kprobe-event tracer, the uprobe event interface expects the
+user to calculate the offset of the probepoint in the object
+
+Synopsis of uprobe_tracer
+-------------------------
+  p[:[GRP/]EVENT] PATH:SYMBOL[+offs] [FETCHARGS]	: Set a probe
+
+ GRP		: Group name. If omitted, use "uprobes" for it.
+ EVENT		: Event name. If omitted, the event name is generated
+		  based on SYMBOL+offs.
+ PATH		: path to an executable or a library.
+ SYMBOL[+offs]	: Symbol+offset where the probe is inserted.
+
+ FETCHARGS	: Arguments. Each probe can have up to 128 args.
+  %REG		: Fetch register REG
+
+Event Profiling
+---------------
+ You can check the total number of probe hits and probe miss-hits via
+/sys/kernel/debug/tracing/uprobe_profile.
+ The first column is event name, the second is the number of probe hits,
+the third is the number of probe miss-hits.
+
+Usage examples
+--------------
+To add a probe as a new event, write a new definition to uprobe_events
+as below.
+
+  echo 'p: /bin/bash:0x4245c0' > /sys/kernel/debug/tracing/uprobe_events
+
+ This sets a uprobe at an offset of 0x4245c0 in the executable /bin/bash
+
+  echo > /sys/kernel/debug/tracing/uprobe_events
+
+ This clears all probe points.
+
+The following example shows how to dump the instruction pointer and %ax
+a register at the probed text address.  Here we are trying to probe
+function zfree in /bin/zsh
+
+    # cd /sys/kernel/debug/tracing/
+    # cat /proc/`pgrep  zsh`/maps | grep /bin/zsh | grep r-xp
+    00400000-0048a000 r-xp 00000000 08:03 130904 /bin/zsh
+    # objdump -T /bin/zsh | grep -w zfree
+    0000000000446420 g    DF .text  0000000000000012  Base        zfree
+
+0x46420 is the offset of zfree in object /bin/zsh that is loaded at
+0x00400000. Hence the command to probe would be :
+
+    # echo 'p /bin/zsh:0x46420 %ip %ax' > uprobe_events
+
+Please note: User has to explicitly calculate the offset of the probepoint
+in the object. We can see the events that are registered by looking at the
+uprobe_events file.
+
+    # cat uprobe_events
+    p:uprobes/p_zsh_0x46420 /bin/zsh:0x00046420 arg1=%ip arg2=%ax
+
+The format of events can be seen by viewing the file events/uprobes/p_zsh_0x46420/format
+
+    # cat events/uprobes/p_zsh_0x46420/format
+    name: p_zsh_0x46420
+    ID: 922
+    format:
+	field:unsigned short common_type;	offset:0;	size:2;	signed:0;
+	field:unsigned char common_flags;	offset:2;	size:1;	signed:0;
+	field:unsigned char common_preempt_count;	offset:3;	size:1;	signed:0;
+	field:int common_pid;	offset:4;	size:4;	signed:1;
+	field:int common_padding;	offset:8;	size:4;	signed:1;
+
+	field:unsigned long __probe_ip;	offset:12;	size:4;	signed:0;
+	field:u32 arg1;	offset:16;	size:4;	signed:0;
+	field:u32 arg2;	offset:20;	size:4;	signed:0;
+
+    print fmt: "(%lx) arg1=%lx arg2=%lx", REC->__probe_ip, REC->arg1, REC->arg2
+
+Right after definition, each event is disabled by default. For tracing these
+events, you need to enable it by:
+
+    # echo 1 > events/uprobes/enable
+
+Lets disable the event after sleeping for some time.
+    # sleep 20
+    # echo 0 > events/uprobes/enable
+
+And you can see the traced information via /sys/kernel/debug/tracing/trace.
+
+    # cat trace
+    # tracer: nop
+    #
+    #           TASK-PID    CPU#    TIMESTAMP  FUNCTION
+    #              | |       |          |         |
+                 zsh-24842 [006] 258544.995456: p_zsh_0x46420: (0x446420) arg1=446421 arg2=79
+                 zsh-24842 [007] 258545.000270: p_zsh_0x46420: (0x446420) arg1=446421 arg2=79
+                 zsh-24842 [002] 258545.043929: p_zsh_0x46420: (0x446420) arg1=446421 arg2=79
+                 zsh-24842 [004] 258547.046129: p_zsh_0x46420: (0x446420) arg1=446421 arg2=79
+
+Each line shows us probes were triggered for a pid 24842 with ip being
+0x446421 and contents of ax register being 79.
diff --git a/Documentation/video4linux/4CCs.txt b/Documentation/video4linux/4CCs.txt
new file mode 100644
index 000000000000..41241af1ebfe
--- /dev/null
+++ b/Documentation/video4linux/4CCs.txt
@@ -0,0 +1,32 @@
+Guidelines for Linux4Linux pixel format 4CCs
+============================================
+
+Guidelines for Video4Linux 4CC codes defined using v4l2_fourcc() are
+specified in this document. First of the characters defines the nature of
+the pixel format, compression and colour space. The interpretation of the
+other three characters depends on the first one.
+
+Existing 4CCs may not obey these guidelines.
+
+Formats
+=======
+
+Raw bayer
+---------
+
+The following first characters are used by raw bayer formats:
+
+	B: raw bayer, uncompressed
+	b: raw bayer, DPCM compressed
+	a: A-law compressed
+	u: u-law compressed
+
+2nd character: pixel order
+	B: BGGR
+	G: GBRG
+	g: GRBG
+	R: RGGB
+
+3rd character: uncompressed bits-per-pixel 0--9, A--
+
+4th character: compressed bits-per-pixel 0--9, A--
diff --git a/Documentation/video4linux/gspca.txt b/Documentation/video4linux/gspca.txt
index e6c2842407a4..1e6b6531bbcc 100644
--- a/Documentation/video4linux/gspca.txt
+++ b/Documentation/video4linux/gspca.txt
@@ -276,6 +276,7 @@ pac7302		093a:2622	Genius Eye 312
 pac7302		093a:2624	PAC7302
 pac7302		093a:2625	Genius iSlim 310
 pac7302		093a:2626	Labtec 2200
+pac7302		093a:2627	Genius FaceCam 300
 pac7302		093a:2628	Genius iLook 300
 pac7302		093a:2629	Genious iSlim 300
 pac7302		093a:262a	Webcam 300k
diff --git a/Documentation/video4linux/v4l2-controls.txt b/Documentation/video4linux/v4l2-controls.txt
index e2492a9d1027..43da22b89728 100644
--- a/Documentation/video4linux/v4l2-controls.txt
+++ b/Documentation/video4linux/v4l2-controls.txt
@@ -130,8 +130,18 @@ Menu controls are added by calling v4l2_ctrl_new_std_menu:
 			const struct v4l2_ctrl_ops *ops,
 			u32 id, s32 max, s32 skip_mask, s32 def);
 
+Or alternatively for integer menu controls, by calling v4l2_ctrl_new_int_menu:
+
+	struct v4l2_ctrl *v4l2_ctrl_new_int_menu(struct v4l2_ctrl_handler *hdl,
+			const struct v4l2_ctrl_ops *ops,
+			u32 id, s32 max, s32 def, const s64 *qmenu_int);
+
 These functions are typically called right after the v4l2_ctrl_handler_init:
 
+	static const s64 exp_bias_qmenu[] = {
+	       -2, -1, 0, 1, 2
+	};
+
 	v4l2_ctrl_handler_init(&foo->ctrl_handler, nr_of_controls);
 	v4l2_ctrl_new_std(&foo->ctrl_handler, &foo_ctrl_ops,
 			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
@@ -141,6 +151,11 @@ These functions are typically called right after the v4l2_ctrl_handler_init:
 			V4L2_CID_POWER_LINE_FREQUENCY,
 			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
 			V4L2_CID_POWER_LINE_FREQUENCY_DISABLED);
+	v4l2_ctrl_new_int_menu(&foo->ctrl_handler, &foo_ctrl_ops,
+			V4L2_CID_EXPOSURE_BIAS,
+			ARRAY_SIZE(exp_bias_qmenu) - 1,
+			ARRAY_SIZE(exp_bias_qmenu) / 2 - 1,
+			exp_bias_qmenu);
 	...
 	if (foo->ctrl_handler.error) {
 		int err = foo->ctrl_handler.error;
@@ -164,6 +179,12 @@ controls. There is no min argument since that is always 0 for menu controls,
 and instead of a step there is a skip_mask argument: if bit X is 1, then menu
 item X is skipped.
 
+The v4l2_ctrl_new_int_menu function creates a new standard integer menu
+control with driver-specific items in the menu. It differs from
+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.
+
 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/Documentation/video4linux/v4l2-framework.txt b/Documentation/video4linux/v4l2-framework.txt
index 659b2ba12a4f..1f5905270050 100644
--- a/Documentation/video4linux/v4l2-framework.txt
+++ b/Documentation/video4linux/v4l2-framework.txt
@@ -182,11 +182,11 @@ static int __devinit drv_probe(struct pci_dev *pdev,
 }
 
 If you have multiple device nodes then it can be difficult to know when it is
-safe to unregister v4l2_device. For this purpose v4l2_device has refcounting
-support. The refcount is increased whenever video_register_device is called and
-it is decreased whenever that device node is released. When the refcount reaches
-zero, then the v4l2_device release() callback is called. You can do your final
-cleanup there.
+safe to unregister v4l2_device for hotpluggable devices. For this purpose
+v4l2_device has refcounting support. The refcount is increased whenever
+video_register_device is called and it is decreased whenever that device node
+is released. When the refcount reaches zero, then the v4l2_device release()
+callback is called. You can do your final cleanup there.
 
 If other device nodes (e.g. ALSA) are created, then you can increase and
 decrease the refcount manually as well by calling:
@@ -197,6 +197,10 @@ or:
 
 int v4l2_device_put(struct v4l2_device *v4l2_dev);
 
+Since the initial refcount is 1 you also need to call v4l2_device_put in the
+disconnect() callback (for USB devices) or in the remove() callback (for e.g.
+PCI devices), otherwise the refcount will never reach 0.
+
 struct v4l2_subdev
 ------------------
 
@@ -262,11 +266,16 @@ struct v4l2_subdev_video_ops {
 	...
 };
 
+struct v4l2_subdev_pad_ops {
+	...
+};
+
 struct v4l2_subdev_ops {
 	const struct v4l2_subdev_core_ops  *core;
 	const struct v4l2_subdev_tuner_ops *tuner;
 	const struct v4l2_subdev_audio_ops *audio;
 	const struct v4l2_subdev_video_ops *video;
+	const struct v4l2_subdev_pad_ops *video;
 };
 
 The core ops are common to all subdevs, the other categories are implemented
@@ -303,6 +312,22 @@ Don't forget to cleanup the media entity before the sub-device is destroyed:
 
 	media_entity_cleanup(&sd->entity);
 
+If the subdev driver intends to process video and integrate with the media
+framework, it must implement format related functionality using
+v4l2_subdev_pad_ops instead of v4l2_subdev_video_ops.
+
+In that case, the subdev driver may set the link_validate field to provide
+its own link validation function. The link validation function is called for
+every link in the pipeline where both of the ends of the links are V4L2
+sub-devices. The driver is still responsible for validating the correctness
+of the format configuration between sub-devices and video nodes.
+
+If link_validate op is not set, the default function
+v4l2_subdev_link_validate_default() is used instead. This function ensures
+that width, height and the media bus pixel code are equal on both source and
+sink of the link. Subdev drivers are also free to use this function to
+perform the checks mentioned above in addition to their own checks.
+
 A device (bridge) driver needs to register the v4l2_subdev with the
 v4l2_device:
 
@@ -555,19 +580,25 @@ allocated memory.
 You should also set these fields:
 
 - v4l2_dev: set to the v4l2_device parent device.
+
 - name: set to something descriptive and unique.
+
 - fops: set to the v4l2_file_operations struct.
+
 - ioctl_ops: if you use the v4l2_ioctl_ops to simplify ioctl maintenance
   (highly recommended to use this and it might become compulsory in the
   future!), then set this to your v4l2_ioctl_ops struct.
+
 - lock: leave to NULL if you want to do all the locking in the driver.
-  Otherwise you give it a pointer to a struct mutex_lock and before any
-  of the v4l2_file_operations is called this lock will be taken by the
-  core and released afterwards.
+  Otherwise you give it a pointer to a struct mutex_lock and before the
+  unlocked_ioctl file operation is called this lock will be taken by the
+  core and released afterwards. See the next section for more details.
+
 - prio: keeps track of the priorities. Used to implement VIDIOC_G/S_PRIORITY.
   If left to NULL, then it will use the struct v4l2_prio_state in v4l2_device.
   If you want to have a separate priority state per (group of) device node(s),
   then you can point it to your own struct v4l2_prio_state.
+
 - parent: you only set this if v4l2_device was registered with NULL as
   the parent device struct. This only happens in cases where one hardware
   device has multiple PCI devices that all share the same v4l2_device core.
@@ -577,6 +608,7 @@ You should also set these fields:
   (cx8802). Since the v4l2_device cannot be associated with a particular
   PCI device it is setup without a parent device. But when the struct
   video_device is setup you do know which parent PCI device to use.
+
 - flags: optional. Set to V4L2_FL_USE_FH_PRIO if you want to let the framework
   handle the VIDIOC_G/S_PRIORITY ioctls. This requires that you use struct
   v4l2_fh. Eventually this flag will disappear once all drivers use the core
@@ -587,6 +619,16 @@ in your v4l2_file_operations struct.
 
 Do not use .ioctl! This is deprecated and will go away in the future.
 
+In some cases you want to tell the core that a function you had specified in
+your v4l2_ioctl_ops should be ignored. You can mark such ioctls by calling this
+function before video_device_register is called:
+
+void v4l2_disable_ioctl(struct video_device *vdev, unsigned int cmd);
+
+This tends to be needed if based on external factors (e.g. which card is
+being used) you want to turns off certain features in v4l2_ioctl_ops without
+having to make a new struct.
+
 The v4l2_file_operations struct is a subset of file_operations. The main
 difference is that the inode argument is omitted since it is never used.
 
@@ -609,8 +651,22 @@ v4l2_file_operations and locking
 --------------------------------
 
 You can set a pointer to a mutex_lock in struct video_device. Usually this
-will be either a top-level mutex or a mutex per device node. If you want
-finer-grained locking then you have to set it to NULL and do you own locking.
+will be either a top-level mutex or a mutex per device node. By default this
+lock will be used for unlocked_ioctl, but you can disable locking for
+selected ioctls by calling:
+
+	void v4l2_disable_ioctl_locking(struct video_device *vdev, unsigned int cmd);
+
+E.g.: v4l2_disable_ioctl_locking(vdev, VIDIOC_DQBUF);
+
+You have to call this before you register the video_device.
+
+Particularly with USB drivers where certain commands such as setting controls
+can take a long time you may want to do your own locking for the buffer queuing
+ioctls.
+
+If you want still finer-grained locking then you have to set mutex_lock to NULL
+and do you own locking completely.
 
 It is up to the driver developer to decide which method to use. However, if
 your driver has high-latency operations (for example, changing the exposure
@@ -618,7 +674,7 @@ of a USB webcam might take a long time), then you might be better off with
 doing your own locking if you want to allow the user to do other things with
 the device while waiting for the high-latency command to finish.
 
-If a lock is specified then all file operations will be serialized on that
+If a lock is specified then all ioctl commands will be serialized on that
 lock. If you use videobuf then you must pass the same lock to the videobuf
 queue initialize function: if videobuf has to wait for a frame to arrive, then
 it will temporarily unlock the lock and relock it afterwards. If your driver
@@ -941,21 +997,35 @@ fast.
 
 Useful functions:
 
-- v4l2_event_queue()
+void v4l2_event_queue(struct video_device *vdev, const struct v4l2_event *ev)
 
   Queue events to video device. The driver's only responsibility is to fill
   in the type and the data fields. The other fields will be filled in by
   V4L2.
 
-- v4l2_event_subscribe()
+int v4l2_event_subscribe(struct v4l2_fh *fh,
+			 struct v4l2_event_subscription *sub, unsigned elems,
+			 const struct v4l2_subscribed_event_ops *ops)
 
   The video_device->ioctl_ops->vidioc_subscribe_event must check the driver
   is able to produce events with specified event id. Then it calls
-  v4l2_event_subscribe() to subscribe the event. The last argument is the
-  size of the event queue for this event. If it is 0, then the framework
-  will fill in a default value (this depends on the event type).
+  v4l2_event_subscribe() to subscribe the event.
+
+  The elems argument is the size of the event queue for this event. If it is 0,
+  then the framework will fill in a default value (this depends on the event
+  type).
+
+  The ops argument allows the driver to specify a number of callbacks:
+  * add:     called when a new listener gets added (subscribing to the same
+             event twice will only cause this callback to get called once)
+  * del:     called when a listener stops listening
+  * replace: replace event 'old' with event 'new'.
+  * merge:   merge event 'old' into event 'new'.
+  All 4 callbacks are optional, if you don't want to specify any callbacks
+  the ops argument itself maybe NULL.
 
-- v4l2_event_unsubscribe()
+int v4l2_event_unsubscribe(struct v4l2_fh *fh,
+			   struct v4l2_event_subscription *sub)
 
   vidioc_unsubscribe_event in struct v4l2_ioctl_ops. A driver may use
   v4l2_event_unsubscribe() directly unless it wants to be involved in
@@ -964,7 +1034,7 @@ Useful functions:
   The special type V4L2_EVENT_ALL may be used to unsubscribe all events. The
   drivers may want to handle this in a special way.
 
-- v4l2_event_pending()
+int v4l2_event_pending(struct v4l2_fh *fh)
 
   Returns the number of pending events. Useful when implementing poll.
 
diff --git a/MAINTAINERS b/MAINTAINERS
index 27a1d3c6eec8..eaff0392eb32 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -2398,10 +2398,10 @@ F:	drivers/gpu/drm/
 F:	include/drm/
 
 INTEL DRM DRIVERS (excluding Poulsbo, Moorestown and derivative chipsets)
-M:	Keith Packard <keithp@keithp.com>
+M:	Daniel Vetter <daniel.vetter@ffwll.ch>
 L:	intel-gfx@lists.freedesktop.org (subscribers-only)
 L:	dri-devel@lists.freedesktop.org
-T:	git git://git.kernel.org/pub/scm/linux/kernel/git/keithp/linux.git
+T:	git git://people.freedesktop.org/~danvet/drm-intel
 S:	Supported
 F:	drivers/gpu/drm/i915
 F:	include/drm/i915*
@@ -2718,6 +2718,13 @@ S:	Maintained
 F:	Documentation/hwmon/f71805f
 F:	drivers/hwmon/f71805f.c
 
+FC0011 TUNER DRIVER
+M:	Michael Buesch <m@bues.ch>
+L:	linux-media@vger.kernel.org
+S:	Maintained
+F:	drivers/media/common/tuners/fc0011.h
+F:	drivers/media/common/tuners/fc0011.c
+
 FANOTIFY
 M:	Eric Paris <eparis@redhat.com>
 S:	Maintained
@@ -3458,6 +3465,8 @@ Q:	http://patchwork.kernel.org/project/linux-input/list/
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/dtor/input.git
 S:	Maintained
 F:	drivers/input/
+F:	include/linux/input.h
+F:	include/linux/input/
 
 INPUT MULTITOUCH (MT) PROTOCOL
 M:	Henrik Rydberg <rydberg@euromail.se>
@@ -7191,7 +7200,7 @@ F:	include/linux/usb/usbnet.h
 
 USB VIDEO CLASS
 M:	Laurent Pinchart <laurent.pinchart@ideasonboard.com>
-L:	linux-uvc-devel@lists.berlios.de (subscribers-only)
+L:	linux-uvc-devel@lists.sourceforge.net (subscribers-only)
 L:	linux-media@vger.kernel.org
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/mchehab/linux-media.git
 W:	http://www.ideasonboard.org/uvc/
diff --git a/arch/Kconfig b/arch/Kconfig
index 1f9461b9cc89..e9a910876cda 100644
--- a/arch/Kconfig
+++ b/arch/Kconfig
@@ -76,6 +76,23 @@ config OPTPROBES
 	depends on KPROBES && HAVE_OPTPROBES
 	depends on !PREEMPT
 
+config UPROBES
+	bool "Transparent user-space probes (EXPERIMENTAL)"
+	depends on UPROBE_EVENT && PERF_EVENTS
+	default n
+	help
+	  Uprobes is the user-space counterpart to kprobes: they
+	  enable instrumentation applications (such as 'perf probe')
+	  to establish unintrusive probes in user-space binaries and
+	  libraries, by executing handler functions when the probes
+	  are hit by user-space applications.
+
+	  ( These probes come in the form of single-byte breakpoints,
+	    managed by the kernel and kept transparent to the probed
+	    application. )
+
+	  If in doubt, say "N".
+
 config HAVE_EFFICIENT_UNALIGNED_ACCESS
 	bool
 	help
diff --git a/arch/alpha/Kconfig b/arch/alpha/Kconfig
index 0893f023efb8..3de74c9f9610 100644
--- a/arch/alpha/Kconfig
+++ b/arch/alpha/Kconfig
@@ -16,6 +16,7 @@ config ALPHA
 	select ARCH_WANT_OPTIONAL_GPIOLIB
 	select ARCH_HAVE_NMI_SAFE_CMPXCHG
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_CMOS_UPDATE
 	help
 	  The Alpha is a 64-bit general-purpose processor designed and
 	  marketed by the Digital Equipment Corporation of blessed memory,
@@ -48,9 +49,6 @@ config GENERIC_CALIBRATE_DELAY
 	bool
 	default y
 
-config GENERIC_CMOS_UPDATE
-        def_bool y
-
 config GENERIC_GPIO
 	bool
 
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 4f4c8115d79b..312450941a1a 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -40,6 +40,8 @@ config ARM
 	select GENERIC_PCI_IOMAP
 	select HAVE_BPF_JIT
 	select GENERIC_SMP_IDLE_THREAD
+	select KTIME_SCALAR
+	select GENERIC_CLOCKEVENTS_BROADCAST if SMP
 	help
 	  The ARM series is a line of low-power-consumption RISC chip designs
 	  licensed by ARM Ltd and targeted at embedded applications and
@@ -63,22 +65,6 @@ config SYS_SUPPORTS_APM_EMULATION
 config GENERIC_GPIO
 	bool
 
-config ARCH_USES_GETTIMEOFFSET
-	bool
-	default n
-
-config GENERIC_CLOCKEVENTS
-	bool
-
-config GENERIC_CLOCKEVENTS_BROADCAST
-	bool
-	depends on GENERIC_CLOCKEVENTS
-	default y if SMP
-
-config KTIME_SCALAR
-	bool
-	default y
-
 config HAVE_TCM
 	bool
 	select GENERIC_ALLOCATOR
@@ -1438,8 +1424,6 @@ endmenu
 
 menu "Kernel Features"
 
-source "kernel/time/Kconfig"
-
 config HAVE_SMP
 	bool
 	help
diff --git a/arch/arm/mach-imx/mach-imx27_visstrim_m10.c b/arch/arm/mach-imx/mach-imx27_visstrim_m10.c
index 748ba2e311b5..dff82eb57cd9 100644
--- a/arch/arm/mach-imx/mach-imx27_visstrim_m10.c
+++ b/arch/arm/mach-imx/mach-imx27_visstrim_m10.c
@@ -178,7 +178,7 @@ static struct soc_camera_link iclink_tvp5150 = {
 
 static struct mx2_camera_platform_data visstrim_camera = {
 	.flags = MX2_CAMERA_CCIR | MX2_CAMERA_CCIR_INTERLACE |
-			MX2_CAMERA_SWAP16 | MX2_CAMERA_PCLK_SAMPLE_RISING,
+		 MX2_CAMERA_PCLK_SAMPLE_RISING,
 	.clk = 100000,
 };
 
diff --git a/arch/arm/plat-mxc/include/mach/mx2_cam.h b/arch/arm/plat-mxc/include/mach/mx2_cam.h
index 3c080a32dbf5..7ded6f1f74bc 100644
--- a/arch/arm/plat-mxc/include/mach/mx2_cam.h
+++ b/arch/arm/plat-mxc/include/mach/mx2_cam.h
@@ -23,7 +23,6 @@
 #ifndef __MACH_MX2_CAM_H_
 #define __MACH_MX2_CAM_H_
 
-#define MX2_CAMERA_SWAP16		(1 << 0)
 #define MX2_CAMERA_EXT_VSYNC		(1 << 1)
 #define MX2_CAMERA_CCIR			(1 << 2)
 #define MX2_CAMERA_CCIR_INTERLACE	(1 << 3)
@@ -31,7 +30,6 @@
 #define MX2_CAMERA_GATED_CLOCK		(1 << 5)
 #define MX2_CAMERA_INV_DATA		(1 << 6)
 #define MX2_CAMERA_PCLK_SAMPLE_RISING	(1 << 7)
-#define MX2_CAMERA_PACK_DIR_MSB		(1 << 8)
 
 /**
  * struct mx2_camera_platform_data - optional platform data for mx2_camera
diff --git a/arch/avr32/Kconfig b/arch/avr32/Kconfig
index 3dea7231f637..f8bc2d27d148 100644
--- a/arch/avr32/Kconfig
+++ b/arch/avr32/Kconfig
@@ -12,6 +12,7 @@ config AVR32
 	select HARDIRQS_SW_RESEND
 	select GENERIC_IRQ_SHOW
 	select ARCH_HAVE_NMI_SAFE_CMPXCHG
+	select GENERIC_CLOCKEVENTS
 	help
 	  AVR32 is a high-performance 32-bit RISC microprocessor core,
 	  designed for cost-sensitive embedded applications, with particular
@@ -35,9 +36,6 @@ config TRACE_IRQFLAGS_SUPPORT
 config RWSEM_GENERIC_SPINLOCK
 	def_bool y
 
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 config RWSEM_XCHGADD_ALGORITHM
 	def_bool n
 
@@ -63,8 +61,6 @@ source "kernel/Kconfig.freezer"
 
 menu "System Type and features"
 
-source "kernel/time/Kconfig"
-
 config SUBARCH_AVR32B
 	bool
 config MMU
diff --git a/arch/blackfin/Kconfig b/arch/blackfin/Kconfig
index 79cfe2614bcc..04ec0d8fbbb5 100644
--- a/arch/blackfin/Kconfig
+++ b/arch/blackfin/Kconfig
@@ -38,6 +38,7 @@ config BLACKFIN
 	select IRQ_PER_CPU if SMP
 	select HAVE_NMI_WATCHDOG if NMI_WATCHDOG
 	select GENERIC_SMP_IDLE_THREAD
+	select ARCH_USES_GETTIMEOFFSET if !GENERIC_CLOCKEVENTS
 
 config GENERIC_CSUM
 	def_bool y
@@ -642,9 +643,10 @@ comment "Kernel Timer/Scheduler"
 
 source kernel/Kconfig.hz
 
-config GENERIC_CLOCKEVENTS
+config SET_GENERIC_CLOCKEVENTS
 	bool "Generic clock events"
 	default y
+	select GENERIC_CLOCKEVENTS
 
 menu "Clock event device"
 	depends on GENERIC_CLOCKEVENTS
@@ -678,12 +680,6 @@ config GPTMR0_CLOCKSOURCE
 	depends on !TICKSOURCE_GPTMR0
 endmenu
 
-config ARCH_USES_GETTIMEOFFSET
-	depends on !GENERIC_CLOCKEVENTS
-	def_bool y
-
-source kernel/time/Kconfig
-
 comment "Misc"
 
 choice
diff --git a/arch/c6x/Kconfig b/arch/c6x/Kconfig
index 1f15b88b537f..052f81a76239 100644
--- a/arch/c6x/Kconfig
+++ b/arch/c6x/Kconfig
@@ -15,6 +15,7 @@ config C6X
 	select IRQ_DOMAIN
 	select OF
 	select OF_EARLY_FLATTREE
+	select GENERIC_CLOCKEVENTS
 
 config MMU
 	def_bool n
@@ -31,12 +32,6 @@ config GENERIC_CALIBRATE_DELAY
 config GENERIC_HWEIGHT
 	def_bool y
 
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
-config GENERIC_CLOCKEVENTS_BROADCAST
-	bool
-
 config GENERIC_BUG
 	def_bool y
 
@@ -125,7 +120,6 @@ source "mm/Kconfig"
 source "kernel/Kconfig.preempt"
 
 source "kernel/Kconfig.hz"
-source "kernel/time/Kconfig"
 
 endmenu
 
diff --git a/arch/cris/Kconfig b/arch/cris/Kconfig
index 2995035812ec..22d34d64cc81 100644
--- a/arch/cris/Kconfig
+++ b/arch/cris/Kconfig
@@ -13,12 +13,6 @@ config RWSEM_GENERIC_SPINLOCK
 config RWSEM_XCHGADD_ALGORITHM
 	bool
 
-config GENERIC_CMOS_UPDATE
-	def_bool y
-
-config ARCH_USES_GETTIMEOFFSET
-	def_bool n
-
 config ARCH_HAS_ILOG2_U32
 	bool
 	default n
@@ -50,6 +44,7 @@ config CRIS
 	select GENERIC_IRQ_SHOW
 	select GENERIC_IOMAP
 	select GENERIC_SMP_IDLE_THREAD if ETRAX_ARCH_V32
+	select GENERIC_CMOS_UPDATE
 
 config HZ
 	int
diff --git a/arch/h8300/Kconfig.cpu b/arch/h8300/Kconfig.cpu
index 15c22286ae79..321f3922728b 100644
--- a/arch/h8300/Kconfig.cpu
+++ b/arch/h8300/Kconfig.cpu
@@ -1,7 +1,5 @@
 menu "Processor type and features"
 
-source "kernel/time/Kconfig"
-
 choice
 	prompt "H8/300 platform"
 	default H8300H_GENERIC
diff --git a/arch/hexagon/Kconfig b/arch/hexagon/Kconfig
index bc979f770980..b2fdfb700f50 100644
--- a/arch/hexagon/Kconfig
+++ b/arch/hexagon/Kconfig
@@ -27,6 +27,9 @@ config HEXAGON
 	select GENERIC_IOMAP
 	select GENERIC_SMP_IDLE_THREAD
 	select STACKTRACE_SUPPORT
+	select KTIME_SCALAR
+	select GENERIC_CLOCKEVENTS
+	select GENERIC_CLOCKEVENTS_BROADCAST
 	---help---
 	  Qualcomm Hexagon is a processor architecture designed for high
 	  performance and low power across a wide variety of applications.
@@ -55,9 +58,6 @@ config PCI
 config EARLY_PRINTK
 	def_bool y
 
-config KTIME_SCALAR
-	def_bool y
-
 config MMU
 	def_bool y
 
@@ -88,15 +88,6 @@ config GENERIC_FIND_NEXT_BIT
 config GENERIC_HWEIGHT
 	def_bool y
 
-config GENERIC_TIME
-	def_bool y
-
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
-config GENERIC_CLOCKEVENTS_BROADCAST
-	def_bool y
-
 config STACKTRACE_SUPPORT
 	def_bool y
 	select STACKTRACE
@@ -179,7 +170,6 @@ endchoice
 source "mm/Kconfig"
 
 source "kernel/Kconfig.hz"
-source "kernel/time/Kconfig"
 
 config GENERIC_GPIO
 	def_bool n
diff --git a/arch/ia64/Kconfig b/arch/ia64/Kconfig
index ba667b60f32d..8186ec5ea151 100644
--- a/arch/ia64/Kconfig
+++ b/arch/ia64/Kconfig
@@ -37,6 +37,8 @@ config IA64
 	select ARCH_INIT_TASK
 	select ARCH_TASK_STRUCT_ALLOCATOR
 	select ARCH_THREAD_INFO_ALLOCATOR
+	select ARCH_CLOCKSOURCE_DATA
+	select GENERIC_TIME_VSYSCALL
 	default y
 	help
 	  The Itanium Processor Family is Intel's 64-bit successor to
@@ -92,10 +94,6 @@ config GENERIC_CALIBRATE_DELAY
 	bool
 	default y
 
-config GENERIC_TIME_VSYSCALL
-	bool
-	default y
-
 config HAVE_SETUP_PER_CPU_AREA
 	def_bool y
 
@@ -110,9 +108,6 @@ config EFI
 	bool
 	default y
 
-config ARCH_CLOCKSOURCE_DATA
-	def_bool y
-
 config SCHED_OMIT_FRAME_POINTER
 	bool
 	default y
diff --git a/arch/m32r/Kconfig b/arch/m32r/Kconfig
index ef80a6546ff2..b638d5bfa14d 100644
--- a/arch/m32r/Kconfig
+++ b/arch/m32r/Kconfig
@@ -11,6 +11,7 @@ config M32R
 	select GENERIC_IRQ_PROBE
 	select GENERIC_IRQ_SHOW
 	select GENERIC_ATOMIC64
+	select ARCH_USES_GETTIMEOFFSET
 
 config SBUS
 	bool
@@ -33,9 +34,6 @@ config HZ
 	int
 	default 100
 
-config ARCH_USES_GETTIMEOFFSET
-	def_bool y
-
 source "init/Kconfig"
 
 source "kernel/Kconfig.freezer"
diff --git a/arch/m68k/Kconfig b/arch/m68k/Kconfig
index d318c606c888..cac5b6be572a 100644
--- a/arch/m68k/Kconfig
+++ b/arch/m68k/Kconfig
@@ -8,6 +8,7 @@ config M68K
 	select ARCH_HAVE_NMI_SAFE_CMPXCHG if RMW_INSNS
 	select GENERIC_CPU_DEVICES
 	select FPU if MMU
+	select ARCH_USES_GETTIMEOFFSET if MMU && !COLDFIRE
 
 config RWSEM_GENERIC_SPINLOCK
 	bool
@@ -22,9 +23,6 @@ config ARCH_HAS_ILOG2_U32
 config ARCH_HAS_ILOG2_U64
 	bool
 
-config GENERIC_CLOCKEVENTS
-	bool
-
 config GENERIC_GPIO
 	bool
 
@@ -43,9 +41,6 @@ config TIME_LOW_RES
 	bool
 	default y
 
-config ARCH_USES_GETTIMEOFFSET
-	def_bool MMU && !COLDFIRE
-
 config NO_IOPORT
 	def_bool y
 
@@ -111,10 +106,6 @@ if COLDFIRE
 source "kernel/Kconfig.preempt"
 endif
 
-if !MMU || COLDFIRE
-source "kernel/time/Kconfig"
-endif
-
 source "mm/Kconfig"
 
 endmenu
diff --git a/arch/microblaze/Kconfig b/arch/microblaze/Kconfig
index ac22dc7f4cab..83460468998d 100644
--- a/arch/microblaze/Kconfig
+++ b/arch/microblaze/Kconfig
@@ -22,6 +22,7 @@ config MICROBLAZE
 	select GENERIC_PCI_IOMAP
 	select GENERIC_CPU_DEVICES
 	select GENERIC_ATOMIC64
+	select GENERIC_CLOCKEVENTS
 
 config SWAP
 	def_bool n
@@ -50,12 +51,6 @@ config GENERIC_HWEIGHT
 config GENERIC_CALIBRATE_DELAY
 	def_bool y
 
-config GENERIC_TIME_VSYSCALL
-	def_bool n
-
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 config GENERIC_GPIO
 	def_bool y
 
@@ -79,8 +74,6 @@ source "arch/microblaze/platform/Kconfig.platform"
 
 menu "Processor type and features"
 
-source "kernel/time/Kconfig"
-
 source "kernel/Kconfig.preempt"
 
 source "kernel/Kconfig.hz"
diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
index 85aad0321397..3aa826bcbf96 100644
--- a/arch/mips/Kconfig
+++ b/arch/mips/Kconfig
@@ -31,6 +31,8 @@ config MIPS
 	select ARCH_DISCARD_MEMBLOCK
 	select GENERIC_SMP_IDLE_THREAD
 	select BUILDTIME_EXTABLE_SORT
+	select GENERIC_CLOCKEVENTS
+	select GENERIC_CMOS_UPDATE
 
 menu "Machine selection"
 
@@ -858,14 +860,6 @@ config GENERIC_CALIBRATE_DELAY
 	bool
 	default y
 
-config GENERIC_CLOCKEVENTS
-	bool
-	default y
-
-config GENERIC_CMOS_UPDATE
-	bool
-	default y
-
 config SCHED_OMIT_FRAME_POINTER
 	bool
 	default y
@@ -2052,9 +2046,6 @@ config CPU_HAS_SYNC
 	depends on !CPU_R3000
 	default y
 
-config GENERIC_CLOCKEVENTS_BROADCAST
-	bool
-
 #
 # CPU non-features
 #
@@ -2216,8 +2207,6 @@ config NR_CPUS
 	  performance should round up your number of processors to the next
 	  power of two.
 
-source "kernel/time/Kconfig"
-
 #
 # Timer Interrupt Frequency Configuration
 #
diff --git a/arch/mips/bcm47xx/setup.c b/arch/mips/bcm47xx/setup.c
index 19780aa91708..95bf4d7bac21 100644
--- a/arch/mips/bcm47xx/setup.c
+++ b/arch/mips/bcm47xx/setup.c
@@ -90,6 +90,7 @@ static int bcm47xx_get_sprom_ssb(struct ssb_bus *bus, struct ssb_sprom *out)
 	char prefix[10];
 
 	if (bus->bustype == SSB_BUSTYPE_PCI) {
+		memset(out, 0, sizeof(struct ssb_sprom));
 		snprintf(prefix, sizeof(prefix), "pci/%u/%u/",
 			 bus->host_pci->bus->number + 1,
 			 PCI_SLOT(bus->host_pci->devfn));
@@ -109,15 +110,9 @@ static int bcm47xx_get_invariants(struct ssb_bus *bus,
 	/* Fill boardinfo structure */
 	memset(&(iv->boardinfo), 0 , sizeof(struct ssb_boardinfo));
 
-	if (nvram_getenv("boardvendor", buf, sizeof(buf)) >= 0)
-		iv->boardinfo.vendor = (u16)simple_strtoul(buf, NULL, 0);
-	else
-		iv->boardinfo.vendor = SSB_BOARDVENDOR_BCM;
-	if (nvram_getenv("boardtype", buf, sizeof(buf)) >= 0)
-		iv->boardinfo.type = (u16)simple_strtoul(buf, NULL, 0);
-	if (nvram_getenv("boardrev", buf, sizeof(buf)) >= 0)
-		iv->boardinfo.rev = (u16)simple_strtoul(buf, NULL, 0);
+	bcm47xx_fill_ssb_boardinfo(&iv->boardinfo, NULL);
 
+	memset(&iv->sprom, 0, sizeof(struct ssb_sprom));
 	bcm47xx_fill_sprom(&iv->sprom, NULL);
 
 	if (nvram_getenv("cardbus", buf, sizeof(buf)) >= 0)
@@ -166,12 +161,14 @@ static int bcm47xx_get_sprom_bcma(struct bcma_bus *bus, struct ssb_sprom *out)
 
 	switch (bus->hosttype) {
 	case BCMA_HOSTTYPE_PCI:
+		memset(out, 0, sizeof(struct ssb_sprom));
 		snprintf(prefix, sizeof(prefix), "pci/%u/%u/",
 			 bus->host_pci->bus->number + 1,
 			 PCI_SLOT(bus->host_pci->devfn));
 		bcm47xx_fill_sprom(out, prefix);
 		return 0;
 	case BCMA_HOSTTYPE_SOC:
+		memset(out, 0, sizeof(struct ssb_sprom));
 		bcm47xx_fill_sprom_ethernet(out, NULL);
 		core = bcma_find_core(bus, BCMA_CORE_80211);
 		if (core) {
@@ -197,6 +194,8 @@ static void __init bcm47xx_register_bcma(void)
 	err = bcma_host_soc_register(&bcm47xx_bus.bcma);
 	if (err)
 		panic("Failed to initialize BCMA bus (err %d)", err);
+
+	bcm47xx_fill_bcma_boardinfo(&bcm47xx_bus.bcma.bus.boardinfo, NULL);
 }
 #endif
 
diff --git a/arch/mips/bcm47xx/sprom.c b/arch/mips/bcm47xx/sprom.c
index 5c8dcd2a8a93..d3a889745e20 100644
--- a/arch/mips/bcm47xx/sprom.c
+++ b/arch/mips/bcm47xx/sprom.c
@@ -165,6 +165,8 @@ static void bcm47xx_fill_sprom_r1234589(struct ssb_sprom *sprom,
 					const char *prefix)
 {
 	nvram_read_u16(prefix, NULL, "boardrev", &sprom->board_rev, 0);
+	if (!sprom->board_rev)
+		nvram_read_u16(NULL, NULL, "boardrev", &sprom->board_rev, 0);
 	nvram_read_u16(prefix, NULL, "boardnum", &sprom->board_num, 0);
 	nvram_read_u8(prefix, NULL, "ledbh0", &sprom->gpio0, 0xff);
 	nvram_read_u8(prefix, NULL, "ledbh1", &sprom->gpio1, 0xff);
@@ -555,8 +557,6 @@ void bcm47xx_fill_sprom_ethernet(struct ssb_sprom *sprom, const char *prefix)
 
 void bcm47xx_fill_sprom(struct ssb_sprom *sprom, const char *prefix)
 {
-	memset(sprom, 0, sizeof(struct ssb_sprom));
-
 	bcm47xx_fill_sprom_ethernet(sprom, prefix);
 
 	nvram_read_u8(prefix, NULL, "sromrev", &sprom->revision, 0);
@@ -618,3 +618,27 @@ void bcm47xx_fill_sprom(struct ssb_sprom *sprom, const char *prefix)
 		bcm47xx_fill_sprom_r1(sprom, prefix);
 	}
 }
+
+#ifdef CONFIG_BCM47XX_SSB
+void bcm47xx_fill_ssb_boardinfo(struct ssb_boardinfo *boardinfo,
+				const char *prefix)
+{
+	nvram_read_u16(prefix, NULL, "boardvendor", &boardinfo->vendor, 0);
+	if (!boardinfo->vendor)
+		boardinfo->vendor = SSB_BOARDVENDOR_BCM;
+
+	nvram_read_u16(prefix, NULL, "boardtype", &boardinfo->type, 0);
+}
+#endif
+
+#ifdef CONFIG_BCM47XX_BCMA
+void bcm47xx_fill_bcma_boardinfo(struct bcma_boardinfo *boardinfo,
+				 const char *prefix)
+{
+	nvram_read_u16(prefix, NULL, "boardvendor", &boardinfo->vendor, 0);
+	if (!boardinfo->vendor)
+		boardinfo->vendor = SSB_BOARDVENDOR_BCM;
+
+	nvram_read_u16(prefix, NULL, "boardtype", &boardinfo->type, 0);
+}
+#endif
diff --git a/arch/mips/include/asm/mach-bcm47xx/bcm47xx.h b/arch/mips/include/asm/mach-bcm47xx/bcm47xx.h
index 5ecaf47b34d2..26fdaf40b930 100644
--- a/arch/mips/include/asm/mach-bcm47xx/bcm47xx.h
+++ b/arch/mips/include/asm/mach-bcm47xx/bcm47xx.h
@@ -47,4 +47,13 @@ extern enum bcm47xx_bus_type bcm47xx_bus_type;
 void bcm47xx_fill_sprom(struct ssb_sprom *sprom, const char *prefix);
 void bcm47xx_fill_sprom_ethernet(struct ssb_sprom *sprom, const char *prefix);
 
+#ifdef CONFIG_BCM47XX_SSB
+void bcm47xx_fill_ssb_boardinfo(struct ssb_boardinfo *boardinfo,
+				const char *prefix);
+#endif
+#ifdef CONFIG_BCM47XX_BCMA
+void bcm47xx_fill_bcma_boardinfo(struct bcma_boardinfo *boardinfo,
+				 const char *prefix);
+#endif
+
 #endif /* __ASM_BCM47XX_H */
diff --git a/arch/mn10300/Kconfig b/arch/mn10300/Kconfig
index 3aa3de017159..687f9b4a2ed6 100644
--- a/arch/mn10300/Kconfig
+++ b/arch/mn10300/Kconfig
@@ -6,6 +6,7 @@ config MN10300
 	select HAVE_ARCH_TRACEHOOK
 	select HAVE_ARCH_KGDB
 	select HAVE_NMI_WATCHDOG if MN10300_WD_TIMER
+	select GENERIC_CLOCKEVENTS
 
 config AM33_2
 	def_bool n
@@ -42,15 +43,9 @@ config RWSEM_XCHGADD_ALGORITHM
 config GENERIC_CALIBRATE_DELAY
 	def_bool y
 
-config GENERIC_CMOS_UPDATE
-        def_bool n
-
 config GENERIC_HWEIGHT
 	def_bool y
 
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 config GENERIC_BUG
 	def_bool y
 
@@ -231,7 +226,6 @@ config MN10300_USING_JTAG
 	  single-stepping, which are taken over completely by the JTAG unit.
 
 source "kernel/Kconfig.hz"
-source "kernel/time/Kconfig"
 
 config MN10300_RTC
 	bool "Using MN10300 RTC"
diff --git a/arch/openrisc/Kconfig b/arch/openrisc/Kconfig
index 297bd38f7c5d..4932247d078a 100644
--- a/arch/openrisc/Kconfig
+++ b/arch/openrisc/Kconfig
@@ -18,6 +18,7 @@ config OPENRISC
 	select GENERIC_IOMAP
 	select GENERIC_CPU_DEVICES
 	select GENERIC_ATOMIC64
+	select GENERIC_CLOCKEVENTS
 
 config MMU
 	def_bool y
@@ -47,9 +48,6 @@ config NO_IOPORT
 config GENERIC_GPIO
 	def_bool y
 
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 config TRACE_IRQFLAGS_SUPPORT
         def_bool y
 
@@ -109,7 +107,6 @@ config OPENRISC_HAVE_INST_DIV
 endmenu
 
 
-source "kernel/time/Kconfig"
 source kernel/Kconfig.hz
 source kernel/Kconfig.preempt
 source "mm/Kconfig"
diff --git a/arch/powerpc/Kconfig b/arch/powerpc/Kconfig
index 0a947bd9c076..00b9874e2240 100644
--- a/arch/powerpc/Kconfig
+++ b/arch/powerpc/Kconfig
@@ -27,15 +27,6 @@ config MMU
 	bool
 	default y
 
-config GENERIC_CMOS_UPDATE
-	def_bool y
-
-config GENERIC_TIME_VSYSCALL
-	def_bool y
-
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 config HAVE_SETUP_PER_CPU_AREA
 	def_bool PPC64
 
@@ -141,6 +132,9 @@ config PPC
 	select HAVE_ARCH_JUMP_LABEL
 	select ARCH_HAVE_NMI_SAFE_CMPXCHG
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_CMOS_UPDATE
+	select GENERIC_TIME_VSYSCALL
+	select GENERIC_CLOCKEVENTS
 
 config EARLY_PRINTK
 	bool
@@ -281,7 +275,6 @@ config HIGHMEM
 	bool "High memory support"
 	depends on PPC32
 
-source kernel/time/Kconfig
 source kernel/Kconfig.hz
 source kernel/Kconfig.preempt
 source "fs/Kconfig.binfmt"
diff --git a/arch/s390/Kconfig b/arch/s390/Kconfig
index e16390c0bca8..b403c533432c 100644
--- a/arch/s390/Kconfig
+++ b/arch/s390/Kconfig
@@ -28,12 +28,6 @@ config ARCH_HAS_ILOG2_U64
 config GENERIC_HWEIGHT
 	def_bool y
 
-config GENERIC_TIME_VSYSCALL
-	def_bool y
-
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 config GENERIC_BUG
 	def_bool y if BUG
 
@@ -123,6 +117,9 @@ config S390
 	select ARCH_INLINE_WRITE_UNLOCK_IRQ
 	select ARCH_INLINE_WRITE_UNLOCK_IRQRESTORE
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_TIME_VSYSCALL
+	select GENERIC_CLOCKEVENTS
+	select KTIME_SCALAR if 32BIT
 
 config SCHED_OMIT_FRAME_POINTER
 	def_bool y
@@ -135,8 +132,6 @@ menu "Base setup"
 
 comment "Processor type and features"
 
-source "kernel/time/Kconfig"
-
 config 64BIT
 	def_bool y
 	prompt "64 bit kernel"
@@ -147,9 +142,6 @@ config 64BIT
 config 32BIT
 	def_bool y if !64BIT
 
-config KTIME_SCALAR
-	def_bool 32BIT
-
 config SMP
 	def_bool y
 	prompt "Symmetric multi-processing support"
diff --git a/arch/score/Kconfig b/arch/score/Kconfig
index 4b285779ac05..ba0f412920be 100644
--- a/arch/score/Kconfig
+++ b/arch/score/Kconfig
@@ -9,6 +9,7 @@ config SCORE
        select HAVE_MEMBLOCK_NODE_MAP
        select ARCH_DISCARD_MEMBLOCK
        select GENERIC_CPU_DEVICES
+       select GENERIC_CLOCKEVENTS
 
 choice
 	prompt "System type"
@@ -51,9 +52,6 @@ config GENERIC_HWEIGHT
 config GENERIC_CALIBRATE_DELAY
 	def_bool y
 
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 menu "Kernel type"
 
 config 32BIT
@@ -68,7 +66,6 @@ config MEMORY_START
 	hex
 	default	0xa0000000
 
-source "kernel/time/Kconfig"
 source "kernel/Kconfig.hz"
 source "kernel/Kconfig.preempt"
 
diff --git a/arch/sh/Kconfig b/arch/sh/Kconfig
index 3e723aaa5e18..5e05c0b445bb 100644
--- a/arch/sh/Kconfig
+++ b/arch/sh/Kconfig
@@ -29,6 +29,8 @@ config SUPERH
 	select GENERIC_ATOMIC64
 	select GENERIC_IRQ_SHOW
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_CLOCKEVENTS
+	select GENERIC_CMOS_UPDATE if SH_SH03 || SH_DREAMCAST
 	help
 	  The SuperH is a RISC processor targeted for use in embedded systems
 	  and consumer electronics; it was also used in the Sega Dreamcast
@@ -87,16 +89,6 @@ config GENERIC_GPIO
 config GENERIC_CALIBRATE_DELAY
 	bool
 
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
-config GENERIC_CLOCKEVENTS_BROADCAST
-	bool
-
-config GENERIC_CMOS_UPDATE
-	def_bool y
-	depends on SH_SH03 || SH_DREAMCAST
-
 config GENERIC_LOCKBREAK
 	def_bool y
 	depends on SMP && PREEMPT
@@ -611,8 +603,6 @@ config SH_CLK_CPG_LEGACY
 		      !CPU_SUBTYPE_SH7734 && !CPU_SUBTYPE_SH7264 && \
 		      !CPU_SUBTYPE_SH7269
 
-source "kernel/time/Kconfig"
-
 endmenu
 
 menu "CPU Frequency scaling"
diff --git a/arch/sparc/Kconfig b/arch/sparc/Kconfig
index 1ea3fd954756..2d493a3bdfe1 100644
--- a/arch/sparc/Kconfig
+++ b/arch/sparc/Kconfig
@@ -32,12 +32,15 @@ config SPARC
 	select HAVE_NMI_WATCHDOG if SPARC64
 	select HAVE_BPF_JIT
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_CMOS_UPDATE
+	select GENERIC_CLOCKEVENTS
 
 config SPARC32
 	def_bool !64BIT
 	select GENERIC_ATOMIC64
 	select CLZ_TAB
 	select ARCH_THREAD_INFO_ALLOCATOR
+	select ARCH_USES_GETTIMEOFFSET
 
 config SPARC64
 	def_bool 64BIT
@@ -77,13 +80,6 @@ config BITS
 	default 32 if SPARC32
 	default 64 if SPARC64
 
-config GENERIC_CMOS_UPDATE
-	bool
-	default y
-
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 config IOMMU_HELPER
 	bool
 	default y if SPARC64
@@ -274,8 +270,6 @@ config HOTPLUG_CPU
 	  can be controlled through /sys/devices/system/cpu/cpu#.
 	  Say N if you want to disable CPU hotplug.
 
-source "kernel/time/Kconfig"
-
 if SPARC64
 source "drivers/cpufreq/Kconfig"
 
diff --git a/arch/tile/Kconfig b/arch/tile/Kconfig
index 74239dd77e06..6ad6219fc47e 100644
--- a/arch/tile/Kconfig
+++ b/arch/tile/Kconfig
@@ -14,6 +14,7 @@ config TILE
 	select HAVE_SYSCALL_WRAPPERS if TILEGX
 	select SYS_HYPERVISOR
 	select ARCH_HAVE_NMI_SAFE_CMPXCHG
+	select GENERIC_CLOCKEVENTS
 
 # FIXME: investigate whether we need/want these options.
 #	select HAVE_IOREMAP_PROT
@@ -47,9 +48,6 @@ config NEED_PER_CPU_PAGE_FIRST_CHUNK
 config SYS_SUPPORTS_HUGETLBFS
 	def_bool y
 
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
 # FIXME: tilegx can implement a more efficient rwsem.
 config RWSEM_GENERIC_SPINLOCK
 	def_bool y
@@ -139,8 +137,6 @@ config NR_CPUS
 	  smaller kernel memory footprint results from using a smaller
 	  value on chips with fewer tiles.
 
-source "kernel/time/Kconfig"
-
 source "kernel/Kconfig.hz"
 
 config KEXEC
diff --git a/arch/um/Kconfig.common b/arch/um/Kconfig.common
index 43ef890d292c..cb837c223922 100644
--- a/arch/um/Kconfig.common
+++ b/arch/um/Kconfig.common
@@ -10,6 +10,7 @@ config UML
 	select GENERIC_IRQ_SHOW
 	select GENERIC_CPU_DEVICES
 	select GENERIC_IO
+	select GENERIC_CLOCKEVENTS
 
 config MMU
 	bool
@@ -52,10 +53,6 @@ config GENERIC_BUG
 	default y
 	depends on BUG
 
-config GENERIC_CLOCKEVENTS
-	bool
-	default y
-
 config HZ
 	int
 	default 100
diff --git a/arch/um/Kconfig.um b/arch/um/Kconfig.um
index 70fd690964e4..bf87f25eb2de 100644
--- a/arch/um/Kconfig.um
+++ b/arch/um/Kconfig.um
@@ -10,7 +10,6 @@ config STATIC_LINK
 	  2.75G) for UML.
 
 source "mm/Kconfig"
-source "kernel/time/Kconfig"
 
 config LD_SCRIPT_STATIC
 	bool
diff --git a/arch/unicore32/Kconfig b/arch/unicore32/Kconfig
index eeb8054c7cd8..47ad5210606f 100644
--- a/arch/unicore32/Kconfig
+++ b/arch/unicore32/Kconfig
@@ -25,9 +25,6 @@ config HAVE_PWM
 config GENERIC_GPIO
 	def_bool y
 
-config GENERIC_CLOCKEVENTS
-	bool
-
 config GENERIC_CSUM
 	def_bool y
 
@@ -146,8 +143,6 @@ endmenu
 
 menu "Kernel Features"
 
-source "kernel/time/Kconfig"
-
 source "kernel/Kconfig.preempt"
 
 source "kernel/Kconfig.hz"
diff --git a/arch/x86/Kconfig b/arch/x86/Kconfig
index d6168994e115..66cc380bebf0 100644
--- a/arch/x86/Kconfig
+++ b/arch/x86/Kconfig
@@ -85,9 +85,16 @@ config X86
 	select GENERIC_SMP_IDLE_THREAD
 	select HAVE_ARCH_SECCOMP_FILTER
 	select BUILDTIME_EXTABLE_SORT
+	select GENERIC_CMOS_UPDATE
+	select CLOCKSOURCE_WATCHDOG
+	select GENERIC_CLOCKEVENTS
+	select ARCH_CLOCKSOURCE_DATA if X86_64
+	select GENERIC_CLOCKEVENTS_BROADCAST if X86_64 || (X86_32 && X86_LOCAL_APIC)
+	select GENERIC_TIME_VSYSCALL if X86_64
+	select KTIME_SCALAR if X86_32
 
 config INSTRUCTION_DECODER
-	def_bool (KPROBES || PERF_EVENTS)
+	def_bool (KPROBES || PERF_EVENTS || UPROBES)
 
 config OUTPUT_FORMAT
 	string
@@ -99,23 +106,6 @@ config ARCH_DEFCONFIG
 	default "arch/x86/configs/i386_defconfig" if X86_32
 	default "arch/x86/configs/x86_64_defconfig" if X86_64
 
-config GENERIC_CMOS_UPDATE
-	def_bool y
-
-config CLOCKSOURCE_WATCHDOG
-	def_bool y
-
-config GENERIC_CLOCKEVENTS
-	def_bool y
-
-config ARCH_CLOCKSOURCE_DATA
-	def_bool y
-	depends on X86_64
-
-config GENERIC_CLOCKEVENTS_BROADCAST
-	def_bool y
-	depends on X86_64 || (X86_32 && X86_LOCAL_APIC)
-
 config LOCKDEP_SUPPORT
 	def_bool y
 
@@ -166,10 +156,6 @@ config RWSEM_XCHGADD_ALGORITHM
 config GENERIC_CALIBRATE_DELAY
 	def_bool y
 
-config GENERIC_TIME_VSYSCALL
-	bool
-	default X86_64
-
 config ARCH_HAS_CPU_RELAX
 	def_bool y
 
@@ -236,13 +222,13 @@ config ARCH_HWEIGHT_CFLAGS
 	default "-fcall-saved-ecx -fcall-saved-edx" if X86_32
 	default "-fcall-saved-rdi -fcall-saved-rsi -fcall-saved-rdx -fcall-saved-rcx -fcall-saved-r8 -fcall-saved-r9 -fcall-saved-r10 -fcall-saved-r11" if X86_64
 
-config KTIME_SCALAR
-	def_bool X86_32
-
 config ARCH_CPU_PROBE_RELEASE
 	def_bool y
 	depends on HOTPLUG_CPU
 
+config ARCH_SUPPORTS_UPROBES
+	def_bool y
+
 source "init/Kconfig"
 source "kernel/Kconfig.freezer"
 
@@ -258,8 +244,6 @@ config ZONE_DMA
 
 	  If unsure, say Y.
 
-source "kernel/time/Kconfig"
-
 config SMP
 	bool "Symmetric multi-processing support"
 	---help---
diff --git a/arch/x86/include/asm/thread_info.h b/arch/x86/include/asm/thread_info.h
index 3c9aebc00d39..5c25de07cba8 100644
--- a/arch/x86/include/asm/thread_info.h
+++ b/arch/x86/include/asm/thread_info.h
@@ -85,6 +85,7 @@ struct thread_info {
 #define TIF_SECCOMP		8	/* secure computing */
 #define TIF_MCE_NOTIFY		10	/* notify userspace of an MCE */
 #define TIF_USER_RETURN_NOTIFY	11	/* notify kernel of userspace return */
+#define TIF_UPROBE		12	/* breakpointed or singlestepping */
 #define TIF_NOTSC		16	/* TSC is not accessible in userland */
 #define TIF_IA32		17	/* IA32 compatibility process */
 #define TIF_FORK		18	/* ret_from_fork */
@@ -109,6 +110,7 @@ struct thread_info {
 #define _TIF_SECCOMP		(1 << TIF_SECCOMP)
 #define _TIF_MCE_NOTIFY		(1 << TIF_MCE_NOTIFY)
 #define _TIF_USER_RETURN_NOTIFY	(1 << TIF_USER_RETURN_NOTIFY)
+#define _TIF_UPROBE		(1 << TIF_UPROBE)
 #define _TIF_NOTSC		(1 << TIF_NOTSC)
 #define _TIF_IA32		(1 << TIF_IA32)
 #define _TIF_FORK		(1 << TIF_FORK)
diff --git a/arch/x86/include/asm/uprobes.h b/arch/x86/include/asm/uprobes.h
new file mode 100644
index 000000000000..1e9bed14f7ae
--- /dev/null
+++ b/arch/x86/include/asm/uprobes.h
@@ -0,0 +1,57 @@
+#ifndef _ASM_UPROBES_H
+#define _ASM_UPROBES_H
+/*
+ * User-space Probes (UProbes) for x86
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Copyright (C) IBM Corporation, 2008-2011
+ * Authors:
+ *	Srikar Dronamraju
+ *	Jim Keniston
+ */
+
+#include <linux/notifier.h>
+
+typedef u8 uprobe_opcode_t;
+
+#define MAX_UINSN_BYTES			  16
+#define UPROBE_XOL_SLOT_BYTES		 128	/* to keep it cache aligned */
+
+#define UPROBE_SWBP_INSN		0xcc
+#define UPROBE_SWBP_INSN_SIZE		   1
+
+struct arch_uprobe {
+	u16				fixups;
+	u8				insn[MAX_UINSN_BYTES];
+#ifdef CONFIG_X86_64
+	unsigned long			rip_rela_target_address;
+#endif
+};
+
+struct arch_uprobe_task {
+	unsigned long			saved_trap_nr;
+#ifdef CONFIG_X86_64
+	unsigned long			saved_scratch_register;
+#endif
+};
+
+extern int  arch_uprobe_analyze_insn(struct arch_uprobe *aup, struct mm_struct *mm);
+extern int  arch_uprobe_pre_xol(struct arch_uprobe *aup, struct pt_regs *regs);
+extern int  arch_uprobe_post_xol(struct arch_uprobe *aup, struct pt_regs *regs);
+extern bool arch_uprobe_xol_was_trapped(struct task_struct *tsk);
+extern int  arch_uprobe_exception_notify(struct notifier_block *self, unsigned long val, void *data);
+extern void arch_uprobe_abort_xol(struct arch_uprobe *aup, struct pt_regs *regs);
+#endif	/* _ASM_UPROBES_H */
diff --git a/arch/x86/include/asm/vga.h b/arch/x86/include/asm/vga.h
index c4b9dc2f67c5..44282fbf7bf9 100644
--- a/arch/x86/include/asm/vga.h
+++ b/arch/x86/include/asm/vga.h
@@ -17,4 +17,10 @@
 #define vga_readb(x) (*(x))
 #define vga_writeb(x, y) (*(y) = (x))
 
+#ifdef CONFIG_FB_EFI
+#define __ARCH_HAS_VGA_DEFAULT_DEVICE
+extern struct pci_dev *vga_default_device(void);
+extern void vga_set_default_device(struct pci_dev *pdev);
+#endif
+
 #endif /* _ASM_X86_VGA_H */
diff --git a/arch/x86/kernel/Makefile b/arch/x86/kernel/Makefile
index bb8529275aab..9bba5b79902b 100644
--- a/arch/x86/kernel/Makefile
+++ b/arch/x86/kernel/Makefile
@@ -100,6 +100,7 @@ obj-$(CONFIG_X86_CHECK_BIOS_CORRUPTION) += check.o
 
 obj-$(CONFIG_SWIOTLB)			+= pci-swiotlb.o
 obj-$(CONFIG_OF)			+= devicetree.o
+obj-$(CONFIG_UPROBES)			+= uprobes.o
 
 ###
 # 64 bit specific files
diff --git a/arch/x86/kernel/hpet.c b/arch/x86/kernel/hpet.c
index ad0de0c2714e..9cc7b4392f7c 100644
--- a/arch/x86/kernel/hpet.c
+++ b/arch/x86/kernel/hpet.c
@@ -94,13 +94,18 @@ static int hpet_verbose;
 
 static int __init hpet_setup(char *str)
 {
-	if (str) {
+	while (str) {
+		char *next = strchr(str, ',');
+
+		if (next)
+			*next++ = 0;
 		if (!strncmp("disable", str, 7))
 			boot_hpet_disable = 1;
 		if (!strncmp("force", str, 5))
 			hpet_force_user = 1;
 		if (!strncmp("verbose", str, 7))
 			hpet_verbose = 1;
+		str = next;
 	}
 	return 1;
 }
@@ -319,8 +324,6 @@ static void hpet_set_mode(enum clock_event_mode mode,
 		now = hpet_readl(HPET_COUNTER);
 		cmp = now + (unsigned int) delta;
 		cfg = hpet_readl(HPET_Tn_CFG(timer));
-		/* Make sure we use edge triggered interrupts */
-		cfg &= ~HPET_TN_LEVEL;
 		cfg |= HPET_TN_ENABLE | HPET_TN_PERIODIC |
 		       HPET_TN_SETVAL | HPET_TN_32BIT;
 		hpet_writel(cfg, HPET_Tn_CFG(timer));
@@ -787,15 +790,16 @@ static int hpet_clocksource_register(void)
 	return 0;
 }
 
+static u32 *hpet_boot_cfg;
+
 /**
  * hpet_enable - Try to setup the HPET timer. Returns 1 on success.
  */
 int __init hpet_enable(void)
 {
-	unsigned long hpet_period;
-	unsigned int id;
+	u32 hpet_period, cfg, id;
 	u64 freq;
-	int i;
+	unsigned int i, last;
 
 	if (!is_hpet_capable())
 		return 0;
@@ -847,15 +851,45 @@ int __init hpet_enable(void)
 	id = hpet_readl(HPET_ID);
 	hpet_print_config();
 
+	last = (id & HPET_ID_NUMBER) >> HPET_ID_NUMBER_SHIFT;
+
 #ifdef CONFIG_HPET_EMULATE_RTC
 	/*
 	 * The legacy routing mode needs at least two channels, tick timer
 	 * and the rtc emulation channel.
 	 */
-	if (!(id & HPET_ID_NUMBER))
+	if (!last)
 		goto out_nohpet;
 #endif
 
+	cfg = hpet_readl(HPET_CFG);
+	hpet_boot_cfg = kmalloc((last + 2) * sizeof(*hpet_boot_cfg),
+				GFP_KERNEL);
+	if (hpet_boot_cfg)
+		*hpet_boot_cfg = cfg;
+	else
+		pr_warn("HPET initial state will not be saved\n");
+	cfg &= ~(HPET_CFG_ENABLE | HPET_CFG_LEGACY);
+	hpet_writel(cfg, HPET_Tn_CFG(i));
+	if (cfg)
+		pr_warn("HPET: Unrecognized bits %#x set in global cfg\n",
+			cfg);
+
+	for (i = 0; i <= last; ++i) {
+		cfg = hpet_readl(HPET_Tn_CFG(i));
+		if (hpet_boot_cfg)
+			hpet_boot_cfg[i + 1] = cfg;
+		cfg &= ~(HPET_TN_ENABLE | HPET_TN_LEVEL | HPET_TN_FSB);
+		hpet_writel(cfg, HPET_Tn_CFG(i));
+		cfg &= ~(HPET_TN_PERIODIC | HPET_TN_PERIODIC_CAP
+			 | HPET_TN_64BIT_CAP | HPET_TN_32BIT | HPET_TN_ROUTE
+			 | HPET_TN_FSB | HPET_TN_FSB_CAP);
+		if (cfg)
+			pr_warn("HPET: Unrecognized bits %#x set in cfg#%u\n",
+				cfg, i);
+	}
+	hpet_print_config();
+
 	if (hpet_clocksource_register())
 		goto out_nohpet;
 
@@ -923,14 +957,28 @@ fs_initcall(hpet_late_init);
 void hpet_disable(void)
 {
 	if (is_hpet_capable() && hpet_virt_address) {
-		unsigned int cfg = hpet_readl(HPET_CFG);
+		unsigned int cfg = hpet_readl(HPET_CFG), id, last;
 
-		if (hpet_legacy_int_enabled) {
+		if (hpet_boot_cfg)
+			cfg = *hpet_boot_cfg;
+		else if (hpet_legacy_int_enabled) {
 			cfg &= ~HPET_CFG_LEGACY;
 			hpet_legacy_int_enabled = 0;
 		}
 		cfg &= ~HPET_CFG_ENABLE;
 		hpet_writel(cfg, HPET_CFG);
+
+		if (!hpet_boot_cfg)
+			return;
+
+		id = hpet_readl(HPET_ID);
+		last = ((id & HPET_ID_NUMBER) >> HPET_ID_NUMBER_SHIFT);
+
+		for (id = 0; id <= last; ++id)
+			hpet_writel(hpet_boot_cfg[id + 1], HPET_Tn_CFG(id));
+
+		if (*hpet_boot_cfg & HPET_CFG_ENABLE)
+			hpet_writel(*hpet_boot_cfg, HPET_CFG);
 	}
 }
 
diff --git a/arch/x86/kernel/signal.c b/arch/x86/kernel/signal.c
index b68ccadd2ff4..965dfda0fd5e 100644
--- a/arch/x86/kernel/signal.c
+++ b/arch/x86/kernel/signal.c
@@ -18,6 +18,7 @@
 #include <linux/personality.h>
 #include <linux/uaccess.h>
 #include <linux/user-return-notifier.h>
+#include <linux/uprobes.h>
 
 #include <asm/processor.h>
 #include <asm/ucontext.h>
@@ -814,6 +815,11 @@ do_notify_resume(struct pt_regs *regs, void *unused, __u32 thread_info_flags)
 		mce_notify_process();
 #endif /* CONFIG_X86_64 && CONFIG_X86_MCE */
 
+	if (thread_info_flags & _TIF_UPROBE) {
+		clear_thread_flag(TIF_UPROBE);
+		uprobe_notify_resume(regs);
+	}
+
 	/* deal with pending signal delivery */
 	if (thread_info_flags & _TIF_SIGPENDING)
 		do_signal(regs);
diff --git a/arch/x86/kernel/uprobes.c b/arch/x86/kernel/uprobes.c
new file mode 100644
index 000000000000..dc4e910a7d96
--- /dev/null
+++ b/arch/x86/kernel/uprobes.c
@@ -0,0 +1,674 @@
+/*
+ * User-space Probes (UProbes) for x86
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Copyright (C) IBM Corporation, 2008-2011
+ * Authors:
+ *	Srikar Dronamraju
+ *	Jim Keniston
+ */
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/ptrace.h>
+#include <linux/uprobes.h>
+#include <linux/uaccess.h>
+
+#include <linux/kdebug.h>
+#include <asm/processor.h>
+#include <asm/insn.h>
+
+/* Post-execution fixups. */
+
+/* No fixup needed */
+#define UPROBE_FIX_NONE		0x0
+
+/* Adjust IP back to vicinity of actual insn */
+#define UPROBE_FIX_IP		0x1
+
+/* Adjust the return address of a call insn */
+#define UPROBE_FIX_CALL	0x2
+
+#define UPROBE_FIX_RIP_AX	0x8000
+#define UPROBE_FIX_RIP_CX	0x4000
+
+#define	UPROBE_TRAP_NR		UINT_MAX
+
+/* Adaptations for mhiramat x86 decoder v14. */
+#define OPCODE1(insn)		((insn)->opcode.bytes[0])
+#define OPCODE2(insn)		((insn)->opcode.bytes[1])
+#define OPCODE3(insn)		((insn)->opcode.bytes[2])
+#define MODRM_REG(insn)		X86_MODRM_REG(insn->modrm.value)
+
+#define W(row, b0, b1, b2, b3, b4, b5, b6, b7, b8, b9, ba, bb, bc, bd, be, bf)\
+	(((b0##UL << 0x0)|(b1##UL << 0x1)|(b2##UL << 0x2)|(b3##UL << 0x3) |   \
+	  (b4##UL << 0x4)|(b5##UL << 0x5)|(b6##UL << 0x6)|(b7##UL << 0x7) |   \
+	  (b8##UL << 0x8)|(b9##UL << 0x9)|(ba##UL << 0xa)|(bb##UL << 0xb) |   \
+	  (bc##UL << 0xc)|(bd##UL << 0xd)|(be##UL << 0xe)|(bf##UL << 0xf))    \
+	 << (row % 32))
+
+/*
+ * Good-instruction tables for 32-bit apps.  This is non-const and volatile
+ * to keep gcc from statically optimizing it out, as variable_test_bit makes
+ * some versions of gcc to think only *(unsigned long*) is used.
+ */
+static volatile u32 good_insns_32[256 / 32] = {
+	/*      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f         */
+	/*      ----------------------------------------------         */
+	W(0x00, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0) | /* 00 */
+	W(0x10, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0) , /* 10 */
+	W(0x20, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1) | /* 20 */
+	W(0x30, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1) , /* 30 */
+	W(0x40, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* 40 */
+	W(0x50, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 50 */
+	W(0x60, 1, 1, 1, 0, 1, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0) | /* 60 */
+	W(0x70, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 70 */
+	W(0x80, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* 80 */
+	W(0x90, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 90 */
+	W(0xa0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* a0 */
+	W(0xb0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* b0 */
+	W(0xc0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0) | /* c0 */
+	W(0xd0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* d0 */
+	W(0xe0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0) | /* e0 */
+	W(0xf0, 0, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1)   /* f0 */
+	/*      ----------------------------------------------         */
+	/*      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f         */
+};
+
+/* Using this for both 64-bit and 32-bit apps */
+static volatile u32 good_2byte_insns[256 / 32] = {
+	/*      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f         */
+	/*      ----------------------------------------------         */
+	W(0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1) | /* 00 */
+	W(0x10, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1) , /* 10 */
+	W(0x20, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1) | /* 20 */
+	W(0x30, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) , /* 30 */
+	W(0x40, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* 40 */
+	W(0x50, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 50 */
+	W(0x60, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* 60 */
+	W(0x70, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1) , /* 70 */
+	W(0x80, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* 80 */
+	W(0x90, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 90 */
+	W(0xa0, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1) | /* a0 */
+	W(0xb0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1) , /* b0 */
+	W(0xc0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* c0 */
+	W(0xd0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* d0 */
+	W(0xe0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* e0 */
+	W(0xf0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0)   /* f0 */
+	/*      ----------------------------------------------         */
+	/*      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f         */
+};
+
+#ifdef CONFIG_X86_64
+/* Good-instruction tables for 64-bit apps */
+static volatile u32 good_insns_64[256 / 32] = {
+	/*      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f         */
+	/*      ----------------------------------------------         */
+	W(0x00, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0) | /* 00 */
+	W(0x10, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0) , /* 10 */
+	W(0x20, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0) | /* 20 */
+	W(0x30, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0) , /* 30 */
+	W(0x40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) | /* 40 */
+	W(0x50, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 50 */
+	W(0x60, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0) | /* 60 */
+	W(0x70, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 70 */
+	W(0x80, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* 80 */
+	W(0x90, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* 90 */
+	W(0xa0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) | /* a0 */
+	W(0xb0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* b0 */
+	W(0xc0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0) | /* c0 */
+	W(0xd0, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1) , /* d0 */
+	W(0xe0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0) | /* e0 */
+	W(0xf0, 0, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1)   /* f0 */
+	/*      ----------------------------------------------         */
+	/*      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f         */
+};
+#endif
+#undef W
+
+/*
+ * opcodes we'll probably never support:
+ *
+ *  6c-6d, e4-e5, ec-ed - in
+ *  6e-6f, e6-e7, ee-ef - out
+ *  cc, cd - int3, int
+ *  cf - iret
+ *  d6 - illegal instruction
+ *  f1 - int1/icebp
+ *  f4 - hlt
+ *  fa, fb - cli, sti
+ *  0f - lar, lsl, syscall, clts, sysret, sysenter, sysexit, invd, wbinvd, ud2
+ *
+ * invalid opcodes in 64-bit mode:
+ *
+ *  06, 0e, 16, 1e, 27, 2f, 37, 3f, 60-62, 82, c4-c5, d4-d5
+ *  63 - we support this opcode in x86_64 but not in i386.
+ *
+ * opcodes we may need to refine support for:
+ *
+ *  0f - 2-byte instructions: For many of these instructions, the validity
+ *  depends on the prefix and/or the reg field.  On such instructions, we
+ *  just consider the opcode combination valid if it corresponds to any
+ *  valid instruction.
+ *
+ *  8f - Group 1 - only reg = 0 is OK
+ *  c6-c7 - Group 11 - only reg = 0 is OK
+ *  d9-df - fpu insns with some illegal encodings
+ *  f2, f3 - repnz, repz prefixes.  These are also the first byte for
+ *  certain floating-point instructions, such as addsd.
+ *
+ *  fe - Group 4 - only reg = 0 or 1 is OK
+ *  ff - Group 5 - only reg = 0-6 is OK
+ *
+ * others -- Do we need to support these?
+ *
+ *  0f - (floating-point?) prefetch instructions
+ *  07, 17, 1f - pop es, pop ss, pop ds
+ *  26, 2e, 36, 3e - es:, cs:, ss:, ds: segment prefixes --
+ *	but 64 and 65 (fs: and gs:) seem to be used, so we support them
+ *  67 - addr16 prefix
+ *  ce - into
+ *  f0 - lock prefix
+ */
+
+/*
+ * TODO:
+ * - Where necessary, examine the modrm byte and allow only valid instructions
+ * in the different Groups and fpu instructions.
+ */
+
+static bool is_prefix_bad(struct insn *insn)
+{
+	int i;
+
+	for (i = 0; i < insn->prefixes.nbytes; i++) {
+		switch (insn->prefixes.bytes[i]) {
+		case 0x26:	/* INAT_PFX_ES   */
+		case 0x2E:	/* INAT_PFX_CS   */
+		case 0x36:	/* INAT_PFX_DS   */
+		case 0x3E:	/* INAT_PFX_SS   */
+		case 0xF0:	/* INAT_PFX_LOCK */
+			return true;
+		}
+	}
+	return false;
+}
+
+static int validate_insn_32bits(struct arch_uprobe *auprobe, struct insn *insn)
+{
+	insn_init(insn, auprobe->insn, false);
+
+	/* Skip good instruction prefixes; reject "bad" ones. */
+	insn_get_opcode(insn);
+	if (is_prefix_bad(insn))
+		return -ENOTSUPP;
+
+	if (test_bit(OPCODE1(insn), (unsigned long *)good_insns_32))
+		return 0;
+
+	if (insn->opcode.nbytes == 2) {
+		if (test_bit(OPCODE2(insn), (unsigned long *)good_2byte_insns))
+			return 0;
+	}
+
+	return -ENOTSUPP;
+}
+
+/*
+ * Figure out which fixups arch_uprobe_post_xol() will need to perform, and
+ * annotate arch_uprobe->fixups accordingly.  To start with,
+ * arch_uprobe->fixups is either zero or it reflects rip-related fixups.
+ */
+static void prepare_fixups(struct arch_uprobe *auprobe, struct insn *insn)
+{
+	bool fix_ip = true, fix_call = false;	/* defaults */
+	int reg;
+
+	insn_get_opcode(insn);	/* should be a nop */
+
+	switch (OPCODE1(insn)) {
+	case 0xc3:		/* ret/lret */
+	case 0xcb:
+	case 0xc2:
+	case 0xca:
+		/* ip is correct */
+		fix_ip = false;
+		break;
+	case 0xe8:		/* call relative - Fix return addr */
+		fix_call = true;
+		break;
+	case 0x9a:		/* call absolute - Fix return addr, not ip */
+		fix_call = true;
+		fix_ip = false;
+		break;
+	case 0xff:
+		insn_get_modrm(insn);
+		reg = MODRM_REG(insn);
+		if (reg == 2 || reg == 3) {
+			/* call or lcall, indirect */
+			/* Fix return addr; ip is correct. */
+			fix_call = true;
+			fix_ip = false;
+		} else if (reg == 4 || reg == 5) {
+			/* jmp or ljmp, indirect */
+			/* ip is correct. */
+			fix_ip = false;
+		}
+		break;
+	case 0xea:		/* jmp absolute -- ip is correct */
+		fix_ip = false;
+		break;
+	default:
+		break;
+	}
+	if (fix_ip)
+		auprobe->fixups |= UPROBE_FIX_IP;
+	if (fix_call)
+		auprobe->fixups |= UPROBE_FIX_CALL;
+}
+
+#ifdef CONFIG_X86_64
+/*
+ * If arch_uprobe->insn doesn't use rip-relative addressing, return
+ * immediately.  Otherwise, rewrite the instruction so that it accesses
+ * its memory operand indirectly through a scratch register.  Set
+ * arch_uprobe->fixups and arch_uprobe->rip_rela_target_address
+ * accordingly.  (The contents of the scratch register will be saved
+ * before we single-step the modified instruction, and restored
+ * afterward.)
+ *
+ * We do this because a rip-relative instruction can access only a
+ * relatively small area (+/- 2 GB from the instruction), and the XOL
+ * area typically lies beyond that area.  At least for instructions
+ * that store to memory, we can't execute the original instruction
+ * and "fix things up" later, because the misdirected store could be
+ * disastrous.
+ *
+ * Some useful facts about rip-relative instructions:
+ *
+ *  - There's always a modrm byte.
+ *  - There's never a SIB byte.
+ *  - The displacement is always 4 bytes.
+ */
+static void
+handle_riprel_insn(struct arch_uprobe *auprobe, struct mm_struct *mm, struct insn *insn)
+{
+	u8 *cursor;
+	u8 reg;
+
+	if (mm->context.ia32_compat)
+		return;
+
+	auprobe->rip_rela_target_address = 0x0;
+	if (!insn_rip_relative(insn))
+		return;
+
+	/*
+	 * insn_rip_relative() would have decoded rex_prefix, modrm.
+	 * Clear REX.b bit (extension of MODRM.rm field):
+	 * we want to encode rax/rcx, not r8/r9.
+	 */
+	if (insn->rex_prefix.nbytes) {
+		cursor = auprobe->insn + insn_offset_rex_prefix(insn);
+		*cursor &= 0xfe;	/* Clearing REX.B bit */
+	}
+
+	/*
+	 * Point cursor at the modrm byte.  The next 4 bytes are the
+	 * displacement.  Beyond the displacement, for some instructions,
+	 * is the immediate operand.
+	 */
+	cursor = auprobe->insn + insn_offset_modrm(insn);
+	insn_get_length(insn);
+
+	/*
+	 * Convert from rip-relative addressing to indirect addressing
+	 * via a scratch register.  Change the r/m field from 0x5 (%rip)
+	 * to 0x0 (%rax) or 0x1 (%rcx), and squeeze out the offset field.
+	 */
+	reg = MODRM_REG(insn);
+	if (reg == 0) {
+		/*
+		 * The register operand (if any) is either the A register
+		 * (%rax, %eax, etc.) or (if the 0x4 bit is set in the
+		 * REX prefix) %r8.  In any case, we know the C register
+		 * is NOT the register operand, so we use %rcx (register
+		 * #1) for the scratch register.
+		 */
+		auprobe->fixups = UPROBE_FIX_RIP_CX;
+		/* Change modrm from 00 000 101 to 00 000 001. */
+		*cursor = 0x1;
+	} else {
+		/* Use %rax (register #0) for the scratch register. */
+		auprobe->fixups = UPROBE_FIX_RIP_AX;
+		/* Change modrm from 00 xxx 101 to 00 xxx 000 */
+		*cursor = (reg << 3);
+	}
+
+	/* Target address = address of next instruction + (signed) offset */
+	auprobe->rip_rela_target_address = (long)insn->length + insn->displacement.value;
+
+	/* Displacement field is gone; slide immediate field (if any) over. */
+	if (insn->immediate.nbytes) {
+		cursor++;
+		memmove(cursor, cursor + insn->displacement.nbytes, insn->immediate.nbytes);
+	}
+	return;
+}
+
+static int validate_insn_64bits(struct arch_uprobe *auprobe, struct insn *insn)
+{
+	insn_init(insn, auprobe->insn, true);
+
+	/* Skip good instruction prefixes; reject "bad" ones. */
+	insn_get_opcode(insn);
+	if (is_prefix_bad(insn))
+		return -ENOTSUPP;
+
+	if (test_bit(OPCODE1(insn), (unsigned long *)good_insns_64))
+		return 0;
+
+	if (insn->opcode.nbytes == 2) {
+		if (test_bit(OPCODE2(insn), (unsigned long *)good_2byte_insns))
+			return 0;
+	}
+	return -ENOTSUPP;
+}
+
+static int validate_insn_bits(struct arch_uprobe *auprobe, struct mm_struct *mm, struct insn *insn)
+{
+	if (mm->context.ia32_compat)
+		return validate_insn_32bits(auprobe, insn);
+	return validate_insn_64bits(auprobe, insn);
+}
+#else /* 32-bit: */
+static void handle_riprel_insn(struct arch_uprobe *auprobe, struct mm_struct *mm, struct insn *insn)
+{
+	/* No RIP-relative addressing on 32-bit */
+}
+
+static int validate_insn_bits(struct arch_uprobe *auprobe, struct mm_struct *mm,  struct insn *insn)
+{
+	return validate_insn_32bits(auprobe, insn);
+}
+#endif /* CONFIG_X86_64 */
+
+/**
+ * arch_uprobe_analyze_insn - instruction analysis including validity and fixups.
+ * @mm: the probed address space.
+ * @arch_uprobe: the probepoint information.
+ * Return 0 on success or a -ve number on error.
+ */
+int arch_uprobe_analyze_insn(struct arch_uprobe *auprobe, struct mm_struct *mm)
+{
+	int ret;
+	struct insn insn;
+
+	auprobe->fixups = 0;
+	ret = validate_insn_bits(auprobe, mm, &insn);
+	if (ret != 0)
+		return ret;
+
+	handle_riprel_insn(auprobe, mm, &insn);
+	prepare_fixups(auprobe, &insn);
+
+	return 0;
+}
+
+#ifdef CONFIG_X86_64
+/*
+ * If we're emulating a rip-relative instruction, save the contents
+ * of the scratch register and store the target address in that register.
+ */
+static void
+pre_xol_rip_insn(struct arch_uprobe *auprobe, struct pt_regs *regs,
+				struct arch_uprobe_task *autask)
+{
+	if (auprobe->fixups & UPROBE_FIX_RIP_AX) {
+		autask->saved_scratch_register = regs->ax;
+		regs->ax = current->utask->vaddr;
+		regs->ax += auprobe->rip_rela_target_address;
+	} else if (auprobe->fixups & UPROBE_FIX_RIP_CX) {
+		autask->saved_scratch_register = regs->cx;
+		regs->cx = current->utask->vaddr;
+		regs->cx += auprobe->rip_rela_target_address;
+	}
+}
+#else
+static void
+pre_xol_rip_insn(struct arch_uprobe *auprobe, struct pt_regs *regs,
+				struct arch_uprobe_task *autask)
+{
+	/* No RIP-relative addressing on 32-bit */
+}
+#endif
+
+/*
+ * arch_uprobe_pre_xol - prepare to execute out of line.
+ * @auprobe: the probepoint information.
+ * @regs: reflects the saved user state of current task.
+ */
+int arch_uprobe_pre_xol(struct arch_uprobe *auprobe, struct pt_regs *regs)
+{
+	struct arch_uprobe_task *autask;
+
+	autask = &current->utask->autask;
+	autask->saved_trap_nr = current->thread.trap_nr;
+	current->thread.trap_nr = UPROBE_TRAP_NR;
+	regs->ip = current->utask->xol_vaddr;
+	pre_xol_rip_insn(auprobe, regs, autask);
+
+	return 0;
+}
+
+/*
+ * This function is called by arch_uprobe_post_xol() to adjust the return
+ * address pushed by a call instruction executed out of line.
+ */
+static int adjust_ret_addr(unsigned long sp, long correction)
+{
+	int rasize, ncopied;
+	long ra = 0;
+
+	if (is_ia32_task())
+		rasize = 4;
+	else
+		rasize = 8;
+
+	ncopied = copy_from_user(&ra, (void __user *)sp, rasize);
+	if (unlikely(ncopied))
+		return -EFAULT;
+
+	ra += correction;
+	ncopied = copy_to_user((void __user *)sp, &ra, rasize);
+	if (unlikely(ncopied))
+		return -EFAULT;
+
+	return 0;
+}
+
+#ifdef CONFIG_X86_64
+static bool is_riprel_insn(struct arch_uprobe *auprobe)
+{
+	return ((auprobe->fixups & (UPROBE_FIX_RIP_AX | UPROBE_FIX_RIP_CX)) != 0);
+}
+
+static void
+handle_riprel_post_xol(struct arch_uprobe *auprobe, struct pt_regs *regs, long *correction)
+{
+	if (is_riprel_insn(auprobe)) {
+		struct arch_uprobe_task *autask;
+
+		autask = &current->utask->autask;
+		if (auprobe->fixups & UPROBE_FIX_RIP_AX)
+			regs->ax = autask->saved_scratch_register;
+		else
+			regs->cx = autask->saved_scratch_register;
+
+		/*
+		 * The original instruction includes a displacement, and so
+		 * is 4 bytes longer than what we've just single-stepped.
+		 * Fall through to handle stuff like "jmpq *...(%rip)" and
+		 * "callq *...(%rip)".
+		 */
+		if (correction)
+			*correction += 4;
+	}
+}
+#else
+static void
+handle_riprel_post_xol(struct arch_uprobe *auprobe, struct pt_regs *regs, long *correction)
+{
+	/* No RIP-relative addressing on 32-bit */
+}
+#endif
+
+/*
+ * If xol insn itself traps and generates a signal(Say,
+ * SIGILL/SIGSEGV/etc), then detect the case where a singlestepped
+ * instruction jumps back to its own address. It is assumed that anything
+ * like do_page_fault/do_trap/etc sets thread.trap_nr != -1.
+ *
+ * arch_uprobe_pre_xol/arch_uprobe_post_xol save/restore thread.trap_nr,
+ * arch_uprobe_xol_was_trapped() simply checks that ->trap_nr is not equal to
+ * UPROBE_TRAP_NR == -1 set by arch_uprobe_pre_xol().
+ */
+bool arch_uprobe_xol_was_trapped(struct task_struct *t)
+{
+	if (t->thread.trap_nr != UPROBE_TRAP_NR)
+		return true;
+
+	return false;
+}
+
+/*
+ * Called after single-stepping. To avoid the SMP problems that can
+ * occur when we temporarily put back the original opcode to
+ * single-step, we single-stepped a copy of the instruction.
+ *
+ * This function prepares to resume execution after the single-step.
+ * We have to fix things up as follows:
+ *
+ * Typically, the new ip is relative to the copied instruction.  We need
+ * to make it relative to the original instruction (FIX_IP).  Exceptions
+ * are return instructions and absolute or indirect jump or call instructions.
+ *
+ * If the single-stepped instruction was a call, the return address that
+ * is atop the stack is the address following the copied instruction.  We
+ * need to make it the address following the original instruction (FIX_CALL).
+ *
+ * If the original instruction was a rip-relative instruction such as
+ * "movl %edx,0xnnnn(%rip)", we have instead executed an equivalent
+ * instruction using a scratch register -- e.g., "movl %edx,(%rax)".
+ * We need to restore the contents of the scratch register and adjust
+ * the ip, keeping in mind that the instruction we executed is 4 bytes
+ * shorter than the original instruction (since we squeezed out the offset
+ * field).  (FIX_RIP_AX or FIX_RIP_CX)
+ */
+int arch_uprobe_post_xol(struct arch_uprobe *auprobe, struct pt_regs *regs)
+{
+	struct uprobe_task *utask;
+	long correction;
+	int result = 0;
+
+	WARN_ON_ONCE(current->thread.trap_nr != UPROBE_TRAP_NR);
+
+	utask = current->utask;
+	current->thread.trap_nr = utask->autask.saved_trap_nr;
+	correction = (long)(utask->vaddr - utask->xol_vaddr);
+	handle_riprel_post_xol(auprobe, regs, &correction);
+	if (auprobe->fixups & UPROBE_FIX_IP)
+		regs->ip += correction;
+
+	if (auprobe->fixups & UPROBE_FIX_CALL)
+		result = adjust_ret_addr(regs->sp, correction);
+
+	return result;
+}
+
+/* callback routine for handling exceptions. */
+int arch_uprobe_exception_notify(struct notifier_block *self, unsigned long val, void *data)
+{
+	struct die_args *args = data;
+	struct pt_regs *regs = args->regs;
+	int ret = NOTIFY_DONE;
+
+	/* We are only interested in userspace traps */
+	if (regs && !user_mode_vm(regs))
+		return NOTIFY_DONE;
+
+	switch (val) {
+	case DIE_INT3:
+		if (uprobe_pre_sstep_notifier(regs))
+			ret = NOTIFY_STOP;
+
+		break;
+
+	case DIE_DEBUG:
+		if (uprobe_post_sstep_notifier(regs))
+			ret = NOTIFY_STOP;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+/*
+ * This function gets called when XOL instruction either gets trapped or
+ * the thread has a fatal signal, so reset the instruction pointer to its
+ * probed address.
+ */
+void arch_uprobe_abort_xol(struct arch_uprobe *auprobe, struct pt_regs *regs)
+{
+	struct uprobe_task *utask = current->utask;
+
+	current->thread.trap_nr = utask->autask.saved_trap_nr;
+	handle_riprel_post_xol(auprobe, regs, NULL);
+	instruction_pointer_set(regs, utask->vaddr);
+}
+
+/*
+ * Skip these instructions as per the currently known x86 ISA.
+ * 0x66* { 0x90 | 0x0f 0x1f | 0x0f 0x19 | 0x87 0xc0 }
+ */
+bool arch_uprobe_skip_sstep(struct arch_uprobe *auprobe, struct pt_regs *regs)
+{
+	int i;
+
+	for (i = 0; i < MAX_UINSN_BYTES; i++) {
+		if ((auprobe->insn[i] == 0x66))
+			continue;
+
+		if (auprobe->insn[i] == 0x90)
+			return true;
+
+		if (i == (MAX_UINSN_BYTES - 1))
+			break;
+
+		if ((auprobe->insn[i] == 0x0f) && (auprobe->insn[i+1] == 0x1f))
+			return true;
+
+		if ((auprobe->insn[i] == 0x0f) && (auprobe->insn[i+1] == 0x19))
+			return true;
+
+		if ((auprobe->insn[i] == 0x87) && (auprobe->insn[i+1] == 0xc0))
+			return true;
+
+		break;
+	}
+	return false;
+}
diff --git a/arch/x86/pci/fixup.c b/arch/x86/pci/fixup.c
index 5dd467bd6121..af8a224db216 100644
--- a/arch/x86/pci/fixup.c
+++ b/arch/x86/pci/fixup.c
@@ -6,6 +6,7 @@
 #include <linux/dmi.h>
 #include <linux/pci.h>
 #include <linux/init.h>
+#include <linux/vgaarb.h>
 #include <asm/pci_x86.h>
 
 static void __devinit pci_fixup_i450nx(struct pci_dev *d)
@@ -348,6 +349,8 @@ static void __devinit pci_fixup_video(struct pci_dev *pdev)
 	if (config & (PCI_COMMAND_IO | PCI_COMMAND_MEMORY)) {
 		pdev->resource[PCI_ROM_RESOURCE].flags |= IORESOURCE_ROM_SHADOW;
 		dev_printk(KERN_DEBUG, &pdev->dev, "Boot video device\n");
+		if (!vga_default_device())
+			vga_set_default_device(pdev);
 	}
 }
 DECLARE_PCI_FIXUP_CLASS_FINAL(PCI_ANY_ID, PCI_ANY_ID,
diff --git a/arch/x86/video/fbdev.c b/arch/x86/video/fbdev.c
index c5ffb6ac8707..d5644bbe8cba 100644
--- a/arch/x86/video/fbdev.c
+++ b/arch/x86/video/fbdev.c
@@ -9,24 +9,34 @@
 #include <linux/fb.h>
 #include <linux/pci.h>
 #include <linux/module.h>
+#include <linux/vgaarb.h>
 
 int fb_is_primary_device(struct fb_info *info)
 {
 	struct device *device = info->device;
 	struct pci_dev *pci_dev = NULL;
+	struct pci_dev *default_device = vga_default_device();
 	struct resource *res = NULL;
-	int retval = 0;
 
 	if (device)
 		pci_dev = to_pci_dev(device);
 
-	if (pci_dev)
-		res = &pci_dev->resource[PCI_ROM_RESOURCE];
+	if (!pci_dev)
+		return 0;
+
+	if (default_device) {
+		if (pci_dev == default_device)
+			return 1;
+		else
+			return 0;
+	}
+
+	res = &pci_dev->resource[PCI_ROM_RESOURCE];
 
 	if (res && res->flags & IORESOURCE_ROM_SHADOW)
-		retval = 1;
+		return 1;
 
-	return retval;
+	return 0;
 }
 EXPORT_SYMBOL(fb_is_primary_device);
 MODULE_LICENSE("GPL");
diff --git a/drivers/bcma/core.c b/drivers/bcma/core.c
index 893f6e0c759f..bc6e89212ad3 100644
--- a/drivers/bcma/core.c
+++ b/drivers/bcma/core.c
@@ -30,6 +30,7 @@ void bcma_core_disable(struct bcma_device *core, u32 flags)
 	udelay(10);
 
 	bcma_awrite32(core, BCMA_RESET_CTL, BCMA_RESET_CTL_RESET);
+	bcma_aread32(core, BCMA_RESET_CTL);
 	udelay(1);
 }
 EXPORT_SYMBOL_GPL(bcma_core_disable);
@@ -77,7 +78,7 @@ void bcma_core_set_clockmode(struct bcma_device *core,
 			pr_err("HT force timeout\n");
 		break;
 	case BCMA_CLKMODE_DYNAMIC:
-		pr_warn("Dynamic clockmode not supported yet!\n");
+		bcma_set32(core, BCMA_CLKCTLST, ~BCMA_CLKCTLST_FORCEHT);
 		break;
 	}
 }
diff --git a/drivers/bcma/driver_pci.c b/drivers/bcma/driver_pci.c
index 4d38ae179b48..9a96f14c8f47 100644
--- a/drivers/bcma/driver_pci.c
+++ b/drivers/bcma/driver_pci.c
@@ -24,14 +24,12 @@ u32 bcma_pcie_read(struct bcma_drv_pci *pc, u32 address)
 	return pcicore_read32(pc, BCMA_CORE_PCI_PCIEIND_DATA);
 }
 
-#if 0
 static void bcma_pcie_write(struct bcma_drv_pci *pc, u32 address, u32 data)
 {
 	pcicore_write32(pc, BCMA_CORE_PCI_PCIEIND_ADDR, address);
 	pcicore_read32(pc, BCMA_CORE_PCI_PCIEIND_ADDR);
 	pcicore_write32(pc, BCMA_CORE_PCI_PCIEIND_DATA, data);
 }
-#endif
 
 static void bcma_pcie_mdio_set_phy(struct bcma_drv_pci *pc, u8 phy)
 {
@@ -170,13 +168,50 @@ static void bcma_pcicore_serdes_workaround(struct bcma_drv_pci *pc)
 		                     tmp & ~BCMA_CORE_PCI_PLL_CTRL_FREQDET_EN);
 }
 
+static void bcma_core_pci_fixcfg(struct bcma_drv_pci *pc)
+{
+	struct bcma_device *core = pc->core;
+	u16 val16, core_index;
+	uint regoff;
+
+	regoff = BCMA_CORE_PCI_SPROM(BCMA_CORE_PCI_SPROM_PI_OFFSET);
+	core_index = (u16)core->core_index;
+
+	val16 = pcicore_read16(pc, regoff);
+	if (((val16 & BCMA_CORE_PCI_SPROM_PI_MASK) >> BCMA_CORE_PCI_SPROM_PI_SHIFT)
+	     != core_index) {
+		val16 = (core_index << BCMA_CORE_PCI_SPROM_PI_SHIFT) |
+			(val16 & ~BCMA_CORE_PCI_SPROM_PI_MASK);
+		pcicore_write16(pc, regoff, val16);
+	}
+}
+
+/* Fix MISC config to allow coming out of L2/L3-Ready state w/o PRST */
+/* Needs to happen when coming out of 'standby'/'hibernate' */
+static void bcma_core_pci_config_fixup(struct bcma_drv_pci *pc)
+{
+	u16 val16;
+	uint regoff;
+
+	regoff = BCMA_CORE_PCI_SPROM(BCMA_CORE_PCI_SPROM_MISC_CONFIG);
+
+	val16 = pcicore_read16(pc, regoff);
+
+	if (!(val16 & BCMA_CORE_PCI_SPROM_L23READY_EXIT_NOPERST)) {
+		val16 |= BCMA_CORE_PCI_SPROM_L23READY_EXIT_NOPERST;
+		pcicore_write16(pc, regoff, val16);
+	}
+}
+
 /**************************************************
  * Init.
  **************************************************/
 
 static void __devinit bcma_core_pci_clientmode_init(struct bcma_drv_pci *pc)
 {
+	bcma_core_pci_fixcfg(pc);
 	bcma_pcicore_serdes_workaround(pc);
+	bcma_core_pci_config_fixup(pc);
 }
 
 void __devinit bcma_core_pci_init(struct bcma_drv_pci *pc)
@@ -224,3 +259,17 @@ out:
 	return err;
 }
 EXPORT_SYMBOL_GPL(bcma_core_pci_irq_ctl);
+
+void bcma_core_pci_extend_L1timer(struct bcma_drv_pci *pc, bool extend)
+{
+	u32 w;
+
+	w = bcma_pcie_read(pc, BCMA_CORE_PCI_DLLP_PMTHRESHREG);
+	if (extend)
+		w |= BCMA_CORE_PCI_ASPMTIMER_EXTEND;
+	else
+		w &= ~BCMA_CORE_PCI_ASPMTIMER_EXTEND;
+	bcma_pcie_write(pc, BCMA_CORE_PCI_DLLP_PMTHRESHREG, w);
+	bcma_pcie_read(pc, BCMA_CORE_PCI_DLLP_PMTHRESHREG);
+}
+EXPORT_SYMBOL_GPL(bcma_core_pci_extend_L1timer);
diff --git a/drivers/bcma/driver_pci_host.c b/drivers/bcma/driver_pci_host.c
index d2097a11c3c7..b9a86edfec39 100644
--- a/drivers/bcma/driver_pci_host.c
+++ b/drivers/bcma/driver_pci_host.c
@@ -119,7 +119,7 @@ static int bcma_extpci_read_config(struct bcma_drv_pci *pc, unsigned int dev,
 		if (unlikely(!addr))
 			goto out;
 		err = -ENOMEM;
-		mmio = ioremap_nocache(addr, len);
+		mmio = ioremap_nocache(addr, sizeof(val));
 		if (!mmio)
 			goto out;
 
@@ -171,7 +171,7 @@ static int bcma_extpci_write_config(struct bcma_drv_pci *pc, unsigned int dev,
 			addr = pc->core->addr + BCMA_CORE_PCI_PCICFG0;
 			addr |= (func << 8);
 			addr |= (off & 0xfc);
-			mmio = ioremap_nocache(addr, len);
+			mmio = ioremap_nocache(addr, sizeof(val));
 			if (!mmio)
 				goto out;
 		}
@@ -180,7 +180,7 @@ static int bcma_extpci_write_config(struct bcma_drv_pci *pc, unsigned int dev,
 		if (unlikely(!addr))
 			goto out;
 		err = -ENOMEM;
-		mmio = ioremap_nocache(addr, len);
+		mmio = ioremap_nocache(addr, sizeof(val));
 		if (!mmio)
 			goto out;
 
@@ -491,8 +491,8 @@ void __devinit bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
 	/* Ok, ready to run, register it to the system.
 	 * The following needs change, if we want to port hostmode
 	 * to non-MIPS platform. */
-	io_map_base = (unsigned long)ioremap_nocache(BCMA_SOC_PCI_MEM,
-						     0x04000000);
+	io_map_base = (unsigned long)ioremap_nocache(pc_host->mem_resource.start,
+						     resource_size(&pc_host->mem_resource));
 	pc_host->pci_controller.io_map_base = io_map_base;
 	set_io_port_base(pc_host->pci_controller.io_map_base);
 	/* Give some time to the PCI controller to configure itself with the new
diff --git a/drivers/bcma/host_pci.c b/drivers/bcma/host_pci.c
index e3928d68802b..6c05cf470f96 100644
--- a/drivers/bcma/host_pci.c
+++ b/drivers/bcma/host_pci.c
@@ -201,6 +201,9 @@ static int __devinit bcma_host_pci_probe(struct pci_dev *dev,
 	bus->hosttype = BCMA_HOSTTYPE_PCI;
 	bus->ops = &bcma_host_pci_ops;
 
+	bus->boardinfo.vendor = bus->host_pci->subsystem_vendor;
+	bus->boardinfo.type = bus->host_pci->subsystem_device;
+
 	/* Register */
 	err = bcma_bus_register(bus);
 	if (err)
@@ -222,7 +225,7 @@ err_kfree_bus:
 	return err;
 }
 
-static void bcma_host_pci_remove(struct pci_dev *dev)
+static void __devexit bcma_host_pci_remove(struct pci_dev *dev)
 {
 	struct bcma_bus *bus = pci_get_drvdata(dev);
 
@@ -277,7 +280,7 @@ static struct pci_driver bcma_pci_bridge_driver = {
 	.name = "bcma-pci-bridge",
 	.id_table = bcma_pci_bridge_tbl,
 	.probe = bcma_host_pci_probe,
-	.remove = bcma_host_pci_remove,
+	.remove = __devexit_p(bcma_host_pci_remove),
 	.driver.pm = BCMA_PM_OPS,
 };
 
diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index 3bea7fe25b20..5ed0718fc660 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -19,7 +19,14 @@ struct bcma_device_id_name {
 	u16 id;
 	const char *name;
 };
-struct bcma_device_id_name bcma_device_names[] = {
+
+static const struct bcma_device_id_name bcma_arm_device_names[] = {
+	{ BCMA_CORE_ARM_1176, "ARM 1176" },
+	{ BCMA_CORE_ARM_7TDMI, "ARM 7TDMI" },
+	{ BCMA_CORE_ARM_CM3, "ARM CM3" },
+};
+
+static const struct bcma_device_id_name bcma_bcm_device_names[] = {
 	{ BCMA_CORE_OOB_ROUTER, "OOB Router" },
 	{ BCMA_CORE_INVALID, "Invalid" },
 	{ BCMA_CORE_CHIPCOMMON, "ChipCommon" },
@@ -27,7 +34,6 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_SRAM, "SRAM" },
 	{ BCMA_CORE_SDRAM, "SDRAM" },
 	{ BCMA_CORE_PCI, "PCI" },
-	{ BCMA_CORE_MIPS, "MIPS" },
 	{ BCMA_CORE_ETHERNET, "Fast Ethernet" },
 	{ BCMA_CORE_V90, "V90" },
 	{ BCMA_CORE_USB11_HOSTDEV, "USB 1.1 Hostdev" },
@@ -44,7 +50,6 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_PHY_A, "PHY A" },
 	{ BCMA_CORE_PHY_B, "PHY B" },
 	{ BCMA_CORE_PHY_G, "PHY G" },
-	{ BCMA_CORE_MIPS_3302, "MIPS 3302" },
 	{ BCMA_CORE_USB11_HOST, "USB 1.1 Host" },
 	{ BCMA_CORE_USB11_DEV, "USB 1.1 Device" },
 	{ BCMA_CORE_USB20_HOST, "USB 2.0 Host" },
@@ -58,15 +63,11 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_PHY_N, "PHY N" },
 	{ BCMA_CORE_SRAM_CTL, "SRAM Controller" },
 	{ BCMA_CORE_MINI_MACPHY, "Mini MACPHY" },
-	{ BCMA_CORE_ARM_1176, "ARM 1176" },
-	{ BCMA_CORE_ARM_7TDMI, "ARM 7TDMI" },
 	{ BCMA_CORE_PHY_LP, "PHY LP" },
 	{ BCMA_CORE_PMU, "PMU" },
 	{ BCMA_CORE_PHY_SSN, "PHY SSN" },
 	{ BCMA_CORE_SDIO_DEV, "SDIO Device" },
-	{ BCMA_CORE_ARM_CM3, "ARM CM3" },
 	{ BCMA_CORE_PHY_HT, "PHY HT" },
-	{ BCMA_CORE_MIPS_74K, "MIPS 74K" },
 	{ BCMA_CORE_MAC_GBIT, "GBit MAC" },
 	{ BCMA_CORE_DDR12_MEM_CTL, "DDR1/DDR2 Memory Controller" },
 	{ BCMA_CORE_PCIE_RC, "PCIe Root Complex" },
@@ -79,16 +80,41 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_SHIM, "SHIM" },
 	{ BCMA_CORE_DEFAULT, "Default" },
 };
-const char *bcma_device_name(struct bcma_device_id *id)
+
+static const struct bcma_device_id_name bcma_mips_device_names[] = {
+	{ BCMA_CORE_MIPS, "MIPS" },
+	{ BCMA_CORE_MIPS_3302, "MIPS 3302" },
+	{ BCMA_CORE_MIPS_74K, "MIPS 74K" },
+};
+
+static const char *bcma_device_name(const struct bcma_device_id *id)
 {
-	int i;
+	const struct bcma_device_id_name *names;
+	int size, i;
+
+	/* search manufacturer specific names */
+	switch (id->manuf) {
+	case BCMA_MANUF_ARM:
+		names = bcma_arm_device_names;
+		size = ARRAY_SIZE(bcma_arm_device_names);
+		break;
+	case BCMA_MANUF_BCM:
+		names = bcma_bcm_device_names;
+		size = ARRAY_SIZE(bcma_bcm_device_names);
+		break;
+	case BCMA_MANUF_MIPS:
+		names = bcma_mips_device_names;
+		size = ARRAY_SIZE(bcma_mips_device_names);
+		break;
+	default:
+		return "UNKNOWN";
+	}
 
-	if (id->manuf == BCMA_MANUF_BCM) {
-		for (i = 0; i < ARRAY_SIZE(bcma_device_names); i++) {
-			if (bcma_device_names[i].id == id->id)
-				return bcma_device_names[i].name;
-		}
+	for (i = 0; i < size; i++) {
+		if (names[i].id == id->id)
+			return names[i].name;
 	}
+
 	return "UNKNOWN";
 }
 
diff --git a/drivers/bcma/sprom.c b/drivers/bcma/sprom.c
index 3e2a6002aae6..c7f93359acb0 100644
--- a/drivers/bcma/sprom.c
+++ b/drivers/bcma/sprom.c
@@ -181,6 +181,22 @@ static int bcma_sprom_valid(const u16 *sprom)
 #define SPEX(_field, _offset, _mask, _shift)	\
 	bus->sprom._field = ((sprom[SPOFF(_offset)] & (_mask)) >> (_shift))
 
+#define SPEX32(_field, _offset, _mask, _shift)	\
+	bus->sprom._field = ((((u32)sprom[SPOFF((_offset)+2)] << 16 | \
+				sprom[SPOFF(_offset)]) & (_mask)) >> (_shift))
+
+#define SPEX_ARRAY8(_field, _offset, _mask, _shift)	\
+	do {	\
+		SPEX(_field[0], _offset +  0, _mask, _shift);	\
+		SPEX(_field[1], _offset +  2, _mask, _shift);	\
+		SPEX(_field[2], _offset +  4, _mask, _shift);	\
+		SPEX(_field[3], _offset +  6, _mask, _shift);	\
+		SPEX(_field[4], _offset +  8, _mask, _shift);	\
+		SPEX(_field[5], _offset + 10, _mask, _shift);	\
+		SPEX(_field[6], _offset + 12, _mask, _shift);	\
+		SPEX(_field[7], _offset + 14, _mask, _shift);	\
+	} while (0)
+
 static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom)
 {
 	u16 v, o;
@@ -243,7 +259,8 @@ static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom)
 	SPEX(boardflags2_lo, SSB_SPROM8_BFL2LO, ~0, 0);
 	SPEX(boardflags2_hi, SSB_SPROM8_BFL2HI, ~0, 0);
 
-	SPEX(country_code, SSB_SPROM8_CCODE, ~0, 0);
+	SPEX(alpha2[0], SSB_SPROM8_CCODE, 0xff00, 8);
+	SPEX(alpha2[1], SSB_SPROM8_CCODE, 0x00ff, 0);
 
 	/* Extract cores power info info */
 	for (i = 0; i < ARRAY_SIZE(pwr_info_offset); i++) {
@@ -298,6 +315,136 @@ static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom)
 	     SSB_SROM8_FEM_TR_ISO_SHIFT);
 	SPEX(fem.ghz5.antswlut, SSB_SPROM8_FEM5G, SSB_SROM8_FEM_ANTSWLUT,
 	     SSB_SROM8_FEM_ANTSWLUT_SHIFT);
+
+	SPEX(ant_available_a, SSB_SPROM8_ANTAVAIL, SSB_SPROM8_ANTAVAIL_A,
+	     SSB_SPROM8_ANTAVAIL_A_SHIFT);
+	SPEX(ant_available_bg, SSB_SPROM8_ANTAVAIL, SSB_SPROM8_ANTAVAIL_BG,
+	     SSB_SPROM8_ANTAVAIL_BG_SHIFT);
+	SPEX(maxpwr_bg, SSB_SPROM8_MAXP_BG, SSB_SPROM8_MAXP_BG_MASK, 0);
+	SPEX(itssi_bg, SSB_SPROM8_MAXP_BG, SSB_SPROM8_ITSSI_BG,
+	     SSB_SPROM8_ITSSI_BG_SHIFT);
+	SPEX(maxpwr_a, SSB_SPROM8_MAXP_A, SSB_SPROM8_MAXP_A_MASK, 0);
+	SPEX(itssi_a, SSB_SPROM8_MAXP_A, SSB_SPROM8_ITSSI_A,
+	     SSB_SPROM8_ITSSI_A_SHIFT);
+	SPEX(maxpwr_ah, SSB_SPROM8_MAXP_AHL, SSB_SPROM8_MAXP_AH_MASK, 0);
+	SPEX(maxpwr_al, SSB_SPROM8_MAXP_AHL, SSB_SPROM8_MAXP_AL_MASK,
+	     SSB_SPROM8_MAXP_AL_SHIFT);
+	SPEX(gpio0, SSB_SPROM8_GPIOA, SSB_SPROM8_GPIOA_P0, 0);
+	SPEX(gpio1, SSB_SPROM8_GPIOA, SSB_SPROM8_GPIOA_P1,
+	     SSB_SPROM8_GPIOA_P1_SHIFT);
+	SPEX(gpio2, SSB_SPROM8_GPIOB, SSB_SPROM8_GPIOB_P2, 0);
+	SPEX(gpio3, SSB_SPROM8_GPIOB, SSB_SPROM8_GPIOB_P3,
+	     SSB_SPROM8_GPIOB_P3_SHIFT);
+	SPEX(tri2g, SSB_SPROM8_TRI25G, SSB_SPROM8_TRI2G, 0);
+	SPEX(tri5g, SSB_SPROM8_TRI25G, SSB_SPROM8_TRI5G,
+	     SSB_SPROM8_TRI5G_SHIFT);
+	SPEX(tri5gl, SSB_SPROM8_TRI5GHL, SSB_SPROM8_TRI5GL, 0);
+	SPEX(tri5gh, SSB_SPROM8_TRI5GHL, SSB_SPROM8_TRI5GH,
+	     SSB_SPROM8_TRI5GH_SHIFT);
+	SPEX(rxpo2g, SSB_SPROM8_RXPO, SSB_SPROM8_RXPO2G,
+	     SSB_SPROM8_RXPO2G_SHIFT);
+	SPEX(rxpo5g, SSB_SPROM8_RXPO, SSB_SPROM8_RXPO5G,
+	     SSB_SPROM8_RXPO5G_SHIFT);
+	SPEX(rssismf2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_RSSISMF2G, 0);
+	SPEX(rssismc2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_RSSISMC2G,
+	     SSB_SPROM8_RSSISMC2G_SHIFT);
+	SPEX(rssisav2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_RSSISAV2G,
+	     SSB_SPROM8_RSSISAV2G_SHIFT);
+	SPEX(bxa2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_BXA2G,
+	     SSB_SPROM8_BXA2G_SHIFT);
+	SPEX(rssismf5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_RSSISMF5G, 0);
+	SPEX(rssismc5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_RSSISMC5G,
+	     SSB_SPROM8_RSSISMC5G_SHIFT);
+	SPEX(rssisav5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_RSSISAV5G,
+	     SSB_SPROM8_RSSISAV5G_SHIFT);
+	SPEX(bxa5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_BXA5G,
+	     SSB_SPROM8_BXA5G_SHIFT);
+
+	SPEX(pa0b0, SSB_SPROM8_PA0B0, ~0, 0);
+	SPEX(pa0b1, SSB_SPROM8_PA0B1, ~0, 0);
+	SPEX(pa0b2, SSB_SPROM8_PA0B2, ~0, 0);
+	SPEX(pa1b0, SSB_SPROM8_PA1B0, ~0, 0);
+	SPEX(pa1b1, SSB_SPROM8_PA1B1, ~0, 0);
+	SPEX(pa1b2, SSB_SPROM8_PA1B2, ~0, 0);
+	SPEX(pa1lob0, SSB_SPROM8_PA1LOB0, ~0, 0);
+	SPEX(pa1lob1, SSB_SPROM8_PA1LOB1, ~0, 0);
+	SPEX(pa1lob2, SSB_SPROM8_PA1LOB2, ~0, 0);
+	SPEX(pa1hib0, SSB_SPROM8_PA1HIB0, ~0, 0);
+	SPEX(pa1hib1, SSB_SPROM8_PA1HIB1, ~0, 0);
+	SPEX(pa1hib2, SSB_SPROM8_PA1HIB2, ~0, 0);
+	SPEX(cck2gpo, SSB_SPROM8_CCK2GPO, ~0, 0);
+	SPEX32(ofdm2gpo, SSB_SPROM8_OFDM2GPO, ~0, 0);
+	SPEX32(ofdm5glpo, SSB_SPROM8_OFDM5GLPO, ~0, 0);
+	SPEX32(ofdm5gpo, SSB_SPROM8_OFDM5GPO, ~0, 0);
+	SPEX32(ofdm5ghpo, SSB_SPROM8_OFDM5GHPO, ~0, 0);
+
+	/* Extract the antenna gain values. */
+	SPEX(antenna_gain.a0, SSB_SPROM8_AGAIN01,
+	     SSB_SPROM8_AGAIN0, SSB_SPROM8_AGAIN0_SHIFT);
+	SPEX(antenna_gain.a1, SSB_SPROM8_AGAIN01,
+	     SSB_SPROM8_AGAIN1, SSB_SPROM8_AGAIN1_SHIFT);
+	SPEX(antenna_gain.a2, SSB_SPROM8_AGAIN23,
+	     SSB_SPROM8_AGAIN2, SSB_SPROM8_AGAIN2_SHIFT);
+	SPEX(antenna_gain.a3, SSB_SPROM8_AGAIN23,
+	     SSB_SPROM8_AGAIN3, SSB_SPROM8_AGAIN3_SHIFT);
+
+	SPEX(leddc_on_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_ON,
+	     SSB_SPROM8_LEDDC_ON_SHIFT);
+	SPEX(leddc_off_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_OFF,
+	     SSB_SPROM8_LEDDC_OFF_SHIFT);
+
+	SPEX(txchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_TXCHAIN,
+	     SSB_SPROM8_TXRXC_TXCHAIN_SHIFT);
+	SPEX(rxchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_RXCHAIN,
+	     SSB_SPROM8_TXRXC_RXCHAIN_SHIFT);
+	SPEX(antswitch, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_SWITCH,
+	     SSB_SPROM8_TXRXC_SWITCH_SHIFT);
+
+	SPEX(opo, SSB_SPROM8_OFDM2GPO, 0x00ff, 0);
+
+	SPEX_ARRAY8(mcs2gpo, SSB_SPROM8_2G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5gpo, SSB_SPROM8_5G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5glpo, SSB_SPROM8_5GL_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5ghpo, SSB_SPROM8_5GH_MCSPO, ~0, 0);
+
+	SPEX(rawtempsense, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_RAWTEMP,
+	     SSB_SPROM8_RAWTS_RAWTEMP_SHIFT);
+	SPEX(measpower, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_MEASPOWER,
+	     SSB_SPROM8_RAWTS_MEASPOWER_SHIFT);
+	SPEX(tempsense_slope, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE_SHIFT);
+	SPEX(tempcorrx, SSB_SPROM8_OPT_CORRX, SSB_SPROM8_OPT_CORRX_TEMPCORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMPCORRX_SHIFT);
+	SPEX(tempsense_option, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION_SHIFT);
+	SPEX(freqoffset_corr, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR_SHIFT);
+	SPEX(iqcal_swp_dis, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP_SHIFT);
+	SPEX(hw_iqcal_en, SSB_SPROM8_HWIQ_IQSWP, SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL,
+	     SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL_SHIFT);
+
+	SPEX(bw40po, SSB_SPROM8_BW40PO, ~0, 0);
+	SPEX(cddpo, SSB_SPROM8_CDDPO, ~0, 0);
+	SPEX(stbcpo, SSB_SPROM8_STBCPO, ~0, 0);
+	SPEX(bwduppo, SSB_SPROM8_BWDUPPO, ~0, 0);
+
+	SPEX(tempthresh, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_TRESH,
+	     SSB_SPROM8_THERMAL_TRESH_SHIFT);
+	SPEX(tempoffset, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_OFFSET,
+	     SSB_SPROM8_THERMAL_OFFSET_SHIFT);
+	SPEX(phycal_tempdelta, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL_SHIFT);
+	SPEX(temps_period, SSB_SPROM8_TEMPDELTA, SSB_SPROM8_TEMPDELTA_PERIOD,
+	     SSB_SPROM8_TEMPDELTA_PERIOD_SHIFT);
+	SPEX(temps_hysteresis, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS_SHIFT);
 }
 
 /*
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index 2812b152d6e9..ad591bd240ec 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -81,6 +81,9 @@ static struct usb_device_id ath3k_table[] = {
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xE02C) },
 
+	/* Atheros AR5BBU22 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xE03C) },
+
 	{ }	/* Terminating entry */
 };
 
@@ -99,6 +102,9 @@ static struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x13d3, 0x3362), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0xe004), .driver_info = BTUSB_ATH3012 },
 
+	/* Atheros AR5BBU22 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xE03C), .driver_info = BTUSB_ATH3012 },
+
 	{ }	/* Terminating entry */
 };
 
diff --git a/drivers/bluetooth/btmrvl_drv.h b/drivers/bluetooth/btmrvl_drv.h
index 90bda50dc446..94f2d65131c4 100644
--- a/drivers/bluetooth/btmrvl_drv.h
+++ b/drivers/bluetooth/btmrvl_drv.h
@@ -67,6 +67,7 @@ struct btmrvl_adapter {
 	u8 wakeup_tries;
 	wait_queue_head_t cmd_wait_q;
 	u8 cmd_complete;
+	bool is_suspended;
 };
 
 struct btmrvl_private {
@@ -139,8 +140,10 @@ void btmrvl_check_evtpkt(struct btmrvl_private *priv, struct sk_buff *skb);
 int btmrvl_process_event(struct btmrvl_private *priv, struct sk_buff *skb);
 
 int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd);
+int btmrvl_send_hscfg_cmd(struct btmrvl_private *priv);
 int btmrvl_enable_ps(struct btmrvl_private *priv);
 int btmrvl_prepare_command(struct btmrvl_private *priv);
+int btmrvl_enable_hs(struct btmrvl_private *priv);
 
 #ifdef CONFIG_DEBUG_FS
 void btmrvl_debugfs_init(struct hci_dev *hdev);
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index d1209adc882d..681ca9d18e12 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -200,6 +200,36 @@ int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd)
 }
 EXPORT_SYMBOL_GPL(btmrvl_send_module_cfg_cmd);
 
+int btmrvl_send_hscfg_cmd(struct btmrvl_private *priv)
+{
+	struct sk_buff *skb;
+	struct btmrvl_cmd *cmd;
+
+	skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
+	if (!skb) {
+		BT_ERR("No free skb");
+		return -ENOMEM;
+	}
+
+	cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
+	cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF,
+						   BT_CMD_HOST_SLEEP_CONFIG));
+	cmd->length = 2;
+	cmd->data[0] = (priv->btmrvl_dev.gpio_gap & 0xff00) >> 8;
+	cmd->data[1] = (u8) (priv->btmrvl_dev.gpio_gap & 0x00ff);
+
+	bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
+
+	skb->dev = (void *) priv->btmrvl_dev.hcidev;
+	skb_queue_head(&priv->adapter->tx_queue, skb);
+
+	BT_DBG("Queue HSCFG Command, gpio=0x%x, gap=0x%x", cmd->data[0],
+	       cmd->data[1]);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btmrvl_send_hscfg_cmd);
+
 int btmrvl_enable_ps(struct btmrvl_private *priv)
 {
 	struct sk_buff *skb;
@@ -232,7 +262,7 @@ int btmrvl_enable_ps(struct btmrvl_private *priv)
 }
 EXPORT_SYMBOL_GPL(btmrvl_enable_ps);
 
-static int btmrvl_enable_hs(struct btmrvl_private *priv)
+int btmrvl_enable_hs(struct btmrvl_private *priv)
 {
 	struct sk_buff *skb;
 	struct btmrvl_cmd *cmd;
@@ -268,35 +298,15 @@ static int btmrvl_enable_hs(struct btmrvl_private *priv)
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(btmrvl_enable_hs);
 
 int btmrvl_prepare_command(struct btmrvl_private *priv)
 {
-	struct sk_buff *skb = NULL;
-	struct btmrvl_cmd *cmd;
 	int ret = 0;
 
 	if (priv->btmrvl_dev.hscfgcmd) {
 		priv->btmrvl_dev.hscfgcmd = 0;
-
-		skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
-		if (skb == NULL) {
-			BT_ERR("No free skb");
-			return -ENOMEM;
-		}
-
-		cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
-		cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF, BT_CMD_HOST_SLEEP_CONFIG));
-		cmd->length = 2;
-		cmd->data[0] = (priv->btmrvl_dev.gpio_gap & 0xff00) >> 8;
-		cmd->data[1] = (u8) (priv->btmrvl_dev.gpio_gap & 0x00ff);
-
-		bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
-
-		skb->dev = (void *) priv->btmrvl_dev.hcidev;
-		skb_queue_head(&priv->adapter->tx_queue, skb);
-
-		BT_DBG("Queue HSCFG Command, gpio=0x%x, gap=0x%x",
-						cmd->data[0], cmd->data[1]);
+		btmrvl_send_hscfg_cmd(priv);
 	}
 
 	if (priv->btmrvl_dev.pscmd) {
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 27b74b0d547b..a853244e7fd7 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -339,9 +339,7 @@ static int btmrvl_sdio_download_helper(struct btmrvl_sdio_card *card)
 
 done:
 	kfree(tmphlprbuf);
-	if (fw_helper)
-		release_firmware(fw_helper);
-
+	release_firmware(fw_helper);
 	return ret;
 }
 
@@ -484,10 +482,7 @@ static int btmrvl_sdio_download_fw_w_helper(struct btmrvl_sdio_card *card)
 
 done:
 	kfree(tmpfwbuf);
-
-	if (fw_firmware)
-		release_firmware(fw_firmware);
-
+	release_firmware(fw_firmware);
 	return ret;
 }
 
@@ -1013,6 +1008,9 @@ static int btmrvl_sdio_probe(struct sdio_func *func,
 	priv->btmrvl_dev.psmode = 1;
 	btmrvl_enable_ps(priv);
 
+	priv->btmrvl_dev.gpio_gap = 0xffff;
+	btmrvl_send_hscfg_cmd(priv);
+
 	return 0;
 
 disable_host_int:
@@ -1048,11 +1046,111 @@ static void btmrvl_sdio_remove(struct sdio_func *func)
 	}
 }
 
+static int btmrvl_sdio_suspend(struct device *dev)
+{
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	struct btmrvl_sdio_card *card;
+	struct btmrvl_private *priv;
+	mmc_pm_flag_t pm_flags;
+	struct hci_dev *hcidev;
+
+	if (func) {
+		pm_flags = sdio_get_host_pm_caps(func);
+		BT_DBG("%s: suspend: PM flags = 0x%x", sdio_func_id(func),
+		       pm_flags);
+		if (!(pm_flags & MMC_PM_KEEP_POWER)) {
+			BT_ERR("%s: cannot remain alive while suspended",
+			       sdio_func_id(func));
+			return -ENOSYS;
+		}
+		card = sdio_get_drvdata(func);
+		if (!card || !card->priv) {
+			BT_ERR("card or priv structure is not valid");
+			return 0;
+		}
+	} else {
+		BT_ERR("sdio_func is not specified");
+		return 0;
+	}
+
+	priv = card->priv;
+
+	if (priv->adapter->hs_state != HS_ACTIVATED) {
+		if (btmrvl_enable_hs(priv)) {
+			BT_ERR("HS not actived, suspend failed!");
+			return -EBUSY;
+		}
+	}
+	hcidev = priv->btmrvl_dev.hcidev;
+	BT_DBG("%s: SDIO suspend", hcidev->name);
+	hci_suspend_dev(hcidev);
+	skb_queue_purge(&priv->adapter->tx_queue);
+
+	priv->adapter->is_suspended = true;
+
+	/* We will keep the power when hs enabled successfully */
+	if (priv->adapter->hs_state == HS_ACTIVATED) {
+		BT_DBG("suspend with MMC_PM_KEEP_POWER");
+		return sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+	} else {
+		BT_DBG("suspend without MMC_PM_KEEP_POWER");
+		return 0;
+	}
+}
+
+static int btmrvl_sdio_resume(struct device *dev)
+{
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	struct btmrvl_sdio_card *card;
+	struct btmrvl_private *priv;
+	mmc_pm_flag_t pm_flags;
+	struct hci_dev *hcidev;
+
+	if (func) {
+		pm_flags = sdio_get_host_pm_caps(func);
+		BT_DBG("%s: resume: PM flags = 0x%x", sdio_func_id(func),
+		       pm_flags);
+		card = sdio_get_drvdata(func);
+		if (!card || !card->priv) {
+			BT_ERR("card or priv structure is not valid");
+			return 0;
+		}
+	} else {
+		BT_ERR("sdio_func is not specified");
+		return 0;
+	}
+	priv = card->priv;
+
+	if (!priv->adapter->is_suspended) {
+		BT_DBG("device already resumed");
+		return 0;
+	}
+
+	priv->adapter->is_suspended = false;
+	hcidev = priv->btmrvl_dev.hcidev;
+	BT_DBG("%s: SDIO resume", hcidev->name);
+	hci_resume_dev(hcidev);
+	priv->hw_wakeup_firmware(priv);
+	priv->adapter->hs_state = HS_DEACTIVATED;
+	BT_DBG("%s: HS DEACTIVATED in resume!", hcidev->name);
+
+	return 0;
+}
+
+static const struct dev_pm_ops btmrvl_sdio_pm_ops = {
+	.suspend	= btmrvl_sdio_suspend,
+	.resume		= btmrvl_sdio_resume,
+};
+
 static struct sdio_driver bt_mrvl_sdio = {
 	.name		= "btmrvl_sdio",
 	.id_table	= btmrvl_sdio_ids,
 	.probe		= btmrvl_sdio_probe,
 	.remove		= btmrvl_sdio_remove,
+	.drv = {
+		.owner = THIS_MODULE,
+		.pm = &btmrvl_sdio_pm_ops,
+	}
 };
 
 static int __init btmrvl_sdio_init_module(void)
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 461c68bc4dd7..c9463af8e564 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -143,6 +143,9 @@ static struct usb_device_id blacklist_table[] = {
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xe02c), .driver_info = BTUSB_IGNORE },
 
+	/* Atheros AR5BBU12 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xe03c), .driver_info = BTUSB_ATH3012 },
+
 	/* Broadcom BCM2035 */
 	{ USB_DEVICE(0x0a5c, 0x2035), .driver_info = BTUSB_WRONG_SCO_MTU },
 	{ USB_DEVICE(0x0a5c, 0x200a), .driver_info = BTUSB_WRONG_SCO_MTU },
@@ -855,6 +858,7 @@ static void btusb_work(struct work_struct *work)
 {
 	struct btusb_data *data = container_of(work, struct btusb_data, work);
 	struct hci_dev *hdev = data->hdev;
+	int new_alts;
 	int err;
 
 	if (hdev->conn_hash.sco_num > 0) {
@@ -868,11 +872,19 @@ static void btusb_work(struct work_struct *work)
 
 			set_bit(BTUSB_DID_ISO_RESUME, &data->flags);
 		}
-		if (data->isoc_altsetting != 2) {
+
+		if (hdev->voice_setting & 0x0020) {
+			static const int alts[3] = { 2, 4, 5 };
+			new_alts = alts[hdev->conn_hash.sco_num - 1];
+		} else {
+			new_alts = hdev->conn_hash.sco_num;
+		}
+
+		if (data->isoc_altsetting != new_alts) {
 			clear_bit(BTUSB_ISOC_RUNNING, &data->flags);
 			usb_kill_anchored_urbs(&data->isoc_anchor);
 
-			if (__set_isoc_interface(hdev, 2) < 0)
+			if (__set_isoc_interface(hdev, new_alts) < 0)
 				return;
 		}
 
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index 98a8c05d4f23..e564579a6115 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -388,7 +388,7 @@ static int hci_uart_register_dev(struct hci_uart *hu)
 	hdev->close = hci_uart_close;
 	hdev->flush = hci_uart_flush;
 	hdev->send  = hci_uart_send_frame;
-	hdev->parent = hu->tty->dev;
+	SET_HCIDEV_DEV(hdev, hu->tty->dev);
 
 	if (test_bit(HCI_UART_RAW_DEVICE, &hu->hdev_flags))
 		set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index 158bfe507da7..3f72595a6017 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -252,8 +252,9 @@ static int vhci_open(struct inode *inode, struct file *file)
 	}
 
 	file->private_data = data;
+	nonseekable_open(inode, file);
 
-	return nonseekable_open(inode, file);
+	return 0;
 }
 
 static int vhci_release(struct inode *inode, struct file *file)
diff --git a/drivers/char/agp/generic.c b/drivers/char/agp/generic.c
index 17e05d1076b3..a0df182f6f7d 100644
--- a/drivers/char/agp/generic.c
+++ b/drivers/char/agp/generic.c
@@ -958,7 +958,7 @@ int agp_generic_create_gatt_table(struct agp_bridge_data *bridge)
 	if (set_memory_uc((unsigned long)table, 1 << page_order))
 		printk(KERN_WARNING "Could not set GATT table memory to UC!\n");
 
-	bridge->gatt_table = (void *)table;
+	bridge->gatt_table = (u32 __iomem *)table;
 #else
 	bridge->gatt_table = ioremap_nocache(virt_to_phys(table),
 					(PAGE_SIZE * (1 << page_order)));
@@ -1010,7 +1010,6 @@ int agp_generic_free_gatt_table(struct agp_bridge_data *bridge)
 	case LVL2_APER_SIZE:
 		/* The generic routines can't deal with 2 level gatt's */
 		return -EINVAL;
-		break;
 	default:
 		page_order = 0;
 		break;
@@ -1077,7 +1076,6 @@ int agp_generic_insert_memory(struct agp_memory * mem, off_t pg_start, int type)
 	case LVL2_APER_SIZE:
 		/* The generic routines can't deal with 2 level gatt's */
 		return -EINVAL;
-		break;
 	default:
 		num_entries = 0;
 		break;
diff --git a/drivers/char/agp/intel-agp.c b/drivers/char/agp/intel-agp.c
index 962e75dc4781..764f70c5e690 100644
--- a/drivers/char/agp/intel-agp.c
+++ b/drivers/char/agp/intel-agp.c
@@ -907,6 +907,11 @@ static struct pci_device_id agp_intel_pci_table[] = {
 	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_HB),
 	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_M_HB),
 	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_HB),
+	ID(PCI_DEVICE_ID_INTEL_VALLEYVIEW_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_M_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_S_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_E_HB),
 	{ }
 };
 
diff --git a/drivers/char/agp/intel-agp.h b/drivers/char/agp/intel-agp.h
index 7ea18a5fe71c..c0091753a0d1 100644
--- a/drivers/char/agp/intel-agp.h
+++ b/drivers/char/agp/intel-agp.h
@@ -96,6 +96,7 @@
 #define G4x_GMCH_SIZE_VT_2M	(G4x_GMCH_SIZE_2M | G4x_GMCH_SIZE_VT_EN)
 
 #define GFX_FLSH_CNTL		0x2170 /* 915+ */
+#define GFX_FLSH_CNTL_VLV	0x101008
 
 #define I810_DRAM_CTL		0x3000
 #define I810_DRAM_ROW_0		0x00000001
@@ -235,6 +236,19 @@
 #define PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_HB		0x0158  /* Server */
 #define PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_GT1_IG		0x015A
 #define PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_GT2_IG		0x016A
+#define PCI_DEVICE_ID_INTEL_VALLEYVIEW_HB		0x0F00 /* VLV1 */
+#define PCI_DEVICE_ID_INTEL_VALLEYVIEW_IG		0x0F30
+#define PCI_DEVICE_ID_INTEL_HASWELL_HB				0x0400 /* Desktop */
+#define PCI_DEVICE_ID_INTEL_HASWELL_D_GT1_IG		0x0402
+#define PCI_DEVICE_ID_INTEL_HASWELL_D_GT2_IG		0x0412
+#define PCI_DEVICE_ID_INTEL_HASWELL_M_HB			0x0404 /* Mobile */
+#define PCI_DEVICE_ID_INTEL_HASWELL_M_GT1_IG		0x0406
+#define PCI_DEVICE_ID_INTEL_HASWELL_M_GT2_IG		0x0416
+#define PCI_DEVICE_ID_INTEL_HASWELL_S_HB			0x0408 /* Server */
+#define PCI_DEVICE_ID_INTEL_HASWELL_S_GT1_IG		0x040a
+#define PCI_DEVICE_ID_INTEL_HASWELL_S_GT2_IG		0x041a
+#define PCI_DEVICE_ID_INTEL_HASWELL_SDV		0x0c16 /* SDV */
+#define PCI_DEVICE_ID_INTEL_HASWELL_E_HB			0x0c04
 
 int intel_gmch_probe(struct pci_dev *pdev,
 			       struct agp_bridge_data *bridge);
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index 7f025fb620de..1237e7575c3f 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -1179,6 +1179,20 @@ static void gen6_write_entry(dma_addr_t addr, unsigned int entry,
 	writel(addr | pte_flags, intel_private.gtt + entry);
 }
 
+static void valleyview_write_entry(dma_addr_t addr, unsigned int entry,
+				   unsigned int flags)
+{
+	u32 pte_flags;
+
+	pte_flags = GEN6_PTE_UNCACHED | I810_PTE_VALID;
+
+	/* gen6 has bit11-4 for physical addr bit39-32 */
+	addr |= (addr >> 28) & 0xff0;
+	writel(addr | pte_flags, intel_private.gtt + entry);
+
+	writel(1, intel_private.registers + GFX_FLSH_CNTL_VLV);
+}
+
 static void gen6_cleanup(void)
 {
 }
@@ -1205,12 +1219,16 @@ static inline int needs_idle_maps(void)
 static int i9xx_setup(void)
 {
 	u32 reg_addr;
+	int size = KB(512);
 
 	pci_read_config_dword(intel_private.pcidev, I915_MMADDR, &reg_addr);
 
 	reg_addr &= 0xfff80000;
 
-	intel_private.registers = ioremap(reg_addr, 128 * 4096);
+	if (INTEL_GTT_GEN >= 7)
+		size = MB(2);
+
+	intel_private.registers = ioremap(reg_addr, size);
 	if (!intel_private.registers)
 		return -ENOMEM;
 
@@ -1354,6 +1372,15 @@ static const struct intel_gtt_driver sandybridge_gtt_driver = {
 	.check_flags = gen6_check_flags,
 	.chipset_flush = i9xx_chipset_flush,
 };
+static const struct intel_gtt_driver valleyview_gtt_driver = {
+	.gen = 7,
+	.setup = i9xx_setup,
+	.cleanup = gen6_cleanup,
+	.write_entry = valleyview_write_entry,
+	.dma_mask_size = 40,
+	.check_flags = gen6_check_flags,
+	.chipset_flush = i9xx_chipset_flush,
+};
 
 /* Table to describe Intel GMCH and AGP/PCIE GART drivers.  At least one of
  * driver and gmch_driver must be non-null, and find_gmch will determine
@@ -1460,6 +1487,22 @@ static const struct intel_gtt_driver_description {
 	    "Ivybridge", &sandybridge_gtt_driver },
 	{ PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_GT2_IG,
 	    "Ivybridge", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_VALLEYVIEW_IG,
+	    "ValleyView", &valleyview_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_D_GT1_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_D_GT2_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_M_GT1_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_M_GT2_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_S_GT1_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_S_GT2_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_SDV,
+	    "Haswell", &sandybridge_gtt_driver },
 	{ 0, NULL, NULL }
 };
 
diff --git a/drivers/char/agp/sgi-agp.c b/drivers/char/agp/sgi-agp.c
index ffa888cd1c88..192000377737 100644
--- a/drivers/char/agp/sgi-agp.c
+++ b/drivers/char/agp/sgi-agp.c
@@ -158,7 +158,6 @@ static int sgi_tioca_insert_memory(struct agp_memory *mem, off_t pg_start,
 		break;
 	case LVL2_APER_SIZE:
 		return -EINVAL;
-		break;
 	default:
 		num_entries = 0;
 		break;
diff --git a/drivers/firewire/core-card.c b/drivers/firewire/core-card.c
index f5552b362efc..57ea7f464178 100644
--- a/drivers/firewire/core-card.c
+++ b/drivers/firewire/core-card.c
@@ -421,8 +421,8 @@ static void bm_work(struct work_struct *work)
 			 * root, and thus, IRM.
 			 */
 			new_root_id = local_id;
-			fw_notice(card, "%s, making local node (%02x) root\n",
-				  "BM lock failed", new_root_id);
+			fw_notice(card, "BM lock failed (%s), making local node (%02x) root\n",
+				  fw_rcode_string(rcode), new_root_id);
 			goto pick_me;
 		}
 	} else if (card->bm_generation != generation) {
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index 2e6b24547e2a..2783f69dada6 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -22,6 +22,7 @@
 #include <linux/compat.h>
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/dma-mapping.h>
 #include <linux/errno.h>
 #include <linux/firewire.h>
 #include <linux/firewire-cdev.h>
@@ -70,6 +71,7 @@ struct client {
 	u64 iso_closure;
 	struct fw_iso_buffer buffer;
 	unsigned long vm_start;
+	bool buffer_is_mapped;
 
 	struct list_head phy_receiver_link;
 	u64 phy_receiver_closure;
@@ -959,11 +961,20 @@ static void iso_mc_callback(struct fw_iso_context *context,
 		    sizeof(e->interrupt), NULL, 0);
 }
 
+static enum dma_data_direction iso_dma_direction(struct fw_iso_context *context)
+{
+		if (context->type == FW_ISO_CONTEXT_TRANSMIT)
+			return DMA_TO_DEVICE;
+		else
+			return DMA_FROM_DEVICE;
+}
+
 static int ioctl_create_iso_context(struct client *client, union ioctl_arg *arg)
 {
 	struct fw_cdev_create_iso_context *a = &arg->create_iso_context;
 	struct fw_iso_context *context;
 	fw_iso_callback_t cb;
+	int ret;
 
 	BUILD_BUG_ON(FW_CDEV_ISO_CONTEXT_TRANSMIT != FW_ISO_CONTEXT_TRANSMIT ||
 		     FW_CDEV_ISO_CONTEXT_RECEIVE  != FW_ISO_CONTEXT_RECEIVE  ||
@@ -1004,8 +1015,21 @@ static int ioctl_create_iso_context(struct client *client, union ioctl_arg *arg)
 	if (client->iso_context != NULL) {
 		spin_unlock_irq(&client->lock);
 		fw_iso_context_destroy(context);
+
 		return -EBUSY;
 	}
+	if (!client->buffer_is_mapped) {
+		ret = fw_iso_buffer_map_dma(&client->buffer,
+					    client->device->card,
+					    iso_dma_direction(context));
+		if (ret < 0) {
+			spin_unlock_irq(&client->lock);
+			fw_iso_context_destroy(context);
+
+			return ret;
+		}
+		client->buffer_is_mapped = true;
+	}
 	client->iso_closure = a->closure;
 	client->iso_context = context;
 	spin_unlock_irq(&client->lock);
@@ -1651,7 +1675,6 @@ static long fw_device_op_compat_ioctl(struct file *file,
 static int fw_device_op_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct client *client = file->private_data;
-	enum dma_data_direction direction;
 	unsigned long size;
 	int page_count, ret;
 
@@ -1674,20 +1697,28 @@ static int fw_device_op_mmap(struct file *file, struct vm_area_struct *vma)
 	if (size & ~PAGE_MASK)
 		return -EINVAL;
 
-	if (vma->vm_flags & VM_WRITE)
-		direction = DMA_TO_DEVICE;
-	else
-		direction = DMA_FROM_DEVICE;
-
-	ret = fw_iso_buffer_init(&client->buffer, client->device->card,
-				 page_count, direction);
+	ret = fw_iso_buffer_alloc(&client->buffer, page_count);
 	if (ret < 0)
 		return ret;
 
-	ret = fw_iso_buffer_map(&client->buffer, vma);
+	spin_lock_irq(&client->lock);
+	if (client->iso_context) {
+		ret = fw_iso_buffer_map_dma(&client->buffer,
+				client->device->card,
+				iso_dma_direction(client->iso_context));
+		client->buffer_is_mapped = (ret == 0);
+	}
+	spin_unlock_irq(&client->lock);
 	if (ret < 0)
-		fw_iso_buffer_destroy(&client->buffer, client->device->card);
+		goto fail;
 
+	ret = fw_iso_buffer_map_vma(&client->buffer, vma);
+	if (ret < 0)
+		goto fail;
+
+	return 0;
+ fail:
+	fw_iso_buffer_destroy(&client->buffer, client->device->card);
 	return ret;
 }
 
diff --git a/drivers/firewire/core-device.c b/drivers/firewire/core-device.c
index 68109e9bb04e..4d460ef87161 100644
--- a/drivers/firewire/core-device.c
+++ b/drivers/firewire/core-device.c
@@ -481,6 +481,7 @@ static int read_rom(struct fw_device *device,
  * generation changes under us, read_config_rom will fail and get retried.
  * It's better to start all over in this case because the node from which we
  * are reading the ROM may have changed the ROM during the reset.
+ * Returns either a result code or a negative error code.
  */
 static int read_config_rom(struct fw_device *device, int generation)
 {
@@ -488,7 +489,7 @@ static int read_config_rom(struct fw_device *device, int generation)
 	const u32 *old_rom, *new_rom;
 	u32 *rom, *stack;
 	u32 sp, key;
-	int i, end, length, ret = -1;
+	int i, end, length, ret;
 
 	rom = kmalloc(sizeof(*rom) * MAX_CONFIG_ROM_SIZE +
 		      sizeof(*stack) * MAX_CONFIG_ROM_SIZE, GFP_KERNEL);
@@ -502,18 +503,21 @@ static int read_config_rom(struct fw_device *device, int generation)
 
 	/* First read the bus info block. */
 	for (i = 0; i < 5; i++) {
-		if (read_rom(device, generation, i, &rom[i]) != RCODE_COMPLETE)
+		ret = read_rom(device, generation, i, &rom[i]);
+		if (ret != RCODE_COMPLETE)
 			goto out;
 		/*
-		 * As per IEEE1212 7.2, during power-up, devices can
+		 * As per IEEE1212 7.2, during initialization, devices can
 		 * reply with a 0 for the first quadlet of the config
 		 * rom to indicate that they are booting (for example,
 		 * if the firmware is on the disk of a external
 		 * harddisk).  In that case we just fail, and the
 		 * retry mechanism will try again later.
 		 */
-		if (i == 0 && rom[i] == 0)
+		if (i == 0 && rom[i] == 0) {
+			ret = RCODE_BUSY;
 			goto out;
+		}
 	}
 
 	device->max_speed = device->node->max_speed;
@@ -563,11 +567,14 @@ static int read_config_rom(struct fw_device *device, int generation)
 		 */
 		key = stack[--sp];
 		i = key & 0xffffff;
-		if (WARN_ON(i >= MAX_CONFIG_ROM_SIZE))
+		if (WARN_ON(i >= MAX_CONFIG_ROM_SIZE)) {
+			ret = -ENXIO;
 			goto out;
+		}
 
 		/* Read header quadlet for the block to get the length. */
-		if (read_rom(device, generation, i, &rom[i]) != RCODE_COMPLETE)
+		ret = read_rom(device, generation, i, &rom[i]);
+		if (ret != RCODE_COMPLETE)
 			goto out;
 		end = i + (rom[i] >> 16) + 1;
 		if (end > MAX_CONFIG_ROM_SIZE) {
@@ -590,8 +597,8 @@ static int read_config_rom(struct fw_device *device, int generation)
 		 * it references another block, and push it in that case.
 		 */
 		for (; i < end; i++) {
-			if (read_rom(device, generation, i, &rom[i]) !=
-			    RCODE_COMPLETE)
+			ret = read_rom(device, generation, i, &rom[i]);
+			if (ret != RCODE_COMPLETE)
 				goto out;
 
 			if ((key >> 30) != 3 || (rom[i] >> 30) < 2)
@@ -619,8 +626,10 @@ static int read_config_rom(struct fw_device *device, int generation)
 
 	old_rom = device->config_rom;
 	new_rom = kmemdup(rom, length * 4, GFP_KERNEL);
-	if (new_rom == NULL)
+	if (new_rom == NULL) {
+		ret = -ENOMEM;
 		goto out;
+	}
 
 	down_write(&fw_device_rwsem);
 	device->config_rom = new_rom;
@@ -628,7 +637,7 @@ static int read_config_rom(struct fw_device *device, int generation)
 	up_write(&fw_device_rwsem);
 
 	kfree(old_rom);
-	ret = 0;
+	ret = RCODE_COMPLETE;
 	device->max_rec	= rom[2] >> 12 & 0xf;
 	device->cmc	= rom[2] >> 30 & 1;
 	device->irmc	= rom[2] >> 31 & 1;
@@ -967,15 +976,17 @@ static void fw_device_init(struct work_struct *work)
 	 * device.
 	 */
 
-	if (read_config_rom(device, device->generation) < 0) {
+	ret = read_config_rom(device, device->generation);
+	if (ret != RCODE_COMPLETE) {
 		if (device->config_rom_retries < MAX_RETRIES &&
 		    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
 			device->config_rom_retries++;
 			fw_schedule_device_work(device, RETRY_DELAY);
 		} else {
 			if (device->node->link_on)
-				fw_notice(card, "giving up on Config ROM for node id %x\n",
-					  device->node_id);
+				fw_notice(card, "giving up on node %x: reading config rom failed: %s\n",
+					  device->node_id,
+					  fw_rcode_string(ret));
 			if (device->node == card->root_node)
 				fw_schedule_bm_work(card, 0);
 			fw_device_release(&device->device);
@@ -1069,31 +1080,30 @@ static void fw_device_init(struct work_struct *work)
 	put_device(&device->device);	/* our reference */
 }
 
-enum {
-	REREAD_BIB_ERROR,
-	REREAD_BIB_GONE,
-	REREAD_BIB_UNCHANGED,
-	REREAD_BIB_CHANGED,
-};
-
 /* Reread and compare bus info block and header of root directory */
-static int reread_config_rom(struct fw_device *device, int generation)
+static int reread_config_rom(struct fw_device *device, int generation,
+			     bool *changed)
 {
 	u32 q;
-	int i;
+	int i, rcode;
 
 	for (i = 0; i < 6; i++) {
-		if (read_rom(device, generation, i, &q) != RCODE_COMPLETE)
-			return REREAD_BIB_ERROR;
+		rcode = read_rom(device, generation, i, &q);
+		if (rcode != RCODE_COMPLETE)
+			return rcode;
 
 		if (i == 0 && q == 0)
-			return REREAD_BIB_GONE;
+			/* inaccessible (see read_config_rom); retry later */
+			return RCODE_BUSY;
 
-		if (q != device->config_rom[i])
-			return REREAD_BIB_CHANGED;
+		if (q != device->config_rom[i]) {
+			*changed = true;
+			return RCODE_COMPLETE;
+		}
 	}
 
-	return REREAD_BIB_UNCHANGED;
+	*changed = false;
+	return RCODE_COMPLETE;
 }
 
 static void fw_device_refresh(struct work_struct *work)
@@ -1101,23 +1111,14 @@ static void fw_device_refresh(struct work_struct *work)
 	struct fw_device *device =
 		container_of(work, struct fw_device, work.work);
 	struct fw_card *card = device->card;
-	int node_id = device->node_id;
-
-	switch (reread_config_rom(device, device->generation)) {
-	case REREAD_BIB_ERROR:
-		if (device->config_rom_retries < MAX_RETRIES / 2 &&
-		    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
-			device->config_rom_retries++;
-			fw_schedule_device_work(device, RETRY_DELAY / 2);
-
-			return;
-		}
-		goto give_up;
+	int ret, node_id = device->node_id;
+	bool changed;
 
-	case REREAD_BIB_GONE:
-		goto gone;
+	ret = reread_config_rom(device, device->generation, &changed);
+	if (ret != RCODE_COMPLETE)
+		goto failed_config_rom;
 
-	case REREAD_BIB_UNCHANGED:
+	if (!changed) {
 		if (atomic_cmpxchg(&device->state,
 				   FW_DEVICE_INITIALIZING,
 				   FW_DEVICE_RUNNING) == FW_DEVICE_GONE)
@@ -1126,9 +1127,6 @@ static void fw_device_refresh(struct work_struct *work)
 		fw_device_update(work);
 		device->config_rom_retries = 0;
 		goto out;
-
-	case REREAD_BIB_CHANGED:
-		break;
 	}
 
 	/*
@@ -1137,16 +1135,9 @@ static void fw_device_refresh(struct work_struct *work)
 	 */
 	device_for_each_child(&device->device, NULL, shutdown_unit);
 
-	if (read_config_rom(device, device->generation) < 0) {
-		if (device->config_rom_retries < MAX_RETRIES &&
-		    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
-			device->config_rom_retries++;
-			fw_schedule_device_work(device, RETRY_DELAY);
-
-			return;
-		}
-		goto give_up;
-	}
+	ret = read_config_rom(device, device->generation);
+	if (ret != RCODE_COMPLETE)
+		goto failed_config_rom;
 
 	fw_device_cdev_update(device);
 	create_units(device);
@@ -1163,9 +1154,16 @@ static void fw_device_refresh(struct work_struct *work)
 	device->config_rom_retries = 0;
 	goto out;
 
- give_up:
-	fw_notice(card, "giving up on refresh of device %s\n",
-		  dev_name(&device->device));
+ failed_config_rom:
+	if (device->config_rom_retries < MAX_RETRIES &&
+	    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
+		device->config_rom_retries++;
+		fw_schedule_device_work(device, RETRY_DELAY);
+		return;
+	}
+
+	fw_notice(card, "giving up on refresh of device %s: %s\n",
+		  dev_name(&device->device), fw_rcode_string(ret));
  gone:
 	atomic_set(&device->state, FW_DEVICE_GONE);
 	PREPARE_DELAYED_WORK(&device->work, fw_device_shutdown);
diff --git a/drivers/firewire/core-iso.c b/drivers/firewire/core-iso.c
index d1565828ae2c..8382e27e9a27 100644
--- a/drivers/firewire/core-iso.c
+++ b/drivers/firewire/core-iso.c
@@ -39,52 +39,73 @@
  * Isochronous DMA context management
  */
 
-int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
-		       int page_count, enum dma_data_direction direction)
+int fw_iso_buffer_alloc(struct fw_iso_buffer *buffer, int page_count)
 {
-	int i, j;
-	dma_addr_t address;
-
-	buffer->page_count = page_count;
-	buffer->direction = direction;
+	int i;
 
+	buffer->page_count = 0;
+	buffer->page_count_mapped = 0;
 	buffer->pages = kmalloc(page_count * sizeof(buffer->pages[0]),
 				GFP_KERNEL);
 	if (buffer->pages == NULL)
-		goto out;
+		return -ENOMEM;
 
-	for (i = 0; i < buffer->page_count; i++) {
+	for (i = 0; i < page_count; i++) {
 		buffer->pages[i] = alloc_page(GFP_KERNEL | GFP_DMA32 | __GFP_ZERO);
 		if (buffer->pages[i] == NULL)
-			goto out_pages;
+			break;
+	}
+	buffer->page_count = i;
+	if (i < page_count) {
+		fw_iso_buffer_destroy(buffer, NULL);
+		return -ENOMEM;
+	}
 
+	return 0;
+}
+
+int fw_iso_buffer_map_dma(struct fw_iso_buffer *buffer, struct fw_card *card,
+			  enum dma_data_direction direction)
+{
+	dma_addr_t address;
+	int i;
+
+	buffer->direction = direction;
+
+	for (i = 0; i < buffer->page_count; i++) {
 		address = dma_map_page(card->device, buffer->pages[i],
 				       0, PAGE_SIZE, direction);
-		if (dma_mapping_error(card->device, address)) {
-			__free_page(buffer->pages[i]);
-			goto out_pages;
-		}
+		if (dma_mapping_error(card->device, address))
+			break;
+
 		set_page_private(buffer->pages[i], address);
 	}
+	buffer->page_count_mapped = i;
+	if (i < buffer->page_count)
+		return -ENOMEM;
 
 	return 0;
+}
 
- out_pages:
-	for (j = 0; j < i; j++) {
-		address = page_private(buffer->pages[j]);
-		dma_unmap_page(card->device, address,
-			       PAGE_SIZE, direction);
-		__free_page(buffer->pages[j]);
-	}
-	kfree(buffer->pages);
- out:
-	buffer->pages = NULL;
+int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
+		       int page_count, enum dma_data_direction direction)
+{
+	int ret;
+
+	ret = fw_iso_buffer_alloc(buffer, page_count);
+	if (ret < 0)
+		return ret;
+
+	ret = fw_iso_buffer_map_dma(buffer, card, direction);
+	if (ret < 0)
+		fw_iso_buffer_destroy(buffer, card);
 
-	return -ENOMEM;
+	return ret;
 }
 EXPORT_SYMBOL(fw_iso_buffer_init);
 
-int fw_iso_buffer_map(struct fw_iso_buffer *buffer, struct vm_area_struct *vma)
+int fw_iso_buffer_map_vma(struct fw_iso_buffer *buffer,
+			  struct vm_area_struct *vma)
 {
 	unsigned long uaddr;
 	int i, err;
@@ -107,15 +128,18 @@ void fw_iso_buffer_destroy(struct fw_iso_buffer *buffer,
 	int i;
 	dma_addr_t address;
 
-	for (i = 0; i < buffer->page_count; i++) {
+	for (i = 0; i < buffer->page_count_mapped; i++) {
 		address = page_private(buffer->pages[i]);
 		dma_unmap_page(card->device, address,
 			       PAGE_SIZE, buffer->direction);
-		__free_page(buffer->pages[i]);
 	}
+	for (i = 0; i < buffer->page_count; i++)
+		__free_page(buffer->pages[i]);
 
 	kfree(buffer->pages);
 	buffer->pages = NULL;
+	buffer->page_count = 0;
+	buffer->page_count_mapped = 0;
 }
 EXPORT_SYMBOL(fw_iso_buffer_destroy);
 
diff --git a/drivers/firewire/core-transaction.c b/drivers/firewire/core-transaction.c
index db8a965cf712..780708dc6e25 100644
--- a/drivers/firewire/core-transaction.c
+++ b/drivers/firewire/core-transaction.c
@@ -1003,6 +1003,32 @@ void fw_core_handle_response(struct fw_card *card, struct fw_packet *p)
 }
 EXPORT_SYMBOL(fw_core_handle_response);
 
+/**
+ * fw_rcode_string - convert a firewire result code to an error description
+ * @rcode: the result code
+ */
+const char *fw_rcode_string(int rcode)
+{
+	static const char *const names[] = {
+		[RCODE_COMPLETE]       = "no error",
+		[RCODE_CONFLICT_ERROR] = "conflict error",
+		[RCODE_DATA_ERROR]     = "data error",
+		[RCODE_TYPE_ERROR]     = "type error",
+		[RCODE_ADDRESS_ERROR]  = "address error",
+		[RCODE_SEND_ERROR]     = "send error",
+		[RCODE_CANCELLED]      = "timeout",
+		[RCODE_BUSY]           = "busy",
+		[RCODE_GENERATION]     = "bus reset",
+		[RCODE_NO_ACK]         = "no ack",
+	};
+
+	if ((unsigned int)rcode < ARRAY_SIZE(names) && names[rcode])
+		return names[rcode];
+	else
+		return "unknown";
+}
+EXPORT_SYMBOL(fw_rcode_string);
+
 static const struct fw_address_region topology_map_region =
 	{ .start = CSR_REGISTER_BASE | CSR_TOPOLOGY_MAP,
 	  .end   = CSR_REGISTER_BASE | CSR_TOPOLOGY_MAP_END, };
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index b5a2f6197053..515a42c786d0 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -3,6 +3,7 @@
 
 #include <linux/compiler.h>
 #include <linux/device.h>
+#include <linux/dma-mapping.h>
 #include <linux/fs.h>
 #include <linux/list.h>
 #include <linux/idr.h>
@@ -154,7 +155,11 @@ void fw_node_event(struct fw_card *card, struct fw_node *node, int event);
 
 /* -iso */
 
-int fw_iso_buffer_map(struct fw_iso_buffer *buffer, struct vm_area_struct *vma);
+int fw_iso_buffer_alloc(struct fw_iso_buffer *buffer, int page_count);
+int fw_iso_buffer_map_dma(struct fw_iso_buffer *buffer, struct fw_card *card,
+			  enum dma_data_direction direction);
+int fw_iso_buffer_map_vma(struct fw_iso_buffer *buffer,
+			  struct vm_area_struct *vma);
 
 
 /* -topology */
diff --git a/drivers/firewire/nosy.c b/drivers/firewire/nosy.c
index a7c4422a688e..4ebfb2273672 100644
--- a/drivers/firewire/nosy.c
+++ b/drivers/firewire/nosy.c
@@ -693,6 +693,8 @@ static struct pci_device_id pci_table[] __devinitdata = {
 	{ }	/* Terminating entry */
 };
 
+MODULE_DEVICE_TABLE(pci, pci_table);
+
 static struct pci_driver lynx_pci_driver = {
 	.name =		driver_name,
 	.id_table =	pci_table,
@@ -700,22 +702,8 @@ static struct pci_driver lynx_pci_driver = {
 	.remove =	remove_card,
 };
 
+module_pci_driver(lynx_pci_driver);
+
 MODULE_AUTHOR("Kristian Hoegsberg");
 MODULE_DESCRIPTION("Snoop mode driver for TI pcilynx 1394 controllers");
 MODULE_LICENSE("GPL");
-MODULE_DEVICE_TABLE(pci, pci_table);
-
-static int __init nosy_init(void)
-{
-	return pci_register_driver(&lynx_pci_driver);
-}
-
-static void __exit nosy_cleanup(void)
-{
-	pci_unregister_driver(&lynx_pci_driver);
-
-	pr_info("Unloaded %s\n", driver_name);
-}
-
-module_init(nosy_init);
-module_exit(nosy_cleanup);
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index 2b5460075a9f..c1af05e834b6 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -1821,9 +1821,8 @@ static void bus_reset_work(struct work_struct *work)
 {
 	struct fw_ohci *ohci =
 		container_of(work, struct fw_ohci, bus_reset_work);
-	int self_id_count, i, j, reg;
-	int generation, new_generation;
-	unsigned long flags;
+	int self_id_count, generation, new_generation, i, j;
+	u32 reg;
 	void *free_rom = NULL;
 	dma_addr_t free_rom_bus = 0;
 	bool is_new_root;
@@ -1930,13 +1929,13 @@ static void bus_reset_work(struct work_struct *work)
 	}
 
 	/* FIXME: Document how the locking works. */
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	ohci->generation = -1; /* prevent AT packet queueing */
 	context_stop(&ohci->at_request_ctx);
 	context_stop(&ohci->at_response_ctx);
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	/*
 	 * Per OHCI 1.2 draft, clause 7.2.3.3, hardware may leave unsent
@@ -1946,7 +1945,7 @@ static void bus_reset_work(struct work_struct *work)
 	at_context_flush(&ohci->at_request_ctx);
 	at_context_flush(&ohci->at_response_ctx);
 
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	ohci->generation = generation;
 	reg_write(ohci, OHCI1394_IntEventClear, OHCI1394_busReset);
@@ -1990,7 +1989,7 @@ static void bus_reset_work(struct work_struct *work)
 	reg_write(ohci, OHCI1394_PhyReqFilterLoSet, ~0);
 #endif
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	if (free_rom)
 		dma_free_coherent(ohci->card.device, CONFIG_ROM_SIZE,
@@ -2402,7 +2401,6 @@ static int ohci_set_config_rom(struct fw_card *card,
 			       const __be32 *config_rom, size_t length)
 {
 	struct fw_ohci *ohci;
-	unsigned long flags;
 	__be32 *next_config_rom;
 	dma_addr_t uninitialized_var(next_config_rom_bus);
 
@@ -2441,7 +2439,7 @@ static int ohci_set_config_rom(struct fw_card *card,
 	if (next_config_rom == NULL)
 		return -ENOMEM;
 
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	/*
 	 * If there is not an already pending config_rom update,
@@ -2467,7 +2465,7 @@ static int ohci_set_config_rom(struct fw_card *card,
 
 	reg_write(ohci, OHCI1394_ConfigROMmap, ohci->next_config_rom_bus);
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	/* If we didn't use the DMA allocation, delete it. */
 	if (next_config_rom != NULL)
@@ -2891,10 +2889,9 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
 	descriptor_callback_t uninitialized_var(callback);
 	u64 *uninitialized_var(channels);
 	u32 *uninitialized_var(mask), uninitialized_var(regs);
-	unsigned long flags;
 	int index, ret = -EBUSY;
 
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	switch (type) {
 	case FW_ISO_CONTEXT_TRANSMIT:
@@ -2938,7 +2935,7 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
 		ret = -ENOSYS;
 	}
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	if (index < 0)
 		return ERR_PTR(ret);
@@ -2964,7 +2961,7 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
  out_with_header:
 	free_page((unsigned long)ctx->header);
  out:
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	switch (type) {
 	case FW_ISO_CONTEXT_RECEIVE:
@@ -2977,7 +2974,7 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
 	}
 	*mask |= 1 << index;
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	return ERR_PTR(ret);
 }
@@ -3789,6 +3786,8 @@ static struct pci_driver fw_ohci_pci_driver = {
 #endif
 };
 
+module_pci_driver(fw_ohci_pci_driver);
+
 MODULE_AUTHOR("Kristian Hoegsberg <krh@bitplanet.net>");
 MODULE_DESCRIPTION("Driver for PCI OHCI IEEE1394 controllers");
 MODULE_LICENSE("GPL");
@@ -3797,16 +3796,3 @@ MODULE_LICENSE("GPL");
 #ifndef CONFIG_IEEE1394_OHCI1394_MODULE
 MODULE_ALIAS("ohci1394");
 #endif
-
-static int __init fw_ohci_init(void)
-{
-	return pci_register_driver(&fw_ohci_pci_driver);
-}
-
-static void __exit fw_ohci_cleanup(void)
-{
-	pci_unregister_driver(&fw_ohci_pci_driver);
-}
-
-module_init(fw_ohci_init);
-module_exit(fw_ohci_cleanup);
diff --git a/drivers/firewire/sbp2.c b/drivers/firewire/sbp2.c
index b7e65d7eab64..1162d6b3bf85 100644
--- a/drivers/firewire/sbp2.c
+++ b/drivers/firewire/sbp2.c
@@ -207,9 +207,8 @@ static const struct device *lu_dev(const struct sbp2_logical_unit *lu)
 #define SBP2_MAX_CDB_SIZE		16
 
 /*
- * The default maximum s/g segment size of a FireWire controller is
- * usually 0x10000, but SBP-2 only allows 0xffff. Since buffers have to
- * be quadlet-aligned, we set the length limit to 0xffff & ~3.
+ * The maximum SBP-2 data buffer size is 0xffff.  We quadlet-align this
+ * for compatibility with earlier versions of this driver.
  */
 #define SBP2_MAX_SEG_SIZE		0xfffc
 
@@ -1163,7 +1162,8 @@ static int sbp2_probe(struct device *dev)
 
 	shost->max_cmd_len = SBP2_MAX_CDB_SIZE;
 
-	if (scsi_add_host(shost, &unit->device) < 0)
+	if (scsi_add_host_with_dma(shost, &unit->device,
+				   device->card->device) < 0)
 		goto fail_shost_put;
 
 	/* implicit directory ID */
@@ -1295,10 +1295,7 @@ static struct fw_driver sbp2_driver = {
 static void sbp2_unmap_scatterlist(struct device *card_device,
 				   struct sbp2_command_orb *orb)
 {
-	if (scsi_sg_count(orb->cmd))
-		dma_unmap_sg(card_device, scsi_sglist(orb->cmd),
-			     scsi_sg_count(orb->cmd),
-			     orb->cmd->sc_data_direction);
+	scsi_dma_unmap(orb->cmd);
 
 	if (orb->request.misc & cpu_to_be32(COMMAND_ORB_PAGE_TABLE_PRESENT))
 		dma_unmap_single(card_device, orb->page_table_bus,
@@ -1404,9 +1401,8 @@ static int sbp2_map_scatterlist(struct sbp2_command_orb *orb,
 	struct scatterlist *sg = scsi_sglist(orb->cmd);
 	int i, n;
 
-	n = dma_map_sg(device->card->device, sg, scsi_sg_count(orb->cmd),
-		       orb->cmd->sc_data_direction);
-	if (n == 0)
+	n = scsi_dma_map(orb->cmd);
+	if (n <= 0)
 		goto fail;
 
 	/*
@@ -1452,8 +1448,7 @@ static int sbp2_map_scatterlist(struct sbp2_command_orb *orb,
 	return 0;
 
  fail_page_table:
-	dma_unmap_sg(device->card->device, scsi_sglist(orb->cmd),
-		     scsi_sg_count(orb->cmd), orb->cmd->sc_data_direction);
+	scsi_dma_unmap(orb->cmd);
  fail:
 	return -ENOMEM;
 }
@@ -1534,7 +1529,10 @@ static int sbp2_scsi_slave_alloc(struct scsi_device *sdev)
 
 	sdev->allow_restart = 1;
 
-	/* SBP-2 requires quadlet alignment of the data buffers. */
+	/*
+	 * SBP-2 does not require any alignment, but we set it anyway
+	 * for compatibility with earlier versions of this driver.
+	 */
 	blk_queue_update_dma_alignment(sdev->request_queue, 4 - 1);
 
 	if (lu->tgt->workarounds & SBP2_WORKAROUND_INQUIRY_36)
@@ -1568,8 +1566,6 @@ static int sbp2_scsi_slave_configure(struct scsi_device *sdev)
 	if (lu->tgt->workarounds & SBP2_WORKAROUND_128K_MAX_TRANS)
 		blk_queue_max_hw_sectors(sdev->request_queue, 128 * 1024 / 512);
 
-	blk_queue_max_segment_size(sdev->request_queue, SBP2_MAX_SEG_SIZE);
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index e354bc0b052a..23120c00a881 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -186,3 +186,9 @@ source "drivers/gpu/drm/vmwgfx/Kconfig"
 source "drivers/gpu/drm/gma500/Kconfig"
 
 source "drivers/gpu/drm/udl/Kconfig"
+
+source "drivers/gpu/drm/ast/Kconfig"
+
+source "drivers/gpu/drm/mgag200/Kconfig"
+
+source "drivers/gpu/drm/cirrus/Kconfig"
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index c20da5bda355..f65f65ed0ddf 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -34,6 +34,8 @@ obj-$(CONFIG_DRM_RADEON)+= radeon/
 obj-$(CONFIG_DRM_MGA)	+= mga/
 obj-$(CONFIG_DRM_I810)	+= i810/
 obj-$(CONFIG_DRM_I915)  += i915/
+obj-$(CONFIG_DRM_MGAG200) += mgag200/
+obj-$(CONFIG_DRM_CIRRUS_QEMU) += cirrus/
 obj-$(CONFIG_DRM_SIS)   += sis/
 obj-$(CONFIG_DRM_SAVAGE)+= savage/
 obj-$(CONFIG_DRM_VMWGFX)+= vmwgfx/
@@ -42,4 +44,5 @@ obj-$(CONFIG_DRM_NOUVEAU) +=nouveau/
 obj-$(CONFIG_DRM_EXYNOS) +=exynos/
 obj-$(CONFIG_DRM_GMA500) += gma500/
 obj-$(CONFIG_DRM_UDL) += udl/
+obj-$(CONFIG_DRM_AST) += ast/
 obj-y			+= i2c/
diff --git a/drivers/gpu/drm/ast/Kconfig b/drivers/gpu/drm/ast/Kconfig
new file mode 100644
index 000000000000..a277b1257888
--- /dev/null
+++ b/drivers/gpu/drm/ast/Kconfig
@@ -0,0 +1,16 @@
+config DRM_AST
+	tristate "AST server chips"
+	depends on DRM && PCI && EXPERIMENTAL
+	select DRM_TTM
+	select FB_SYS_COPYAREA
+	select FB_SYS_FILLRECT
+	select FB_SYS_IMAGEBLIT
+	select DRM_KMS_HELPER
+	select DRM_TTM
+	help
+	 Say yes for experimental AST GPU driver. Do not enable
+	 this driver without having a working -modesetting,
+	 and a version of AST that knows to fail if KMS
+	 is bound to the driver. These GPUs are commonly found
+	 in server chipsets.
+
diff --git a/drivers/gpu/drm/ast/Makefile b/drivers/gpu/drm/ast/Makefile
new file mode 100644
index 000000000000..8df4f284ee24
--- /dev/null
+++ b/drivers/gpu/drm/ast/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the drm device driver.  This driver provides support for the
+# Direct Rendering Infrastructure (DRI) in XFree86 4.1.0 and higher.
+
+ccflags-y := -Iinclude/drm
+
+ast-y := ast_drv.o ast_main.o ast_mode.o ast_fb.o ast_ttm.o ast_post.o
+
+obj-$(CONFIG_DRM_AST) := ast.o
\ No newline at end of file
diff --git a/drivers/gpu/drm/ast/ast_dram_tables.h b/drivers/gpu/drm/ast/ast_dram_tables.h
new file mode 100644
index 000000000000..cc04539c0ff3
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_dram_tables.h
@@ -0,0 +1,144 @@
+#ifndef AST_DRAM_TABLES_H
+#define AST_DRAM_TABLES_H
+
+/* DRAM timing tables */
+struct ast_dramstruct {
+	u16 index;
+	u32 data;
+};
+
+static const struct ast_dramstruct ast2000_dram_table_data[] = {
+	{ 0x0108, 0x00000000 },
+	{ 0x0120, 0x00004a21 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0000, 0xFFFFFFFF },
+	{ 0x0004, 0x00000089 },
+	{ 0x0008, 0x22331353 },
+	{ 0x000C, 0x0d07000b },
+	{ 0x0010, 0x11113333 },
+	{ 0x0020, 0x00110350 },
+	{ 0x0028, 0x1e0828f0 },
+	{ 0x0024, 0x00000001 },
+	{ 0x001C, 0x00000000 },
+	{ 0x0014, 0x00000003 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0018, 0x00000131 },
+	{ 0x0014, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0018, 0x00000031 },
+	{ 0x0014, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0028, 0x1e0828f1 },
+	{ 0x0024, 0x00000003 },
+	{ 0x002C, 0x1f0f28fb },
+	{ 0x0030, 0xFFFFFE01 },
+	{ 0xFFFF, 0xFFFFFFFF }
+};
+
+static const struct ast_dramstruct ast1100_dram_table_data[] = {
+	{ 0x2000, 0x1688a8a8 },
+	{ 0x2020, 0x000041f0 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0000, 0xfc600309 },
+	{ 0x006C, 0x00909090 },
+	{ 0x0064, 0x00050000 },
+	{ 0x0004, 0x00000585 },
+	{ 0x0008, 0x0011030f },
+	{ 0x0010, 0x22201724 },
+	{ 0x0018, 0x1e29011a },
+	{ 0x0020, 0x00c82222 },
+	{ 0x0014, 0x01001523 },
+	{ 0x001C, 0x1024010d },
+	{ 0x0024, 0x00cb2522 },
+	{ 0x0038, 0xffffff82 },
+	{ 0x003C, 0x00000000 },
+	{ 0x0040, 0x00000000 },
+	{ 0x0044, 0x00000000 },
+	{ 0x0048, 0x00000000 },
+	{ 0x004C, 0x00000000 },
+	{ 0x0050, 0x00000000 },
+	{ 0x0054, 0x00000000 },
+	{ 0x0058, 0x00000000 },
+	{ 0x005C, 0x00000000 },
+	{ 0x0060, 0x032aa02a },
+	{ 0x0064, 0x002d3000 },
+	{ 0x0068, 0x00000000 },
+	{ 0x0070, 0x00000000 },
+	{ 0x0074, 0x00000000 },
+	{ 0x0078, 0x00000000 },
+	{ 0x007C, 0x00000000 },
+	{ 0x0034, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x002C, 0x00000732 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000005 },
+	{ 0x0028, 0x00000007 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0028, 0x00000001 },
+	{ 0x000C, 0x00005a08 },
+	{ 0x002C, 0x00000632 },
+	{ 0x0028, 0x00000001 },
+	{ 0x0030, 0x000003c0 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000003 },
+	{ 0x000C, 0x00005a21 },
+	{ 0x0034, 0x00007c03 },
+	{ 0x0120, 0x00004c41 },
+	{ 0xffff, 0xffffffff },
+};
+
+static const struct ast_dramstruct ast2100_dram_table_data[] = {
+	{ 0x2000, 0x1688a8a8 },
+	{ 0x2020, 0x00004120 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0000, 0xfc600309 },
+	{ 0x006C, 0x00909090 },
+	{ 0x0064, 0x00070000 },
+	{ 0x0004, 0x00000489 },
+	{ 0x0008, 0x0011030f },
+	{ 0x0010, 0x32302926 },
+	{ 0x0018, 0x274c0122 },
+	{ 0x0020, 0x00ce2222 },
+	{ 0x0014, 0x01001523 },
+	{ 0x001C, 0x1024010d },
+	{ 0x0024, 0x00cb2522 },
+	{ 0x0038, 0xffffff82 },
+	{ 0x003C, 0x00000000 },
+	{ 0x0040, 0x00000000 },
+	{ 0x0044, 0x00000000 },
+	{ 0x0048, 0x00000000 },
+	{ 0x004C, 0x00000000 },
+	{ 0x0050, 0x00000000 },
+	{ 0x0054, 0x00000000 },
+	{ 0x0058, 0x00000000 },
+	{ 0x005C, 0x00000000 },
+	{ 0x0060, 0x0f2aa02a },
+	{ 0x0064, 0x003f3005 },
+	{ 0x0068, 0x02020202 },
+	{ 0x0070, 0x00000000 },
+	{ 0x0074, 0x00000000 },
+	{ 0x0078, 0x00000000 },
+	{ 0x007C, 0x00000000 },
+	{ 0x0034, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x002C, 0x00000942 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000005 },
+	{ 0x0028, 0x00000007 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0028, 0x00000001 },
+	{ 0x000C, 0x00005a08 },
+	{ 0x002C, 0x00000842 },
+	{ 0x0028, 0x00000001 },
+	{ 0x0030, 0x000003c0 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000003 },
+	{ 0x000C, 0x00005a21 },
+	{ 0x0034, 0x00007c03 },
+	{ 0x0120, 0x00005061 },
+	{ 0xffff, 0xffffffff },
+};
+
+#endif
diff --git a/drivers/gpu/drm/ast/ast_drv.c b/drivers/gpu/drm/ast/ast_drv.c
new file mode 100644
index 000000000000..d0c4574ef49c
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_drv.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/module.h>
+#include <linux/console.h>
+
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include "ast_drv.h"
+
+int ast_modeset = -1;
+
+MODULE_PARM_DESC(modeset, "Disable/Enable modesetting");
+module_param_named(modeset, ast_modeset, int, 0400);
+
+#define PCI_VENDOR_ASPEED 0x1a03
+
+static struct drm_driver driver;
+
+#define AST_VGA_DEVICE(id, info) {		\
+	.class = PCI_BASE_CLASS_DISPLAY << 16,	\
+	.class_mask = 0xff0000,			\
+	.vendor = PCI_VENDOR_ASPEED,			\
+	.device = id,				\
+	.subvendor = PCI_ANY_ID,		\
+	.subdevice = PCI_ANY_ID,		\
+	.driver_data = (unsigned long) info }
+
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	AST_VGA_DEVICE(PCI_CHIP_AST2000, NULL),
+	AST_VGA_DEVICE(PCI_CHIP_AST2100, NULL),
+	/*	AST_VGA_DEVICE(PCI_CHIP_AST1180, NULL), - don't bind to 1180 for now */
+	{0, 0, 0},
+};
+
+MODULE_DEVICE_TABLE(pci, pciidlist);
+
+static int __devinit
+ast_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static void
+ast_pci_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	drm_put_dev(dev);
+}
+
+
+
+static int ast_drm_freeze(struct drm_device *dev)
+{
+	drm_kms_helper_poll_disable(dev);
+
+	pci_save_state(dev->pdev);
+
+	console_lock();
+	ast_fbdev_set_suspend(dev, 1);
+	console_unlock();
+	return 0;
+}
+
+static int ast_drm_thaw(struct drm_device *dev)
+{
+	int error = 0;
+
+	ast_post_gpu(dev);
+
+	drm_mode_config_reset(dev);
+	mutex_lock(&dev->mode_config.mutex);
+	drm_helper_resume_force_mode(dev);
+	mutex_unlock(&dev->mode_config.mutex);
+
+	console_lock();
+	ast_fbdev_set_suspend(dev, 0);
+	console_unlock();
+	return error;
+}
+
+static int ast_drm_resume(struct drm_device *dev)
+{
+	int ret;
+
+	if (pci_enable_device(dev->pdev))
+		return -EIO;
+
+	ret = ast_drm_thaw(dev);
+	if (ret)
+		return ret;
+
+	drm_kms_helper_poll_enable(dev);
+	return 0;
+}
+
+static int ast_pm_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+	int error;
+
+	error = ast_drm_freeze(ddev);
+	if (error)
+		return error;
+
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, PCI_D3hot);
+	return 0;
+}
+static int ast_pm_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+	return ast_drm_resume(ddev);
+}
+
+static int ast_pm_freeze(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+
+	if (!ddev || !ddev->dev_private)
+		return -ENODEV;
+	return ast_drm_freeze(ddev);
+
+}
+
+static int ast_pm_thaw(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+	return ast_drm_thaw(ddev);
+}
+
+static int ast_pm_poweroff(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+
+	return ast_drm_freeze(ddev);
+}
+
+static const struct dev_pm_ops ast_pm_ops = {
+	.suspend = ast_pm_suspend,
+	.resume = ast_pm_resume,
+	.freeze = ast_pm_freeze,
+	.thaw = ast_pm_thaw,
+	.poweroff = ast_pm_poweroff,
+	.restore = ast_pm_resume,
+};
+
+static struct pci_driver ast_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = ast_pci_probe,
+	.remove = ast_pci_remove,
+	.driver.pm = &ast_pm_ops,
+};
+
+static const struct file_operations ast_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = ast_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+};
+
+static struct drm_driver driver = {
+	.driver_features = DRIVER_USE_MTRR | DRIVER_MODESET | DRIVER_GEM,
+	.dev_priv_size = 0,
+
+	.load = ast_driver_load,
+	.unload = ast_driver_unload,
+
+	.fops = &ast_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+
+	.gem_init_object = ast_gem_init_object,
+	.gem_free_object = ast_gem_free_object,
+	.dumb_create = ast_dumb_create,
+	.dumb_map_offset = ast_dumb_mmap_offset,
+	.dumb_destroy = ast_dumb_destroy,
+
+};
+
+static int __init ast_init(void)
+{
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force() && ast_modeset == -1)
+		return -EINVAL;
+#endif
+
+	if (ast_modeset == 0)
+		return -EINVAL;
+	return drm_pci_init(&driver, &ast_pci_driver);
+}
+static void __exit ast_exit(void)
+{
+	drm_pci_exit(&driver, &ast_pci_driver);
+}
+
+module_init(ast_init);
+module_exit(ast_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL and additional rights");
+
diff --git a/drivers/gpu/drm/ast/ast_drv.h b/drivers/gpu/drm/ast/ast_drv.h
new file mode 100644
index 000000000000..d4af9edcbb97
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_drv.h
@@ -0,0 +1,356 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#ifndef __AST_DRV_H__
+#define __AST_DRV_H__
+
+#include "drm_fb_helper.h"
+
+#include "ttm/ttm_bo_api.h"
+#include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_placement.h"
+#include "ttm/ttm_memory.h"
+#include "ttm/ttm_module.h"
+
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
+#define DRIVER_AUTHOR		"Dave Airlie"
+
+#define DRIVER_NAME		"ast"
+#define DRIVER_DESC		"AST"
+#define DRIVER_DATE		"20120228"
+
+#define DRIVER_MAJOR		0
+#define DRIVER_MINOR		1
+#define DRIVER_PATCHLEVEL	0
+
+#define PCI_CHIP_AST2000 0x2000
+#define PCI_CHIP_AST2100 0x2010
+#define PCI_CHIP_AST1180 0x1180
+
+
+enum ast_chip {
+	AST2000,
+	AST2100,
+	AST1100,
+	AST2200,
+	AST2150,
+	AST2300,
+	AST1180,
+};
+
+#define AST_DRAM_512Mx16 0
+#define AST_DRAM_1Gx16   1
+#define AST_DRAM_512Mx32 2
+#define AST_DRAM_1Gx32   3
+#define AST_DRAM_2Gx16   6
+#define AST_DRAM_4Gx16   7
+
+struct ast_fbdev;
+
+struct ast_private {
+	struct drm_device *dev;
+
+	void __iomem *regs;
+	void __iomem *ioregs;
+
+	enum ast_chip chip;
+	bool vga2_clone;
+	uint32_t dram_bus_width;
+	uint32_t dram_type;
+	uint32_t mclk;
+	uint32_t vram_size;
+
+	struct ast_fbdev *fbdev;
+
+	int fb_mtrr;
+
+	struct {
+		struct drm_global_reference mem_global_ref;
+		struct ttm_bo_global_ref bo_global_ref;
+		struct ttm_bo_device bdev;
+		atomic_t validate_sequence;
+	} ttm;
+
+	struct drm_gem_object *cursor_cache;
+	uint64_t cursor_cache_gpu_addr;
+	struct ttm_bo_kmap_obj cache_kmap;
+	int next_cursor;
+};
+
+int ast_driver_load(struct drm_device *dev, unsigned long flags);
+int ast_driver_unload(struct drm_device *dev);
+
+struct ast_gem_object;
+
+#define AST_IO_AR_PORT_WRITE		(0x40)
+#define AST_IO_MISC_PORT_WRITE		(0x42)
+#define AST_IO_SEQ_PORT			(0x44)
+#define AST_DAC_INDEX_READ		(0x3c7)
+#define AST_IO_DAC_INDEX_WRITE		(0x48)
+#define AST_IO_DAC_DATA		        (0x49)
+#define AST_IO_GR_PORT			(0x4E)
+#define AST_IO_CRTC_PORT		(0x54)
+#define AST_IO_INPUT_STATUS1_READ	(0x5A)
+#define AST_IO_MISC_PORT_READ		(0x4C)
+
+#define __ast_read(x) \
+static inline u##x ast_read##x(struct ast_private *ast, u32 reg) { \
+u##x val = 0;\
+val = ioread##x(ast->regs + reg); \
+return val;\
+}
+
+__ast_read(8);
+__ast_read(16);
+__ast_read(32)
+
+#define __ast_io_read(x) \
+static inline u##x ast_io_read##x(struct ast_private *ast, u32 reg) { \
+u##x val = 0;\
+val = ioread##x(ast->ioregs + reg); \
+return val;\
+}
+
+__ast_io_read(8);
+__ast_io_read(16);
+__ast_io_read(32);
+
+#define __ast_write(x) \
+static inline void ast_write##x(struct ast_private *ast, u32 reg, u##x val) {\
+	iowrite##x(val, ast->regs + reg);\
+	}
+
+__ast_write(8);
+__ast_write(16);
+__ast_write(32);
+
+#define __ast_io_write(x) \
+static inline void ast_io_write##x(struct ast_private *ast, u32 reg, u##x val) {\
+	iowrite##x(val, ast->ioregs + reg);\
+	}
+
+__ast_io_write(8);
+__ast_io_write(16);
+#undef __ast_io_write
+
+static inline void ast_set_index_reg(struct ast_private *ast,
+				     uint32_t base, uint8_t index,
+				     uint8_t val)
+{
+	ast_io_write16(ast, base, ((u16)val << 8) | index);
+}
+
+void ast_set_index_reg_mask(struct ast_private *ast,
+			    uint32_t base, uint8_t index,
+			    uint8_t mask, uint8_t val);
+uint8_t ast_get_index_reg(struct ast_private *ast,
+			  uint32_t base, uint8_t index);
+uint8_t ast_get_index_reg_mask(struct ast_private *ast,
+			       uint32_t base, uint8_t index, uint8_t mask);
+
+static inline void ast_open_key(struct ast_private *ast)
+{
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xA1, 0xFF, 0x04);
+}
+
+#define AST_VIDMEM_SIZE_8M    0x00800000
+#define AST_VIDMEM_SIZE_16M   0x01000000
+#define AST_VIDMEM_SIZE_32M   0x02000000
+#define AST_VIDMEM_SIZE_64M   0x04000000
+#define AST_VIDMEM_SIZE_128M  0x08000000
+
+#define AST_VIDMEM_DEFAULT_SIZE AST_VIDMEM_SIZE_8M
+
+#define AST_MAX_HWC_WIDTH 64
+#define AST_MAX_HWC_HEIGHT 64
+
+#define AST_HWC_SIZE                (AST_MAX_HWC_WIDTH*AST_MAX_HWC_HEIGHT*2)
+#define AST_HWC_SIGNATURE_SIZE      32
+
+#define AST_DEFAULT_HWC_NUM 2
+/* define for signature structure */
+#define AST_HWC_SIGNATURE_CHECKSUM  0x00
+#define AST_HWC_SIGNATURE_SizeX     0x04
+#define AST_HWC_SIGNATURE_SizeY     0x08
+#define AST_HWC_SIGNATURE_X         0x0C
+#define AST_HWC_SIGNATURE_Y         0x10
+#define AST_HWC_SIGNATURE_HOTSPOTX  0x14
+#define AST_HWC_SIGNATURE_HOTSPOTY  0x18
+
+
+struct ast_i2c_chan {
+	struct i2c_adapter adapter;
+	struct drm_device *dev;
+	struct i2c_algo_bit_data bit;
+};
+
+struct ast_connector {
+	struct drm_connector base;
+	struct ast_i2c_chan *i2c;
+};
+
+struct ast_crtc {
+	struct drm_crtc base;
+	u8 lut_r[256], lut_g[256], lut_b[256];
+	struct drm_gem_object *cursor_bo;
+	uint64_t cursor_addr;
+	int cursor_width, cursor_height;
+	u8 offset_x, offset_y;
+};
+
+struct ast_encoder {
+	struct drm_encoder base;
+};
+
+struct ast_framebuffer {
+	struct drm_framebuffer base;
+	struct drm_gem_object *obj;
+};
+
+struct ast_fbdev {
+	struct drm_fb_helper helper;
+	struct ast_framebuffer afb;
+	struct list_head fbdev_list;
+	void *sysram;
+	int size;
+	struct ttm_bo_kmap_obj mapping;
+};
+
+#define to_ast_crtc(x) container_of(x, struct ast_crtc, base)
+#define to_ast_connector(x) container_of(x, struct ast_connector, base)
+#define to_ast_encoder(x) container_of(x, struct ast_encoder, base)
+#define to_ast_framebuffer(x) container_of(x, struct ast_framebuffer, base)
+
+struct ast_vbios_stdtable {
+	u8 misc;
+	u8 seq[4];
+	u8 crtc[25];
+	u8 ar[20];
+	u8 gr[9];
+};
+
+struct ast_vbios_enhtable {
+	u32 ht;
+	u32 hde;
+	u32 hfp;
+	u32 hsync;
+	u32 vt;
+	u32 vde;
+	u32 vfp;
+	u32 vsync;
+	u32 dclk_index;
+	u32 flags;
+	u32 refresh_rate;
+	u32 refresh_rate_index;
+	u32 mode_id;
+};
+
+struct ast_vbios_dclk_info {
+	u8 param1;
+	u8 param2;
+	u8 param3;
+};
+
+struct ast_vbios_mode_info {
+	struct ast_vbios_stdtable *std_table;
+	struct ast_vbios_enhtable *enh_table;
+};
+
+extern int ast_mode_init(struct drm_device *dev);
+extern void ast_mode_fini(struct drm_device *dev);
+
+int ast_framebuffer_init(struct drm_device *dev,
+			 struct ast_framebuffer *ast_fb,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 struct drm_gem_object *obj);
+
+int ast_fbdev_init(struct drm_device *dev);
+void ast_fbdev_fini(struct drm_device *dev);
+void ast_fbdev_set_suspend(struct drm_device *dev, int state);
+
+struct ast_bo {
+	struct ttm_buffer_object bo;
+	struct ttm_placement placement;
+	struct ttm_bo_kmap_obj kmap;
+	struct drm_gem_object gem;
+	u32 placements[3];
+	int pin_count;
+};
+#define gem_to_ast_bo(gobj) container_of((gobj), struct ast_bo, gem)
+
+static inline struct ast_bo *
+ast_bo(struct ttm_buffer_object *bo)
+{
+	return container_of(bo, struct ast_bo, bo);
+}
+
+
+#define to_ast_obj(x) container_of(x, struct ast_gem_object, base)
+
+#define AST_MM_ALIGN_SHIFT 4
+#define AST_MM_ALIGN_MASK ((1 << AST_MM_ALIGN_SHIFT) - 1)
+
+extern int ast_dumb_create(struct drm_file *file,
+			   struct drm_device *dev,
+			   struct drm_mode_create_dumb *args);
+extern int ast_dumb_destroy(struct drm_file *file,
+			    struct drm_device *dev,
+			    uint32_t handle);
+
+extern int ast_gem_init_object(struct drm_gem_object *obj);
+extern void ast_gem_free_object(struct drm_gem_object *obj);
+extern int ast_dumb_mmap_offset(struct drm_file *file,
+				struct drm_device *dev,
+				uint32_t handle,
+				uint64_t *offset);
+
+#define DRM_FILE_PAGE_OFFSET (0x100000000ULL >> PAGE_SHIFT)
+
+int ast_mm_init(struct ast_private *ast);
+void ast_mm_fini(struct ast_private *ast);
+
+int ast_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct ast_bo **pastbo);
+
+int ast_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj);
+
+int ast_bo_pin(struct ast_bo *bo, u32 pl_flag, u64 *gpu_addr);
+int ast_bo_unpin(struct ast_bo *bo);
+
+int ast_bo_reserve(struct ast_bo *bo, bool no_wait);
+void ast_bo_unreserve(struct ast_bo *bo);
+void ast_ttm_placement(struct ast_bo *bo, int domain);
+int ast_bo_push_sysram(struct ast_bo *bo);
+int ast_mmap(struct file *filp, struct vm_area_struct *vma);
+
+/* ast post */
+void ast_post_gpu(struct drm_device *dev);
+#endif
diff --git a/drivers/gpu/drm/ast/ast_fb.c b/drivers/gpu/drm/ast/ast_fb.c
new file mode 100644
index 000000000000..2fc8e9e860b1
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_fb.c
@@ -0,0 +1,341 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/tty.h>
+#include <linux/sysrq.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+
+
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc.h"
+#include "drm_fb_helper.h"
+#include "ast_drv.h"
+
+static void ast_dirty_update(struct ast_fbdev *afbdev,
+			     int x, int y, int width, int height)
+{
+	int i;
+	struct drm_gem_object *obj;
+	struct ast_bo *bo;
+	int src_offset, dst_offset;
+	int bpp = (afbdev->afb.base.bits_per_pixel + 7)/8;
+	int ret;
+	bool unmap = false;
+
+	obj = afbdev->afb.obj;
+	bo = gem_to_ast_bo(obj);
+
+	ret = ast_bo_reserve(bo, true);
+	if (ret) {
+		DRM_ERROR("failed to reserve fb bo\n");
+		return;
+	}
+
+	if (!bo->kmap.virtual) {
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret) {
+			DRM_ERROR("failed to kmap fb updates\n");
+			ast_bo_unreserve(bo);
+			return;
+		}
+		unmap = true;
+	}
+	for (i = y; i < y + height; i++) {
+		/* assume equal stride for now */
+		src_offset = dst_offset = i * afbdev->afb.base.pitches[0] + (x * bpp);
+		memcpy_toio(bo->kmap.virtual + src_offset, afbdev->sysram + src_offset, width * bpp);
+
+	}
+	if (unmap)
+		ttm_bo_kunmap(&bo->kmap);
+
+	ast_bo_unreserve(bo);
+}
+
+static void ast_fillrect(struct fb_info *info,
+			 const struct fb_fillrect *rect)
+{
+	struct ast_fbdev *afbdev = info->par;
+	sys_fillrect(info, rect);
+	ast_dirty_update(afbdev, rect->dx, rect->dy, rect->width,
+			 rect->height);
+}
+
+static void ast_copyarea(struct fb_info *info,
+			 const struct fb_copyarea *area)
+{
+	struct ast_fbdev *afbdev = info->par;
+	sys_copyarea(info, area);
+	ast_dirty_update(afbdev, area->dx, area->dy, area->width,
+			 area->height);
+}
+
+static void ast_imageblit(struct fb_info *info,
+			  const struct fb_image *image)
+{
+	struct ast_fbdev *afbdev = info->par;
+	sys_imageblit(info, image);
+	ast_dirty_update(afbdev, image->dx, image->dy, image->width,
+			 image->height);
+}
+
+static struct fb_ops astfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_fillrect = ast_fillrect,
+	.fb_copyarea = ast_copyarea,
+	.fb_imageblit = ast_imageblit,
+	.fb_pan_display = drm_fb_helper_pan_display,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcmap = drm_fb_helper_setcmap,
+	.fb_debug_enter = drm_fb_helper_debug_enter,
+	.fb_debug_leave = drm_fb_helper_debug_leave,
+};
+
+static int astfb_create_object(struct ast_fbdev *afbdev,
+			       struct drm_mode_fb_cmd2 *mode_cmd,
+			       struct drm_gem_object **gobj_p)
+{
+	struct drm_device *dev = afbdev->helper.dev;
+	u32 bpp, depth;
+	u32 size;
+	struct drm_gem_object *gobj;
+
+	int ret = 0;
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
+
+	size = mode_cmd->pitches[0] * mode_cmd->height;
+	ret = ast_gem_create(dev, size, true, &gobj);
+	if (ret)
+		return ret;
+
+	*gobj_p = gobj;
+	return ret;
+}
+
+static int astfb_create(struct ast_fbdev *afbdev,
+			struct drm_fb_helper_surface_size *sizes)
+{
+	struct drm_device *dev = afbdev->helper.dev;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct drm_framebuffer *fb;
+	struct fb_info *info;
+	int size, ret;
+	struct device *device = &dev->pdev->dev;
+	void *sysram;
+	struct drm_gem_object *gobj = NULL;
+	struct ast_bo *bo = NULL;
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	mode_cmd.pitches[0] = mode_cmd.width * ((sizes->surface_bpp + 7)/8);
+
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	ret = astfb_create_object(afbdev, &mode_cmd, &gobj);
+	if (ret) {
+		DRM_ERROR("failed to create fbcon backing object %d\n", ret);
+		return ret;
+	}
+	bo = gem_to_ast_bo(gobj);
+
+	sysram = vmalloc(size);
+	if (!sysram)
+		return -ENOMEM;
+
+	info = framebuffer_alloc(0, device);
+	if (!info) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	info->par = afbdev;
+
+	ret = ast_framebuffer_init(dev, &afbdev->afb, &mode_cmd, gobj);
+	if (ret)
+		goto out;
+
+	afbdev->sysram = sysram;
+	afbdev->size = size;
+
+	fb = &afbdev->afb.base;
+	afbdev->helper.fb = fb;
+	afbdev->helper.fbdev = info;
+
+	strcpy(info->fix.id, "astdrmfb");
+
+	info->flags = FBINFO_DEFAULT | FBINFO_CAN_FORCE_OUTPUT;
+	info->fbops = &astfb_ops;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	info->apertures = alloc_apertures(1);
+	if (!info->apertures) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	info->apertures->ranges[0].base = pci_resource_start(dev->pdev, 0);
+	info->apertures->ranges[0].size = pci_resource_len(dev->pdev, 0);
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, &afbdev->helper, sizes->fb_width, sizes->fb_height);
+
+	info->screen_base = sysram;
+	info->screen_size = size;
+
+	info->pixmap.flags = FB_PIXMAP_SYSTEM;
+
+	DRM_DEBUG_KMS("allocated %dx%d\n",
+		      fb->width, fb->height);
+
+	return 0;
+out:
+	return ret;
+}
+
+static void ast_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			       u16 blue, int regno)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	ast_crtc->lut_r[regno] = red >> 8;
+	ast_crtc->lut_g[regno] = green >> 8;
+	ast_crtc->lut_b[regno] = blue >> 8;
+}
+
+static void ast_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			       u16 *blue, int regno)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	*red = ast_crtc->lut_r[regno] << 8;
+	*green = ast_crtc->lut_g[regno] << 8;
+	*blue = ast_crtc->lut_b[regno] << 8;
+}
+
+static int ast_find_or_create_single(struct drm_fb_helper *helper,
+					  struct drm_fb_helper_surface_size *sizes)
+{
+	struct ast_fbdev *afbdev = (struct ast_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = astfb_create(afbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static struct drm_fb_helper_funcs ast_fb_helper_funcs = {
+	.gamma_set = ast_fb_gamma_set,
+	.gamma_get = ast_fb_gamma_get,
+	.fb_probe = ast_find_or_create_single,
+};
+
+static void ast_fbdev_destroy(struct drm_device *dev,
+			      struct ast_fbdev *afbdev)
+{
+	struct fb_info *info;
+	struct ast_framebuffer *afb = &afbdev->afb;
+	if (afbdev->helper.fbdev) {
+		info = afbdev->helper.fbdev;
+		unregister_framebuffer(info);
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+
+	if (afb->obj) {
+		drm_gem_object_unreference_unlocked(afb->obj);
+		afb->obj = NULL;
+	}
+	drm_fb_helper_fini(&afbdev->helper);
+
+	vfree(afbdev->sysram);
+	drm_framebuffer_cleanup(&afb->base);
+}
+
+int ast_fbdev_init(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast_fbdev *afbdev;
+	int ret;
+
+	afbdev = kzalloc(sizeof(struct ast_fbdev), GFP_KERNEL);
+	if (!afbdev)
+		return -ENOMEM;
+
+	ast->fbdev = afbdev;
+	afbdev->helper.funcs = &ast_fb_helper_funcs;
+	ret = drm_fb_helper_init(dev, &afbdev->helper,
+				 1, 1);
+	if (ret) {
+		kfree(afbdev);
+		return ret;
+	}
+
+	drm_fb_helper_single_add_all_connectors(&afbdev->helper);
+	drm_fb_helper_initial_config(&afbdev->helper, 32);
+	return 0;
+}
+
+void ast_fbdev_fini(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	if (!ast->fbdev)
+		return;
+
+	ast_fbdev_destroy(dev, ast->fbdev);
+	kfree(ast->fbdev);
+	ast->fbdev = NULL;
+}
+
+void ast_fbdev_set_suspend(struct drm_device *dev, int state)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	if (!ast->fbdev)
+		return;
+
+	fb_set_suspend(ast->fbdev->helper.fbdev, state);
+}
diff --git a/drivers/gpu/drm/ast/ast_main.c b/drivers/gpu/drm/ast/ast_main.c
new file mode 100644
index 000000000000..95ae55b8214b
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_main.c
@@ -0,0 +1,527 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "ast_drv.h"
+
+
+#include "drm_fb_helper.h"
+#include "drm_crtc_helper.h"
+
+#include "ast_dram_tables.h"
+
+void ast_set_index_reg_mask(struct ast_private *ast,
+			    uint32_t base, uint8_t index,
+			    uint8_t mask, uint8_t val)
+{
+	u8 tmp;
+	ast_io_write8(ast, base, index);
+	tmp = (ast_io_read8(ast, base + 1) & mask) | val;
+	ast_set_index_reg(ast, base, index, tmp);
+}
+
+uint8_t ast_get_index_reg(struct ast_private *ast,
+			  uint32_t base, uint8_t index)
+{
+	uint8_t ret;
+	ast_io_write8(ast, base, index);
+	ret = ast_io_read8(ast, base + 1);
+	return ret;
+}
+
+uint8_t ast_get_index_reg_mask(struct ast_private *ast,
+			       uint32_t base, uint8_t index, uint8_t mask)
+{
+	uint8_t ret;
+	ast_io_write8(ast, base, index);
+	ret = ast_io_read8(ast, base + 1) & mask;
+	return ret;
+}
+
+
+static int ast_detect_chip(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	if (dev->pdev->device == PCI_CHIP_AST1180) {
+		ast->chip = AST1100;
+		DRM_INFO("AST 1180 detected\n");
+	} else {
+		if (dev->pdev->revision >= 0x20) {
+			ast->chip = AST2300;
+			DRM_INFO("AST 2300 detected\n");
+		} else if (dev->pdev->revision >= 0x10) {
+			uint32_t data;
+			ast_write32(ast, 0xf004, 0x1e6e0000);
+			ast_write32(ast, 0xf000, 0x1);
+
+			data = ast_read32(ast, 0x1207c);
+			switch (data & 0x0300) {
+			case 0x0200:
+				ast->chip = AST1100;
+				DRM_INFO("AST 1100 detected\n");
+				break;
+			case 0x0100:
+				ast->chip = AST2200;
+				DRM_INFO("AST 2200 detected\n");
+				break;
+			case 0x0000:
+				ast->chip = AST2150;
+				DRM_INFO("AST 2150 detected\n");
+				break;
+			default:
+				ast->chip = AST2100;
+				DRM_INFO("AST 2100 detected\n");
+				break;
+			}
+			ast->vga2_clone = false;
+		} else {
+			ast->chip = 2000;
+			DRM_INFO("AST 2000 detected\n");
+		}
+	}
+	return 0;
+}
+
+static int ast_get_dram_info(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	uint32_t data, data2;
+	uint32_t denum, num, div, ref_pll;
+
+	ast_write32(ast, 0xf004, 0x1e6e0000);
+	ast_write32(ast, 0xf000, 0x1);
+
+
+	ast_write32(ast, 0x10000, 0xfc600309);
+
+	do {
+		;
+	} while (ast_read32(ast, 0x10000) != 0x01);
+	data = ast_read32(ast, 0x10004);
+
+	if (data & 0x400)
+		ast->dram_bus_width = 16;
+	else
+		ast->dram_bus_width = 32;
+
+	if (ast->chip == AST2300) {
+		switch (data & 0x03) {
+		case 0:
+			ast->dram_type = AST_DRAM_512Mx16;
+			break;
+		default:
+		case 1:
+			ast->dram_type = AST_DRAM_1Gx16;
+			break;
+		case 2:
+			ast->dram_type = AST_DRAM_2Gx16;
+			break;
+		case 3:
+			ast->dram_type = AST_DRAM_4Gx16;
+			break;
+		}
+	} else {
+		switch (data & 0x0c) {
+		case 0:
+		case 4:
+			ast->dram_type = AST_DRAM_512Mx16;
+			break;
+		case 8:
+			if (data & 0x40)
+				ast->dram_type = AST_DRAM_1Gx16;
+			else
+				ast->dram_type = AST_DRAM_512Mx32;
+			break;
+		case 0xc:
+			ast->dram_type = AST_DRAM_1Gx32;
+			break;
+		}
+	}
+
+	data = ast_read32(ast, 0x10120);
+	data2 = ast_read32(ast, 0x10170);
+	if (data2 & 0x2000)
+		ref_pll = 14318;
+	else
+		ref_pll = 12000;
+
+	denum = data & 0x1f;
+	num = (data & 0x3fe0) >> 5;
+	data = (data & 0xc000) >> 14;
+	switch (data) {
+	case 3:
+		div = 0x4;
+		break;
+	case 2:
+	case 1:
+		div = 0x2;
+		break;
+	default:
+		div = 0x1;
+		break;
+	}
+	ast->mclk = ref_pll * (num + 2) / (denum + 2) * (div * 1000);
+	return 0;
+}
+
+uint32_t ast_get_max_dclk(struct drm_device *dev, int bpp)
+{
+	struct ast_private *ast = dev->dev_private;
+	uint32_t dclk, jreg;
+	uint32_t dram_bus_width, mclk, dram_bandwidth, actual_dram_bandwidth, dram_efficency = 500;
+
+	dram_bus_width = ast->dram_bus_width;
+	mclk = ast->mclk;
+
+	if (ast->chip == AST2100 ||
+	    ast->chip == AST1100 ||
+	    ast->chip == AST2200 ||
+	    ast->chip == AST2150 ||
+	    ast->dram_bus_width == 16)
+		dram_efficency = 600;
+	else if (ast->chip == AST2300)
+		dram_efficency = 400;
+
+	dram_bandwidth = mclk * dram_bus_width * 2 / 8;
+	actual_dram_bandwidth = dram_bandwidth * dram_efficency / 1000;
+
+	if (ast->chip == AST1180)
+		dclk = actual_dram_bandwidth / ((bpp + 1) / 8);
+	else {
+		jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+		if ((jreg & 0x08) && (ast->chip == AST2000))
+			dclk = actual_dram_bandwidth / ((bpp + 1 + 16) / 8);
+		else if ((jreg & 0x08) && (bpp == 8))
+			dclk = actual_dram_bandwidth / ((bpp + 1 + 24) / 8);
+		else
+			dclk = actual_dram_bandwidth / ((bpp + 1) / 8);
+	}
+
+	if (ast->chip == AST2100 ||
+	    ast->chip == AST2200 ||
+	    ast->chip == AST2300 ||
+	    ast->chip == AST1180) {
+		if (dclk > 200)
+			dclk = 200;
+	} else {
+		if (dclk > 165)
+			dclk = 165;
+	}
+
+	return dclk;
+}
+
+static void ast_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct ast_framebuffer *ast_fb = to_ast_framebuffer(fb);
+	if (ast_fb->obj)
+		drm_gem_object_unreference_unlocked(ast_fb->obj);
+
+	drm_framebuffer_cleanup(fb);
+	kfree(fb);
+}
+
+static int ast_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+					      struct drm_file *file,
+					      unsigned int *handle)
+{
+	return -EINVAL;
+}
+
+static const struct drm_framebuffer_funcs ast_fb_funcs = {
+	.destroy = ast_user_framebuffer_destroy,
+	.create_handle = ast_user_framebuffer_create_handle,
+};
+
+
+int ast_framebuffer_init(struct drm_device *dev,
+			 struct ast_framebuffer *ast_fb,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 struct drm_gem_object *obj)
+{
+	int ret;
+
+	ret = drm_framebuffer_init(dev, &ast_fb->base, &ast_fb_funcs);
+	if (ret) {
+		DRM_ERROR("framebuffer init failed %d\n", ret);
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&ast_fb->base, mode_cmd);
+	ast_fb->obj = obj;
+	return 0;
+}
+
+static struct drm_framebuffer *
+ast_user_framebuffer_create(struct drm_device *dev,
+	       struct drm_file *filp,
+	       struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct ast_framebuffer *ast_fb;
+	int ret;
+
+	obj = drm_gem_object_lookup(dev, filp, mode_cmd->handles[0]);
+	if (obj == NULL)
+		return ERR_PTR(-ENOENT);
+
+	ast_fb = kzalloc(sizeof(*ast_fb), GFP_KERNEL);
+	if (!ast_fb) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	ret = ast_framebuffer_init(dev, ast_fb, mode_cmd, obj);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(obj);
+		kfree(ast_fb);
+		return ERR_PTR(ret);
+	}
+	return &ast_fb->base;
+}
+
+static const struct drm_mode_config_funcs ast_mode_funcs = {
+	.fb_create = ast_user_framebuffer_create,
+};
+
+static u32 ast_get_vram_info(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 jreg;
+
+	ast_open_key(ast);
+
+	jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xaa, 0xff);
+	switch (jreg & 3) {
+	case 0: return AST_VIDMEM_SIZE_8M;
+	case 1: return AST_VIDMEM_SIZE_16M;
+	case 2: return AST_VIDMEM_SIZE_32M;
+	case 3: return AST_VIDMEM_SIZE_64M;
+	}
+	return AST_VIDMEM_DEFAULT_SIZE;
+}
+
+int ast_driver_load(struct drm_device *dev, unsigned long flags)
+{
+	struct ast_private *ast;
+	int ret = 0;
+
+	ast = kzalloc(sizeof(struct ast_private), GFP_KERNEL);
+	if (!ast)
+		return -ENOMEM;
+
+	dev->dev_private = ast;
+	ast->dev = dev;
+
+	ast->regs = pci_iomap(dev->pdev, 1, 0);
+	if (!ast->regs) {
+		ret = -EIO;
+		goto out_free;
+	}
+	ast->ioregs = pci_iomap(dev->pdev, 2, 0);
+	if (!ast->ioregs) {
+		ret = -EIO;
+		goto out_free;
+	}
+
+	ast_detect_chip(dev);
+
+	if (ast->chip != AST1180) {
+		ast_get_dram_info(dev);
+		ast->vram_size = ast_get_vram_info(dev);
+		DRM_INFO("dram %d %d %d %08x\n", ast->mclk, ast->dram_type, ast->dram_bus_width, ast->vram_size);
+	}
+
+	ret = ast_mm_init(ast);
+	if (ret)
+		goto out_free;
+
+	drm_mode_config_init(dev);
+
+	dev->mode_config.funcs = (void *)&ast_mode_funcs;
+	dev->mode_config.min_width = 0;
+	dev->mode_config.min_height = 0;
+	dev->mode_config.preferred_depth = 24;
+	dev->mode_config.prefer_shadow = 1;
+
+	if (ast->chip == AST2100 ||
+	    ast->chip == AST2200 ||
+	    ast->chip == AST2300 ||
+	    ast->chip == AST1180) {
+		dev->mode_config.max_width = 1920;
+		dev->mode_config.max_height = 2048;
+	} else {
+		dev->mode_config.max_width = 1600;
+		dev->mode_config.max_height = 1200;
+	}
+
+	ret = ast_mode_init(dev);
+	if (ret)
+		goto out_free;
+
+	ret = ast_fbdev_init(dev);
+	if (ret)
+		goto out_free;
+
+	return 0;
+out_free:
+	kfree(ast);
+	dev->dev_private = NULL;
+	return ret;
+}
+
+int ast_driver_unload(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	ast_mode_fini(dev);
+	ast_fbdev_fini(dev);
+	drm_mode_config_cleanup(dev);
+
+	ast_mm_fini(ast);
+	pci_iounmap(dev->pdev, ast->ioregs);
+	pci_iounmap(dev->pdev, ast->regs);
+	kfree(ast);
+	return 0;
+}
+
+int ast_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj)
+{
+	struct ast_bo *astbo;
+	int ret;
+
+	*obj = NULL;
+
+	size = roundup(size, PAGE_SIZE);
+	if (size == 0)
+		return -EINVAL;
+
+	ret = ast_bo_create(dev, size, 0, 0, &astbo);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("failed to allocate GEM object\n");
+		return ret;
+	}
+	*obj = &astbo->gem;
+	return 0;
+}
+
+int ast_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		    struct drm_mode_create_dumb *args)
+{
+	int ret;
+	struct drm_gem_object *gobj;
+	u32 handle;
+
+	args->pitch = args->width * ((args->bpp + 7) / 8);
+	args->size = args->pitch * args->height;
+
+	ret = ast_gem_create(dev, args->size, false,
+			     &gobj);
+	if (ret)
+		return ret;
+
+	ret = drm_gem_handle_create(file, gobj, &handle);
+	drm_gem_object_unreference_unlocked(gobj);
+	if (ret)
+		return ret;
+
+	args->handle = handle;
+	return 0;
+}
+
+int ast_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
+
+int ast_gem_init_object(struct drm_gem_object *obj)
+{
+	BUG();
+	return 0;
+}
+
+void ast_bo_unref(struct ast_bo **bo)
+{
+	struct ttm_buffer_object *tbo;
+
+	if ((*bo) == NULL)
+		return;
+
+	tbo = &((*bo)->bo);
+	ttm_bo_unref(&tbo);
+	if (tbo == NULL)
+		*bo = NULL;
+
+}
+void ast_gem_free_object(struct drm_gem_object *obj)
+{
+	struct ast_bo *ast_bo = gem_to_ast_bo(obj);
+
+	if (!ast_bo)
+		return;
+	ast_bo_unref(&ast_bo);
+}
+
+
+static inline u64 ast_bo_mmap_offset(struct ast_bo *bo)
+{
+	return bo->bo.addr_space_offset;
+}
+int
+ast_dumb_mmap_offset(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle,
+		     uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret;
+	struct ast_bo *bo;
+
+	mutex_lock(&dev->struct_mutex);
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto out_unlock;
+	}
+
+	bo = gem_to_ast_bo(obj);
+	*offset = ast_bo_mmap_offset(bo);
+
+	drm_gem_object_unreference(obj);
+	ret = 0;
+out_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+
+}
+
diff --git a/drivers/gpu/drm/ast/ast_mode.c b/drivers/gpu/drm/ast/ast_mode.c
new file mode 100644
index 000000000000..65f9d231af14
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_mode.c
@@ -0,0 +1,1160 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ * Parts based on xf86-video-ast
+ * Copyright (c) 2005 ASPEED Technology Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/export.h>
+#include "drmP.h"
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+#include "ast_drv.h"
+
+#include "ast_tables.h"
+
+static struct ast_i2c_chan *ast_i2c_create(struct drm_device *dev);
+static void ast_i2c_destroy(struct ast_i2c_chan *i2c);
+static int ast_cursor_set(struct drm_crtc *crtc,
+			  struct drm_file *file_priv,
+			  uint32_t handle,
+			  uint32_t width,
+			  uint32_t height);
+static int ast_cursor_move(struct drm_crtc *crtc,
+			   int x, int y);
+
+static inline void ast_load_palette_index(struct ast_private *ast,
+				     u8 index, u8 red, u8 green,
+				     u8 blue)
+{
+	ast_io_write8(ast, AST_IO_DAC_INDEX_WRITE, index);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+	ast_io_write8(ast, AST_IO_DAC_DATA, red);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+	ast_io_write8(ast, AST_IO_DAC_DATA, green);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+	ast_io_write8(ast, AST_IO_DAC_DATA, blue);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+}
+
+static void ast_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	int i;
+
+	if (!crtc->enabled)
+		return;
+
+	for (i = 0; i < 256; i++)
+		ast_load_palette_index(ast, i, ast_crtc->lut_r[i],
+				       ast_crtc->lut_g[i], ast_crtc->lut_b[i]);
+}
+
+static bool ast_get_vbios_mode_info(struct drm_crtc *crtc, struct drm_display_mode *mode,
+				    struct drm_display_mode *adjusted_mode,
+				    struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u32 refresh_rate_index = 0, mode_id, color_index, refresh_rate;
+	u32 hborder, vborder;
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		vbios_mode->std_table = &vbios_stdtable[VGAModeIndex];
+		color_index = VGAModeIndex - 1;
+		break;
+	case 16:
+		vbios_mode->std_table = &vbios_stdtable[HiCModeIndex];
+		color_index = HiCModeIndex;
+		break;
+	case 24:
+	case 32:
+		vbios_mode->std_table = &vbios_stdtable[TrueCModeIndex];
+		color_index = TrueCModeIndex;
+		break;
+	default:
+		return false;
+	}
+
+	switch (crtc->mode.crtc_hdisplay) {
+	case 640:
+		vbios_mode->enh_table = &res_640x480[refresh_rate_index];
+		break;
+	case 800:
+		vbios_mode->enh_table = &res_800x600[refresh_rate_index];
+		break;
+	case 1024:
+		vbios_mode->enh_table = &res_1024x768[refresh_rate_index];
+		break;
+	case 1280:
+		if (crtc->mode.crtc_vdisplay == 800)
+			vbios_mode->enh_table = &res_1280x800[refresh_rate_index];
+		else
+			vbios_mode->enh_table = &res_1280x1024[refresh_rate_index];
+		break;
+	case 1440:
+		vbios_mode->enh_table = &res_1440x900[refresh_rate_index];
+		break;
+	case 1600:
+		vbios_mode->enh_table = &res_1600x1200[refresh_rate_index];
+		break;
+	case 1680:
+		vbios_mode->enh_table = &res_1680x1050[refresh_rate_index];
+		break;
+	case 1920:
+		if (crtc->mode.crtc_vdisplay == 1080)
+			vbios_mode->enh_table = &res_1920x1080[refresh_rate_index];
+		else
+			vbios_mode->enh_table = &res_1920x1200[refresh_rate_index];
+		break;
+	default:
+		return false;
+	}
+
+	refresh_rate = drm_mode_vrefresh(mode);
+	while (vbios_mode->enh_table->refresh_rate < refresh_rate) {
+		vbios_mode->enh_table++;
+		if ((vbios_mode->enh_table->refresh_rate > refresh_rate) ||
+		    (vbios_mode->enh_table->refresh_rate == 0xff)) {
+			vbios_mode->enh_table--;
+			break;
+		}
+	}
+
+	hborder = (vbios_mode->enh_table->flags & HBorder) ? 8 : 0;
+	vborder = (vbios_mode->enh_table->flags & VBorder) ? 8 : 0;
+
+	adjusted_mode->crtc_htotal = vbios_mode->enh_table->ht;
+	adjusted_mode->crtc_hblank_start = vbios_mode->enh_table->hde + hborder;
+	adjusted_mode->crtc_hblank_end = vbios_mode->enh_table->ht - hborder;
+	adjusted_mode->crtc_hsync_start = vbios_mode->enh_table->hde + hborder +
+		vbios_mode->enh_table->hfp;
+	adjusted_mode->crtc_hsync_end = (vbios_mode->enh_table->hde + hborder +
+					 vbios_mode->enh_table->hfp +
+					 vbios_mode->enh_table->hsync);
+
+	adjusted_mode->crtc_vtotal = vbios_mode->enh_table->vt;
+	adjusted_mode->crtc_vblank_start = vbios_mode->enh_table->vde + vborder;
+	adjusted_mode->crtc_vblank_end = vbios_mode->enh_table->vt - vborder;
+	adjusted_mode->crtc_vsync_start = vbios_mode->enh_table->vde + vborder +
+		vbios_mode->enh_table->vfp;
+	adjusted_mode->crtc_vsync_end = (vbios_mode->enh_table->vde + vborder +
+					 vbios_mode->enh_table->vfp +
+					 vbios_mode->enh_table->vsync);
+
+	refresh_rate_index = vbios_mode->enh_table->refresh_rate_index;
+	mode_id = vbios_mode->enh_table->mode_id;
+
+	if (ast->chip == AST1180) {
+		/* TODO 1180 */
+	} else {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x8c, (u8)((color_index & 0xf) << 4));
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x8d, refresh_rate_index & 0xff);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x8e, mode_id & 0xff);
+
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x91, 0xa8);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x92, crtc->fb->bits_per_pixel);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x93, adjusted_mode->clock / 1000);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x94, adjusted_mode->crtc_hdisplay);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x95, adjusted_mode->crtc_hdisplay >> 8);
+
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x96, adjusted_mode->crtc_vdisplay);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x97, adjusted_mode->crtc_vdisplay >> 8);
+	}
+
+	return true;
+
+
+}
+static void ast_set_std_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+			    struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct ast_vbios_stdtable *stdtable;
+	u32 i;
+	u8 jreg;
+
+	stdtable = vbios_mode->std_table;
+
+	jreg = stdtable->misc;
+	ast_io_write8(ast, AST_IO_MISC_PORT_WRITE, jreg);
+
+	/* Set SEQ */
+	ast_set_index_reg(ast, AST_IO_SEQ_PORT, 0x00, 0x03);
+	for (i = 0; i < 4; i++) {
+		jreg = stdtable->seq[i];
+		if (!i)
+			jreg |= 0x20;
+		ast_set_index_reg(ast, AST_IO_SEQ_PORT, (i + 1) , jreg);
+	}
+
+	/* Set CRTC */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x7f, 0x00);
+	for (i = 0; i < 25; i++)
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, i, stdtable->crtc[i]);
+
+	/* set AR */
+	jreg = ast_io_read8(ast, AST_IO_INPUT_STATUS1_READ);
+	for (i = 0; i < 20; i++) {
+		jreg = stdtable->ar[i];
+		ast_io_write8(ast, AST_IO_AR_PORT_WRITE, (u8)i);
+		ast_io_write8(ast, AST_IO_AR_PORT_WRITE, jreg);
+	}
+	ast_io_write8(ast, AST_IO_AR_PORT_WRITE, 0x14);
+	ast_io_write8(ast, AST_IO_AR_PORT_WRITE, 0x00);
+
+	jreg = ast_io_read8(ast, AST_IO_INPUT_STATUS1_READ);
+	ast_io_write8(ast, AST_IO_AR_PORT_WRITE, 0x20);
+
+	/* Set GR */
+	for (i = 0; i < 9; i++)
+		ast_set_index_reg(ast, AST_IO_GR_PORT, i, stdtable->gr[i]);
+}
+
+static void ast_set_crtc_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+			     struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u8 jreg05 = 0, jreg07 = 0, jreg09 = 0, jregAC = 0, jregAD = 0, jregAE = 0;
+	u16 temp;
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x7f, 0x00);
+
+	temp = (mode->crtc_htotal >> 3) - 5;
+	if (temp & 0x100)
+		jregAC |= 0x01; /* HT D[8] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x00, 0x00, temp);
+
+	temp = (mode->crtc_hdisplay >> 3) - 1;
+	if (temp & 0x100)
+		jregAC |= 0x04; /* HDE D[8] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x01, 0x00, temp);
+
+	temp = (mode->crtc_hblank_start >> 3) - 1;
+	if (temp & 0x100)
+		jregAC |= 0x10; /* HBS D[8] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x02, 0x00, temp);
+
+	temp = ((mode->crtc_hblank_end >> 3) - 1) & 0x7f;
+	if (temp & 0x20)
+		jreg05 |= 0x80;  /* HBE D[5] */
+	if (temp & 0x40)
+		jregAD |= 0x01;  /* HBE D[5] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x03, 0xE0, (temp & 0x1f));
+
+	temp = (mode->crtc_hsync_start >> 3) - 1;
+	if (temp & 0x100)
+		jregAC |= 0x40; /* HRS D[5] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x04, 0x00, temp);
+
+	temp = ((mode->crtc_hsync_end >> 3) - 1) & 0x3f;
+	if (temp & 0x20)
+		jregAD |= 0x04; /* HRE D[5] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x05, 0x60, (u8)((temp & 0x1f) | jreg05));
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xAC, 0x00, jregAC);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xAD, 0x00, jregAD);
+
+	/* vert timings */
+	temp = (mode->crtc_vtotal) - 2;
+	if (temp & 0x100)
+		jreg07 |= 0x01;
+	if (temp & 0x200)
+		jreg07 |= 0x20;
+	if (temp & 0x400)
+		jregAE |= 0x01;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x06, 0x00, temp);
+
+	temp = (mode->crtc_vsync_start) - 1;
+	if (temp & 0x100)
+		jreg07 |= 0x04;
+	if (temp & 0x200)
+		jreg07 |= 0x80;
+	if (temp & 0x400)
+		jregAE |= 0x08;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x10, 0x00, temp);
+
+	temp = (mode->crtc_vsync_end - 1) & 0x3f;
+	if (temp & 0x10)
+		jregAE |= 0x20;
+	if (temp & 0x20)
+		jregAE |= 0x40;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x70, temp & 0xf);
+
+	temp = mode->crtc_vdisplay - 1;
+	if (temp & 0x100)
+		jreg07 |= 0x02;
+	if (temp & 0x200)
+		jreg07 |= 0x40;
+	if (temp & 0x400)
+		jregAE |= 0x02;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x12, 0x00, temp);
+
+	temp = mode->crtc_vblank_start - 1;
+	if (temp & 0x100)
+		jreg07 |= 0x08;
+	if (temp & 0x200)
+		jreg09 |= 0x20;
+	if (temp & 0x400)
+		jregAE |= 0x04;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x15, 0x00, temp);
+
+	temp = mode->crtc_vblank_end - 1;
+	if (temp & 0x100)
+		jregAE |= 0x10;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x16, 0x00, temp);
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x07, 0x00, jreg07);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x09, 0xdf, jreg09);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xAE, 0x00, (jregAE | 0x80));
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x7f, 0x80);
+}
+
+static void ast_set_offset_reg(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+
+	u16 offset;
+
+	offset = crtc->fb->pitches[0] >> 3;
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x13, (offset & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xb0, (offset >> 8) & 0x3f);
+}
+
+static void ast_set_dclk_reg(struct drm_device *dev, struct drm_display_mode *mode,
+			     struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast_vbios_dclk_info *clk_info;
+
+	clk_info = &dclk_table[vbios_mode->enh_table->dclk_index];
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xc0, 0x00, clk_info->param1);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xc1, 0x00, clk_info->param2);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xbb, 0x0f,
+			       (clk_info->param3 & 0x80) | ((clk_info->param3 & 0x3) << 4));
+}
+
+static void ast_set_ext_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+			     struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u8 jregA0 = 0, jregA3 = 0, jregA8 = 0;
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		jregA0 = 0x70;
+		jregA3 = 0x01;
+		jregA8 = 0x00;
+		break;
+	case 15:
+	case 16:
+		jregA0 = 0x70;
+		jregA3 = 0x04;
+		jregA8 = 0x02;
+		break;
+	case 32:
+		jregA0 = 0x70;
+		jregA3 = 0x08;
+		jregA8 = 0x02;
+		break;
+	}
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa0, 0x8f, jregA0);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa3, 0xf0, jregA3);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa8, 0xfd, jregA8);
+
+	/* Set Threshold */
+	if (ast->chip == AST2300) {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa7, 0x78);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa6, 0x60);
+	} else if (ast->chip == AST2100 ||
+		   ast->chip == AST1100 ||
+		   ast->chip == AST2200 ||
+		   ast->chip == AST2150) {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa7, 0x3f);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa6, 0x2f);
+	} else {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa7, 0x2f);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa6, 0x1f);
+	}
+}
+
+void ast_set_sync_reg(struct drm_device *dev, struct drm_display_mode *mode,
+		      struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 jreg;
+
+	jreg = ast_io_read8(ast, AST_IO_MISC_PORT_READ);
+	jreg |= (vbios_mode->enh_table->flags & SyncNN);
+	ast_io_write8(ast, AST_IO_MISC_PORT_WRITE, jreg);
+}
+
+bool ast_set_dac_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+		     struct ast_vbios_mode_info *vbios_mode)
+{
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		break;
+	default:
+		return false;
+	}
+	return true;
+}
+
+void ast_set_start_address_crt1(struct drm_crtc *crtc, unsigned offset)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u32 addr;
+
+	addr = offset >> 2;
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x0d, (u8)(addr & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x0c, (u8)((addr >> 8) & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xaf, (u8)((addr >> 16) & 0xff));
+
+}
+
+static void ast_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+
+	if (ast->chip == AST1180)
+		return;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+		ast_set_index_reg_mask(ast, AST_IO_SEQ_PORT, 0x1, 0xdf, 0);
+		ast_crtc_load_lut(crtc);
+		break;
+	case DRM_MODE_DPMS_OFF:
+		ast_set_index_reg_mask(ast, AST_IO_SEQ_PORT, 0x1, 0xdf, 0x20);
+		break;
+	}
+}
+
+static bool ast_crtc_mode_fixup(struct drm_crtc *crtc,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+/* ast is different - we will force move buffers out of VRAM */
+static int ast_crtc_do_set_base(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				int x, int y, int atomic)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct drm_gem_object *obj;
+	struct ast_framebuffer *ast_fb;
+	struct ast_bo *bo;
+	int ret;
+	u64 gpu_addr;
+
+	/* push the previous fb to system ram */
+	if (!atomic && fb) {
+		ast_fb = to_ast_framebuffer(fb);
+		obj = ast_fb->obj;
+		bo = gem_to_ast_bo(obj);
+		ret = ast_bo_reserve(bo, false);
+		if (ret)
+			return ret;
+		ast_bo_push_sysram(bo);
+		ast_bo_unreserve(bo);
+	}
+
+	ast_fb = to_ast_framebuffer(crtc->fb);
+	obj = ast_fb->obj;
+	bo = gem_to_ast_bo(obj);
+
+	ret = ast_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	ret = ast_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	if (ret) {
+		ast_bo_unreserve(bo);
+		return ret;
+	}
+
+	if (&ast->fbdev->afb == ast_fb) {
+		/* if pushing console in kmap it */
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret)
+			DRM_ERROR("failed to kmap fbcon\n");
+	}
+	ast_bo_unreserve(bo);
+
+	ast_set_start_address_crt1(crtc, (u32)gpu_addr);
+
+	return 0;
+}
+
+static int ast_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+			     struct drm_framebuffer *old_fb)
+{
+	return ast_crtc_do_set_base(crtc, old_fb, x, y, 0);
+}
+
+static int ast_crtc_mode_set(struct drm_crtc *crtc,
+			     struct drm_display_mode *mode,
+			     struct drm_display_mode *adjusted_mode,
+			     int x, int y,
+			     struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct ast_vbios_mode_info vbios_mode;
+	bool ret;
+	if (ast->chip == AST1180) {
+		DRM_ERROR("AST 1180 modesetting not supported\n");
+		return -EINVAL;
+	}
+
+	ret = ast_get_vbios_mode_info(crtc, mode, adjusted_mode, &vbios_mode);
+	if (ret == false)
+		return -EINVAL;
+	ast_open_key(ast);
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa1, 0xff, 0x04);
+
+	ast_set_std_reg(crtc, adjusted_mode, &vbios_mode);
+	ast_set_crtc_reg(crtc, adjusted_mode, &vbios_mode);
+	ast_set_offset_reg(crtc);
+	ast_set_dclk_reg(dev, adjusted_mode, &vbios_mode);
+	ast_set_ext_reg(crtc, adjusted_mode, &vbios_mode);
+	ast_set_sync_reg(dev, adjusted_mode, &vbios_mode);
+	ast_set_dac_reg(crtc, adjusted_mode, &vbios_mode);
+
+	ast_crtc_mode_set_base(crtc, x, y, old_fb);
+
+	return 0;
+}
+
+static void ast_crtc_disable(struct drm_crtc *crtc)
+{
+
+}
+
+static void ast_crtc_prepare(struct drm_crtc *crtc)
+{
+
+}
+
+static void ast_crtc_commit(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	ast_set_index_reg_mask(ast, AST_IO_SEQ_PORT, 0x1, 0xdf, 0);
+}
+
+
+static const struct drm_crtc_helper_funcs ast_crtc_helper_funcs = {
+	.dpms = ast_crtc_dpms,
+	.mode_fixup = ast_crtc_mode_fixup,
+	.mode_set = ast_crtc_mode_set,
+	.mode_set_base = ast_crtc_mode_set_base,
+	.disable = ast_crtc_disable,
+	.load_lut = ast_crtc_load_lut,
+	.disable = ast_crtc_disable,
+	.prepare = ast_crtc_prepare,
+	.commit = ast_crtc_commit,
+
+};
+
+static void ast_crtc_reset(struct drm_crtc *crtc)
+{
+
+}
+
+static void ast_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
+				 u16 *blue, uint32_t start, uint32_t size)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	int end = (start + size > 256) ? 256 : start + size, i;
+
+	/* userspace palettes are always correct as is */
+	for (i = start; i < end; i++) {
+		ast_crtc->lut_r[i] = red[i] >> 8;
+		ast_crtc->lut_g[i] = green[i] >> 8;
+		ast_crtc->lut_b[i] = blue[i] >> 8;
+	}
+	ast_crtc_load_lut(crtc);
+}
+
+
+static void ast_crtc_destroy(struct drm_crtc *crtc)
+{
+	drm_crtc_cleanup(crtc);
+	kfree(crtc);
+}
+
+static const struct drm_crtc_funcs ast_crtc_funcs = {
+	.cursor_set = ast_cursor_set,
+	.cursor_move = ast_cursor_move,
+	.reset = ast_crtc_reset,
+	.set_config = drm_crtc_helper_set_config,
+	.gamma_set = ast_crtc_gamma_set,
+	.destroy = ast_crtc_destroy,
+};
+
+int ast_crtc_init(struct drm_device *dev)
+{
+	struct ast_crtc *crtc;
+	int i;
+
+	crtc = kzalloc(sizeof(struct ast_crtc), GFP_KERNEL);
+	if (!crtc)
+		return -ENOMEM;
+
+	drm_crtc_init(dev, &crtc->base, &ast_crtc_funcs);
+	drm_mode_crtc_set_gamma_size(&crtc->base, 256);
+	drm_crtc_helper_add(&crtc->base, &ast_crtc_helper_funcs);
+
+	for (i = 0; i < 256; i++) {
+		crtc->lut_r[i] = i;
+		crtc->lut_g[i] = i;
+		crtc->lut_b[i] = i;
+	}
+	return 0;
+}
+
+static void ast_encoder_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+	kfree(encoder);
+}
+
+
+static struct drm_encoder *ast_best_single_encoder(struct drm_connector *connector)
+{
+	int enc_id = connector->encoder_ids[0];
+	struct drm_mode_object *obj;
+	struct drm_encoder *encoder;
+
+	/* pick the encoder ids */
+	if (enc_id) {
+		obj = drm_mode_object_find(connector->dev, enc_id, DRM_MODE_OBJECT_ENCODER);
+		if (!obj)
+			return NULL;
+		encoder = obj_to_encoder(obj);
+		return encoder;
+	}
+	return NULL;
+}
+
+
+static const struct drm_encoder_funcs ast_enc_funcs = {
+	.destroy = ast_encoder_destroy,
+};
+
+static void ast_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+
+}
+
+static bool ast_mode_fixup(struct drm_encoder *encoder,
+			   struct drm_display_mode *mode,
+			   struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void ast_encoder_mode_set(struct drm_encoder *encoder,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode)
+{
+}
+
+static void ast_encoder_prepare(struct drm_encoder *encoder)
+{
+
+}
+
+static void ast_encoder_commit(struct drm_encoder *encoder)
+{
+
+}
+
+
+static const struct drm_encoder_helper_funcs ast_enc_helper_funcs = {
+	.dpms = ast_encoder_dpms,
+	.mode_fixup = ast_mode_fixup,
+	.prepare = ast_encoder_prepare,
+	.commit = ast_encoder_commit,
+	.mode_set = ast_encoder_mode_set,
+};
+
+int ast_encoder_init(struct drm_device *dev)
+{
+	struct ast_encoder *ast_encoder;
+
+	ast_encoder = kzalloc(sizeof(struct ast_encoder), GFP_KERNEL);
+	if (!ast_encoder)
+		return -ENOMEM;
+
+	drm_encoder_init(dev, &ast_encoder->base, &ast_enc_funcs,
+			 DRM_MODE_ENCODER_DAC);
+	drm_encoder_helper_add(&ast_encoder->base, &ast_enc_helper_funcs);
+
+	ast_encoder->base.possible_crtcs = 1;
+	return 0;
+}
+
+static int ast_get_modes(struct drm_connector *connector)
+{
+	struct ast_connector *ast_connector = to_ast_connector(connector);
+	struct edid *edid;
+	int ret;
+
+	edid = drm_get_edid(connector, &ast_connector->i2c->adapter);
+	if (edid) {
+		drm_mode_connector_update_edid_property(&ast_connector->base, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		return ret;
+	} else
+		drm_mode_connector_update_edid_property(&ast_connector->base, NULL);
+	return 0;
+}
+
+static int ast_mode_valid(struct drm_connector *connector,
+			  struct drm_display_mode *mode)
+{
+	return MODE_OK;
+}
+
+static void ast_connector_destroy(struct drm_connector *connector)
+{
+	struct ast_connector *ast_connector = to_ast_connector(connector);
+	ast_i2c_destroy(ast_connector->i2c);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+static enum drm_connector_status
+ast_connector_detect(struct drm_connector *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static const struct drm_connector_helper_funcs ast_connector_helper_funcs = {
+	.mode_valid = ast_mode_valid,
+	.get_modes = ast_get_modes,
+	.best_encoder = ast_best_single_encoder,
+};
+
+static const struct drm_connector_funcs ast_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = ast_connector_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = ast_connector_destroy,
+};
+
+int ast_connector_init(struct drm_device *dev)
+{
+	struct ast_connector *ast_connector;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+
+	ast_connector = kzalloc(sizeof(struct ast_connector), GFP_KERNEL);
+	if (!ast_connector)
+		return -ENOMEM;
+
+	connector = &ast_connector->base;
+	drm_connector_init(dev, connector, &ast_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+	drm_connector_helper_add(connector, &ast_connector_helper_funcs);
+
+	connector->interlace_allowed = 0;
+	connector->doublescan_allowed = 0;
+
+	drm_sysfs_connector_add(connector);
+
+	connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+
+	encoder = list_first_entry(&dev->mode_config.encoder_list, struct drm_encoder, head);
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	ast_connector->i2c = ast_i2c_create(dev);
+	if (!ast_connector->i2c)
+		DRM_ERROR("failed to add ddc bus for connector\n");
+
+	return 0;
+}
+
+/* allocate cursor cache and pin at start of VRAM */
+int ast_cursor_init(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	int size;
+	int ret;
+	struct drm_gem_object *obj;
+	struct ast_bo *bo;
+	uint64_t gpu_addr;
+
+	size = (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE) * AST_DEFAULT_HWC_NUM;
+
+	ret = ast_gem_create(dev, size, true, &obj);
+	if (ret)
+		return ret;
+	bo = gem_to_ast_bo(obj);
+	ret = ast_bo_reserve(bo, false);
+	if (unlikely(ret != 0))
+		goto fail;
+
+	ret = ast_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	ast_bo_unreserve(bo);
+	if (ret)
+		goto fail;
+
+	/* kmap the object */
+	ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &ast->cache_kmap);
+	if (ret)
+		goto fail;
+
+	ast->cursor_cache = obj;
+	ast->cursor_cache_gpu_addr = gpu_addr;
+	DRM_ERROR("pinned cursor cache at %llx\n", ast->cursor_cache_gpu_addr);
+	return 0;
+fail:
+	return ret;
+}
+
+void ast_cursor_fini(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	ttm_bo_kunmap(&ast->cache_kmap);
+	drm_gem_object_unreference_unlocked(ast->cursor_cache);
+}
+
+int ast_mode_init(struct drm_device *dev)
+{
+	ast_cursor_init(dev);
+	ast_crtc_init(dev);
+	ast_encoder_init(dev);
+	ast_connector_init(dev);
+	return 0;
+}
+
+void ast_mode_fini(struct drm_device *dev)
+{
+	ast_cursor_fini(dev);
+}
+
+static int get_clock(void *i2c_priv)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	uint32_t val;
+
+	val = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x10) >> 4;
+	return val & 1 ? 1 : 0;
+}
+
+static int get_data(void *i2c_priv)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	uint32_t val;
+
+	val = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x20) >> 5;
+	return val & 1 ? 1 : 0;
+}
+
+static void set_clock(void *i2c_priv, int clock)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	int i;
+	u8 ujcrb7, jtemp;
+
+	for (i = 0; i < 0x10000; i++) {
+		ujcrb7 = ((clock & 0x01) ? 0 : 1);
+		ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0xfe, ujcrb7);
+		jtemp = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x01);
+		if (ujcrb7 == jtemp)
+			break;
+	}
+}
+
+static void set_data(void *i2c_priv, int data)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	int i;
+	u8 ujcrb7, jtemp;
+
+	for (i = 0; i < 0x10000; i++) {
+		ujcrb7 = ((data & 0x01) ? 0 : 1) << 2;
+		ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0xfb, ujcrb7);
+		jtemp = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x04);
+		if (ujcrb7 == jtemp)
+			break;
+	}
+}
+
+static struct ast_i2c_chan *ast_i2c_create(struct drm_device *dev)
+{
+	struct ast_i2c_chan *i2c;
+	int ret;
+
+	i2c = kzalloc(sizeof(struct ast_i2c_chan), GFP_KERNEL);
+	if (!i2c)
+		return NULL;
+
+	i2c->adapter.owner = THIS_MODULE;
+	i2c->adapter.class = I2C_CLASS_DDC;
+	i2c->adapter.dev.parent = &dev->pdev->dev;
+	i2c->dev = dev;
+	i2c_set_adapdata(&i2c->adapter, i2c);
+	snprintf(i2c->adapter.name, sizeof(i2c->adapter.name),
+		 "AST i2c bit bus");
+	i2c->adapter.algo_data = &i2c->bit;
+
+	i2c->bit.udelay = 20;
+	i2c->bit.timeout = 2;
+	i2c->bit.data = i2c;
+	i2c->bit.setsda = set_data;
+	i2c->bit.setscl = set_clock;
+	i2c->bit.getsda = get_data;
+	i2c->bit.getscl = get_clock;
+	ret = i2c_bit_add_bus(&i2c->adapter);
+	if (ret) {
+		DRM_ERROR("Failed to register bit i2c\n");
+		goto out_free;
+	}
+
+	return i2c;
+out_free:
+	kfree(i2c);
+	return NULL;
+}
+
+static void ast_i2c_destroy(struct ast_i2c_chan *i2c)
+{
+	if (!i2c)
+		return;
+	i2c_del_adapter(&i2c->adapter);
+	kfree(i2c);
+}
+
+void ast_show_cursor(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u8 jreg;
+
+	jreg = 0x2;
+	/* enable ARGB cursor */
+	jreg |= 1;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xcb, 0xfc, jreg);
+}
+
+void ast_hide_cursor(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xcb, 0xfc, 0x00);
+}
+
+static u32 copy_cursor_image(u8 *src, u8 *dst, int width, int height)
+{
+	union {
+		u32 ul;
+		u8 b[4];
+	} srcdata32[2], data32;
+	union {
+		u16 us;
+		u8 b[2];
+	} data16;
+	u32 csum = 0;
+	s32 alpha_dst_delta, last_alpha_dst_delta;
+	u8 *srcxor, *dstxor;
+	int i, j;
+	u32 per_pixel_copy, two_pixel_copy;
+
+	alpha_dst_delta = AST_MAX_HWC_WIDTH << 1;
+	last_alpha_dst_delta = alpha_dst_delta - (width << 1);
+
+	srcxor = src;
+	dstxor = (u8 *)dst + last_alpha_dst_delta + (AST_MAX_HWC_HEIGHT - height) * alpha_dst_delta;
+	per_pixel_copy = width & 1;
+	two_pixel_copy = width >> 1;
+
+	for (j = 0; j < height; j++) {
+		for (i = 0; i < two_pixel_copy; i++) {
+			srcdata32[0].ul = *((u32 *)srcxor) & 0xf0f0f0f0;
+			srcdata32[1].ul = *((u32 *)(srcxor + 4)) & 0xf0f0f0f0;
+			data32.b[0] = srcdata32[0].b[1] | (srcdata32[0].b[0] >> 4);
+			data32.b[1] = srcdata32[0].b[3] | (srcdata32[0].b[2] >> 4);
+			data32.b[2] = srcdata32[0].b[1] | (srcdata32[1].b[0] >> 4);
+			data32.b[3] = srcdata32[0].b[3] | (srcdata32[1].b[2] >> 4);
+
+			writel(data32.ul, dstxor);
+			csum += data32.ul;
+
+			dstxor += 4;
+			srcxor += 8;
+
+		}
+
+		for (i = 0; i < per_pixel_copy; i++) {
+			srcdata32[0].ul = *((u32 *)srcxor) & 0xf0f0f0f0;
+			data16.b[0] = srcdata32[0].b[1] | (srcdata32[0].b[0] >> 4);
+			data16.b[1] = srcdata32[0].b[3] | (srcdata32[0].b[2] >> 4);
+			writew(data16.us, dstxor);
+			csum += (u32)data16.us;
+
+			dstxor += 2;
+			srcxor += 4;
+		}
+		dstxor += last_alpha_dst_delta;
+	}
+	return csum;
+}
+
+static int ast_cursor_set(struct drm_crtc *crtc,
+			  struct drm_file *file_priv,
+			  uint32_t handle,
+			  uint32_t width,
+			  uint32_t height)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	struct drm_gem_object *obj;
+	struct ast_bo *bo;
+	uint64_t gpu_addr;
+	u32 csum;
+	int ret;
+	struct ttm_bo_kmap_obj uobj_map;
+	u8 *src, *dst;
+	bool src_isiomem, dst_isiomem;
+	if (!handle) {
+		ast_hide_cursor(crtc);
+		return 0;
+	}
+
+	if (width > AST_MAX_HWC_WIDTH || height > AST_MAX_HWC_HEIGHT)
+		return -EINVAL;
+
+	obj = drm_gem_object_lookup(crtc->dev, file_priv, handle);
+	if (!obj) {
+		DRM_ERROR("Cannot find cursor object %x for crtc\n", handle);
+		return -ENOENT;
+	}
+	bo = gem_to_ast_bo(obj);
+
+	ret = ast_bo_reserve(bo, false);
+	if (ret)
+		goto fail;
+
+	ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &uobj_map);
+
+	src = ttm_kmap_obj_virtual(&uobj_map, &src_isiomem);
+	dst = ttm_kmap_obj_virtual(&ast->cache_kmap, &dst_isiomem);
+
+	if (src_isiomem == true)
+		DRM_ERROR("src cursor bo should be in main memory\n");
+	if (dst_isiomem == false)
+		DRM_ERROR("dst bo should be in VRAM\n");
+
+	dst += (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor;
+
+	/* do data transfer to cursor cache */
+	csum = copy_cursor_image(src, dst, width, height);
+
+	/* write checksum + signature */
+	ttm_bo_kunmap(&uobj_map);
+	ast_bo_unreserve(bo);
+	{
+		u8 *dst = (u8 *)ast->cache_kmap.virtual + (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor + AST_HWC_SIZE;
+		writel(csum, dst);
+		writel(width, dst + AST_HWC_SIGNATURE_SizeX);
+		writel(height, dst + AST_HWC_SIGNATURE_SizeY);
+		writel(0, dst + AST_HWC_SIGNATURE_HOTSPOTX);
+		writel(0, dst + AST_HWC_SIGNATURE_HOTSPOTY);
+
+		/* set pattern offset */
+		gpu_addr = ast->cursor_cache_gpu_addr;
+		gpu_addr += (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor;
+		gpu_addr >>= 3;
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc8, gpu_addr & 0xff);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc9, (gpu_addr >> 8) & 0xff);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xca, (gpu_addr >> 16) & 0xff);
+	}
+	ast_crtc->cursor_width = width;
+	ast_crtc->cursor_height = height;
+	ast_crtc->offset_x = AST_MAX_HWC_WIDTH - width;
+	ast_crtc->offset_y = AST_MAX_HWC_WIDTH - height;
+
+	ast->next_cursor = (ast->next_cursor + 1) % AST_DEFAULT_HWC_NUM;
+
+	ast_show_cursor(crtc);
+
+	drm_gem_object_unreference_unlocked(obj);
+	return 0;
+fail:
+	drm_gem_object_unreference_unlocked(obj);
+	return ret;
+}
+
+static int ast_cursor_move(struct drm_crtc *crtc,
+			   int x, int y)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	struct ast_private *ast = crtc->dev->dev_private;
+	int x_offset, y_offset;
+	u8 *sig;
+
+	sig = (u8 *)ast->cache_kmap.virtual + (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor + AST_HWC_SIZE;
+	writel(x, sig + AST_HWC_SIGNATURE_X);
+	writel(y, sig + AST_HWC_SIGNATURE_Y);
+
+	x_offset = ast_crtc->offset_x;
+	y_offset = ast_crtc->offset_y;
+	if (x < 0) {
+		x_offset = (-x) + ast_crtc->offset_x;
+		x = 0;
+	}
+
+	if (y < 0) {
+		y_offset = (-y) + ast_crtc->offset_y;
+		y = 0;
+	}
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc2, x_offset);
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc3, y_offset);
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc4, (x & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc5, ((x >> 8) & 0x0f));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc6, (y & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc7, ((y >> 8) & 0x07));
+
+	/* dummy write to fire HWC */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xCB, 0xFF, 0x00);
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/ast/ast_post.c b/drivers/gpu/drm/ast/ast_post.c
new file mode 100644
index 000000000000..6edbee63b0cb
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_post.c
@@ -0,0 +1,1780 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+
+#include "drmP.h"
+#include "ast_drv.h"
+
+#include "ast_dram_tables.h"
+
+static void ast_init_dram_2300(struct drm_device *dev);
+
+static void
+ast_enable_vga(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	ast_io_write8(ast, 0x43, 0x01);
+	ast_io_write8(ast, 0x42, 0x01);
+}
+
+#if 0 /* will use later */
+static bool
+ast_is_vga_enabled(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 ch;
+
+	if (ast->chip == AST1180) {
+		/* TODO 1180 */
+	} else {
+		ch = ast_io_read8(ast, 0x43);
+		if (ch) {
+			ast_open_key(ast);
+			ch = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb6, 0xff);
+			return ch & 0x04;
+		}
+	}
+	return 0;
+}
+#endif
+
+static const u8 extreginfo[] = { 0x0f, 0x04, 0x1c, 0xff };
+static const u8 extreginfo_ast2300a0[] = { 0x0f, 0x04, 0x1c, 0xff };
+static const u8 extreginfo_ast2300[] = { 0x0f, 0x04, 0x1f, 0xff };
+
+static void
+ast_set_def_ext_reg(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 i, index, reg;
+	const u8 *ext_reg_info;
+
+	/* reset scratch */
+	for (i = 0x81; i <= 0x8f; i++)
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, i, 0x00);
+
+	if (ast->chip == AST2300) {
+		if (dev->pdev->revision >= 0x20)
+			ext_reg_info = extreginfo_ast2300;
+		else
+			ext_reg_info = extreginfo_ast2300a0;
+	} else
+		ext_reg_info = extreginfo;
+
+	index = 0xa0;
+	while (*ext_reg_info != 0xff) {
+		ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, index, 0x00, *ext_reg_info);
+		index++;
+		ext_reg_info++;
+	}
+
+	/* disable standard IO/MEM decode if secondary */
+	/* ast_set_index_reg-mask(ast, AST_IO_CRTC_PORT, 0xa1, 0xff, 0x3); */
+
+	/* Set Ext. Default */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x8c, 0x00, 0x01);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x00, 0x00);
+
+	/* Enable RAMDAC for A1 */
+	reg = 0x04;
+	if (ast->chip == AST2300)
+		reg |= 0x20;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb6, 0xff, reg);
+}
+
+static inline u32 mindwm(struct ast_private *ast, u32 r)
+{
+	ast_write32(ast, 0xf004, r & 0xffff0000);
+	ast_write32(ast, 0xf000, 0x1);
+
+	return ast_read32(ast, 0x10000 + (r & 0x0000ffff));
+}
+
+static inline void moutdwm(struct ast_private *ast, u32 r, u32 v)
+{
+	ast_write32(ast, 0xf004, r & 0xffff0000);
+	ast_write32(ast, 0xf000, 0x1);
+	ast_write32(ast, 0x10000 + (r & 0x0000ffff), v);
+}
+
+/*
+ * AST2100/2150 DLL CBR Setting
+ */
+#define CBR_SIZE_AST2150	     ((16 << 10) - 1)
+#define CBR_PASSNUM_AST2150          5
+#define CBR_THRESHOLD_AST2150        10
+#define CBR_THRESHOLD2_AST2150       10
+#define TIMEOUT_AST2150              5000000
+
+#define CBR_PATNUM_AST2150           8
+
+static const u32 pattern_AST2150[14] = {
+	0xFF00FF00,
+	0xCC33CC33,
+	0xAA55AA55,
+	0xFFFE0001,
+	0x683501FE,
+	0x0F1929B0,
+	0x2D0B4346,
+	0x60767F02,
+	0x6FBE36A6,
+	0x3A253035,
+	0x3019686D,
+	0x41C6167E,
+	0x620152BF,
+	0x20F050E0
+};
+
+static u32 mmctestburst2_ast2150(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000001 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x40;
+		if (++timeout > TIMEOUT_AST2150) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0xffffffff;
+		}
+	} while (!data);
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000003 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x40;
+		if (++timeout > TIMEOUT_AST2150) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0xffffffff;
+		}
+	} while (!data);
+	data = (mindwm(ast, 0x1e6e0070) & 0x80) >> 7;
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	return data;
+}
+
+#if 0 /* unused in DDX driver - here for completeness */
+static u32 mmctestsingle2_ast2150(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000005 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x40;
+		if (++timeout > TIMEOUT_AST2150) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0xffffffff;
+		}
+	} while (!data);
+	data = (mindwm(ast, 0x1e6e0070) & 0x80) >> 7;
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	return data;
+}
+#endif
+
+static int cbrtest_ast2150(struct ast_private *ast)
+{
+	int i;
+
+	for (i = 0; i < 8; i++)
+		if (mmctestburst2_ast2150(ast, i))
+			return 0;
+	return 1;
+}
+
+static int cbrscan_ast2150(struct ast_private *ast, int busw)
+{
+	u32 patcnt, loop;
+
+	for (patcnt = 0; patcnt < CBR_PATNUM_AST2150; patcnt++) {
+		moutdwm(ast, 0x1e6e007c, pattern_AST2150[patcnt]);
+		for (loop = 0; loop < CBR_PASSNUM_AST2150; loop++) {
+			if (cbrtest_ast2150(ast))
+				break;
+		}
+		if (loop == CBR_PASSNUM_AST2150)
+			return 0;
+	}
+	return 1;
+}
+
+
+static void cbrdlli_ast2150(struct ast_private *ast, int busw)
+{
+	u32 dll_min[4], dll_max[4], dlli, data, passcnt;
+
+cbr_start:
+	dll_min[0] = dll_min[1] = dll_min[2] = dll_min[3] = 0xff;
+	dll_max[0] = dll_max[1] = dll_max[2] = dll_max[3] = 0x0;
+	passcnt = 0;
+
+	for (dlli = 0; dlli < 100; dlli++) {
+		moutdwm(ast, 0x1e6e0068, dlli | (dlli << 8) | (dlli << 16) | (dlli << 24));
+		data = cbrscan_ast2150(ast, busw);
+		if (data != 0) {
+			if (data & 0x1) {
+				if (dll_min[0] > dlli)
+					dll_min[0] = dlli;
+				if (dll_max[0] < dlli)
+					dll_max[0] = dlli;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD_AST2150)
+			goto cbr_start;
+	}
+	if (dll_max[0] == 0 || (dll_max[0]-dll_min[0]) < CBR_THRESHOLD_AST2150)
+		goto cbr_start;
+
+	dlli = dll_min[0] + (((dll_max[0] - dll_min[0]) * 7) >> 4);
+	moutdwm(ast, 0x1e6e0068, dlli | (dlli << 8) | (dlli << 16) | (dlli << 24));
+}
+
+
+
+static void ast_init_dram_reg(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 j;
+	u32 data, temp, i;
+	const struct ast_dramstruct *dram_reg_info;
+
+	j = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+
+	if ((j & 0x80) == 0) { /* VGA only */
+		if (ast->chip == AST2000) {
+			dram_reg_info = ast2000_dram_table_data;
+			ast_write32(ast, 0xf004, 0x1e6e0000);
+			ast_write32(ast, 0xf000, 0x1);
+			ast_write32(ast, 0x10100, 0xa8);
+
+			do {
+				;
+			} while (ast_read32(ast, 0x10100) != 0xa8);
+		} else {/* AST2100/1100 */
+			if (ast->chip == AST2100 || ast->chip == 2200)
+				dram_reg_info = ast2100_dram_table_data;
+			else
+				dram_reg_info = ast1100_dram_table_data;
+
+			ast_write32(ast, 0xf004, 0x1e6e0000);
+			ast_write32(ast, 0xf000, 0x1);
+			ast_write32(ast, 0x12000, 0x1688A8A8);
+			do {
+				;
+			} while (ast_read32(ast, 0x12000) != 0x01);
+
+			ast_write32(ast, 0x10000, 0xfc600309);
+			do {
+				;
+			} while (ast_read32(ast, 0x10000) != 0x01);
+		}
+
+		while (dram_reg_info->index != 0xffff) {
+			if (dram_reg_info->index == 0xff00) {/* delay fn */
+				for (i = 0; i < 15; i++)
+					udelay(dram_reg_info->data);
+			} else if (dram_reg_info->index == 0x4 && ast->chip != AST2000) {
+				data = dram_reg_info->data;
+				if (ast->dram_type == AST_DRAM_1Gx16)
+					data = 0x00000d89;
+				else if (ast->dram_type == AST_DRAM_1Gx32)
+					data = 0x00000c8d;
+
+				temp = ast_read32(ast, 0x12070);
+				temp &= 0xc;
+				temp <<= 2;
+				ast_write32(ast, 0x10000 + dram_reg_info->index, data | temp);
+			} else
+				ast_write32(ast, 0x10000 + dram_reg_info->index, dram_reg_info->data);
+			dram_reg_info++;
+		}
+
+		/* AST 2100/2150 DRAM calibration */
+		data = ast_read32(ast, 0x10120);
+		if (data == 0x5061) { /* 266Mhz */
+			data = ast_read32(ast, 0x10004);
+			if (data & 0x40)
+				cbrdlli_ast2150(ast, 16); /* 16 bits */
+			else
+				cbrdlli_ast2150(ast, 32); /* 32 bits */
+		}
+
+		switch (ast->chip) {
+		case AST2000:
+			temp = ast_read32(ast, 0x10140);
+			ast_write32(ast, 0x10140, temp | 0x40);
+			break;
+		case AST1100:
+		case AST2100:
+		case AST2200:
+		case AST2150:
+			temp = ast_read32(ast, 0x1200c);
+			ast_write32(ast, 0x1200c, temp & 0xfffffffd);
+			temp = ast_read32(ast, 0x12040);
+			ast_write32(ast, 0x12040, temp | 0x40);
+			break;
+		default:
+			break;
+		}
+	}
+
+	/* wait ready */
+	do {
+		j = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+	} while ((j & 0x40) == 0);
+}
+
+void ast_post_gpu(struct drm_device *dev)
+{
+	u32 reg;
+	struct ast_private *ast = dev->dev_private;
+
+	pci_read_config_dword(ast->dev->pdev, 0x04, &reg);
+	reg |= 0x3;
+	pci_write_config_dword(ast->dev->pdev, 0x04, reg);
+
+	ast_enable_vga(dev);
+	ast_open_key(ast);
+	ast_set_def_ext_reg(dev);
+
+	if (ast->chip == AST2300)
+		ast_init_dram_2300(dev);
+	else
+		ast_init_dram_reg(dev);
+}
+
+/* AST 2300 DRAM settings */
+#define AST_DDR3 0
+#define AST_DDR2 1
+
+struct ast2300_dram_param {
+	u32 dram_type;
+	u32 dram_chipid;
+	u32 dram_freq;
+	u32 vram_size;
+	u32 odt;
+	u32 wodt;
+	u32 rodt;
+	u32 dram_config;
+	u32 reg_PERIOD;
+	u32 reg_MADJ;
+	u32 reg_SADJ;
+	u32 reg_MRS;
+	u32 reg_EMRS;
+	u32 reg_AC1;
+	u32 reg_AC2;
+	u32 reg_DQSIC;
+	u32 reg_DRV;
+	u32 reg_IOZ;
+	u32 reg_DQIDLY;
+	u32 reg_FREQ;
+	u32 madj_max;
+	u32 dll2_finetune_step;
+};
+
+/*
+ * DQSI DLL CBR Setting
+ */
+#define CBR_SIZE1            ((4  << 10) - 1)
+#define CBR_SIZE2            ((64 << 10) - 1)
+#define CBR_PASSNUM          5
+#define CBR_PASSNUM2         5
+#define CBR_THRESHOLD        10
+#define CBR_THRESHOLD2       10
+#define TIMEOUT              5000000
+#define CBR_PATNUM           8
+
+static const u32 pattern[8] = {
+	0xFF00FF00,
+	0xCC33CC33,
+	0xAA55AA55,
+	0x88778877,
+	0x92CC4D6E,
+	0x543D3CDE,
+	0xF1E843C7,
+	0x7C61D253
+};
+
+#if 0 /* unused in DDX, included for completeness */
+static int mmc_test_burst(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x000000c1 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x3000;
+		if (data & 0x2000) {
+			return 0;
+		}
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0;
+		}
+	} while (!data);
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	return 1;
+}
+#endif
+
+static int mmc_test_burst2(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000041 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x1000;
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x0);
+			return -1;
+		}
+	} while (!data);
+	data = mindwm(ast, 0x1e6e0078);
+	data = (data | (data >> 16)) & 0xffff;
+	moutdwm(ast, 0x1e6e0070, 0x0);
+	return data;
+}
+
+#if 0 /* Unused in DDX here for completeness */
+static int mmc_test_single(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x000000c5 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x3000;
+		if (data & 0x2000)
+			return 0;
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x0);
+			return 0;
+		}
+	} while (!data);
+	moutdwm(ast, 0x1e6e0070, 0x0);
+	return 1;
+}
+#endif
+
+static int mmc_test_single2(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000005 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x1000;
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x0);
+			return -1;
+		}
+	} while (!data);
+	data = mindwm(ast, 0x1e6e0078);
+	data = (data | (data >> 16)) & 0xffff;
+	moutdwm(ast, 0x1e6e0070, 0x0);
+	return data;
+}
+
+static int cbr_test(struct ast_private *ast)
+{
+	u32 data;
+	int i;
+	data = mmc_test_single2(ast, 0);
+	if ((data & 0xff) && (data & 0xff00))
+		return 0;
+	for (i = 0; i < 8; i++) {
+		data = mmc_test_burst2(ast, i);
+		if ((data & 0xff) && (data & 0xff00))
+			return 0;
+	}
+	if (!data)
+		return 3;
+	else if (data & 0xff)
+		return 2;
+	return 1;
+}
+
+static int cbr_scan(struct ast_private *ast)
+{
+	u32 data, data2, patcnt, loop;
+
+	data2 = 3;
+	for (patcnt = 0; patcnt < CBR_PATNUM; patcnt++) {
+		moutdwm(ast, 0x1e6e007c, pattern[patcnt]);
+		for (loop = 0; loop < CBR_PASSNUM2; loop++) {
+			if ((data = cbr_test(ast)) != 0) {
+				data2 &= data;
+				if (!data2)
+					return 0;
+				break;
+			}
+		}
+		if (loop == CBR_PASSNUM2)
+			return 0;
+	}
+	return data2;
+}
+
+static u32 cbr_test2(struct ast_private *ast)
+{
+	u32 data;
+
+	data = mmc_test_burst2(ast, 0);
+	if (data == 0xffff)
+		return 0;
+	data |= mmc_test_single2(ast, 0);
+	if (data == 0xffff)
+		return 0;
+
+	return ~data & 0xffff;
+}
+
+static u32 cbr_scan2(struct ast_private *ast)
+{
+	u32 data, data2, patcnt, loop;
+
+	data2 = 0xffff;
+	for (patcnt = 0; patcnt < CBR_PATNUM; patcnt++) {
+		moutdwm(ast, 0x1e6e007c, pattern[patcnt]);
+		for (loop = 0; loop < CBR_PASSNUM2; loop++) {
+			if ((data = cbr_test2(ast)) != 0) {
+				data2 &= data;
+				if (!data)
+					return 0;
+				break;
+			}
+		}
+		if (loop == CBR_PASSNUM2)
+			return 0;
+	}
+	return data2;
+}
+
+#if 0 /* unused in DDX - added for completeness */
+static void finetuneDQI(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 gold_sadj[2], dllmin[16], dllmax[16], dlli, data, cnt, mask, passcnt;
+
+	gold_sadj[0] = (mindwm(ast, 0x1E6E0024) >> 16) & 0xffff;
+	gold_sadj[1] = gold_sadj[0] >> 8;
+	gold_sadj[0] = gold_sadj[0] & 0xff;
+	gold_sadj[0] = (gold_sadj[0] + gold_sadj[1]) >> 1;
+	gold_sadj[1] = gold_sadj[0];
+
+	for (cnt = 0; cnt < 16; cnt++) {
+		dllmin[cnt] = 0xff;
+		dllmax[cnt] = 0x0;
+	}
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001400 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE1);
+		data = cbr_scan2(ast);
+		if (data != 0) {
+			mask = 0x00010001;
+			for (cnt = 0; cnt < 16; cnt++) {
+				if (data & mask) {
+					if (dllmin[cnt] > dlli) {
+						dllmin[cnt] = dlli;
+					}
+					if (dllmax[cnt] < dlli) {
+						dllmax[cnt] = dlli;
+					}
+				}
+				mask <<= 1;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD) {
+			break;
+		}
+	}
+	data = 0;
+	for (cnt = 0; cnt < 8; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[0] >= dlli) {
+				dlli = (gold_sadj[0] - dlli) >> 1;
+				if (dlli > 3) {
+					dlli = 3;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[0]) >> 1;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0080, data);
+
+	data = 0;
+	for (cnt = 8; cnt < 16; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[1] >= dlli) {
+				dlli = (gold_sadj[1] - dlli) >> 1;
+				if (dlli > 3) {
+					dlli = 3;
+				} else {
+					dlli = (dlli - 1) & 0x7;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[1]) >> 1;
+				dlli += 1;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0084, data);
+
+} /* finetuneDQI */
+#endif
+
+static void finetuneDQI_L(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 gold_sadj[2], dllmin[16], dllmax[16], dlli, data, cnt, mask, passcnt;
+
+FINETUNE_START:
+	for (cnt = 0; cnt < 16; cnt++) {
+		dllmin[cnt] = 0xff;
+		dllmax[cnt] = 0x0;
+	}
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001400 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE1);
+		data = cbr_scan2(ast);
+		if (data != 0) {
+			mask = 0x00010001;
+			for (cnt = 0; cnt < 16; cnt++) {
+				if (data & mask) {
+					if (dllmin[cnt] > dlli) {
+						dllmin[cnt] = dlli;
+					}
+					if (dllmax[cnt] < dlli) {
+						dllmax[cnt] = dlli;
+					}
+				}
+				mask <<= 1;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD2) {
+			break;
+		}
+	}
+	gold_sadj[0] = 0x0;
+	passcnt = 0;
+	for (cnt = 0; cnt < 16; cnt++) {
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			gold_sadj[0] += dllmin[cnt];
+			passcnt++;
+		}
+	}
+	if (passcnt != 16) {
+		goto FINETUNE_START;
+	}
+	gold_sadj[0] = gold_sadj[0] >> 4;
+	gold_sadj[1] = gold_sadj[0];
+
+	data = 0;
+	for (cnt = 0; cnt < 8; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = dllmin[cnt];
+			if (gold_sadj[0] >= dlli) {
+				dlli = ((gold_sadj[0] - dlli) * 19) >> 5;
+				if (dlli > 3) {
+					dlli = 3;
+				}
+			} else {
+				dlli = ((dlli - gold_sadj[0]) * 19) >> 5;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0080, data);
+
+	data = 0;
+	for (cnt = 8; cnt < 16; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = dllmin[cnt];
+			if (gold_sadj[1] >= dlli) {
+				dlli = ((gold_sadj[1] - dlli) * 19) >> 5;
+				if (dlli > 3) {
+					dlli = 3;
+				} else {
+					dlli = (dlli - 1) & 0x7;
+				}
+			} else {
+				dlli = ((dlli - gold_sadj[1]) * 19) >> 5;
+				dlli += 1;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0084, data);
+
+} /* finetuneDQI_L */
+
+static void finetuneDQI_L2(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 gold_sadj[2], dllmin[16], dllmax[16], dlli, data, cnt, mask, passcnt, data2;
+
+	for (cnt = 0; cnt < 16; cnt++) {
+		dllmin[cnt] = 0xff;
+		dllmax[cnt] = 0x0;
+	}
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001400 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE2);
+		data = cbr_scan2(ast);
+		if (data != 0) {
+			mask = 0x00010001;
+			for (cnt = 0; cnt < 16; cnt++) {
+				if (data & mask) {
+					if (dllmin[cnt] > dlli) {
+						dllmin[cnt] = dlli;
+					}
+					if (dllmax[cnt] < dlli) {
+						dllmax[cnt] = dlli;
+					}
+				}
+				mask <<= 1;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD2) {
+			break;
+		}
+	}
+	gold_sadj[0] = 0x0;
+	gold_sadj[1] = 0xFF;
+	for (cnt = 0; cnt < 8; cnt++) {
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			if (gold_sadj[0] < dllmin[cnt]) {
+				gold_sadj[0] = dllmin[cnt];
+			}
+			if (gold_sadj[1] > dllmax[cnt]) {
+				gold_sadj[1] = dllmax[cnt];
+			}
+		}
+	}
+	gold_sadj[0] = (gold_sadj[1] + gold_sadj[0]) >> 1;
+	gold_sadj[1] = mindwm(ast, 0x1E6E0080);
+
+	data = 0;
+	for (cnt = 0; cnt < 8; cnt++) {
+		data >>= 3;
+		data2 = gold_sadj[1] & 0x7;
+		gold_sadj[1] >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[0] >= dlli) {
+				dlli = (gold_sadj[0] - dlli) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 3) {
+					data2 = (data2 + dlli) & 0x7;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[0]) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 4) {
+					data2 = (data2 - dlli) & 0x7;
+				}
+			}
+		}
+		data |= data2 << 21;
+	}
+	moutdwm(ast, 0x1E6E0080, data);
+
+	gold_sadj[0] = 0x0;
+	gold_sadj[1] = 0xFF;
+	for (cnt = 8; cnt < 16; cnt++) {
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			if (gold_sadj[0] < dllmin[cnt]) {
+				gold_sadj[0] = dllmin[cnt];
+			}
+			if (gold_sadj[1] > dllmax[cnt]) {
+				gold_sadj[1] = dllmax[cnt];
+			}
+		}
+	}
+	gold_sadj[0] = (gold_sadj[1] + gold_sadj[0]) >> 1;
+	gold_sadj[1] = mindwm(ast, 0x1E6E0084);
+
+	data = 0;
+	for (cnt = 8; cnt < 16; cnt++) {
+		data >>= 3;
+		data2 = gold_sadj[1] & 0x7;
+		gold_sadj[1] >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[0] >= dlli) {
+				dlli = (gold_sadj[0] - dlli) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 3) {
+					data2 = (data2 + dlli) & 0x7;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[0]) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 4) {
+					data2 = (data2 - dlli) & 0x7;
+				}
+			}
+		}
+		data |= data2 << 21;
+	}
+	moutdwm(ast, 0x1E6E0084, data);
+
+} /* finetuneDQI_L2 */
+
+static void cbr_dll2(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 dllmin[2], dllmax[2], dlli, data, data2, passcnt;
+
+
+	finetuneDQI_L(ast, param);
+	finetuneDQI_L2(ast, param);
+
+CBR_START2:
+	dllmin[0] = dllmin[1] = 0xff;
+	dllmax[0] = dllmax[1] = 0x0;
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001300 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE2);
+		data = cbr_scan(ast);
+		if (data != 0) {
+			if (data & 0x1) {
+				if (dllmin[0] > dlli) {
+					dllmin[0] = dlli;
+				}
+				if (dllmax[0] < dlli) {
+					dllmax[0] = dlli;
+				}
+			}
+			if (data & 0x2) {
+				if (dllmin[1] > dlli) {
+					dllmin[1] = dlli;
+				}
+				if (dllmax[1] < dlli) {
+					dllmax[1] = dlli;
+				}
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD) {
+			break;
+		}
+	}
+	if (dllmax[0] == 0 || (dllmax[0]-dllmin[0]) < CBR_THRESHOLD) {
+		goto CBR_START2;
+	}
+	if (dllmax[1] == 0 || (dllmax[1]-dllmin[1]) < CBR_THRESHOLD) {
+		goto CBR_START2;
+	}
+	dlli  = (dllmin[1] + dllmax[1]) >> 1;
+	dlli <<= 8;
+	dlli += (dllmin[0] + dllmax[0]) >> 1;
+	moutdwm(ast, 0x1E6E0068, (mindwm(ast, 0x1E6E0068) & 0xFFFF) | (dlli << 16));
+
+	data  = (mindwm(ast, 0x1E6E0080) >> 24) & 0x1F;
+	data2 = (mindwm(ast, 0x1E6E0018) & 0xff80ffff) | (data << 16);
+	moutdwm(ast, 0x1E6E0018, data2);
+	moutdwm(ast, 0x1E6E0024, 0x8001 | (data << 1) | (param->dll2_finetune_step << 8));
+
+	/* Wait DQSI latch phase calibration */
+	moutdwm(ast, 0x1E6E0074, 0x00000010);
+	moutdwm(ast, 0x1E6E0070, 0x00000003);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0070, 0x00000003);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+} /* CBRDLL2 */
+
+static void get_ddr3_info(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 trap, trap_AC2, trap_MRS;
+
+	moutdwm(ast, 0x1E6E2000, 0x1688A8A8);
+
+	/* Ger trap info */
+	trap = (mindwm(ast, 0x1E6E2070) >> 25) & 0x3;
+	trap_AC2  = 0x00020000 + (trap << 16);
+	trap_AC2 |= 0x00300000 + ((trap & 0x2) << 19);
+	trap_MRS  = 0x00000010 + (trap << 4);
+	trap_MRS |= ((trap & 0x2) << 18);
+
+	param->reg_MADJ       = 0x00034C4C;
+	param->reg_SADJ       = 0x00001800;
+	param->reg_DRV        = 0x000000F0;
+	param->reg_PERIOD     = param->dram_freq;
+	param->rodt           = 0;
+
+	switch (param->dram_freq) {
+	case 336:
+		moutdwm(ast, 0x1E6E2020, 0x0190);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x22202725;
+		param->reg_AC2       = 0xAA007613 | trap_AC2;
+		param->reg_DQSIC     = 0x000000BA;
+		param->reg_MRS       = 0x04001400 | trap_MRS;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000074;
+		param->reg_FREQ      = 0x00004DC0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 3;
+		break;
+	default:
+	case 396:
+		moutdwm(ast, 0x1E6E2020, 0x03F1);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x33302825;
+		param->reg_AC2       = 0xCC009617 | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x04001600 | trap_MRS;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		default:
+		case AST_DRAM_512Mx16:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC009617 | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC009622 | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00963F | trap_AC2;
+			break;
+		}
+		break;
+
+	case 408:
+		moutdwm(ast, 0x1E6E2020, 0x01F0);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x33302825;
+		param->reg_AC2       = 0xCC009617 | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x04001600 | trap_MRS;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		default:
+		case AST_DRAM_512Mx16:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC009617 | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC009622 | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00963F | trap_AC2;
+			break;
+		}
+
+		break;
+	case 456:
+		moutdwm(ast, 0x1E6E2020, 0x0230);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x33302926;
+		param->reg_AC2       = 0xCD44961A;
+		param->reg_DQSIC     = 0x000000FC;
+		param->reg_MRS       = 0x00081830;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x00000097;
+		param->reg_FREQ      = 0x000052C0;
+		param->madj_max      = 88;
+		param->dll2_finetune_step = 4;
+		break;
+	case 504:
+		moutdwm(ast, 0x1E6E2020, 0x0270);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x33302926;
+		param->reg_AC2       = 0xDE44A61D;
+		param->reg_DQSIC     = 0x00000117;
+		param->reg_MRS       = 0x00081A30;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x070000BB;
+		param->reg_DQIDLY    = 0x000000A0;
+		param->reg_FREQ      = 0x000054C0;
+		param->madj_max      = 79;
+		param->dll2_finetune_step = 4;
+		break;
+	case 528:
+		moutdwm(ast, 0x1E6E2020, 0x0290);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302926;
+		param->reg_AC2       = 0xEF44B61E;
+		param->reg_DQSIC     = 0x00000125;
+		param->reg_MRS       = 0x00081A30;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000023;
+		param->reg_DQIDLY    = 0x00000088;
+		param->reg_FREQ      = 0x000055C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	case 576:
+		moutdwm(ast, 0x1E6E2020, 0x0140);
+		param->reg_MADJ      = 0x00136868;
+		param->reg_SADJ      = 0x00004534;
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302A37;
+		param->reg_AC2       = 0xEF56B61E;
+		param->reg_DQSIC     = 0x0000013F;
+		param->reg_MRS       = 0x00101A50;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000023;
+		param->reg_DQIDLY    = 0x00000078;
+		param->reg_FREQ      = 0x000057C0;
+		param->madj_max      = 136;
+		param->dll2_finetune_step = 3;
+		break;
+	case 600:
+		moutdwm(ast, 0x1E6E2020, 0x02E1);
+		param->reg_MADJ      = 0x00136868;
+		param->reg_SADJ      = 0x00004534;
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x32302A37;
+		param->reg_AC2       = 0xDF56B61F;
+		param->reg_DQSIC     = 0x0000014D;
+		param->reg_MRS       = 0x00101A50;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000023;
+		param->reg_DQIDLY    = 0x00000078;
+		param->reg_FREQ      = 0x000058C0;
+		param->madj_max      = 132;
+		param->dll2_finetune_step = 3;
+		break;
+	case 624:
+		moutdwm(ast, 0x1E6E2020, 0x0160);
+		param->reg_MADJ      = 0x00136868;
+		param->reg_SADJ      = 0x00004534;
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x32302A37;
+		param->reg_AC2       = 0xEF56B621;
+		param->reg_DQSIC     = 0x0000015A;
+		param->reg_MRS       = 0x02101A50;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000078;
+		param->reg_FREQ      = 0x000059C0;
+		param->madj_max      = 128;
+		param->dll2_finetune_step = 3;
+		break;
+	} /* switch freq */
+
+	switch (param->dram_chipid) {
+	case AST_DRAM_512Mx16:
+		param->dram_config = 0x130;
+		break;
+	default:
+	case AST_DRAM_1Gx16:
+		param->dram_config = 0x131;
+		break;
+	case AST_DRAM_2Gx16:
+		param->dram_config = 0x132;
+		break;
+	case AST_DRAM_4Gx16:
+		param->dram_config = 0x133;
+		break;
+	}; /* switch size */
+
+	switch (param->vram_size) {
+	default:
+	case AST_VIDMEM_SIZE_8M:
+		param->dram_config |= 0x00;
+		break;
+	case AST_VIDMEM_SIZE_16M:
+		param->dram_config |= 0x04;
+		break;
+	case AST_VIDMEM_SIZE_32M:
+		param->dram_config |= 0x08;
+		break;
+	case AST_VIDMEM_SIZE_64M:
+		param->dram_config |= 0x0c;
+		break;
+	}
+
+}
+
+static void ddr3_init(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 data, data2;
+
+	moutdwm(ast, 0x1E6E0000, 0xFC600309);
+	moutdwm(ast, 0x1E6E0018, 0x00000100);
+	moutdwm(ast, 0x1E6E0024, 0x00000000);
+	moutdwm(ast, 0x1E6E0034, 0x00000000);
+	udelay(10);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ);
+	moutdwm(ast, 0x1E6E0068, param->reg_SADJ);
+	udelay(10);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ | 0xC0000);
+	udelay(10);
+
+	moutdwm(ast, 0x1E6E0004, param->dram_config);
+	moutdwm(ast, 0x1E6E0008, 0x90040f);
+	moutdwm(ast, 0x1E6E0010, param->reg_AC1);
+	moutdwm(ast, 0x1E6E0014, param->reg_AC2);
+	moutdwm(ast, 0x1E6E0020, param->reg_DQSIC);
+	moutdwm(ast, 0x1E6E0080, 0x00000000);
+	moutdwm(ast, 0x1E6E0084, 0x00000000);
+	moutdwm(ast, 0x1E6E0088, param->reg_DQIDLY);
+	moutdwm(ast, 0x1E6E0018, 0x4040A170);
+	moutdwm(ast, 0x1E6E0018, 0x20402370);
+	moutdwm(ast, 0x1E6E0038, 0x00000000);
+	moutdwm(ast, 0x1E6E0040, 0xFF444444);
+	moutdwm(ast, 0x1E6E0044, 0x22222222);
+	moutdwm(ast, 0x1E6E0048, 0x22222222);
+	moutdwm(ast, 0x1E6E004C, 0x00000002);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+	moutdwm(ast, 0x1E6E0054, 0);
+	moutdwm(ast, 0x1E6E0060, param->reg_DRV);
+	moutdwm(ast, 0x1E6E006C, param->reg_IOZ);
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0074, 0x00000000);
+	moutdwm(ast, 0x1E6E0078, 0x00000000);
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+	/* Wait MCLK2X lock to MCLK */
+	do {
+		data = mindwm(ast, 0x1E6E001C);
+	} while (!(data & 0x08000000));
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00005C04);
+	udelay(10);
+	moutdwm(ast, 0x1E6E000C, 0x00000000);
+	moutdwm(ast, 0x1E6E0034, 0x00000000);
+	data = mindwm(ast, 0x1E6E001C);
+	data = (data >> 8) & 0xff;
+	while ((data & 0x08) || ((data & 0x7) < 2) || (data < 4)) {
+		data2 = (mindwm(ast, 0x1E6E0064) & 0xfff3ffff) + 4;
+		if ((data2 & 0xff) > param->madj_max) {
+			break;
+		}
+		moutdwm(ast, 0x1E6E0064, data2);
+		if (data2 & 0x00100000) {
+			data2 = ((data2 & 0xff) >> 3) + 3;
+		} else {
+			data2 = ((data2 & 0xff) >> 2) + 5;
+		}
+		data = mindwm(ast, 0x1E6E0068) & 0xffff00ff;
+		data2 += data & 0xff;
+		data = data | (data2 << 8);
+		moutdwm(ast, 0x1E6E0068, data);
+		udelay(10);
+		moutdwm(ast, 0x1E6E0064, mindwm(ast, 0x1E6E0064) | 0xC0000);
+		udelay(10);
+		data = mindwm(ast, 0x1E6E0018) & 0xfffff1ff;
+		moutdwm(ast, 0x1E6E0018, data);
+		data = data | 0x200;
+		moutdwm(ast, 0x1E6E0018, data);
+		do {
+			data = mindwm(ast, 0x1E6E001C);
+		} while (!(data & 0x08000000));
+
+		moutdwm(ast, 0x1E6E0034, 0x00000001);
+		moutdwm(ast, 0x1E6E000C, 0x00005C04);
+		udelay(10);
+		moutdwm(ast, 0x1E6E000C, 0x00000000);
+		moutdwm(ast, 0x1E6E0034, 0x00000000);
+		data = mindwm(ast, 0x1E6E001C);
+		data = (data >> 8) & 0xff;
+	}
+	data = mindwm(ast, 0x1E6E0018) | 0xC00;
+	moutdwm(ast, 0x1E6E0018, data);
+
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00000040);
+	udelay(50);
+	/* Mode Register Setting */
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS | 0x100);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000005);
+	moutdwm(ast, 0x1E6E0028, 0x00000007);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS);
+	moutdwm(ast, 0x1E6E000C, 0x00005C08);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+
+	moutdwm(ast, 0x1E6E000C, 0x7FFF5C01);
+	data = 0;
+	if (param->wodt) {
+		data = 0x300;
+	}
+	if (param->rodt) {
+		data = data | 0x3000 | ((param->reg_AC2 & 0x60000) >> 3);
+	}
+	moutdwm(ast, 0x1E6E0034, data | 0x3);
+
+	/* Wait DQI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0080);
+	} while (!(data & 0x40000000));
+	/* Wait DQSI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0020);
+	} while (!(data & 0x00000800));
+	/* Calibrate the DQSI delay */
+	cbr_dll2(ast, param);
+
+	moutdwm(ast, 0x1E6E0120, param->reg_FREQ);
+	/* ECC Memory Initialization */
+#ifdef ECC
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+	moutdwm(ast, 0x1E6E0070, 0x221);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+#endif
+
+
+}
+
+static void get_ddr2_info(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 trap, trap_AC2, trap_MRS;
+
+	moutdwm(ast, 0x1E6E2000, 0x1688A8A8);
+
+	/* Ger trap info */
+	trap = (mindwm(ast, 0x1E6E2070) >> 25) & 0x3;
+	trap_AC2  = (trap << 20) | (trap << 16);
+	trap_AC2 += 0x00110000;
+	trap_MRS  = 0x00000040 | (trap << 4);
+
+
+	param->reg_MADJ       = 0x00034C4C;
+	param->reg_SADJ       = 0x00001800;
+	param->reg_DRV        = 0x000000F0;
+	param->reg_PERIOD     = param->dram_freq;
+	param->rodt           = 0;
+
+	switch (param->dram_freq) {
+	case 264:
+		moutdwm(ast, 0x1E6E2020, 0x0130);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x11101513;
+		param->reg_AC2       = 0x78117011;
+		param->reg_DQSIC     = 0x00000092;
+		param->reg_MRS       = 0x00000842;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_DRV       = 0x000000F0;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x0000005A;
+		param->reg_FREQ      = 0x00004AC0;
+		param->madj_max      = 138;
+		param->dll2_finetune_step = 3;
+		break;
+	case 336:
+		moutdwm(ast, 0x1E6E2020, 0x0190);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x22202613;
+		param->reg_AC2       = 0xAA009016 | trap_AC2;
+		param->reg_DQSIC     = 0x000000BA;
+		param->reg_MRS       = 0x00000A02 | trap_MRS;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000074;
+		param->reg_FREQ      = 0x00004DC0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 3;
+		break;
+	default:
+	case 396:
+		moutdwm(ast, 0x1E6E2020, 0x03F1);
+		param->wodt          = 1;
+		param->rodt          = 0;
+		param->reg_AC1       = 0x33302714;
+		param->reg_AC2       = 0xCC00B01B | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x00000C02 | trap_MRS;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		case AST_DRAM_512Mx16:
+			param->reg_AC2   = 0xCC00B016 | trap_AC2;
+			break;
+		default:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC00B01B | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC00B02B | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00B03F | trap_AC2;
+			break;
+		}
+
+		break;
+
+	case 408:
+		moutdwm(ast, 0x1E6E2020, 0x01F0);
+		param->wodt          = 1;
+		param->rodt          = 0;
+		param->reg_AC1       = 0x33302714;
+		param->reg_AC2       = 0xCC00B01B | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x00000C02 | trap_MRS;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		case AST_DRAM_512Mx16:
+			param->reg_AC2   = 0xCC00B016 | trap_AC2;
+			break;
+		default:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC00B01B | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC00B02B | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00B03F | trap_AC2;
+			break;
+		}
+
+		break;
+	case 456:
+		moutdwm(ast, 0x1E6E2020, 0x0230);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x33302815;
+		param->reg_AC2       = 0xCD44B01E;
+		param->reg_DQSIC     = 0x000000FC;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_DRV       = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000097;
+		param->reg_FREQ      = 0x000052C0;
+		param->madj_max      = 88;
+		param->dll2_finetune_step = 3;
+		break;
+	case 504:
+		moutdwm(ast, 0x1E6E2020, 0x0261);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302815;
+		param->reg_AC2       = 0xDE44C022;
+		param->reg_DQSIC     = 0x00000117;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x0000000A;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000A0;
+		param->reg_FREQ      = 0x000054C0;
+		param->madj_max      = 79;
+		param->dll2_finetune_step = 3;
+		break;
+	case 528:
+		moutdwm(ast, 0x1E6E2020, 0x0120);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302815;
+		param->reg_AC2       = 0xEF44D024;
+		param->reg_DQSIC     = 0x00000125;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F9;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000A7;
+		param->reg_FREQ      = 0x000055C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	case 552:
+		moutdwm(ast, 0x1E6E2020, 0x02A1);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x43402915;
+		param->reg_AC2       = 0xFF44E025;
+		param->reg_DQSIC     = 0x00000132;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x0000000A;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000AD;
+		param->reg_FREQ      = 0x000056C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	case 576:
+		moutdwm(ast, 0x1E6E2020, 0x0140);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x43402915;
+		param->reg_AC2       = 0xFF44E027;
+		param->reg_DQSIC     = 0x0000013F;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000B3;
+		param->reg_FREQ      = 0x000057C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	}
+
+	switch (param->dram_chipid) {
+	case AST_DRAM_512Mx16:
+		param->dram_config = 0x100;
+		break;
+	default:
+	case AST_DRAM_1Gx16:
+		param->dram_config = 0x121;
+		break;
+	case AST_DRAM_2Gx16:
+		param->dram_config = 0x122;
+		break;
+	case AST_DRAM_4Gx16:
+		param->dram_config = 0x123;
+		break;
+	}; /* switch size */
+
+	switch (param->vram_size) {
+	default:
+	case AST_VIDMEM_SIZE_8M:
+		param->dram_config |= 0x00;
+		break;
+	case AST_VIDMEM_SIZE_16M:
+		param->dram_config |= 0x04;
+		break;
+	case AST_VIDMEM_SIZE_32M:
+		param->dram_config |= 0x08;
+		break;
+	case AST_VIDMEM_SIZE_64M:
+		param->dram_config |= 0x0c;
+		break;
+	}
+}
+
+static void ddr2_init(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 data, data2;
+
+	moutdwm(ast, 0x1E6E0000, 0xFC600309);
+	moutdwm(ast, 0x1E6E0018, 0x00000100);
+	moutdwm(ast, 0x1E6E0024, 0x00000000);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ);
+	moutdwm(ast, 0x1E6E0068, param->reg_SADJ);
+	udelay(10);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ | 0xC0000);
+	udelay(10);
+
+	moutdwm(ast, 0x1E6E0004, param->dram_config);
+	moutdwm(ast, 0x1E6E0008, 0x90040f);
+	moutdwm(ast, 0x1E6E0010, param->reg_AC1);
+	moutdwm(ast, 0x1E6E0014, param->reg_AC2);
+	moutdwm(ast, 0x1E6E0020, param->reg_DQSIC);
+	moutdwm(ast, 0x1E6E0080, 0x00000000);
+	moutdwm(ast, 0x1E6E0084, 0x00000000);
+	moutdwm(ast, 0x1E6E0088, param->reg_DQIDLY);
+	moutdwm(ast, 0x1E6E0018, 0x4040A130);
+	moutdwm(ast, 0x1E6E0018, 0x20402330);
+	moutdwm(ast, 0x1E6E0038, 0x00000000);
+	moutdwm(ast, 0x1E6E0040, 0xFF808000);
+	moutdwm(ast, 0x1E6E0044, 0x88848466);
+	moutdwm(ast, 0x1E6E0048, 0x44440008);
+	moutdwm(ast, 0x1E6E004C, 0x00000000);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+	moutdwm(ast, 0x1E6E0054, 0);
+	moutdwm(ast, 0x1E6E0060, param->reg_DRV);
+	moutdwm(ast, 0x1E6E006C, param->reg_IOZ);
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0074, 0x00000000);
+	moutdwm(ast, 0x1E6E0078, 0x00000000);
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+
+	/* Wait MCLK2X lock to MCLK */
+	do {
+		data = mindwm(ast, 0x1E6E001C);
+	} while (!(data & 0x08000000));
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00005C04);
+	udelay(10);
+	moutdwm(ast, 0x1E6E000C, 0x00000000);
+	moutdwm(ast, 0x1E6E0034, 0x00000000);
+	data = mindwm(ast, 0x1E6E001C);
+	data = (data >> 8) & 0xff;
+	while ((data & 0x08) || ((data & 0x7) < 2) || (data < 4)) {
+		data2 = (mindwm(ast, 0x1E6E0064) & 0xfff3ffff) + 4;
+		if ((data2 & 0xff) > param->madj_max) {
+			break;
+		}
+		moutdwm(ast, 0x1E6E0064, data2);
+		if (data2 & 0x00100000) {
+			data2 = ((data2 & 0xff) >> 3) + 3;
+		} else {
+			data2 = ((data2 & 0xff) >> 2) + 5;
+		}
+		data = mindwm(ast, 0x1E6E0068) & 0xffff00ff;
+		data2 += data & 0xff;
+		data = data | (data2 << 8);
+		moutdwm(ast, 0x1E6E0068, data);
+		udelay(10);
+		moutdwm(ast, 0x1E6E0064, mindwm(ast, 0x1E6E0064) | 0xC0000);
+		udelay(10);
+		data = mindwm(ast, 0x1E6E0018) & 0xfffff1ff;
+		moutdwm(ast, 0x1E6E0018, data);
+		data = data | 0x200;
+		moutdwm(ast, 0x1E6E0018, data);
+		do {
+			data = mindwm(ast, 0x1E6E001C);
+		} while (!(data & 0x08000000));
+
+		moutdwm(ast, 0x1E6E0034, 0x00000001);
+		moutdwm(ast, 0x1E6E000C, 0x00005C04);
+		udelay(10);
+		moutdwm(ast, 0x1E6E000C, 0x00000000);
+		moutdwm(ast, 0x1E6E0034, 0x00000000);
+		data = mindwm(ast, 0x1E6E001C);
+		data = (data >> 8) & 0xff;
+	}
+	data = mindwm(ast, 0x1E6E0018) | 0xC00;
+	moutdwm(ast, 0x1E6E0018, data);
+
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00000000);
+	udelay(50);
+	/* Mode Register Setting */
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS | 0x100);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000005);
+	moutdwm(ast, 0x1E6E0028, 0x00000007);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+
+	moutdwm(ast, 0x1E6E000C, 0x00005C08);
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS | 0x380);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+
+	moutdwm(ast, 0x1E6E000C, 0x7FFF5C01);
+	data = 0;
+	if (param->wodt) {
+		data = 0x500;
+	}
+	if (param->rodt) {
+		data = data | 0x3000 | ((param->reg_AC2 & 0x60000) >> 3);
+	}
+	moutdwm(ast, 0x1E6E0034, data | 0x3);
+	moutdwm(ast, 0x1E6E0120, param->reg_FREQ);
+
+	/* Wait DQI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0080);
+	} while (!(data & 0x40000000));
+	/* Wait DQSI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0020);
+	} while (!(data & 0x00000800));
+	/* Calibrate the DQSI delay */
+	cbr_dll2(ast, param);
+
+	/* ECC Memory Initialization */
+#ifdef ECC
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+	moutdwm(ast, 0x1E6E0070, 0x221);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+#endif
+
+}
+
+static void ast_init_dram_2300(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast2300_dram_param param;
+	u32 temp;
+	u8 reg;
+
+	reg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+	if ((reg & 0x80) == 0) {/* vga only */
+		ast_write32(ast, 0xf004, 0x1e6e0000);
+		ast_write32(ast, 0xf000, 0x1);
+		ast_write32(ast, 0x12000, 0x1688a8a8);
+		do {
+			;
+		} while (ast_read32(ast, 0x12000) != 0x1);
+
+		ast_write32(ast, 0x10000, 0xfc600309);
+		do {
+			;
+		} while (ast_read32(ast, 0x10000) != 0x1);
+
+		/* Slow down CPU/AHB CLK in VGA only mode */
+		temp = ast_read32(ast, 0x12008);
+		temp |= 0x73;
+		ast_write32(ast, 0x12008, temp);
+
+		param.dram_type = AST_DDR3;
+		if (temp & 0x01000000)
+			param.dram_type = AST_DDR2;
+		param.dram_chipid = ast->dram_type;
+		param.dram_freq = ast->mclk;
+		param.vram_size = ast->vram_size;
+
+		if (param.dram_type == AST_DDR3) {
+			get_ddr3_info(ast, &param);
+			ddr3_init(ast, &param);
+		} else {
+			get_ddr2_info(ast, &param);
+			ddr2_init(ast, &param);
+		}
+
+		temp = mindwm(ast, 0x1e6e2040);
+		moutdwm(ast, 0x1e6e2040, temp | 0x40);
+	}
+
+	/* wait ready */
+	do {
+		reg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+	} while ((reg & 0x40) == 0);
+}
+
diff --git a/drivers/gpu/drm/ast/ast_tables.h b/drivers/gpu/drm/ast/ast_tables.h
new file mode 100644
index 000000000000..95fa6aba26bc
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_tables.h
@@ -0,0 +1,265 @@
+/*
+ * Copyright (c) 2005 ASPEED Technology Inc.
+ *
+ * Permission to use, copy, modify, distribute, and sell this software and its
+ * documentation for any purpose is hereby granted without fee, provided that
+ * the above copyright notice appear in all copies and that both that
+ * copyright notice and this permission notice appear in supporting
+ * documentation, and that the name of the authors not be used in
+ * advertising or publicity pertaining to distribution of the software without
+ * specific, written prior permission.  The authors makes no representations
+ * about the suitability of this software for any purpose.  It is provided
+ * "as is" without express or implied warranty.
+ *
+ * THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+ * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+ * EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+ * CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
+ * DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
+ * TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+ * PERFORMANCE OF THIS SOFTWARE.
+ */
+/* Ported from xf86-video-ast driver */
+
+#ifndef AST_TABLES_H
+#define AST_TABLES_H
+
+/* Std. Table Index Definition */
+#define TextModeIndex		0
+#define EGAModeIndex		1
+#define VGAModeIndex		2
+#define HiCModeIndex		3
+#define TrueCModeIndex		4
+
+#define Charx8Dot               0x00000001
+#define HalfDCLK                0x00000002
+#define DoubleScanMode          0x00000004
+#define LineCompareOff          0x00000008
+#define SyncPP                  0x00000000
+#define SyncPN                  0x00000040
+#define SyncNP                  0x00000080
+#define SyncNN                  0x000000C0
+#define HBorder                 0x00000020
+#define VBorder                 0x00000010
+#define WideScreenMode		0x00000100
+
+
+/* DCLK Index */
+#define VCLK25_175     		0x00
+#define VCLK28_322     		0x01
+#define VCLK31_5       		0x02
+#define VCLK36         		0x03
+#define VCLK40         		0x04
+#define VCLK49_5       		0x05
+#define VCLK50         		0x06
+#define VCLK56_25      		0x07
+#define VCLK65		 	0x08
+#define VCLK75	        	0x09
+#define VCLK78_75      		0x0A
+#define VCLK94_5       		0x0B
+#define VCLK108        		0x0C
+#define VCLK135        		0x0D
+#define VCLK157_5      		0x0E
+#define VCLK162        		0x0F
+/* #define VCLK193_25     		0x10 */
+#define VCLK154     		0x10
+#define VCLK83_5    		0x11
+#define VCLK106_5   		0x12
+#define VCLK146_25  		0x13
+#define VCLK148_5   		0x14
+
+static struct ast_vbios_dclk_info dclk_table[] = {
+	{0x2C, 0xE7, 0x03},					/* 00: VCLK25_175	*/
+	{0x95, 0x62, 0x03},				        /* 01: VCLK28_322	*/
+	{0x67, 0x63, 0x01},				        /* 02: VCLK31_5         */
+	{0x76, 0x63, 0x01},				        /* 03: VCLK36         	*/
+	{0xEE, 0x67, 0x01},				        /* 04: VCLK40          	*/
+	{0x82, 0x62, 0x01}, 			        /* 05: VCLK49_5        	*/
+	{0xC6, 0x64, 0x01},                        	        /* 06: VCLK50          	*/
+	{0x94, 0x62, 0x01},                        	        /* 07: VCLK56_25       	*/
+	{0x80, 0x64, 0x00},                        	        /* 08: VCLK65		*/
+	{0x7B, 0x63, 0x00},                        	        /* 09: VCLK75	        */
+	{0x67, 0x62, 0x00},				        /* 0A: VCLK78_75       	*/
+	{0x7C, 0x62, 0x00},                        	        /* 0B: VCLK94_5        	*/
+	{0x8E, 0x62, 0x00},                        	        /* 0C: VCLK108         	*/
+	{0x85, 0x24, 0x00},                        	        /* 0D: VCLK135         	*/
+	{0x67, 0x22, 0x00},                        	        /* 0E: VCLK157_5       	*/
+	{0x6A, 0x22, 0x00},				        /* 0F: VCLK162         	*/
+	{0x4d, 0x4c, 0x80},				        /* 10: VCLK154      	*/
+	{0xa7, 0x78, 0x80},					/* 11: VCLK83.5         */
+	{0x28, 0x49, 0x80},					/* 12: VCLK106.5        */
+	{0x37, 0x49, 0x80},					/* 13: VCLK146.25       */
+	{0x1f, 0x45, 0x80},					/* 14: VCLK148.5        */
+};
+
+static struct ast_vbios_stdtable vbios_stdtable[] = {
+	/* MD_2_3_400 */
+	{
+		0x67,
+		{0x00,0x03,0x00,0x02},
+		{0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+		 0x00,0x4f,0x0d,0x0e,0x00,0x00,0x00,0x00,
+		 0x9c,0x8e,0x8f,0x28,0x1f,0x96,0xb9,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+		 0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+		 0x0c,0x00,0x0f,0x08},
+		{0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+		 0xff}
+	},
+	/* Mode12/ExtEGATable */
+	{
+		0xe3,
+		{0x01,0x0f,0x00,0x06},
+		{0x5f,0x4f,0x50,0x82,0x55,0x81,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xe9,0x8b,0xdf,0x28,0x00,0xe7,0x04,0xe3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+		 0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+		 0x01,0x00,0x0f,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+		 0xff}
+	},
+	/* ExtVGATable */
+	{
+		0x2f,
+		{0x01,0x0f,0x00,0x0e},
+		{0x5f,0x4f,0x50,0x82,0x54,0x80,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xea,0x8c,0xdf,0x28,0x40,0xe7,0x04,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+		 0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+		 0x01,0x00,0x00,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x40,0x05,0x0f,
+		 0xff}
+	},
+	/* ExtHiCTable */
+	{
+		0x2f,
+		{0x01,0x0f,0x00,0x0e},
+		{0x5f,0x4f,0x50,0x82,0x54,0x80,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xea,0x8c,0xdf,0x28,0x40,0xe7,0x04,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+		 0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+		 0x01,0x00,0x00,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+		 0xff}
+	},
+	/* ExtTrueCTable */
+	{
+		0x2f,
+		{0x01,0x0f,0x00,0x0e},
+		{0x5f,0x4f,0x50,0x82,0x54,0x80,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xea,0x8c,0xdf,0x28,0x40,0xe7,0x04,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+		 0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+		 0x01,0x00,0x00,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+		 0xff}
+	},
+};
+
+static struct ast_vbios_enhtable res_640x480[] = {
+	{ 800, 640, 8, 96, 525, 480, 2, 2, VCLK25_175,	/* 60Hz */
+	  (SyncNN | HBorder | VBorder | Charx8Dot), 60, 1, 0x2E },
+	{ 832, 640, 16, 40, 520, 480, 1, 3, VCLK31_5,	/* 72Hz */
+	  (SyncNN | HBorder | VBorder | Charx8Dot), 72, 2, 0x2E  },
+	{ 840, 640, 16, 64, 500, 480, 1, 3, VCLK31_5,	/* 75Hz */
+	  (SyncNN | Charx8Dot) , 75, 3, 0x2E },
+	{ 832, 640, 56, 56, 509, 480, 1, 3, VCLK36,		/* 85Hz */
+	  (SyncNN | Charx8Dot) , 85, 4, 0x2E },
+	{ 832, 640, 56, 56, 509, 480, 1, 3, VCLK36,		/* end */
+	  (SyncNN | Charx8Dot) , 0xFF, 4, 0x2E },
+};
+
+static struct ast_vbios_enhtable res_800x600[] = {
+	{1024, 800, 24, 72, 625, 600, 1, 2, VCLK36,		/* 56Hz */
+	 (SyncPP | Charx8Dot), 56, 1, 0x30 },
+	{1056, 800, 40, 128, 628, 600, 1, 4, VCLK40,	/* 60Hz */
+	 (SyncPP | Charx8Dot), 60, 2, 0x30 },
+	{1040, 800, 56, 120, 666, 600, 37, 6, VCLK50,	/* 72Hz */
+	 (SyncPP | Charx8Dot), 72, 3, 0x30 },
+	{1056, 800, 16, 80, 625, 600, 1, 3, VCLK49_5,	/* 75Hz */
+	 (SyncPP | Charx8Dot), 75, 4, 0x30 },
+	{1048, 800, 32, 64, 631, 600, 1, 3, VCLK56_25,	/* 85Hz */
+	 (SyncPP | Charx8Dot), 84, 5, 0x30 },
+	{1048, 800, 32, 64, 631, 600, 1, 3, VCLK56_25,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 5, 0x30 },
+};
+
+
+static struct ast_vbios_enhtable res_1024x768[] = {
+	{1344, 1024, 24, 136, 806, 768, 3, 6, VCLK65,	/* 60Hz */
+	 (SyncNN | Charx8Dot), 60, 1, 0x31 },
+	{1328, 1024, 24, 136, 806, 768, 3, 6, VCLK75,	/* 70Hz */
+	 (SyncNN | Charx8Dot), 70, 2, 0x31 },
+	{1312, 1024, 16, 96, 800, 768, 1, 3, VCLK78_75,	/* 75Hz */
+	 (SyncPP | Charx8Dot), 75, 3, 0x31 },
+	{1376, 1024, 48, 96, 808, 768, 1, 3, VCLK94_5,	/* 85Hz */
+	 (SyncPP | Charx8Dot), 84, 4, 0x31 },
+	{1376, 1024, 48, 96, 808, 768, 1, 3, VCLK94_5,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 4, 0x31 },
+};
+
+static struct ast_vbios_enhtable res_1280x1024[] = {
+	{1688, 1280, 48, 112, 1066, 1024, 1, 3, VCLK108,	/* 60Hz */
+	 (SyncPP | Charx8Dot), 60, 1, 0x32 },
+	{1688, 1280, 16, 144, 1066, 1024, 1, 3, VCLK135,	/* 75Hz */
+	 (SyncPP | Charx8Dot), 75, 2, 0x32 },
+	{1728, 1280, 64, 160, 1072, 1024, 1, 3, VCLK157_5,	/* 85Hz */
+	 (SyncPP | Charx8Dot), 85, 3, 0x32 },
+	{1728, 1280, 64, 160, 1072, 1024, 1, 3, VCLK157_5,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 3, 0x32 },
+};
+
+static struct ast_vbios_enhtable res_1600x1200[] = {
+	{2160, 1600, 64, 192, 1250, 1200, 1, 3, VCLK162,	/* 60Hz */
+	 (SyncPP | Charx8Dot), 60, 1, 0x33 },
+	{2160, 1600, 64, 192, 1250, 1200, 1, 3, VCLK162,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 1, 0x33 },
+};
+
+static struct ast_vbios_enhtable res_1920x1200[] = {
+	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz */
+	 (SyncNP | Charx8Dot), 60, 1, 0x34 },
+	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz */
+	 (SyncNP | Charx8Dot), 0xFF, 1, 0x34 },
+};
+
+/* 16:10 */
+static struct ast_vbios_enhtable res_1280x800[] = {
+	{1680, 1280, 72,128,  831,  800, 3, 6, VCLK83_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x35 },
+	{1680, 1280, 72,128,  831,  800, 3, 6, VCLK83_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x35 },
+
+};
+
+static struct ast_vbios_enhtable res_1440x900[] = {
+	{1904, 1440, 80,152,  934,  900, 3, 6, VCLK106_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x36 },
+	{1904, 1440, 80,152,  934,  900, 3, 6, VCLK106_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x36 },
+};
+
+static struct ast_vbios_enhtable res_1680x1050[] = {
+	{2240, 1680,104,176, 1089, 1050, 3, 6, VCLK146_25,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x37 },
+	{2240, 1680,104,176, 1089, 1050, 3, 6, VCLK146_25,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x37 },
+};
+
+/* HDTV */
+static struct ast_vbios_enhtable res_1920x1080[] = {
+	{2200, 1920, 88, 44, 1125, 1080, 4, 5, VCLK148_5,	/* 60Hz */
+	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x38 },
+	{2200, 1920, 88, 44, 1125, 1080, 4, 5, VCLK148_5,	/* 60Hz */
+	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x38 },
+};
+#endif
diff --git a/drivers/gpu/drm/ast/ast_ttm.c b/drivers/gpu/drm/ast/ast_ttm.c
new file mode 100644
index 000000000000..6cf2adea66bc
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_ttm.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "ast_drv.h"
+#include <ttm/ttm_page_alloc.h>
+
+static inline struct ast_private *
+ast_bdev(struct ttm_bo_device *bd)
+{
+	return container_of(bd, struct ast_private, ttm.bdev);
+}
+
+static int
+ast_ttm_mem_global_init(struct drm_global_reference *ref)
+{
+	return ttm_mem_global_init(ref->object);
+}
+
+static void
+ast_ttm_mem_global_release(struct drm_global_reference *ref)
+{
+	ttm_mem_global_release(ref->object);
+}
+
+static int ast_ttm_global_init(struct ast_private *ast)
+{
+	struct drm_global_reference *global_ref;
+	int r;
+
+	global_ref = &ast->ttm.mem_global_ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_MEM;
+	global_ref->size = sizeof(struct ttm_mem_global);
+	global_ref->init = &ast_ttm_mem_global_init;
+	global_ref->release = &ast_ttm_mem_global_release;
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM memory accounting "
+			  "subsystem.\n");
+		return r;
+	}
+
+	ast->ttm.bo_global_ref.mem_glob =
+		ast->ttm.mem_global_ref.object;
+	global_ref = &ast->ttm.bo_global_ref.ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_BO;
+	global_ref->size = sizeof(struct ttm_bo_global);
+	global_ref->init = &ttm_bo_global_init;
+	global_ref->release = &ttm_bo_global_release;
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM BO subsystem.\n");
+		drm_global_item_unref(&ast->ttm.mem_global_ref);
+		return r;
+	}
+	return 0;
+}
+
+void
+ast_ttm_global_release(struct ast_private *ast)
+{
+	if (ast->ttm.mem_global_ref.release == NULL)
+		return;
+
+	drm_global_item_unref(&ast->ttm.bo_global_ref.ref);
+	drm_global_item_unref(&ast->ttm.mem_global_ref);
+	ast->ttm.mem_global_ref.release = NULL;
+}
+
+
+static void ast_bo_ttm_destroy(struct ttm_buffer_object *tbo)
+{
+	struct ast_bo *bo;
+
+	bo = container_of(tbo, struct ast_bo, bo);
+
+	drm_gem_object_release(&bo->gem);
+	kfree(bo);
+}
+
+bool ast_ttm_bo_is_ast_bo(struct ttm_buffer_object *bo)
+{
+	if (bo->destroy == &ast_bo_ttm_destroy)
+		return true;
+	return false;
+}
+
+static int
+ast_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
+		     struct ttm_mem_type_manager *man)
+{
+	switch (type) {
+	case TTM_PL_SYSTEM:
+		man->flags = TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_MASK_CACHING;
+		man->default_caching = TTM_PL_FLAG_CACHED;
+		break;
+	case TTM_PL_VRAM:
+		man->func = &ttm_bo_manager_func;
+		man->flags = TTM_MEMTYPE_FLAG_FIXED |
+			TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_FLAG_UNCACHED |
+			TTM_PL_FLAG_WC;
+		man->default_caching = TTM_PL_FLAG_WC;
+		break;
+	default:
+		DRM_ERROR("Unsupported memory type %u\n", (unsigned)type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void
+ast_bo_evict_flags(struct ttm_buffer_object *bo, struct ttm_placement *pl)
+{
+	struct ast_bo *astbo = ast_bo(bo);
+
+	if (!ast_ttm_bo_is_ast_bo(bo))
+		return;
+
+	ast_ttm_placement(astbo, TTM_PL_FLAG_SYSTEM);
+	*pl = astbo->placement;
+}
+
+static int ast_bo_verify_access(struct ttm_buffer_object *bo, struct file *filp)
+{
+	return 0;
+}
+
+static int ast_ttm_io_mem_reserve(struct ttm_bo_device *bdev,
+				  struct ttm_mem_reg *mem)
+{
+	struct ttm_mem_type_manager *man = &bdev->man[mem->mem_type];
+	struct ast_private *ast = ast_bdev(bdev);
+
+	mem->bus.addr = NULL;
+	mem->bus.offset = 0;
+	mem->bus.size = mem->num_pages << PAGE_SHIFT;
+	mem->bus.base = 0;
+	mem->bus.is_iomem = false;
+	if (!(man->flags & TTM_MEMTYPE_FLAG_MAPPABLE))
+		return -EINVAL;
+	switch (mem->mem_type) {
+	case TTM_PL_SYSTEM:
+		/* system memory */
+		return 0;
+	case TTM_PL_VRAM:
+		mem->bus.offset = mem->start << PAGE_SHIFT;
+		mem->bus.base = pci_resource_start(ast->dev->pdev, 0);
+		mem->bus.is_iomem = true;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
+
+static void ast_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
+{
+}
+
+static int ast_bo_move(struct ttm_buffer_object *bo,
+		       bool evict, bool interruptible,
+		       bool no_wait_reserve, bool no_wait_gpu,
+		       struct ttm_mem_reg *new_mem)
+{
+	int r;
+	r = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
+	return r;
+}
+
+
+static void ast_ttm_backend_destroy(struct ttm_tt *tt)
+{
+	ttm_tt_fini(tt);
+	kfree(tt);
+}
+
+static struct ttm_backend_func ast_tt_backend_func = {
+	.destroy = &ast_ttm_backend_destroy,
+};
+
+
+struct ttm_tt *ast_ttm_tt_create(struct ttm_bo_device *bdev,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
+{
+	struct ttm_tt *tt;
+
+	tt = kzalloc(sizeof(struct ttm_tt), GFP_KERNEL);
+	if (tt == NULL)
+		return NULL;
+	tt->func = &ast_tt_backend_func;
+	if (ttm_tt_init(tt, bdev, size, page_flags, dummy_read_page)) {
+		kfree(tt);
+		return NULL;
+	}
+	return tt;
+}
+
+static int ast_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	return ttm_pool_populate(ttm);
+}
+
+static void ast_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
+}
+
+struct ttm_bo_driver ast_bo_driver = {
+	.ttm_tt_create = ast_ttm_tt_create,
+	.ttm_tt_populate = ast_ttm_tt_populate,
+	.ttm_tt_unpopulate = ast_ttm_tt_unpopulate,
+	.init_mem_type = ast_bo_init_mem_type,
+	.evict_flags = ast_bo_evict_flags,
+	.move = ast_bo_move,
+	.verify_access = ast_bo_verify_access,
+	.io_mem_reserve = &ast_ttm_io_mem_reserve,
+	.io_mem_free = &ast_ttm_io_mem_free,
+};
+
+int ast_mm_init(struct ast_private *ast)
+{
+	int ret;
+	struct drm_device *dev = ast->dev;
+	struct ttm_bo_device *bdev = &ast->ttm.bdev;
+
+	ret = ast_ttm_global_init(ast);
+	if (ret)
+		return ret;
+
+	ret = ttm_bo_device_init(&ast->ttm.bdev,
+				 ast->ttm.bo_global_ref.ref.object,
+				 &ast_bo_driver, DRM_FILE_PAGE_OFFSET,
+				 true);
+	if (ret) {
+		DRM_ERROR("Error initialising bo driver; %d\n", ret);
+		return ret;
+	}
+
+	ret = ttm_bo_init_mm(bdev, TTM_PL_VRAM,
+			     ast->vram_size >> PAGE_SHIFT);
+	if (ret) {
+		DRM_ERROR("Failed ttm VRAM init: %d\n", ret);
+		return ret;
+	}
+
+	ast->fb_mtrr = drm_mtrr_add(pci_resource_start(dev->pdev, 0),
+				    pci_resource_len(dev->pdev, 0),
+				    DRM_MTRR_WC);
+
+	return 0;
+}
+
+void ast_mm_fini(struct ast_private *ast)
+{
+	struct drm_device *dev = ast->dev;
+	ttm_bo_device_release(&ast->ttm.bdev);
+
+	ast_ttm_global_release(ast);
+
+	if (ast->fb_mtrr >= 0) {
+		drm_mtrr_del(ast->fb_mtrr,
+			     pci_resource_start(dev->pdev, 0),
+			     pci_resource_len(dev->pdev, 0), DRM_MTRR_WC);
+		ast->fb_mtrr = -1;
+	}
+}
+
+void ast_ttm_placement(struct ast_bo *bo, int domain)
+{
+	u32 c = 0;
+	bo->placement.fpfn = 0;
+	bo->placement.lpfn = 0;
+	bo->placement.placement = bo->placements;
+	bo->placement.busy_placement = bo->placements;
+	if (domain & TTM_PL_FLAG_VRAM)
+		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+	if (domain & TTM_PL_FLAG_SYSTEM)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	if (!c)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	bo->placement.num_placement = c;
+	bo->placement.num_busy_placement = c;
+}
+
+int ast_bo_reserve(struct ast_bo *bo, bool no_wait)
+{
+	int ret;
+
+	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("reserve failed %p\n", bo);
+		return ret;
+	}
+	return 0;
+}
+
+void ast_bo_unreserve(struct ast_bo *bo)
+{
+	ttm_bo_unreserve(&bo->bo);
+}
+
+int ast_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct ast_bo **pastbo)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast_bo *astbo;
+	size_t acc_size;
+	int ret;
+
+	astbo = kzalloc(sizeof(struct ast_bo), GFP_KERNEL);
+	if (!astbo)
+		return -ENOMEM;
+
+	ret = drm_gem_object_init(dev, &astbo->gem, size);
+	if (ret) {
+		kfree(astbo);
+		return ret;
+	}
+
+	astbo->gem.driver_private = NULL;
+	astbo->bo.bdev = &ast->ttm.bdev;
+
+	ast_ttm_placement(astbo, TTM_PL_FLAG_VRAM | TTM_PL_FLAG_SYSTEM);
+
+	acc_size = ttm_bo_dma_acc_size(&ast->ttm.bdev, size,
+				       sizeof(struct ast_bo));
+
+	ret = ttm_bo_init(&ast->ttm.bdev, &astbo->bo, size,
+			  ttm_bo_type_device, &astbo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  NULL, ast_bo_ttm_destroy);
+	if (ret)
+		return ret;
+
+	*pastbo = astbo;
+	return 0;
+}
+
+static inline u64 ast_bo_gpu_offset(struct ast_bo *bo)
+{
+	return bo->bo.offset;
+}
+
+int ast_bo_pin(struct ast_bo *bo, u32 pl_flag, u64 *gpu_addr)
+{
+	int i, ret;
+
+	if (bo->pin_count) {
+		bo->pin_count++;
+		if (gpu_addr)
+			*gpu_addr = ast_bo_gpu_offset(bo);
+	}
+
+	ast_ttm_placement(bo, pl_flag);
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	bo->pin_count = 1;
+	if (gpu_addr)
+		*gpu_addr = ast_bo_gpu_offset(bo);
+	return 0;
+}
+
+int ast_bo_unpin(struct ast_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int ast_bo_push_sysram(struct ast_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	if (bo->kmap.virtual)
+		ttm_bo_kunmap(&bo->kmap);
+
+	ast_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret) {
+		DRM_ERROR("pushing to VRAM failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+int ast_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *file_priv;
+	struct ast_private *ast;
+
+	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
+		return drm_mmap(filp, vma);
+
+	file_priv = filp->private_data;
+	ast = file_priv->minor->dev->dev_private;
+	return ttm_bo_mmap(filp, vma, &ast->ttm.bdev);
+}
diff --git a/drivers/gpu/drm/cirrus/Kconfig b/drivers/gpu/drm/cirrus/Kconfig
new file mode 100644
index 000000000000..fc154dd75296
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/Kconfig
@@ -0,0 +1,12 @@
+config DRM_CIRRUS_QEMU
+	tristate "Cirrus driver for QEMU emulated device"
+	depends on DRM && PCI && EXPERIMENTAL
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select DRM_KMS_HELPER
+	select DRM_TTM
+	help
+	 This is a KMS driver for emulated cirrus device in qemu.
+	 It is *NOT* intended for real cirrus devices. This requires
+	 the modesetting userspace X.org driver.
diff --git a/drivers/gpu/drm/cirrus/Makefile b/drivers/gpu/drm/cirrus/Makefile
new file mode 100644
index 000000000000..69ffe7006d55
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/Makefile
@@ -0,0 +1,5 @@
+ccflags-y := -Iinclude/drm
+cirrus-y  := cirrus_main.o cirrus_mode.o \
+	cirrus_drv.o cirrus_fbdev.o cirrus_ttm.o
+
+obj-$(CONFIG_DRM_CIRRUS_QEMU) += cirrus.o
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.c b/drivers/gpu/drm/cirrus/cirrus_drv.c
new file mode 100644
index 000000000000..d7038230b71e
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright 2012 Red Hat <mjg@redhat.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include <linux/console.h>
+#include "drmP.h"
+#include "drm.h"
+
+#include "cirrus_drv.h"
+
+int cirrus_modeset = -1;
+
+MODULE_PARM_DESC(modeset, "Disable/Enable modesetting");
+module_param_named(modeset, cirrus_modeset, int, 0400);
+
+/*
+ * This is the generic driver code. This binds the driver to the drm core,
+ * which then performs further device association and calls our graphics init
+ * functions
+ */
+
+static struct drm_driver driver;
+
+/* only bind to the cirrus chip in qemu */
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	{ PCI_VENDOR_ID_CIRRUS, PCI_DEVICE_ID_CIRRUS_5446, 0x1af4, 0x1100, 0,
+	  0, 0 },
+	{0,}
+};
+
+static int __devinit
+cirrus_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static void cirrus_pci_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	drm_put_dev(dev);
+}
+
+static const struct file_operations cirrus_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = cirrus_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+};
+static struct drm_driver driver = {
+	.driver_features = DRIVER_MODESET | DRIVER_GEM | DRIVER_USE_MTRR,
+	.load = cirrus_driver_load,
+	.unload = cirrus_driver_unload,
+	.fops = &cirrus_driver_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+	.gem_init_object = cirrus_gem_init_object,
+	.gem_free_object = cirrus_gem_free_object,
+	.dumb_create = cirrus_dumb_create,
+	.dumb_map_offset = cirrus_dumb_mmap_offset,
+	.dumb_destroy = cirrus_dumb_destroy,
+};
+
+static struct pci_driver cirrus_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = cirrus_pci_probe,
+	.remove = cirrus_pci_remove,
+};
+
+static int __init cirrus_init(void)
+{
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force() && cirrus_modeset == -1)
+		return -EINVAL;
+#endif
+
+	if (cirrus_modeset == 0)
+		return -EINVAL;
+	return drm_pci_init(&driver, &cirrus_pci_driver);
+}
+
+static void __exit cirrus_exit(void)
+{
+	drm_pci_exit(&driver, &cirrus_pci_driver);
+}
+
+module_init(cirrus_init);
+module_exit(cirrus_exit);
+
+MODULE_DEVICE_TABLE(pci, pciidlist);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.h b/drivers/gpu/drm/cirrus/cirrus_drv.h
new file mode 100644
index 000000000000..21bdfa8836f7
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.h
@@ -0,0 +1,246 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#ifndef __CIRRUS_DRV_H__
+#define __CIRRUS_DRV_H__
+
+#include <video/vga.h>
+
+#include <drm/drm_fb_helper.h>
+
+#include "ttm/ttm_bo_api.h"
+#include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_placement.h"
+#include "ttm/ttm_memory.h"
+#include "ttm/ttm_module.h"
+
+#define DRIVER_AUTHOR		"Matthew Garrett"
+
+#define DRIVER_NAME		"cirrus"
+#define DRIVER_DESC		"qemu Cirrus emulation"
+#define DRIVER_DATE		"20110418"
+
+#define DRIVER_MAJOR		1
+#define DRIVER_MINOR		0
+#define DRIVER_PATCHLEVEL	0
+
+#define CIRRUSFB_CONN_LIMIT 1
+
+#define RREG8(reg) ioread8(((void __iomem *)cdev->rmmio) + (reg))
+#define WREG8(reg, v) iowrite8(v, ((void __iomem *)cdev->rmmio) + (reg))
+#define RREG32(reg) ioread32(((void __iomem *)cdev->rmmio) + (reg))
+#define WREG32(reg, v) iowrite32(v, ((void __iomem *)cdev->rmmio) + (reg))
+
+#define SEQ_INDEX 4
+#define SEQ_DATA 5
+
+#define WREG_SEQ(reg, v)					\
+	do {							\
+		WREG8(SEQ_INDEX, reg);				\
+		WREG8(SEQ_DATA, v);				\
+	} while (0)						\
+
+#define CRT_INDEX 0x14
+#define CRT_DATA 0x15
+
+#define WREG_CRT(reg, v)					\
+	do {							\
+		WREG8(CRT_INDEX, reg);				\
+		WREG8(CRT_DATA, v);				\
+	} while (0)						\
+
+#define GFX_INDEX 0xe
+#define GFX_DATA 0xf
+
+#define WREG_GFX(reg, v)					\
+	do {							\
+		WREG8(GFX_INDEX, reg);				\
+		WREG8(GFX_DATA, v);				\
+	} while (0)						\
+
+/*
+ * Cirrus has a "hidden" DAC register that can be accessed by writing to
+ * the pixel mask register to reset the state, then reading from the register
+ * four times. The next write will then pass to the DAC
+ */
+#define VGA_DAC_MASK 0x6
+
+#define WREG_HDR(v)						\
+	do {							\
+		RREG8(VGA_DAC_MASK);					\
+		RREG8(VGA_DAC_MASK);					\
+		RREG8(VGA_DAC_MASK);					\
+		RREG8(VGA_DAC_MASK);					\
+		WREG8(VGA_DAC_MASK, v);					\
+	} while (0)						\
+
+
+#define CIRRUS_MAX_FB_HEIGHT 4096
+#define CIRRUS_MAX_FB_WIDTH 4096
+
+#define CIRRUS_DPMS_CLEARED (-1)
+
+#define to_cirrus_crtc(x) container_of(x, struct cirrus_crtc, base)
+#define to_cirrus_encoder(x) container_of(x, struct cirrus_encoder, base)
+#define to_cirrus_framebuffer(x) container_of(x, struct cirrus_framebuffer, base)
+
+struct cirrus_crtc {
+	struct drm_crtc			base;
+	u8				lut_r[256], lut_g[256], lut_b[256];
+	int				last_dpms;
+	bool				enabled;
+};
+
+struct cirrus_fbdev;
+struct cirrus_mode_info {
+	bool				mode_config_initialized;
+	struct cirrus_crtc		*crtc;
+	/* pointer to fbdev info structure */
+	struct cirrus_fbdev		*gfbdev;
+};
+
+struct cirrus_encoder {
+	struct drm_encoder		base;
+	int				last_dpms;
+};
+
+struct cirrus_connector {
+	struct drm_connector		base;
+};
+
+struct cirrus_framebuffer {
+	struct drm_framebuffer		base;
+	struct drm_gem_object *obj;
+};
+
+struct cirrus_mc {
+	resource_size_t			vram_size;
+	resource_size_t			vram_base;
+};
+
+struct cirrus_device {
+	struct drm_device		*dev;
+	unsigned long			flags;
+
+	resource_size_t			rmmio_base;
+	resource_size_t			rmmio_size;
+	void __iomem			*rmmio;
+
+	struct cirrus_mc			mc;
+	struct cirrus_mode_info		mode_info;
+
+	int				num_crtc;
+	int fb_mtrr;
+
+	struct {
+		struct drm_global_reference mem_global_ref;
+		struct ttm_bo_global_ref bo_global_ref;
+		struct ttm_bo_device bdev;
+		atomic_t validate_sequence;
+	} ttm;
+
+};
+
+
+struct cirrus_fbdev {
+	struct drm_fb_helper helper;
+	struct cirrus_framebuffer gfb;
+	struct list_head fbdev_list;
+	void *sysram;
+	int size;
+};
+
+struct cirrus_bo {
+	struct ttm_buffer_object bo;
+	struct ttm_placement placement;
+	struct ttm_bo_kmap_obj kmap;
+	struct drm_gem_object gem;
+	u32 placements[3];
+	int pin_count;
+};
+#define gem_to_cirrus_bo(gobj) container_of((gobj), struct cirrus_bo, gem)
+
+static inline struct cirrus_bo *
+cirrus_bo(struct ttm_buffer_object *bo)
+{
+	return container_of(bo, struct cirrus_bo, bo);
+}
+
+
+#define to_cirrus_obj(x) container_of(x, struct cirrus_gem_object, base)
+#define DRM_FILE_PAGE_OFFSET (0x100000000ULL >> PAGE_SHIFT)
+
+				/* cirrus_mode.c */
+void cirrus_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			     u16 blue, int regno);
+void cirrus_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			     u16 *blue, int regno);
+
+
+				/* cirrus_main.c */
+int cirrus_device_init(struct cirrus_device *cdev,
+		      struct drm_device *ddev,
+		      struct pci_dev *pdev,
+		      uint32_t flags);
+void cirrus_device_fini(struct cirrus_device *cdev);
+int cirrus_gem_init_object(struct drm_gem_object *obj);
+void cirrus_gem_free_object(struct drm_gem_object *obj);
+int cirrus_dumb_mmap_offset(struct drm_file *file,
+			    struct drm_device *dev,
+			    uint32_t handle,
+			    uint64_t *offset);
+int cirrus_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		      struct drm_gem_object **obj);
+int cirrus_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		       struct drm_mode_create_dumb *args);
+int cirrus_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+			uint32_t handle);
+
+int cirrus_framebuffer_init(struct drm_device *dev,
+			   struct cirrus_framebuffer *gfb,
+			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    struct drm_gem_object *obj);
+
+				/* cirrus_display.c */
+int cirrus_modeset_init(struct cirrus_device *cdev);
+void cirrus_modeset_fini(struct cirrus_device *cdev);
+
+				/* cirrus_fbdev.c */
+int cirrus_fbdev_init(struct cirrus_device *cdev);
+void cirrus_fbdev_fini(struct cirrus_device *cdev);
+
+
+
+				/* cirrus_irq.c */
+void cirrus_driver_irq_preinstall(struct drm_device *dev);
+int cirrus_driver_irq_postinstall(struct drm_device *dev);
+void cirrus_driver_irq_uninstall(struct drm_device *dev);
+irqreturn_t cirrus_driver_irq_handler(DRM_IRQ_ARGS);
+
+				/* cirrus_kms.c */
+int cirrus_driver_load(struct drm_device *dev, unsigned long flags);
+int cirrus_driver_unload(struct drm_device *dev);
+extern struct drm_ioctl_desc cirrus_ioctls[];
+extern int cirrus_max_ioctl;
+
+int cirrus_mm_init(struct cirrus_device *cirrus);
+void cirrus_mm_fini(struct cirrus_device *cirrus);
+void cirrus_ttm_placement(struct cirrus_bo *bo, int domain);
+int cirrus_bo_create(struct drm_device *dev, int size, int align,
+		     uint32_t flags, struct cirrus_bo **pcirrusbo);
+int cirrus_mmap(struct file *filp, struct vm_area_struct *vma);
+int cirrus_bo_reserve(struct cirrus_bo *bo, bool no_wait);
+void cirrus_bo_unreserve(struct cirrus_bo *bo);
+int cirrus_bo_push_sysram(struct cirrus_bo *bo);
+int cirrus_bo_pin(struct cirrus_bo *bo, u32 pl_flag, u64 *gpu_addr);
+#endif				/* __CIRRUS_DRV_H__ */
diff --git a/drivers/gpu/drm/cirrus/cirrus_fbdev.c b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
new file mode 100644
index 000000000000..9a276a536992
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
@@ -0,0 +1,307 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include "drmP.h"
+#include "drm.h"
+#include "drm_fb_helper.h"
+
+#include <linux/fb.h>
+
+#include "cirrus_drv.h"
+
+static void cirrus_dirty_update(struct cirrus_fbdev *afbdev,
+			     int x, int y, int width, int height)
+{
+	int i;
+	struct drm_gem_object *obj;
+	struct cirrus_bo *bo;
+	int src_offset, dst_offset;
+	int bpp = (afbdev->gfb.base.bits_per_pixel + 7)/8;
+	int ret;
+	bool unmap = false;
+
+	obj = afbdev->gfb.obj;
+	bo = gem_to_cirrus_bo(obj);
+
+	ret = cirrus_bo_reserve(bo, true);
+	if (ret) {
+		DRM_ERROR("failed to reserve fb bo\n");
+		return;
+	}
+
+	if (!bo->kmap.virtual) {
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret) {
+			DRM_ERROR("failed to kmap fb updates\n");
+			cirrus_bo_unreserve(bo);
+			return;
+		}
+		unmap = true;
+	}
+	for (i = y; i < y + height; i++) {
+		/* assume equal stride for now */
+		src_offset = dst_offset = i * afbdev->gfb.base.pitches[0] + (x * bpp);
+		memcpy_toio(bo->kmap.virtual + src_offset, afbdev->sysram + src_offset, width * bpp);
+
+	}
+	if (unmap)
+		ttm_bo_kunmap(&bo->kmap);
+
+	cirrus_bo_unreserve(bo);
+}
+
+static void cirrus_fillrect(struct fb_info *info,
+			 const struct fb_fillrect *rect)
+{
+	struct cirrus_fbdev *afbdev = info->par;
+	sys_fillrect(info, rect);
+	cirrus_dirty_update(afbdev, rect->dx, rect->dy, rect->width,
+			 rect->height);
+}
+
+static void cirrus_copyarea(struct fb_info *info,
+			 const struct fb_copyarea *area)
+{
+	struct cirrus_fbdev *afbdev = info->par;
+	sys_copyarea(info, area);
+	cirrus_dirty_update(afbdev, area->dx, area->dy, area->width,
+			 area->height);
+}
+
+static void cirrus_imageblit(struct fb_info *info,
+			  const struct fb_image *image)
+{
+	struct cirrus_fbdev *afbdev = info->par;
+	sys_imageblit(info, image);
+	cirrus_dirty_update(afbdev, image->dx, image->dy, image->width,
+			 image->height);
+}
+
+
+static struct fb_ops cirrusfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_fillrect = cirrus_fillrect,
+	.fb_copyarea = cirrus_copyarea,
+	.fb_imageblit = cirrus_imageblit,
+	.fb_pan_display = drm_fb_helper_pan_display,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcmap = drm_fb_helper_setcmap,
+};
+
+static int cirrusfb_create_object(struct cirrus_fbdev *afbdev,
+			       struct drm_mode_fb_cmd2 *mode_cmd,
+			       struct drm_gem_object **gobj_p)
+{
+	struct drm_device *dev = afbdev->helper.dev;
+	u32 bpp, depth;
+	u32 size;
+	struct drm_gem_object *gobj;
+
+	int ret = 0;
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
+
+	if (bpp > 24)
+		return -EINVAL;
+	size = mode_cmd->pitches[0] * mode_cmd->height;
+	ret = cirrus_gem_create(dev, size, true, &gobj);
+	if (ret)
+		return ret;
+
+	*gobj_p = gobj;
+	return ret;
+}
+
+static int cirrusfb_create(struct cirrus_fbdev *gfbdev,
+			   struct drm_fb_helper_surface_size *sizes)
+{
+	struct drm_device *dev = gfbdev->helper.dev;
+	struct cirrus_device *cdev = gfbdev->helper.dev->dev_private;
+	struct fb_info *info;
+	struct drm_framebuffer *fb;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct device *device = &dev->pdev->dev;
+	void *sysram;
+	struct drm_gem_object *gobj = NULL;
+	struct cirrus_bo *bo = NULL;
+	int size, ret;
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	mode_cmd.pitches[0] = mode_cmd.width * ((sizes->surface_bpp + 7) / 8);
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	ret = cirrusfb_create_object(gfbdev, &mode_cmd, &gobj);
+	if (ret) {
+		DRM_ERROR("failed to create fbcon backing object %d\n", ret);
+		return ret;
+	}
+
+	bo = gem_to_cirrus_bo(gobj);
+
+	sysram = vmalloc(size);
+	if (!sysram)
+		return -ENOMEM;
+
+	info = framebuffer_alloc(0, device);
+	if (info == NULL)
+		return -ENOMEM;
+
+	info->par = gfbdev;
+
+	ret = cirrus_framebuffer_init(cdev->dev, &gfbdev->gfb, &mode_cmd, gobj);
+	if (ret)
+		return ret;
+
+	gfbdev->sysram = sysram;
+	gfbdev->size = size;
+
+	fb = &gfbdev->gfb.base;
+	if (!fb) {
+		DRM_INFO("fb is NULL\n");
+		return -EINVAL;
+	}
+
+	/* setup helper */
+	gfbdev->helper.fb = fb;
+	gfbdev->helper.fbdev = info;
+
+	strcpy(info->fix.id, "cirrusdrmfb");
+
+
+	info->flags = FBINFO_DEFAULT;
+	info->fbops = &cirrusfb_ops;
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, &gfbdev->helper, sizes->fb_width,
+			       sizes->fb_height);
+
+	/* setup aperture base/size for vesafb takeover */
+	info->apertures = alloc_apertures(1);
+	if (!info->apertures) {
+		ret = -ENOMEM;
+		goto out_iounmap;
+	}
+	info->apertures->ranges[0].base = cdev->dev->mode_config.fb_base;
+	info->apertures->ranges[0].size = cdev->mc.vram_size;
+
+	info->screen_base = sysram;
+	info->screen_size = size;
+
+	info->fix.mmio_start = 0;
+	info->fix.mmio_len = 0;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		DRM_ERROR("%s: can't allocate color map\n", info->fix.id);
+		ret = -ENOMEM;
+		goto out_iounmap;
+	}
+
+	DRM_INFO("fb mappable at 0x%lX\n", info->fix.smem_start);
+	DRM_INFO("vram aper at 0x%lX\n", (unsigned long)info->fix.smem_start);
+	DRM_INFO("size %lu\n", (unsigned long)info->fix.smem_len);
+	DRM_INFO("fb depth is %d\n", fb->depth);
+	DRM_INFO("   pitch is %d\n", fb->pitches[0]);
+
+	return 0;
+out_iounmap:
+	return ret;
+}
+
+static int cirrus_fb_find_or_create_single(struct drm_fb_helper *helper,
+					   struct drm_fb_helper_surface_size
+					   *sizes)
+{
+	struct cirrus_fbdev *gfbdev = (struct cirrus_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = cirrusfb_create(gfbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static int cirrus_fbdev_destroy(struct drm_device *dev,
+				struct cirrus_fbdev *gfbdev)
+{
+	struct fb_info *info;
+	struct cirrus_framebuffer *gfb = &gfbdev->gfb;
+
+	if (gfbdev->helper.fbdev) {
+		info = gfbdev->helper.fbdev;
+
+		unregister_framebuffer(info);
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+
+	if (gfb->obj) {
+		drm_gem_object_unreference_unlocked(gfb->obj);
+		gfb->obj = NULL;
+	}
+
+	vfree(gfbdev->sysram);
+	drm_fb_helper_fini(&gfbdev->helper);
+	drm_framebuffer_cleanup(&gfb->base);
+
+	return 0;
+}
+
+static struct drm_fb_helper_funcs cirrus_fb_helper_funcs = {
+	.gamma_set = cirrus_crtc_fb_gamma_set,
+	.gamma_get = cirrus_crtc_fb_gamma_get,
+	.fb_probe = cirrus_fb_find_or_create_single,
+};
+
+int cirrus_fbdev_init(struct cirrus_device *cdev)
+{
+	struct cirrus_fbdev *gfbdev;
+	int ret;
+	int bpp_sel = 24;
+
+	/*bpp_sel = 8;*/
+	gfbdev = kzalloc(sizeof(struct cirrus_fbdev), GFP_KERNEL);
+	if (!gfbdev)
+		return -ENOMEM;
+
+	cdev->mode_info.gfbdev = gfbdev;
+	gfbdev->helper.funcs = &cirrus_fb_helper_funcs;
+
+	ret = drm_fb_helper_init(cdev->dev, &gfbdev->helper,
+				 cdev->num_crtc, CIRRUSFB_CONN_LIMIT);
+	if (ret) {
+		kfree(gfbdev);
+		return ret;
+	}
+	drm_fb_helper_single_add_all_connectors(&gfbdev->helper);
+	drm_fb_helper_initial_config(&gfbdev->helper, bpp_sel);
+
+	return 0;
+}
+
+void cirrus_fbdev_fini(struct cirrus_device *cdev)
+{
+	if (!cdev->mode_info.gfbdev)
+		return;
+
+	cirrus_fbdev_destroy(cdev->dev, cdev->mode_info.gfbdev);
+	kfree(cdev->mode_info.gfbdev);
+	cdev->mode_info.gfbdev = NULL;
+}
diff --git a/drivers/gpu/drm/cirrus/cirrus_main.c b/drivers/gpu/drm/cirrus/cirrus_main.c
new file mode 100644
index 000000000000..e3c122578417
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_main.c
@@ -0,0 +1,335 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include "cirrus_drv.h"
+
+
+static void cirrus_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct cirrus_framebuffer *cirrus_fb = to_cirrus_framebuffer(fb);
+	if (cirrus_fb->obj)
+		drm_gem_object_unreference_unlocked(cirrus_fb->obj);
+	drm_framebuffer_cleanup(fb);
+	kfree(fb);
+}
+
+static int cirrus_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+						 struct drm_file *file_priv,
+						 unsigned int *handle)
+{
+	return 0;
+}
+
+static const struct drm_framebuffer_funcs cirrus_fb_funcs = {
+	.destroy = cirrus_user_framebuffer_destroy,
+	.create_handle = cirrus_user_framebuffer_create_handle,
+};
+
+int cirrus_framebuffer_init(struct drm_device *dev,
+			    struct cirrus_framebuffer *gfb,
+			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    struct drm_gem_object *obj)
+{
+	int ret;
+
+	ret = drm_framebuffer_init(dev, &gfb->base, &cirrus_fb_funcs);
+	if (ret) {
+		DRM_ERROR("drm_framebuffer_init failed: %d\n", ret);
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&gfb->base, mode_cmd);
+	gfb->obj = obj;
+	return 0;
+}
+
+static struct drm_framebuffer *
+cirrus_user_framebuffer_create(struct drm_device *dev,
+			       struct drm_file *filp,
+			       struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct cirrus_framebuffer *cirrus_fb;
+	int ret;
+	u32 bpp, depth;
+
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
+	/* cirrus can't handle > 24bpp framebuffers at all */
+	if (bpp > 24)
+		return ERR_PTR(-EINVAL);
+
+	obj = drm_gem_object_lookup(dev, filp, mode_cmd->handles[0]);
+	if (obj == NULL)
+		return ERR_PTR(-ENOENT);
+
+	cirrus_fb = kzalloc(sizeof(*cirrus_fb), GFP_KERNEL);
+	if (!cirrus_fb) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	ret = cirrus_framebuffer_init(dev, cirrus_fb, mode_cmd, obj);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(obj);
+		kfree(cirrus_fb);
+		return ERR_PTR(ret);
+	}
+	return &cirrus_fb->base;
+}
+
+static const struct drm_mode_config_funcs cirrus_mode_funcs = {
+	.fb_create = cirrus_user_framebuffer_create,
+};
+
+/* Unmap the framebuffer from the core and release the memory */
+static void cirrus_vram_fini(struct cirrus_device *cdev)
+{
+	iounmap(cdev->rmmio);
+	cdev->rmmio = NULL;
+	if (cdev->mc.vram_base)
+		release_mem_region(cdev->mc.vram_base, cdev->mc.vram_size);
+}
+
+/* Map the framebuffer from the card and configure the core */
+static int cirrus_vram_init(struct cirrus_device *cdev)
+{
+	/* BAR 0 is VRAM */
+	cdev->mc.vram_base = pci_resource_start(cdev->dev->pdev, 0);
+	/* We have 4MB of VRAM */
+	cdev->mc.vram_size = 4 * 1024 * 1024;
+
+	if (!request_mem_region(cdev->mc.vram_base, cdev->mc.vram_size,
+				"cirrusdrmfb_vram")) {
+		DRM_ERROR("can't reserve VRAM\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+/*
+ * Our emulated hardware has two sets of memory. One is video RAM and can
+ * simply be used as a linear framebuffer - the other provides mmio access
+ * to the display registers. The latter can also be accessed via IO port
+ * access, but we map the range and use mmio to program them instead
+ */
+
+int cirrus_device_init(struct cirrus_device *cdev,
+		       struct drm_device *ddev,
+		       struct pci_dev *pdev, uint32_t flags)
+{
+	int ret;
+
+	cdev->dev = ddev;
+	cdev->flags = flags;
+
+	/* Hardcode the number of CRTCs to 1 */
+	cdev->num_crtc = 1;
+
+	/* BAR 0 is the framebuffer, BAR 1 contains registers */
+	cdev->rmmio_base = pci_resource_start(cdev->dev->pdev, 1);
+	cdev->rmmio_size = pci_resource_len(cdev->dev->pdev, 1);
+
+	if (!request_mem_region(cdev->rmmio_base, cdev->rmmio_size,
+				"cirrusdrmfb_mmio")) {
+		DRM_ERROR("can't reserve mmio registers\n");
+		return -ENOMEM;
+	}
+
+	cdev->rmmio = ioremap(cdev->rmmio_base, cdev->rmmio_size);
+
+	if (cdev->rmmio == NULL)
+		return -ENOMEM;
+
+	ret = cirrus_vram_init(cdev);
+	if (ret) {
+		release_mem_region(cdev->rmmio_base, cdev->rmmio_size);
+		return ret;
+	}
+
+	return 0;
+}
+
+void cirrus_device_fini(struct cirrus_device *cdev)
+{
+	release_mem_region(cdev->rmmio_base, cdev->rmmio_size);
+	cirrus_vram_fini(cdev);
+}
+
+/*
+ * Functions here will be called by the core once it's bound the driver to
+ * a PCI device
+ */
+
+int cirrus_driver_load(struct drm_device *dev, unsigned long flags)
+{
+	struct cirrus_device *cdev;
+	int r;
+
+	cdev = kzalloc(sizeof(struct cirrus_device), GFP_KERNEL);
+	if (cdev == NULL)
+		return -ENOMEM;
+	dev->dev_private = (void *)cdev;
+
+	r = cirrus_device_init(cdev, dev, dev->pdev, flags);
+	if (r) {
+		dev_err(&dev->pdev->dev, "Fatal error during GPU init: %d\n", r);
+		goto out;
+	}
+
+	r = cirrus_mm_init(cdev);
+	if (r)
+		dev_err(&dev->pdev->dev, "fatal err on mm init\n");
+
+	r = cirrus_modeset_init(cdev);
+	if (r)
+		dev_err(&dev->pdev->dev, "Fatal error during modeset init: %d\n", r);
+
+	dev->mode_config.funcs = (void *)&cirrus_mode_funcs;
+out:
+	if (r)
+		cirrus_driver_unload(dev);
+	return r;
+}
+
+int cirrus_driver_unload(struct drm_device *dev)
+{
+	struct cirrus_device *cdev = dev->dev_private;
+
+	if (cdev == NULL)
+		return 0;
+	cirrus_modeset_fini(cdev);
+	cirrus_mm_fini(cdev);
+	cirrus_device_fini(cdev);
+	kfree(cdev);
+	dev->dev_private = NULL;
+	return 0;
+}
+
+int cirrus_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj)
+{
+	struct cirrus_bo *cirrusbo;
+	int ret;
+
+	*obj = NULL;
+
+	size = roundup(size, PAGE_SIZE);
+	if (size == 0)
+		return -EINVAL;
+
+	ret = cirrus_bo_create(dev, size, 0, 0, &cirrusbo);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("failed to allocate GEM object\n");
+		return ret;
+	}
+	*obj = &cirrusbo->gem;
+	return 0;
+}
+
+int cirrus_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		    struct drm_mode_create_dumb *args)
+{
+	int ret;
+	struct drm_gem_object *gobj;
+	u32 handle;
+
+	args->pitch = args->width * ((args->bpp + 7) / 8);
+	args->size = args->pitch * args->height;
+
+	ret = cirrus_gem_create(dev, args->size, false,
+			     &gobj);
+	if (ret)
+		return ret;
+
+	ret = drm_gem_handle_create(file, gobj, &handle);
+	drm_gem_object_unreference_unlocked(gobj);
+	if (ret)
+		return ret;
+
+	args->handle = handle;
+	return 0;
+}
+
+int cirrus_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
+
+int cirrus_gem_init_object(struct drm_gem_object *obj)
+{
+	BUG();
+	return 0;
+}
+
+void cirrus_bo_unref(struct cirrus_bo **bo)
+{
+	struct ttm_buffer_object *tbo;
+
+	if ((*bo) == NULL)
+		return;
+
+	tbo = &((*bo)->bo);
+	ttm_bo_unref(&tbo);
+	if (tbo == NULL)
+		*bo = NULL;
+
+}
+
+void cirrus_gem_free_object(struct drm_gem_object *obj)
+{
+	struct cirrus_bo *cirrus_bo = gem_to_cirrus_bo(obj);
+
+	if (!cirrus_bo)
+		return;
+	cirrus_bo_unref(&cirrus_bo);
+}
+
+
+static inline u64 cirrus_bo_mmap_offset(struct cirrus_bo *bo)
+{
+	return bo->bo.addr_space_offset;
+}
+
+int
+cirrus_dumb_mmap_offset(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle,
+		     uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret;
+	struct cirrus_bo *bo;
+
+	mutex_lock(&dev->struct_mutex);
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto out_unlock;
+	}
+
+	bo = gem_to_cirrus_bo(obj);
+	*offset = cirrus_bo_mmap_offset(bo);
+
+	drm_gem_object_unreference(obj);
+	ret = 0;
+out_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+
+}
diff --git a/drivers/gpu/drm/cirrus/cirrus_mode.c b/drivers/gpu/drm/cirrus/cirrus_mode.c
new file mode 100644
index 000000000000..100f6308c509
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_mode.c
@@ -0,0 +1,629 @@
+
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ *
+ * Portions of this code derived from cirrusfb.c:
+ * drivers/video/cirrusfb.c - driver for Cirrus Logic chipsets
+ *
+ * Copyright 1999-2001 Jeff Garzik <jgarzik@pobox.com>
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include <video/cirrus.h>
+
+#include "cirrus_drv.h"
+
+#define CIRRUS_LUT_SIZE 256
+
+#define PALETTE_INDEX 0x8
+#define PALETTE_DATA 0x9
+
+/*
+ * This file contains setup code for the CRTC.
+ */
+
+static void cirrus_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct cirrus_device *cdev = dev->dev_private;
+	int i;
+
+	if (!crtc->enabled)
+		return;
+
+	for (i = 0; i < CIRRUS_LUT_SIZE; i++) {
+		/* VGA registers */
+		WREG8(PALETTE_INDEX, i);
+		WREG8(PALETTE_DATA, cirrus_crtc->lut_r[i]);
+		WREG8(PALETTE_DATA, cirrus_crtc->lut_g[i]);
+		WREG8(PALETTE_DATA, cirrus_crtc->lut_b[i]);
+	}
+}
+
+/*
+ * The DRM core requires DPMS functions, but they make little sense in our
+ * case and so are just stubs
+ */
+
+static void cirrus_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct cirrus_device *cdev = dev->dev_private;
+	u8 sr01, gr0e;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		sr01 = 0x00;
+		gr0e = 0x00;
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		sr01 = 0x20;
+		gr0e = 0x02;
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		sr01 = 0x20;
+		gr0e = 0x04;
+		break;
+	case DRM_MODE_DPMS_OFF:
+		sr01 = 0x20;
+		gr0e = 0x06;
+		break;
+	default:
+		return;
+	}
+
+	WREG8(SEQ_INDEX, 0x1);
+	sr01 |= RREG8(SEQ_DATA) & ~0x20;
+	WREG_SEQ(0x1, sr01);
+
+	WREG8(GFX_INDEX, 0xe);
+	gr0e |= RREG8(GFX_DATA) & ~0x06;
+	WREG_GFX(0xe, gr0e);
+}
+
+/*
+ * The core passes the desired mode to the CRTC code to see whether any
+ * CRTC-specific modifications need to be made to it. We're in a position
+ * to just pass that straight through, so this does nothing
+ */
+static bool cirrus_crtc_mode_fixup(struct drm_crtc *crtc,
+				   struct drm_display_mode *mode,
+				   struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+void cirrus_set_start_address(struct drm_crtc *crtc, unsigned offset)
+{
+	struct cirrus_device *cdev = crtc->dev->dev_private;
+	u32 addr;
+	u8 tmp;
+
+	addr = offset >> 2;
+	WREG_CRT(0x0c, (u8)((addr >> 8) & 0xff));
+	WREG_CRT(0x0d, (u8)(addr & 0xff));
+
+	WREG8(CRT_INDEX, 0x1b);
+	tmp = RREG8(CRT_DATA);
+	tmp &= 0xf2;
+	tmp |= (addr >> 16) & 0x01;
+	tmp |= (addr >> 15) & 0x0c;
+	WREG_CRT(0x1b, tmp);
+	WREG8(CRT_INDEX, 0x1d);
+	tmp = RREG8(CRT_DATA);
+	tmp &= 0x7f;
+	tmp |= (addr >> 12) & 0x80;
+	WREG_CRT(0x1d, tmp);
+}
+
+/* cirrus is different - we will force move buffers out of VRAM */
+static int cirrus_crtc_do_set_base(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				int x, int y, int atomic)
+{
+	struct cirrus_device *cdev = crtc->dev->dev_private;
+	struct drm_gem_object *obj;
+	struct cirrus_framebuffer *cirrus_fb;
+	struct cirrus_bo *bo;
+	int ret;
+	u64 gpu_addr;
+
+	/* push the previous fb to system ram */
+	if (!atomic && fb) {
+		cirrus_fb = to_cirrus_framebuffer(fb);
+		obj = cirrus_fb->obj;
+		bo = gem_to_cirrus_bo(obj);
+		ret = cirrus_bo_reserve(bo, false);
+		if (ret)
+			return ret;
+		cirrus_bo_push_sysram(bo);
+		cirrus_bo_unreserve(bo);
+	}
+
+	cirrus_fb = to_cirrus_framebuffer(crtc->fb);
+	obj = cirrus_fb->obj;
+	bo = gem_to_cirrus_bo(obj);
+
+	ret = cirrus_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	ret = cirrus_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	if (ret) {
+		cirrus_bo_unreserve(bo);
+		return ret;
+	}
+
+	if (&cdev->mode_info.gfbdev->gfb == cirrus_fb) {
+		/* if pushing console in kmap it */
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret)
+			DRM_ERROR("failed to kmap fbcon\n");
+	}
+	cirrus_bo_unreserve(bo);
+
+	cirrus_set_start_address(crtc, (u32)gpu_addr);
+	return 0;
+}
+
+static int cirrus_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+			     struct drm_framebuffer *old_fb)
+{
+	return cirrus_crtc_do_set_base(crtc, old_fb, x, y, 0);
+}
+
+/*
+ * The meat of this driver. The core passes us a mode and we have to program
+ * it. The modesetting here is the bare minimum required to satisfy the qemu
+ * emulation of this hardware, and running this against a real device is
+ * likely to result in an inadequately programmed mode. We've already had
+ * the opportunity to modify the mode, so whatever we receive here should
+ * be something that can be correctly programmed and displayed
+ */
+static int cirrus_crtc_mode_set(struct drm_crtc *crtc,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode,
+				int x, int y, struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct cirrus_device *cdev = dev->dev_private;
+	int hsyncstart, hsyncend, htotal, hdispend;
+	int vtotal, vdispend;
+	int tmp;
+	int sr07 = 0, hdr = 0;
+
+	htotal = mode->htotal / 8;
+	hsyncend = mode->hsync_end / 8;
+	hsyncstart = mode->hsync_start / 8;
+	hdispend = mode->hdisplay / 8;
+
+	vtotal = mode->vtotal;
+	vdispend = mode->vdisplay;
+
+	vdispend -= 1;
+	vtotal -= 2;
+
+	htotal -= 5;
+	hdispend -= 1;
+	hsyncstart += 1;
+	hsyncend += 1;
+
+	WREG_CRT(VGA_CRTC_V_SYNC_END, 0x20);
+	WREG_CRT(VGA_CRTC_H_TOTAL, htotal);
+	WREG_CRT(VGA_CRTC_H_DISP, hdispend);
+	WREG_CRT(VGA_CRTC_H_SYNC_START, hsyncstart);
+	WREG_CRT(VGA_CRTC_H_SYNC_END, hsyncend);
+	WREG_CRT(VGA_CRTC_V_TOTAL, vtotal & 0xff);
+	WREG_CRT(VGA_CRTC_V_DISP_END, vdispend & 0xff);
+
+	tmp = 0x40;
+	if ((vdispend + 1) & 512)
+		tmp |= 0x20;
+	WREG_CRT(VGA_CRTC_MAX_SCAN, tmp);
+
+	/*
+	 * Overflow bits for values that don't fit in the standard registers
+	 */
+	tmp = 16;
+	if (vtotal & 256)
+		tmp |= 1;
+	if (vdispend & 256)
+		tmp |= 2;
+	if ((vdispend + 1) & 256)
+		tmp |= 8;
+	if (vtotal & 512)
+		tmp |= 32;
+	if (vdispend & 512)
+		tmp |= 64;
+	WREG_CRT(VGA_CRTC_OVERFLOW, tmp);
+
+	tmp = 0;
+
+	/* More overflow bits */
+
+	if ((htotal + 5) & 64)
+		tmp |= 16;
+	if ((htotal + 5) & 128)
+		tmp |= 32;
+	if (vtotal & 256)
+		tmp |= 64;
+	if (vtotal & 512)
+		tmp |= 128;
+
+	WREG_CRT(CL_CRT1A, tmp);
+
+	/* Disable Hercules/CGA compatibility */
+	WREG_CRT(VGA_CRTC_MODE, 0x03);
+
+	WREG8(SEQ_INDEX, 0x7);
+	sr07 = RREG8(SEQ_DATA);
+	sr07 &= 0xe0;
+	hdr = 0;
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		sr07 |= 0x11;
+		break;
+	case 16:
+		sr07 |= 0xc1;
+		hdr = 0xc0;
+		break;
+	case 24:
+		sr07 |= 0x15;
+		hdr = 0xc5;
+		break;
+	case 32:
+		sr07 |= 0x19;
+		hdr = 0xc5;
+		break;
+	default:
+		return -1;
+	}
+
+	WREG_SEQ(0x7, sr07);
+
+	/* Program the pitch */
+	tmp = crtc->fb->pitches[0] / 8;
+	WREG_CRT(VGA_CRTC_OFFSET, tmp);
+
+	/* Enable extended blanking and pitch bits, and enable full memory */
+	tmp = 0x22;
+	tmp |= (crtc->fb->pitches[0] >> 7) & 0x10;
+	tmp |= (crtc->fb->pitches[0] >> 6) & 0x40;
+	WREG_CRT(0x1b, tmp);
+
+	/* Enable high-colour modes */
+	WREG_GFX(VGA_GFX_MODE, 0x40);
+
+	/* And set graphics mode */
+	WREG_GFX(VGA_GFX_MISC, 0x01);
+
+	WREG_HDR(hdr);
+	cirrus_crtc_do_set_base(crtc, old_fb, x, y, 0);
+	return 0;
+}
+
+/*
+ * This is called before a mode is programmed. A typical use might be to
+ * enable DPMS during the programming to avoid seeing intermediate stages,
+ * but that's not relevant to us
+ */
+static void cirrus_crtc_prepare(struct drm_crtc *crtc)
+{
+}
+
+/*
+ * This is called after a mode is programmed. It should reverse anything done
+ * by the prepare function
+ */
+static void cirrus_crtc_commit(struct drm_crtc *crtc)
+{
+}
+
+/*
+ * The core can pass us a set of gamma values to program. We actually only
+ * use this for 8-bit mode so can't perform smooth fades on deeper modes,
+ * but it's a requirement that we provide the function
+ */
+static void cirrus_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
+				  u16 *blue, uint32_t start, uint32_t size)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+	int i;
+
+	if (size != CIRRUS_LUT_SIZE)
+		return;
+
+	for (i = 0; i < CIRRUS_LUT_SIZE; i++) {
+		cirrus_crtc->lut_r[i] = red[i];
+		cirrus_crtc->lut_g[i] = green[i];
+		cirrus_crtc->lut_b[i] = blue[i];
+	}
+	cirrus_crtc_load_lut(crtc);
+}
+
+/* Simple cleanup function */
+static void cirrus_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+
+	drm_crtc_cleanup(crtc);
+	kfree(cirrus_crtc);
+}
+
+/* These provide the minimum set of functions required to handle a CRTC */
+static const struct drm_crtc_funcs cirrus_crtc_funcs = {
+	.gamma_set = cirrus_crtc_gamma_set,
+	.set_config = drm_crtc_helper_set_config,
+	.destroy = cirrus_crtc_destroy,
+};
+
+static const struct drm_crtc_helper_funcs cirrus_helper_funcs = {
+	.dpms = cirrus_crtc_dpms,
+	.mode_fixup = cirrus_crtc_mode_fixup,
+	.mode_set = cirrus_crtc_mode_set,
+	.mode_set_base = cirrus_crtc_mode_set_base,
+	.prepare = cirrus_crtc_prepare,
+	.commit = cirrus_crtc_commit,
+	.load_lut = cirrus_crtc_load_lut,
+};
+
+/* CRTC setup */
+static void cirrus_crtc_init(struct drm_device *dev)
+{
+	struct cirrus_device *cdev = dev->dev_private;
+	struct cirrus_crtc *cirrus_crtc;
+	int i;
+
+	cirrus_crtc = kzalloc(sizeof(struct cirrus_crtc) +
+			      (CIRRUSFB_CONN_LIMIT * sizeof(struct drm_connector *)),
+			      GFP_KERNEL);
+
+	if (cirrus_crtc == NULL)
+		return;
+
+	drm_crtc_init(dev, &cirrus_crtc->base, &cirrus_crtc_funcs);
+
+	drm_mode_crtc_set_gamma_size(&cirrus_crtc->base, CIRRUS_LUT_SIZE);
+	cdev->mode_info.crtc = cirrus_crtc;
+
+	for (i = 0; i < CIRRUS_LUT_SIZE; i++) {
+		cirrus_crtc->lut_r[i] = i;
+		cirrus_crtc->lut_g[i] = i;
+		cirrus_crtc->lut_b[i] = i;
+	}
+
+	drm_crtc_helper_add(&cirrus_crtc->base, &cirrus_helper_funcs);
+}
+
+/** Sets the color ramps on behalf of fbcon */
+void cirrus_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			      u16 blue, int regno)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+
+	cirrus_crtc->lut_r[regno] = red;
+	cirrus_crtc->lut_g[regno] = green;
+	cirrus_crtc->lut_b[regno] = blue;
+}
+
+/** Gets the color ramps on behalf of fbcon */
+void cirrus_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			      u16 *blue, int regno)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+
+	*red = cirrus_crtc->lut_r[regno];
+	*green = cirrus_crtc->lut_g[regno];
+	*blue = cirrus_crtc->lut_b[regno];
+}
+
+
+static bool cirrus_encoder_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void cirrus_encoder_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+}
+
+static void cirrus_encoder_dpms(struct drm_encoder *encoder, int state)
+{
+	return;
+}
+
+static void cirrus_encoder_prepare(struct drm_encoder *encoder)
+{
+}
+
+static void cirrus_encoder_commit(struct drm_encoder *encoder)
+{
+}
+
+void cirrus_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct cirrus_encoder *cirrus_encoder = to_cirrus_encoder(encoder);
+	drm_encoder_cleanup(encoder);
+	kfree(cirrus_encoder);
+}
+
+static const struct drm_encoder_helper_funcs cirrus_encoder_helper_funcs = {
+	.dpms = cirrus_encoder_dpms,
+	.mode_fixup = cirrus_encoder_mode_fixup,
+	.mode_set = cirrus_encoder_mode_set,
+	.prepare = cirrus_encoder_prepare,
+	.commit = cirrus_encoder_commit,
+};
+
+static const struct drm_encoder_funcs cirrus_encoder_encoder_funcs = {
+	.destroy = cirrus_encoder_destroy,
+};
+
+static struct drm_encoder *cirrus_encoder_init(struct drm_device *dev)
+{
+	struct drm_encoder *encoder;
+	struct cirrus_encoder *cirrus_encoder;
+
+	cirrus_encoder = kzalloc(sizeof(struct cirrus_encoder), GFP_KERNEL);
+	if (!cirrus_encoder)
+		return NULL;
+
+	encoder = &cirrus_encoder->base;
+	encoder->possible_crtcs = 0x1;
+
+	drm_encoder_init(dev, encoder, &cirrus_encoder_encoder_funcs,
+			 DRM_MODE_ENCODER_DAC);
+	drm_encoder_helper_add(encoder, &cirrus_encoder_helper_funcs);
+
+	return encoder;
+}
+
+
+int cirrus_vga_get_modes(struct drm_connector *connector)
+{
+	/* Just add a static list of modes */
+	drm_add_modes_noedid(connector, 640, 480);
+	drm_add_modes_noedid(connector, 800, 600);
+	drm_add_modes_noedid(connector, 1024, 768);
+	drm_add_modes_noedid(connector, 1280, 1024);
+
+	return 4;
+}
+
+static int cirrus_vga_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	/* Any mode we've added is valid */
+	return MODE_OK;
+}
+
+struct drm_encoder *cirrus_connector_best_encoder(struct drm_connector
+						  *connector)
+{
+	int enc_id = connector->encoder_ids[0];
+	struct drm_mode_object *obj;
+	struct drm_encoder *encoder;
+
+	/* pick the encoder ids */
+	if (enc_id) {
+		obj =
+		    drm_mode_object_find(connector->dev, enc_id,
+					 DRM_MODE_OBJECT_ENCODER);
+		if (!obj)
+			return NULL;
+		encoder = obj_to_encoder(obj);
+		return encoder;
+	}
+	return NULL;
+}
+
+static enum drm_connector_status cirrus_vga_detect(struct drm_connector
+						   *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static void cirrus_connector_destroy(struct drm_connector *connector)
+{
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+struct drm_connector_helper_funcs cirrus_vga_connector_helper_funcs = {
+	.get_modes = cirrus_vga_get_modes,
+	.mode_valid = cirrus_vga_mode_valid,
+	.best_encoder = cirrus_connector_best_encoder,
+};
+
+struct drm_connector_funcs cirrus_vga_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = cirrus_vga_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = cirrus_connector_destroy,
+};
+
+static struct drm_connector *cirrus_vga_init(struct drm_device *dev)
+{
+	struct drm_connector *connector;
+	struct cirrus_connector *cirrus_connector;
+
+	cirrus_connector = kzalloc(sizeof(struct cirrus_connector), GFP_KERNEL);
+	if (!cirrus_connector)
+		return NULL;
+
+	connector = &cirrus_connector->base;
+
+	drm_connector_init(dev, connector,
+			   &cirrus_vga_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+	drm_connector_helper_add(connector, &cirrus_vga_connector_helper_funcs);
+
+	return connector;
+}
+
+
+int cirrus_modeset_init(struct cirrus_device *cdev)
+{
+	struct drm_encoder *encoder;
+	struct drm_connector *connector;
+	int ret;
+
+	drm_mode_config_init(cdev->dev);
+	cdev->mode_info.mode_config_initialized = true;
+
+	cdev->dev->mode_config.max_width = CIRRUS_MAX_FB_WIDTH;
+	cdev->dev->mode_config.max_height = CIRRUS_MAX_FB_HEIGHT;
+
+	cdev->dev->mode_config.fb_base = cdev->mc.vram_base;
+	cdev->dev->mode_config.preferred_depth = 24;
+	/* don't prefer a shadow on virt GPU */
+	cdev->dev->mode_config.prefer_shadow = 0;
+
+	cirrus_crtc_init(cdev->dev);
+
+	encoder = cirrus_encoder_init(cdev->dev);
+	if (!encoder) {
+		DRM_ERROR("cirrus_encoder_init failed\n");
+		return -1;
+	}
+
+	connector = cirrus_vga_init(cdev->dev);
+	if (!connector) {
+		DRM_ERROR("cirrus_vga_init failed\n");
+		return -1;
+	}
+
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	ret = cirrus_fbdev_init(cdev);
+	if (ret) {
+		DRM_ERROR("cirrus_fbdev_init failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+void cirrus_modeset_fini(struct cirrus_device *cdev)
+{
+	cirrus_fbdev_fini(cdev);
+
+	if (cdev->mode_info.mode_config_initialized) {
+		drm_mode_config_cleanup(cdev->dev);
+		cdev->mode_info.mode_config_initialized = false;
+	}
+}
diff --git a/drivers/gpu/drm/cirrus/cirrus_ttm.c b/drivers/gpu/drm/cirrus/cirrus_ttm.c
new file mode 100644
index 000000000000..2ebcd11a5023
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_ttm.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "cirrus_drv.h"
+#include <ttm/ttm_page_alloc.h>
+
+static inline struct cirrus_device *
+cirrus_bdev(struct ttm_bo_device *bd)
+{
+	return container_of(bd, struct cirrus_device, ttm.bdev);
+}
+
+static int
+cirrus_ttm_mem_global_init(struct drm_global_reference *ref)
+{
+	return ttm_mem_global_init(ref->object);
+}
+
+static void
+cirrus_ttm_mem_global_release(struct drm_global_reference *ref)
+{
+	ttm_mem_global_release(ref->object);
+}
+
+static int cirrus_ttm_global_init(struct cirrus_device *cirrus)
+{
+	struct drm_global_reference *global_ref;
+	int r;
+
+	global_ref = &cirrus->ttm.mem_global_ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_MEM;
+	global_ref->size = sizeof(struct ttm_mem_global);
+	global_ref->init = &cirrus_ttm_mem_global_init;
+	global_ref->release = &cirrus_ttm_mem_global_release;
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM memory accounting "
+			  "subsystem.\n");
+		return r;
+	}
+
+	cirrus->ttm.bo_global_ref.mem_glob =
+		cirrus->ttm.mem_global_ref.object;
+	global_ref = &cirrus->ttm.bo_global_ref.ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_BO;
+	global_ref->size = sizeof(struct ttm_bo_global);
+	global_ref->init = &ttm_bo_global_init;
+	global_ref->release = &ttm_bo_global_release;
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM BO subsystem.\n");
+		drm_global_item_unref(&cirrus->ttm.mem_global_ref);
+		return r;
+	}
+	return 0;
+}
+
+void
+cirrus_ttm_global_release(struct cirrus_device *cirrus)
+{
+	if (cirrus->ttm.mem_global_ref.release == NULL)
+		return;
+
+	drm_global_item_unref(&cirrus->ttm.bo_global_ref.ref);
+	drm_global_item_unref(&cirrus->ttm.mem_global_ref);
+	cirrus->ttm.mem_global_ref.release = NULL;
+}
+
+
+static void cirrus_bo_ttm_destroy(struct ttm_buffer_object *tbo)
+{
+	struct cirrus_bo *bo;
+
+	bo = container_of(tbo, struct cirrus_bo, bo);
+
+	drm_gem_object_release(&bo->gem);
+	kfree(bo);
+}
+
+bool cirrus_ttm_bo_is_cirrus_bo(struct ttm_buffer_object *bo)
+{
+	if (bo->destroy == &cirrus_bo_ttm_destroy)
+		return true;
+	return false;
+}
+
+static int
+cirrus_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
+		     struct ttm_mem_type_manager *man)
+{
+	switch (type) {
+	case TTM_PL_SYSTEM:
+		man->flags = TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_MASK_CACHING;
+		man->default_caching = TTM_PL_FLAG_CACHED;
+		break;
+	case TTM_PL_VRAM:
+		man->func = &ttm_bo_manager_func;
+		man->flags = TTM_MEMTYPE_FLAG_FIXED |
+			TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_FLAG_UNCACHED |
+			TTM_PL_FLAG_WC;
+		man->default_caching = TTM_PL_FLAG_WC;
+		break;
+	default:
+		DRM_ERROR("Unsupported memory type %u\n", (unsigned)type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void
+cirrus_bo_evict_flags(struct ttm_buffer_object *bo, struct ttm_placement *pl)
+{
+	struct cirrus_bo *cirrusbo = cirrus_bo(bo);
+
+	if (!cirrus_ttm_bo_is_cirrus_bo(bo))
+		return;
+
+	cirrus_ttm_placement(cirrusbo, TTM_PL_FLAG_SYSTEM);
+	*pl = cirrusbo->placement;
+}
+
+static int cirrus_bo_verify_access(struct ttm_buffer_object *bo, struct file *filp)
+{
+	return 0;
+}
+
+static int cirrus_ttm_io_mem_reserve(struct ttm_bo_device *bdev,
+				  struct ttm_mem_reg *mem)
+{
+	struct ttm_mem_type_manager *man = &bdev->man[mem->mem_type];
+	struct cirrus_device *cirrus = cirrus_bdev(bdev);
+
+	mem->bus.addr = NULL;
+	mem->bus.offset = 0;
+	mem->bus.size = mem->num_pages << PAGE_SHIFT;
+	mem->bus.base = 0;
+	mem->bus.is_iomem = false;
+	if (!(man->flags & TTM_MEMTYPE_FLAG_MAPPABLE))
+		return -EINVAL;
+	switch (mem->mem_type) {
+	case TTM_PL_SYSTEM:
+		/* system memory */
+		return 0;
+	case TTM_PL_VRAM:
+		mem->bus.offset = mem->start << PAGE_SHIFT;
+		mem->bus.base = pci_resource_start(cirrus->dev->pdev, 0);
+		mem->bus.is_iomem = true;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
+
+static void cirrus_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
+{
+}
+
+static int cirrus_bo_move(struct ttm_buffer_object *bo,
+		       bool evict, bool interruptible,
+		       bool no_wait_reserve, bool no_wait_gpu,
+		       struct ttm_mem_reg *new_mem)
+{
+	int r;
+	r = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
+	return r;
+}
+
+
+static void cirrus_ttm_backend_destroy(struct ttm_tt *tt)
+{
+	ttm_tt_fini(tt);
+	kfree(tt);
+}
+
+static struct ttm_backend_func cirrus_tt_backend_func = {
+	.destroy = &cirrus_ttm_backend_destroy,
+};
+
+
+struct ttm_tt *cirrus_ttm_tt_create(struct ttm_bo_device *bdev,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
+{
+	struct ttm_tt *tt;
+
+	tt = kzalloc(sizeof(struct ttm_tt), GFP_KERNEL);
+	if (tt == NULL)
+		return NULL;
+	tt->func = &cirrus_tt_backend_func;
+	if (ttm_tt_init(tt, bdev, size, page_flags, dummy_read_page)) {
+		kfree(tt);
+		return NULL;
+	}
+	return tt;
+}
+
+static int cirrus_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	return ttm_pool_populate(ttm);
+}
+
+static void cirrus_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
+}
+
+struct ttm_bo_driver cirrus_bo_driver = {
+	.ttm_tt_create = cirrus_ttm_tt_create,
+	.ttm_tt_populate = cirrus_ttm_tt_populate,
+	.ttm_tt_unpopulate = cirrus_ttm_tt_unpopulate,
+	.init_mem_type = cirrus_bo_init_mem_type,
+	.evict_flags = cirrus_bo_evict_flags,
+	.move = cirrus_bo_move,
+	.verify_access = cirrus_bo_verify_access,
+	.io_mem_reserve = &cirrus_ttm_io_mem_reserve,
+	.io_mem_free = &cirrus_ttm_io_mem_free,
+};
+
+int cirrus_mm_init(struct cirrus_device *cirrus)
+{
+	int ret;
+	struct drm_device *dev = cirrus->dev;
+	struct ttm_bo_device *bdev = &cirrus->ttm.bdev;
+
+	ret = cirrus_ttm_global_init(cirrus);
+	if (ret)
+		return ret;
+
+	ret = ttm_bo_device_init(&cirrus->ttm.bdev,
+				 cirrus->ttm.bo_global_ref.ref.object,
+				 &cirrus_bo_driver, DRM_FILE_PAGE_OFFSET,
+				 true);
+	if (ret) {
+		DRM_ERROR("Error initialising bo driver; %d\n", ret);
+		return ret;
+	}
+
+	ret = ttm_bo_init_mm(bdev, TTM_PL_VRAM,
+			     cirrus->mc.vram_size >> PAGE_SHIFT);
+	if (ret) {
+		DRM_ERROR("Failed ttm VRAM init: %d\n", ret);
+		return ret;
+	}
+
+	cirrus->fb_mtrr = drm_mtrr_add(pci_resource_start(dev->pdev, 0),
+				    pci_resource_len(dev->pdev, 0),
+				    DRM_MTRR_WC);
+
+	return 0;
+}
+
+void cirrus_mm_fini(struct cirrus_device *cirrus)
+{
+	struct drm_device *dev = cirrus->dev;
+	ttm_bo_device_release(&cirrus->ttm.bdev);
+
+	cirrus_ttm_global_release(cirrus);
+
+	if (cirrus->fb_mtrr >= 0) {
+		drm_mtrr_del(cirrus->fb_mtrr,
+			     pci_resource_start(dev->pdev, 0),
+			     pci_resource_len(dev->pdev, 0), DRM_MTRR_WC);
+		cirrus->fb_mtrr = -1;
+	}
+}
+
+void cirrus_ttm_placement(struct cirrus_bo *bo, int domain)
+{
+	u32 c = 0;
+	bo->placement.fpfn = 0;
+	bo->placement.lpfn = 0;
+	bo->placement.placement = bo->placements;
+	bo->placement.busy_placement = bo->placements;
+	if (domain & TTM_PL_FLAG_VRAM)
+		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+	if (domain & TTM_PL_FLAG_SYSTEM)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	if (!c)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	bo->placement.num_placement = c;
+	bo->placement.num_busy_placement = c;
+}
+
+int cirrus_bo_reserve(struct cirrus_bo *bo, bool no_wait)
+{
+	int ret;
+
+	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("reserve failed %p\n", bo);
+		return ret;
+	}
+	return 0;
+}
+
+void cirrus_bo_unreserve(struct cirrus_bo *bo)
+{
+	ttm_bo_unreserve(&bo->bo);
+}
+
+int cirrus_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct cirrus_bo **pcirrusbo)
+{
+	struct cirrus_device *cirrus = dev->dev_private;
+	struct cirrus_bo *cirrusbo;
+	size_t acc_size;
+	int ret;
+
+	cirrusbo = kzalloc(sizeof(struct cirrus_bo), GFP_KERNEL);
+	if (!cirrusbo)
+		return -ENOMEM;
+
+	ret = drm_gem_object_init(dev, &cirrusbo->gem, size);
+	if (ret) {
+		kfree(cirrusbo);
+		return ret;
+	}
+
+	cirrusbo->gem.driver_private = NULL;
+	cirrusbo->bo.bdev = &cirrus->ttm.bdev;
+
+	cirrus_ttm_placement(cirrusbo, TTM_PL_FLAG_VRAM | TTM_PL_FLAG_SYSTEM);
+
+	acc_size = ttm_bo_dma_acc_size(&cirrus->ttm.bdev, size,
+				       sizeof(struct cirrus_bo));
+
+	ret = ttm_bo_init(&cirrus->ttm.bdev, &cirrusbo->bo, size,
+			  ttm_bo_type_device, &cirrusbo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  NULL, cirrus_bo_ttm_destroy);
+	if (ret)
+		return ret;
+
+	*pcirrusbo = cirrusbo;
+	return 0;
+}
+
+static inline u64 cirrus_bo_gpu_offset(struct cirrus_bo *bo)
+{
+	return bo->bo.offset;
+}
+
+int cirrus_bo_pin(struct cirrus_bo *bo, u32 pl_flag, u64 *gpu_addr)
+{
+	int i, ret;
+
+	if (bo->pin_count) {
+		bo->pin_count++;
+		if (gpu_addr)
+			*gpu_addr = cirrus_bo_gpu_offset(bo);
+	}
+
+	cirrus_ttm_placement(bo, pl_flag);
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	bo->pin_count = 1;
+	if (gpu_addr)
+		*gpu_addr = cirrus_bo_gpu_offset(bo);
+	return 0;
+}
+
+int cirrus_bo_unpin(struct cirrus_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int cirrus_bo_push_sysram(struct cirrus_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	if (bo->kmap.virtual)
+		ttm_bo_kunmap(&bo->kmap);
+
+	cirrus_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret) {
+		DRM_ERROR("pushing to VRAM failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+int cirrus_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *file_priv;
+	struct cirrus_device *cirrus;
+
+	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
+		return drm_mmap(filp, vma);
+
+	file_priv = filp->private_data;
+	cirrus = file_priv->minor->dev->dev_private;
+	return ttm_bo_mmap(filp, vma, &cirrus->ttm.bdev);
+}
diff --git a/drivers/gpu/drm/drm_cache.c b/drivers/gpu/drm/drm_cache.c
index 4b8653b932f9..08758e061478 100644
--- a/drivers/gpu/drm/drm_cache.c
+++ b/drivers/gpu/drm/drm_cache.c
@@ -98,3 +98,26 @@ drm_clflush_pages(struct page *pages[], unsigned long num_pages)
 #endif
 }
 EXPORT_SYMBOL(drm_clflush_pages);
+
+void
+drm_clflush_virt_range(char *addr, unsigned long length)
+{
+#if defined(CONFIG_X86)
+	if (cpu_has_clflush) {
+		char *end = addr + length;
+		mb();
+		for (; addr < end; addr += boot_cpu_data.x86_clflush_size)
+			clflush(addr);
+		clflush(end - 1);
+		mb();
+		return;
+	}
+
+	if (on_each_cpu(drm_clflush_ipi_handler, NULL, 1) != 0)
+		printk(KERN_ERR "Timed out waiting for cache flush.\n");
+#else
+	printk(KERN_ERR "Architecture has no drm_cache.c support\n");
+	WARN_ON_ONCE(1);
+#endif
+}
+EXPORT_SYMBOL(drm_clflush_virt_range);
diff --git a/drivers/gpu/drm/drm_context.c b/drivers/gpu/drm/drm_context.c
index 325365f6d355..affa629589ac 100644
--- a/drivers/gpu/drm/drm_context.c
+++ b/drivers/gpu/drm/drm_context.c
@@ -85,11 +85,12 @@ again:
 	mutex_lock(&dev->struct_mutex);
 	ret = idr_get_new_above(&dev->ctx_idr, NULL,
 				DRM_RESERVED_CONTEXTS, &new_id);
-	if (ret == -EAGAIN) {
-		mutex_unlock(&dev->struct_mutex);
-		goto again;
-	}
 	mutex_unlock(&dev->struct_mutex);
+	if (ret == -EAGAIN)
+		goto again;
+	else if (ret)
+		return ret;
+
 	return new_id;
 }
 
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index c79870a75c2f..92cea9d77ec9 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -227,7 +227,7 @@ static int drm_mode_object_get(struct drm_device *dev,
 again:
 	if (idr_pre_get(&dev->mode_config.crtc_idr, GFP_KERNEL) == 0) {
 		DRM_ERROR("Ran out memory getting a mode number\n");
-		return -EINVAL;
+		return -ENOMEM;
 	}
 
 	mutex_lock(&dev->mode_config.idr_mutex);
@@ -235,6 +235,8 @@ again:
 	mutex_unlock(&dev->mode_config.idr_mutex);
 	if (ret == -EAGAIN)
 		goto again;
+	else if (ret)
+		return ret;
 
 	obj->id = new_id;
 	obj->type = obj_type;
@@ -361,7 +363,7 @@ EXPORT_SYMBOL(drm_framebuffer_cleanup);
  * @funcs: callbacks for the new CRTC
  *
  * LOCKING:
- * Caller must hold mode config lock.
+ * Takes mode_config lock.
  *
  * Inits a new object created as base part of an driver crtc object.
  *
@@ -382,6 +384,8 @@ int drm_crtc_init(struct drm_device *dev, struct drm_crtc *crtc,
 	if (ret)
 		goto out;
 
+	crtc->base.properties = &crtc->properties;
+
 	list_add_tail(&crtc->head, &dev->mode_config.crtc_list);
 	dev->mode_config.num_crtc++;
 
@@ -481,6 +485,7 @@ int drm_connector_init(struct drm_device *dev,
 	if (ret)
 		goto out;
 
+	connector->base.properties = &connector->properties;
 	connector->dev = dev;
 	connector->funcs = funcs;
 	connector->connector_type = connector_type;
@@ -603,6 +608,7 @@ int drm_plane_init(struct drm_device *dev, struct drm_plane *plane,
 	if (ret)
 		goto out;
 
+	plane->base.properties = &plane->properties;
 	plane->dev = dev;
 	plane->funcs = funcs;
 	plane->format_types = kmalloc(sizeof(uint32_t) * format_count,
@@ -1422,11 +1428,7 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	}
 	connector = obj_to_connector(obj);
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] != 0) {
-			props_count++;
-		}
-	}
+	props_count = connector->properties.count;
 
 	for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++) {
 		if (connector->encoder_ids[i] != 0) {
@@ -1479,21 +1481,19 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 		copied = 0;
 		prop_ptr = (uint32_t __user *)(unsigned long)(out_resp->props_ptr);
 		prop_values = (uint64_t __user *)(unsigned long)(out_resp->prop_values_ptr);
-		for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-			if (connector->property_ids[i] != 0) {
-				if (put_user(connector->property_ids[i],
-					     prop_ptr + copied)) {
-					ret = -EFAULT;
-					goto out;
-				}
+		for (i = 0; i < connector->properties.count; i++) {
+			if (put_user(connector->properties.ids[i],
+				     prop_ptr + copied)) {
+				ret = -EFAULT;
+				goto out;
+			}
 
-				if (put_user(connector->property_values[i],
-					     prop_values + copied)) {
-					ret = -EFAULT;
-					goto out;
-				}
-				copied++;
+			if (put_user(connector->properties.values[i],
+				     prop_values + copied)) {
+				ret = -EFAULT;
+				goto out;
 			}
+			copied++;
 		}
 	}
 	out_resp->count_props = props_count;
@@ -1830,7 +1830,7 @@ int drm_mode_setcrtc(struct drm_device *dev, void *data,
 	struct drm_display_mode *mode = NULL;
 	struct drm_mode_set set;
 	uint32_t __user *set_connectors_ptr;
-	int ret = 0;
+	int ret;
 	int i;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
@@ -2102,7 +2102,7 @@ int drm_mode_addfb(struct drm_device *dev,
 
 	fb = dev->mode_config.funcs->fb_create(dev, file_priv, &r);
 	if (IS_ERR(fb)) {
-		DRM_ERROR("could not create framebuffer\n");
+		DRM_DEBUG_KMS("could not create framebuffer\n");
 		ret = PTR_ERR(fb);
 		goto out;
 	}
@@ -2185,6 +2185,47 @@ static int format_check(struct drm_mode_fb_cmd2 *r)
 	}
 }
 
+static int framebuffer_check(struct drm_mode_fb_cmd2 *r)
+{
+	int ret, hsub, vsub, num_planes, i;
+
+	ret = format_check(r);
+	if (ret) {
+		DRM_DEBUG_KMS("bad framebuffer format 0x%08x\n", r->pixel_format);
+		return ret;
+	}
+
+	hsub = drm_format_horz_chroma_subsampling(r->pixel_format);
+	vsub = drm_format_vert_chroma_subsampling(r->pixel_format);
+	num_planes = drm_format_num_planes(r->pixel_format);
+
+	if (r->width == 0 || r->width % hsub) {
+		DRM_DEBUG_KMS("bad framebuffer width %u\n", r->height);
+		return -EINVAL;
+	}
+
+	if (r->height == 0 || r->height % vsub) {
+		DRM_DEBUG_KMS("bad framebuffer height %u\n", r->height);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < num_planes; i++) {
+		unsigned int width = r->width / (i != 0 ? hsub : 1);
+
+		if (!r->handles[i]) {
+			DRM_DEBUG_KMS("no buffer object handle for plane %d\n", i);
+			return -EINVAL;
+		}
+
+		if (r->pitches[i] < drm_format_plane_cpp(r->pixel_format, i) * width) {
+			DRM_DEBUG_KMS("bad pitch %u for plane %d\n", r->pitches[i], i);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 /**
  * drm_mode_addfb2 - add an FB to the graphics configuration
  * @inode: inode from the ioctl
@@ -2208,33 +2249,31 @@ int drm_mode_addfb2(struct drm_device *dev,
 	struct drm_mode_fb_cmd2 *r = data;
 	struct drm_mode_config *config = &dev->mode_config;
 	struct drm_framebuffer *fb;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
 	if ((config->min_width > r->width) || (r->width > config->max_width)) {
-		DRM_ERROR("bad framebuffer width %d, should be >= %d && <= %d\n",
+		DRM_DEBUG_KMS("bad framebuffer width %d, should be >= %d && <= %d\n",
 			  r->width, config->min_width, config->max_width);
 		return -EINVAL;
 	}
 	if ((config->min_height > r->height) || (r->height > config->max_height)) {
-		DRM_ERROR("bad framebuffer height %d, should be >= %d && <= %d\n",
+		DRM_DEBUG_KMS("bad framebuffer height %d, should be >= %d && <= %d\n",
 			  r->height, config->min_height, config->max_height);
 		return -EINVAL;
 	}
 
-	ret = format_check(r);
-	if (ret) {
-		DRM_ERROR("bad framebuffer format 0x%08x\n", r->pixel_format);
+	ret = framebuffer_check(r);
+	if (ret)
 		return ret;
-	}
 
 	mutex_lock(&dev->mode_config.mutex);
 
 	fb = dev->mode_config.funcs->fb_create(dev, file_priv, r);
 	if (IS_ERR(fb)) {
-		DRM_ERROR("could not create framebuffer\n");
+		DRM_DEBUG_KMS("could not create framebuffer\n");
 		ret = PTR_ERR(fb);
 		goto out;
 	}
@@ -2365,7 +2404,7 @@ int drm_mode_dirtyfb_ioctl(struct drm_device *dev,
 	struct drm_framebuffer *fb;
 	unsigned flags;
 	int num_clips;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -2564,7 +2603,7 @@ int drm_mode_attachmode_ioctl(struct drm_device *dev,
 	struct drm_display_mode *mode;
 	struct drm_mode_object *obj;
 	struct drm_mode_modeinfo *umode = &mode_cmd->mode;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -2618,7 +2657,7 @@ int drm_mode_detachmode_ioctl(struct drm_device *dev,
 	struct drm_connector *connector;
 	struct drm_display_mode mode;
 	struct drm_mode_modeinfo *umode = &mode_cmd->mode;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -2710,6 +2749,34 @@ struct drm_property *drm_property_create_enum(struct drm_device *dev, int flags,
 }
 EXPORT_SYMBOL(drm_property_create_enum);
 
+struct drm_property *drm_property_create_bitmask(struct drm_device *dev,
+					 int flags, const char *name,
+					 const struct drm_prop_enum_list *props,
+					 int num_values)
+{
+	struct drm_property *property;
+	int i, ret;
+
+	flags |= DRM_MODE_PROP_BITMASK;
+
+	property = drm_property_create(dev, flags, name, num_values);
+	if (!property)
+		return NULL;
+
+	for (i = 0; i < num_values; i++) {
+		ret = drm_property_add_enum(property, i,
+				      props[i].type,
+				      props[i].name);
+		if (ret) {
+			drm_property_destroy(dev, property);
+			return NULL;
+		}
+	}
+
+	return property;
+}
+EXPORT_SYMBOL(drm_property_create_bitmask);
+
 struct drm_property *drm_property_create_range(struct drm_device *dev, int flags,
 					 const char *name,
 					 uint64_t min, uint64_t max)
@@ -2734,7 +2801,14 @@ int drm_property_add_enum(struct drm_property *property, int index,
 {
 	struct drm_property_enum *prop_enum;
 
-	if (!(property->flags & DRM_MODE_PROP_ENUM))
+	if (!(property->flags & (DRM_MODE_PROP_ENUM | DRM_MODE_PROP_BITMASK)))
+		return -EINVAL;
+
+	/*
+	 * Bitmask enum properties have the additional constraint of values
+	 * from 0 to 63
+	 */
+	if ((property->flags & DRM_MODE_PROP_BITMASK) && (value > 63))
 		return -EINVAL;
 
 	if (!list_empty(&property->enum_blob_list)) {
@@ -2778,60 +2852,78 @@ void drm_property_destroy(struct drm_device *dev, struct drm_property *property)
 }
 EXPORT_SYMBOL(drm_property_destroy);
 
-int drm_connector_attach_property(struct drm_connector *connector,
+void drm_connector_attach_property(struct drm_connector *connector,
 			       struct drm_property *property, uint64_t init_val)
 {
-	int i;
-
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == 0) {
-			connector->property_ids[i] = property->base.id;
-			connector->property_values[i] = init_val;
-			break;
-		}
-	}
-
-	if (i == DRM_CONNECTOR_MAX_PROPERTY)
-		return -EINVAL;
-	return 0;
+	drm_object_attach_property(&connector->base, property, init_val);
 }
 EXPORT_SYMBOL(drm_connector_attach_property);
 
 int drm_connector_property_set_value(struct drm_connector *connector,
 				  struct drm_property *property, uint64_t value)
 {
+	return drm_object_property_set_value(&connector->base, property, value);
+}
+EXPORT_SYMBOL(drm_connector_property_set_value);
+
+int drm_connector_property_get_value(struct drm_connector *connector,
+				  struct drm_property *property, uint64_t *val)
+{
+	return drm_object_property_get_value(&connector->base, property, val);
+}
+EXPORT_SYMBOL(drm_connector_property_get_value);
+
+void drm_object_attach_property(struct drm_mode_object *obj,
+				struct drm_property *property,
+				uint64_t init_val)
+{
+	int count = obj->properties->count;
+
+	if (count == DRM_OBJECT_MAX_PROPERTY) {
+		WARN(1, "Failed to attach object property (type: 0x%x). Please "
+			"increase DRM_OBJECT_MAX_PROPERTY by 1 for each time "
+			"you see this message on the same object type.\n",
+			obj->type);
+		return;
+	}
+
+	obj->properties->ids[count] = property->base.id;
+	obj->properties->values[count] = init_val;
+	obj->properties->count++;
+}
+EXPORT_SYMBOL(drm_object_attach_property);
+
+int drm_object_property_set_value(struct drm_mode_object *obj,
+				  struct drm_property *property, uint64_t val)
+{
 	int i;
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == property->base.id) {
-			connector->property_values[i] = value;
-			break;
+	for (i = 0; i < obj->properties->count; i++) {
+		if (obj->properties->ids[i] == property->base.id) {
+			obj->properties->values[i] = val;
+			return 0;
 		}
 	}
 
-	if (i == DRM_CONNECTOR_MAX_PROPERTY)
-		return -EINVAL;
-	return 0;
+	return -EINVAL;
 }
-EXPORT_SYMBOL(drm_connector_property_set_value);
+EXPORT_SYMBOL(drm_object_property_set_value);
 
-int drm_connector_property_get_value(struct drm_connector *connector,
+int drm_object_property_get_value(struct drm_mode_object *obj,
 				  struct drm_property *property, uint64_t *val)
 {
 	int i;
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == property->base.id) {
-			*val = connector->property_values[i];
-			break;
+	for (i = 0; i < obj->properties->count; i++) {
+		if (obj->properties->ids[i] == property->base.id) {
+			*val = obj->properties->values[i];
+			return 0;
 		}
 	}
 
-	if (i == DRM_CONNECTOR_MAX_PROPERTY)
-		return -EINVAL;
-	return 0;
+	return -EINVAL;
 }
-EXPORT_SYMBOL(drm_connector_property_get_value);
+EXPORT_SYMBOL(drm_object_property_get_value);
 
 int drm_mode_getproperty_ioctl(struct drm_device *dev,
 			       void *data, struct drm_file *file_priv)
@@ -2862,7 +2954,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	}
 	property = obj_to_property(obj);
 
-	if (property->flags & DRM_MODE_PROP_ENUM) {
+	if (property->flags & (DRM_MODE_PROP_ENUM | DRM_MODE_PROP_BITMASK)) {
 		list_for_each_entry(prop_enum, &property->enum_blob_list, head)
 			enum_count++;
 	} else if (property->flags & DRM_MODE_PROP_BLOB) {
@@ -2887,7 +2979,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	}
 	out_resp->count_values = value_count;
 
-	if (property->flags & DRM_MODE_PROP_ENUM) {
+	if (property->flags & (DRM_MODE_PROP_ENUM | DRM_MODE_PROP_BITMASK)) {
 		if ((out_resp->count_enum_blobs >= enum_count) && enum_count) {
 			copied = 0;
 			enum_ptr = (struct drm_mode_property_enum __user *)(unsigned long)out_resp->enum_blob_ptr;
@@ -3009,7 +3101,7 @@ int drm_mode_connector_update_edid_property(struct drm_connector *connector,
 					    struct edid *edid)
 {
 	struct drm_device *dev = connector->dev;
-	int ret = 0, size;
+	int ret, size;
 
 	if (connector->edid_blob_ptr)
 		drm_property_destroy_blob(dev, connector->edid_blob_ptr);
@@ -3033,75 +3125,202 @@ int drm_mode_connector_update_edid_property(struct drm_connector *connector,
 }
 EXPORT_SYMBOL(drm_mode_connector_update_edid_property);
 
+static bool drm_property_change_is_valid(struct drm_property *property,
+					 __u64 value)
+{
+	if (property->flags & DRM_MODE_PROP_IMMUTABLE)
+		return false;
+	if (property->flags & DRM_MODE_PROP_RANGE) {
+		if (value < property->values[0] || value > property->values[1])
+			return false;
+		return true;
+	} else if (property->flags & DRM_MODE_PROP_BITMASK) {
+		int i;
+		__u64 valid_mask = 0;
+		for (i = 0; i < property->num_values; i++)
+			valid_mask |= (1ULL << property->values[i]);
+		return !(value & ~valid_mask);
+	} else {
+		int i;
+		for (i = 0; i < property->num_values; i++)
+			if (property->values[i] == value)
+				return true;
+		return false;
+	}
+}
+
 int drm_mode_connector_property_set_ioctl(struct drm_device *dev,
 				       void *data, struct drm_file *file_priv)
 {
-	struct drm_mode_connector_set_property *out_resp = data;
-	struct drm_mode_object *obj;
-	struct drm_property *property;
-	struct drm_connector *connector;
+	struct drm_mode_connector_set_property *conn_set_prop = data;
+	struct drm_mode_obj_set_property obj_set_prop = {
+		.value = conn_set_prop->value,
+		.prop_id = conn_set_prop->prop_id,
+		.obj_id = conn_set_prop->connector_id,
+		.obj_type = DRM_MODE_OBJECT_CONNECTOR
+	};
+
+	/* It does all the locking and checking we need */
+	return drm_mode_obj_set_property_ioctl(dev, &obj_set_prop, file_priv);
+}
+
+static int drm_mode_connector_set_obj_prop(struct drm_mode_object *obj,
+					   struct drm_property *property,
+					   uint64_t value)
+{
 	int ret = -EINVAL;
+	struct drm_connector *connector = obj_to_connector(obj);
+
+	/* Do DPMS ourselves */
+	if (property == connector->dev->mode_config.dpms_property) {
+		if (connector->funcs->dpms)
+			(*connector->funcs->dpms)(connector, (int)value);
+		ret = 0;
+	} else if (connector->funcs->set_property)
+		ret = connector->funcs->set_property(connector, property, value);
+
+	/* store the property value if successful */
+	if (!ret)
+		drm_connector_property_set_value(connector, property, value);
+	return ret;
+}
+
+static int drm_mode_crtc_set_obj_prop(struct drm_mode_object *obj,
+				      struct drm_property *property,
+				      uint64_t value)
+{
+	int ret = -EINVAL;
+	struct drm_crtc *crtc = obj_to_crtc(obj);
+
+	if (crtc->funcs->set_property)
+		ret = crtc->funcs->set_property(crtc, property, value);
+	if (!ret)
+		drm_object_property_set_value(obj, property, value);
+
+	return ret;
+}
+
+static int drm_mode_plane_set_obj_prop(struct drm_mode_object *obj,
+				      struct drm_property *property,
+				      uint64_t value)
+{
+	int ret = -EINVAL;
+	struct drm_plane *plane = obj_to_plane(obj);
+
+	if (plane->funcs->set_property)
+		ret = plane->funcs->set_property(plane, property, value);
+	if (!ret)
+		drm_object_property_set_value(obj, property, value);
+
+	return ret;
+}
+
+int drm_mode_obj_get_properties_ioctl(struct drm_device *dev, void *data,
+				      struct drm_file *file_priv)
+{
+	struct drm_mode_obj_get_properties *arg = data;
+	struct drm_mode_object *obj;
+	int ret = 0;
 	int i;
+	int copied = 0;
+	int props_count = 0;
+	uint32_t __user *props_ptr;
+	uint64_t __user *prop_values_ptr;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
 	mutex_lock(&dev->mode_config.mutex);
 
-	obj = drm_mode_object_find(dev, out_resp->connector_id, DRM_MODE_OBJECT_CONNECTOR);
+	obj = drm_mode_object_find(dev, arg->obj_id, arg->obj_type);
 	if (!obj) {
+		ret = -EINVAL;
+		goto out;
+	}
+	if (!obj->properties) {
+		ret = -EINVAL;
 		goto out;
 	}
-	connector = obj_to_connector(obj);
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == out_resp->prop_id)
-			break;
+	props_count = obj->properties->count;
+
+	/* This ioctl is called twice, once to determine how much space is
+	 * needed, and the 2nd time to fill it. */
+	if ((arg->count_props >= props_count) && props_count) {
+		copied = 0;
+		props_ptr = (uint32_t __user *)(unsigned long)(arg->props_ptr);
+		prop_values_ptr = (uint64_t __user *)(unsigned long)
+				  (arg->prop_values_ptr);
+		for (i = 0; i < props_count; i++) {
+			if (put_user(obj->properties->ids[i],
+				     props_ptr + copied)) {
+				ret = -EFAULT;
+				goto out;
+			}
+			if (put_user(obj->properties->values[i],
+				     prop_values_ptr + copied)) {
+				ret = -EFAULT;
+				goto out;
+			}
+			copied++;
+		}
 	}
+	arg->count_props = props_count;
+out:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
+
+int drm_mode_obj_set_property_ioctl(struct drm_device *dev, void *data,
+				    struct drm_file *file_priv)
+{
+	struct drm_mode_obj_set_property *arg = data;
+	struct drm_mode_object *arg_obj;
+	struct drm_mode_object *prop_obj;
+	struct drm_property *property;
+	int ret = -EINVAL;
+	int i;
+
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
 
-	if (i == DRM_CONNECTOR_MAX_PROPERTY) {
+	arg_obj = drm_mode_object_find(dev, arg->obj_id, arg->obj_type);
+	if (!arg_obj)
+		goto out;
+	if (!arg_obj->properties)
 		goto out;
-	}
 
-	obj = drm_mode_object_find(dev, out_resp->prop_id, DRM_MODE_OBJECT_PROPERTY);
-	if (!obj) {
+	for (i = 0; i < arg_obj->properties->count; i++)
+		if (arg_obj->properties->ids[i] == arg->prop_id)
+			break;
+
+	if (i == arg_obj->properties->count)
 		goto out;
-	}
-	property = obj_to_property(obj);
 
-	if (property->flags & DRM_MODE_PROP_IMMUTABLE)
+	prop_obj = drm_mode_object_find(dev, arg->prop_id,
+					DRM_MODE_OBJECT_PROPERTY);
+	if (!prop_obj)
 		goto out;
+	property = obj_to_property(prop_obj);
 
-	if (property->flags & DRM_MODE_PROP_RANGE) {
-		if (out_resp->value < property->values[0])
-			goto out;
+	if (!drm_property_change_is_valid(property, arg->value))
+		goto out;
 
-		if (out_resp->value > property->values[1])
-			goto out;
-	} else {
-		int found = 0;
-		for (i = 0; i < property->num_values; i++) {
-			if (property->values[i] == out_resp->value) {
-				found = 1;
-				break;
-			}
-		}
-		if (!found) {
-			goto out;
-		}
+	switch (arg_obj->type) {
+	case DRM_MODE_OBJECT_CONNECTOR:
+		ret = drm_mode_connector_set_obj_prop(arg_obj, property,
+						      arg->value);
+		break;
+	case DRM_MODE_OBJECT_CRTC:
+		ret = drm_mode_crtc_set_obj_prop(arg_obj, property, arg->value);
+		break;
+	case DRM_MODE_OBJECT_PLANE:
+		ret = drm_mode_plane_set_obj_prop(arg_obj, property, arg->value);
+		break;
 	}
 
-	/* Do DPMS ourselves */
-	if (property == connector->dev->mode_config.dpms_property) {
-		if (connector->funcs->dpms)
-			(*connector->funcs->dpms)(connector, (int) out_resp->value);
-		ret = 0;
-	} else if (connector->funcs->set_property)
-		ret = connector->funcs->set_property(connector, property, out_resp->value);
-
-	/* store the property value if successful */
-	if (!ret)
-		drm_connector_property_set_value(connector, property, out_resp->value);
 out:
 	mutex_unlock(&dev->mode_config.mutex);
 	return ret;
@@ -3173,6 +3392,11 @@ int drm_mode_gamma_set_ioctl(struct drm_device *dev,
 	}
 	crtc = obj_to_crtc(obj);
 
+	if (crtc->funcs->gamma_set == NULL) {
+		ret = -ENOSYS;
+		goto out;
+	}
+
 	/* memcpy into gamma store */
 	if (crtc_lut->gamma_size != crtc->gamma_size) {
 		ret = -EINVAL;
@@ -3468,3 +3692,140 @@ void drm_fb_get_bpp_depth(uint32_t format, unsigned int *depth,
 	}
 }
 EXPORT_SYMBOL(drm_fb_get_bpp_depth);
+
+/**
+ * drm_format_num_planes - get the number of planes for format
+ * @format: pixel format (DRM_FORMAT_*)
+ *
+ * RETURNS:
+ * The number of planes used by the specified pixel format.
+ */
+int drm_format_num_planes(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV444:
+	case DRM_FORMAT_YVU444:
+		return 3;
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+		return 2;
+	default:
+		return 1;
+	}
+}
+EXPORT_SYMBOL(drm_format_num_planes);
+
+/**
+ * drm_format_plane_cpp - determine the bytes per pixel value
+ * @format: pixel format (DRM_FORMAT_*)
+ * @plane: plane index
+ *
+ * RETURNS:
+ * The bytes per pixel value for the specified plane.
+ */
+int drm_format_plane_cpp(uint32_t format, int plane)
+{
+	unsigned int depth;
+	int bpp;
+
+	if (plane >= drm_format_num_planes(format))
+		return 0;
+
+	switch (format) {
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_YVYU:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_VYUY:
+		return 2;
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+		return plane ? 2 : 1;
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV444:
+	case DRM_FORMAT_YVU444:
+		return 1;
+	default:
+		drm_fb_get_bpp_depth(format, &depth, &bpp);
+		return bpp >> 3;
+	}
+}
+EXPORT_SYMBOL(drm_format_plane_cpp);
+
+/**
+ * drm_format_horz_chroma_subsampling - get the horizontal chroma subsampling factor
+ * @format: pixel format (DRM_FORMAT_*)
+ *
+ * RETURNS:
+ * The horizontal chroma subsampling factor for the
+ * specified pixel format.
+ */
+int drm_format_horz_chroma_subsampling(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+		return 4;
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_YVYU:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_VYUY:
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+		return 2;
+	default:
+		return 1;
+	}
+}
+EXPORT_SYMBOL(drm_format_horz_chroma_subsampling);
+
+/**
+ * drm_format_vert_chroma_subsampling - get the vertical chroma subsampling factor
+ * @format: pixel format (DRM_FORMAT_*)
+ *
+ * RETURNS:
+ * The vertical chroma subsampling factor for the
+ * specified pixel format.
+ */
+int drm_format_vert_chroma_subsampling(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+		return 4;
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+		return 2;
+	default:
+		return 1;
+	}
+}
+EXPORT_SYMBOL(drm_format_vert_chroma_subsampling);
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index 81118893264c..3252e7067d8b 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -518,7 +518,7 @@ int drm_crtc_helper_set_config(struct drm_mode_set *set)
 	int count = 0, ro, fail = 0;
 	struct drm_crtc_helper_funcs *crtc_funcs;
 	struct drm_mode_set save_set;
-	int ret = 0;
+	int ret;
 	int i;
 
 	DRM_DEBUG_KMS("\n");
@@ -1023,36 +1023,3 @@ void drm_helper_hpd_irq_event(struct drm_device *dev)
 		queue_delayed_work(system_nrt_wq, &dev->mode_config.output_poll_work, 0);
 }
 EXPORT_SYMBOL(drm_helper_hpd_irq_event);
-
-
-/**
- * drm_format_num_planes - get the number of planes for format
- * @format: pixel format (DRM_FORMAT_*)
- *
- * RETURNS:
- * The number of planes used by the specified pixel format.
- */
-int drm_format_num_planes(uint32_t format)
-{
-	switch (format) {
-	case DRM_FORMAT_YUV410:
-	case DRM_FORMAT_YVU410:
-	case DRM_FORMAT_YUV411:
-	case DRM_FORMAT_YVU411:
-	case DRM_FORMAT_YUV420:
-	case DRM_FORMAT_YVU420:
-	case DRM_FORMAT_YUV422:
-	case DRM_FORMAT_YVU422:
-	case DRM_FORMAT_YUV444:
-	case DRM_FORMAT_YVU444:
-		return 3;
-	case DRM_FORMAT_NV12:
-	case DRM_FORMAT_NV21:
-	case DRM_FORMAT_NV16:
-	case DRM_FORMAT_NV61:
-		return 2;
-	default:
-		return 1;
-	}
-}
-EXPORT_SYMBOL(drm_format_num_planes);
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 6116e3b75393..8a9d0792e4ec 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -163,7 +163,9 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DIRTYFB, drm_mode_dirtyfb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CREATE_DUMB, drm_mode_create_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_MAP_DUMB, drm_mode_mmap_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DESTROY_DUMB, drm_mode_destroy_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED)
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DESTROY_DUMB, drm_mode_destroy_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_GETPROPERTIES, drm_mode_obj_get_properties_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_SETPROPERTY, drm_mode_obj_set_property_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 };
 
 #define DRM_CORE_IOCTL_COUNT	ARRAY_SIZE( drm_ioctls )
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index 5a18b0df8285..608bddfc7e35 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -81,7 +81,7 @@ struct detailed_mode_closure {
 #define LEVEL_CVT	3
 
 static struct edid_quirk {
-	char *vendor;
+	char vendor[4];
 	int product_id;
 	u32 quirks;
 } edid_quirk_list[] = {
@@ -149,13 +149,13 @@ EXPORT_SYMBOL(drm_edid_header_is_valid);
  * Sanity check the EDID block (base or extension).  Return 0 if the block
  * doesn't check out, or 1 if it's valid.
  */
-bool drm_edid_block_valid(u8 *raw_edid)
+bool drm_edid_block_valid(u8 *raw_edid, int block)
 {
 	int i;
 	u8 csum = 0;
 	struct edid *edid = (struct edid *)raw_edid;
 
-	if (raw_edid[0] == 0x00) {
+	if (block == 0) {
 		int score = drm_edid_header_is_valid(raw_edid);
 		if (score == 8) ;
 		else if (score >= 6) {
@@ -219,7 +219,7 @@ bool drm_edid_is_valid(struct edid *edid)
 		return false;
 
 	for (i = 0; i <= edid->extensions; i++)
-		if (!drm_edid_block_valid(raw + i * EDID_LENGTH))
+		if (!drm_edid_block_valid(raw + i * EDID_LENGTH, i))
 			return false;
 
 	return true;
@@ -299,7 +299,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 	for (i = 0; i < 4; i++) {
 		if (drm_do_probe_ddc_edid(adapter, block, 0, EDID_LENGTH))
 			goto out;
-		if (drm_edid_block_valid(block))
+		if (drm_edid_block_valid(block, 0))
 			break;
 		if (i == 0 && drm_edid_is_zero(block, EDID_LENGTH)) {
 			connector->null_edid_counter++;
@@ -324,7 +324,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 				  block + (valid_extensions + 1) * EDID_LENGTH,
 				  j, EDID_LENGTH))
 				goto out;
-			if (drm_edid_block_valid(block + (valid_extensions + 1) * EDID_LENGTH)) {
+			if (drm_edid_block_valid(block + (valid_extensions + 1) * EDID_LENGTH, j)) {
 				valid_extensions++;
 				break;
 			}
@@ -486,23 +486,47 @@ static void edid_fixup_preferred(struct drm_connector *connector,
 	preferred_mode->type |= DRM_MODE_TYPE_PREFERRED;
 }
 
+static bool
+mode_is_rb(const struct drm_display_mode *mode)
+{
+	return (mode->htotal - mode->hdisplay == 160) &&
+	       (mode->hsync_end - mode->hdisplay == 80) &&
+	       (mode->hsync_end - mode->hsync_start == 32) &&
+	       (mode->vsync_start - mode->vdisplay == 3);
+}
+
+/*
+ * drm_mode_find_dmt - Create a copy of a mode if present in DMT
+ * @dev: Device to duplicate against
+ * @hsize: Mode width
+ * @vsize: Mode height
+ * @fresh: Mode refresh rate
+ * @rb: Mode reduced-blanking-ness
+ *
+ * Walk the DMT mode list looking for a match for the given parameters.
+ * Return a newly allocated copy of the mode, or NULL if not found.
+ */
 struct drm_display_mode *drm_mode_find_dmt(struct drm_device *dev,
-					   int hsize, int vsize, int fresh)
+					   int hsize, int vsize, int fresh,
+					   bool rb)
 {
-	struct drm_display_mode *mode = NULL;
 	int i;
 
 	for (i = 0; i < drm_num_dmt_modes; i++) {
 		const struct drm_display_mode *ptr = &drm_dmt_modes[i];
-		if (hsize == ptr->hdisplay &&
-			vsize == ptr->vdisplay &&
-			fresh == drm_mode_vrefresh(ptr)) {
-			/* get the expected default mode */
-			mode = drm_mode_duplicate(dev, ptr);
-			break;
-		}
+		if (hsize != ptr->hdisplay)
+			continue;
+		if (vsize != ptr->vdisplay)
+			continue;
+		if (fresh != drm_mode_vrefresh(ptr))
+			continue;
+		if (rb != mode_is_rb(ptr))
+			continue;
+
+		return drm_mode_duplicate(dev, ptr);
 	}
-	return mode;
+
+	return NULL;
 }
 EXPORT_SYMBOL(drm_mode_find_dmt);
 
@@ -731,10 +755,17 @@ drm_mode_std(struct drm_connector *connector, struct edid *edid,
 	}
 
 	/* check whether it can be found in default mode table */
-	mode = drm_mode_find_dmt(dev, hsize, vsize, vrefresh_rate);
+	if (drm_monitor_supports_rb(edid)) {
+		mode = drm_mode_find_dmt(dev, hsize, vsize, vrefresh_rate,
+					 true);
+		if (mode)
+			return mode;
+	}
+	mode = drm_mode_find_dmt(dev, hsize, vsize, vrefresh_rate, false);
 	if (mode)
 		return mode;
 
+	/* okay, generate it */
 	switch (timing_level) {
 	case LEVEL_DMT:
 		break;
@@ -748,6 +779,8 @@ drm_mode_std(struct drm_connector *connector, struct edid *edid,
 		 * secondary GTF curve.  Please don't do that.
 		 */
 		mode = drm_gtf_mode(dev, hsize, vsize, vrefresh_rate, 0, 0);
+		if (!mode)
+			return NULL;
 		if (drm_mode_hsync(mode) > drm_gtf2_hbreak(edid)) {
 			drm_mode_destroy(dev, mode);
 			mode = drm_gtf_mode_complex(dev, hsize, vsize,
@@ -909,15 +942,6 @@ static struct drm_display_mode *drm_mode_detailed(struct drm_device *dev,
 }
 
 static bool
-mode_is_rb(const struct drm_display_mode *mode)
-{
-	return (mode->htotal - mode->hdisplay == 160) &&
-	       (mode->hsync_end - mode->hdisplay == 80) &&
-	       (mode->hsync_end - mode->hsync_start == 32) &&
-	       (mode->vsync_start - mode->vdisplay == 3);
-}
-
-static bool
 mode_in_hsync_range(const struct drm_display_mode *mode,
 		    struct edid *edid, u8 *t)
 {
@@ -994,12 +1018,8 @@ mode_in_range(const struct drm_display_mode *mode, struct edid *edid,
 	return true;
 }
 
-/*
- * XXX If drm_dmt_modes ever regrows the CVT-R modes (and it will) this will
- * need to account for them.
- */
 static int
-drm_gtf_modes_for_range(struct drm_connector *connector, struct edid *edid,
+drm_dmt_modes_for_range(struct drm_connector *connector, struct edid *edid,
 			struct detailed_timing *timing)
 {
 	int i, modes = 0;
@@ -1019,17 +1039,110 @@ drm_gtf_modes_for_range(struct drm_connector *connector, struct edid *edid,
 	return modes;
 }
 
+/* fix up 1366x768 mode from 1368x768;
+ * GFT/CVT can't express 1366 width which isn't dividable by 8
+ */
+static void fixup_mode_1366x768(struct drm_display_mode *mode)
+{
+	if (mode->hdisplay == 1368 && mode->vdisplay == 768) {
+		mode->hdisplay = 1366;
+		mode->hsync_start--;
+		mode->hsync_end--;
+		drm_mode_set_name(mode);
+	}
+}
+
+static int
+drm_gtf_modes_for_range(struct drm_connector *connector, struct edid *edid,
+			struct detailed_timing *timing)
+{
+	int i, modes = 0;
+	struct drm_display_mode *newmode;
+	struct drm_device *dev = connector->dev;
+
+	for (i = 0; i < num_extra_modes; i++) {
+		const struct minimode *m = &extra_modes[i];
+		newmode = drm_gtf_mode(dev, m->w, m->h, m->r, 0, 0);
+		if (!newmode)
+			return modes;
+
+		fixup_mode_1366x768(newmode);
+		if (!mode_in_range(newmode, edid, timing)) {
+			drm_mode_destroy(dev, newmode);
+			continue;
+		}
+
+		drm_mode_probed_add(connector, newmode);
+		modes++;
+	}
+
+	return modes;
+}
+
+static int
+drm_cvt_modes_for_range(struct drm_connector *connector, struct edid *edid,
+			struct detailed_timing *timing)
+{
+	int i, modes = 0;
+	struct drm_display_mode *newmode;
+	struct drm_device *dev = connector->dev;
+	bool rb = drm_monitor_supports_rb(edid);
+
+	for (i = 0; i < num_extra_modes; i++) {
+		const struct minimode *m = &extra_modes[i];
+		newmode = drm_cvt_mode(dev, m->w, m->h, m->r, rb, 0, 0);
+		if (!newmode)
+			return modes;
+
+		fixup_mode_1366x768(newmode);
+		if (!mode_in_range(newmode, edid, timing)) {
+			drm_mode_destroy(dev, newmode);
+			continue;
+		}
+
+		drm_mode_probed_add(connector, newmode);
+		modes++;
+	}
+
+	return modes;
+}
+
 static void
 do_inferred_modes(struct detailed_timing *timing, void *c)
 {
 	struct detailed_mode_closure *closure = c;
 	struct detailed_non_pixel *data = &timing->data.other_data;
-	int gtf = (closure->edid->features & DRM_EDID_FEATURE_DEFAULT_GTF);
+	struct detailed_data_monitor_range *range = &data->data.range;
+
+	if (data->type != EDID_DETAIL_MONITOR_RANGE)
+		return;
 
-	if (gtf && data->type == EDID_DETAIL_MONITOR_RANGE)
+	closure->modes += drm_dmt_modes_for_range(closure->connector,
+						  closure->edid,
+						  timing);
+	
+	if (!version_greater(closure->edid, 1, 1))
+		return; /* GTF not defined yet */
+
+	switch (range->flags) {
+	case 0x02: /* secondary gtf, XXX could do more */
+	case 0x00: /* default gtf */
 		closure->modes += drm_gtf_modes_for_range(closure->connector,
 							  closure->edid,
 							  timing);
+		break;
+	case 0x04: /* cvt, only in 1.4+ */
+		if (!version_greater(closure->edid, 1, 3))
+			break;
+
+		closure->modes += drm_cvt_modes_for_range(closure->connector,
+							  closure->edid,
+							  timing);
+		break;
+	case 0x01: /* just the ranges, no formula */
+	default:
+		break;
+	}
 }
 
 static int
@@ -1062,8 +1175,8 @@ drm_est3_modes(struct drm_connector *connector, struct detailed_timing *timing)
 				mode = drm_mode_find_dmt(connector->dev,
 							 est3_modes[m].w,
 							 est3_modes[m].h,
-							 est3_modes[m].r
-							 /*, est3_modes[m].rb */);
+							 est3_modes[m].r,
+							 est3_modes[m].rb);
 				if (mode) {
 					drm_mode_probed_add(connector, mode);
 					modes++;
@@ -1312,6 +1425,8 @@ add_detailed_modes(struct drm_connector *connector, struct edid *edid,
 #define VENDOR_BLOCK    0x03
 #define SPEAKER_BLOCK	0x04
 #define EDID_BASIC_AUDIO	(1 << 6)
+#define EDID_CEA_YCRCB444	(1 << 5)
+#define EDID_CEA_YCRCB422	(1 << 4)
 
 /**
  * Search EDID for CEA extension block.
@@ -1666,13 +1781,29 @@ static void drm_add_display_info(struct edid *edid,
 	info->bpc = 0;
 	info->color_formats = 0;
 
-	/* Only defined for 1.4 with digital displays */
-	if (edid->revision < 4)
+	if (edid->revision < 3)
 		return;
 
 	if (!(edid->input & DRM_EDID_INPUT_DIGITAL))
 		return;
 
+	/* Get data from CEA blocks if present */
+	edid_ext = drm_find_cea_extension(edid);
+	if (edid_ext) {
+		info->cea_rev = edid_ext[1];
+
+		/* The existence of a CEA block should imply RGB support */
+		info->color_formats = DRM_COLOR_FORMAT_RGB444;
+		if (edid_ext[3] & EDID_CEA_YCRCB444)
+			info->color_formats |= DRM_COLOR_FORMAT_YCRCB444;
+		if (edid_ext[3] & EDID_CEA_YCRCB422)
+			info->color_formats |= DRM_COLOR_FORMAT_YCRCB422;
+	}
+
+	/* Only defined for 1.4 with digital displays */
+	if (edid->revision < 4)
+		return;
+
 	switch (edid->input & DRM_EDID_DIGITAL_DEPTH_MASK) {
 	case DRM_EDID_DIGITAL_DEPTH_6:
 		info->bpc = 6;
@@ -1698,18 +1829,11 @@ static void drm_add_display_info(struct edid *edid,
 		break;
 	}
 
-	info->color_formats = DRM_COLOR_FORMAT_RGB444;
-	if (info->color_formats & DRM_EDID_FEATURE_RGB_YCRCB444)
-		info->color_formats = DRM_COLOR_FORMAT_YCRCB444;
-	if (info->color_formats & DRM_EDID_FEATURE_RGB_YCRCB422)
-		info->color_formats = DRM_COLOR_FORMAT_YCRCB422;
-
-	/* Get data from CEA blocks if present */
-	edid_ext = drm_find_cea_extension(edid);
-	if (!edid_ext)
-		return;
-
-	info->cea_rev = edid_ext[1];
+	info->color_formats |= DRM_COLOR_FORMAT_RGB444;
+	if (edid->features & DRM_EDID_FEATURE_RGB_YCRCB444)
+		info->color_formats |= DRM_COLOR_FORMAT_YCRCB444;
+	if (edid->features & DRM_EDID_FEATURE_RGB_YCRCB422)
+		info->color_formats |= DRM_COLOR_FORMAT_YCRCB422;
 }
 
 /**
diff --git a/drivers/gpu/drm/drm_edid_load.c b/drivers/gpu/drm/drm_edid_load.c
index da9acba2dd6c..66d4a28ad5a2 100644
--- a/drivers/gpu/drm/drm_edid_load.c
+++ b/drivers/gpu/drm/drm_edid_load.c
@@ -173,7 +173,7 @@ static int edid_load(struct drm_connector *connector, char *name,
 	}
 	memcpy(edid, fwdata, fwsize);
 
-	if (!drm_edid_block_valid(edid)) {
+	if (!drm_edid_block_valid(edid, 0)) {
 		DRM_ERROR("Base block of EDID firmware \"%s\" is invalid ",
 		    name);
 		kfree(edid);
@@ -185,7 +185,7 @@ static int edid_load(struct drm_connector *connector, char *name,
 		if (i != valid_extensions + 1)
 			memcpy(edid + (valid_extensions + 1) * EDID_LENGTH,
 			    edid + i * EDID_LENGTH, EDID_LENGTH);
-		if (drm_edid_block_valid(edid + i * EDID_LENGTH))
+		if (drm_edid_block_valid(edid + i * EDID_LENGTH, i))
 			valid_extensions++;
 	}
 
@@ -220,18 +220,18 @@ int drm_load_edid_firmware(struct drm_connector *connector)
 {
 	char *connector_name = drm_get_connector_name(connector);
 	char *edidname = edid_firmware, *last, *colon;
-	int ret = 0;
+	int ret;
 
 	if (*edidname == '\0')
-		return ret;
+		return 0;
 
 	colon = strchr(edidname, ':');
 	if (colon != NULL) {
 		if (strncmp(connector_name, edidname, colon - edidname))
-			return ret;
+			return 0;
 		edidname = colon + 1;
 		if (*edidname == '\0')
-			return ret;
+			return 0;
 	}
 
 	last = edidname + strlen(edidname) - 1;
diff --git a/drivers/gpu/drm/drm_edid_modes.h b/drivers/gpu/drm/drm_edid_modes.h
index a91ffb117220..ff98a7eb38dd 100644
--- a/drivers/gpu/drm/drm_edid_modes.h
+++ b/drivers/gpu/drm/drm_edid_modes.h
@@ -30,7 +30,6 @@
 /*
  * Autogenerated from the DMT spec.
  * This table is copied from xfree86/modes/xf86EdidModes.c.
- * But the mode with Reduced blank feature is deleted.
  */
 static const struct drm_display_mode drm_dmt_modes[] = {
 	/* 640x350@85Hz */
@@ -81,6 +80,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("800x600", DRM_MODE_TYPE_DRIVER, 56250, 800, 832,
 		   896, 1048, 0, 600, 601, 604, 631, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 800x600@120Hz RB */
+	{ DRM_MODE("800x600", DRM_MODE_TYPE_DRIVER, 73250, 800, 848,
+		   880, 960, 0, 600, 603, 607, 636, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 848x480@60Hz */
 	{ DRM_MODE("848x480", DRM_MODE_TYPE_DRIVER, 33750, 848, 864,
 		   976, 1088, 0, 480, 486, 494, 517, 0,
@@ -106,10 +109,18 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 94500, 1024, 1072,
 		   1168, 1376, 0, 768, 769, 772, 808, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1024x768@120Hz RB */
+	{ DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 115500, 1024, 1072,
+		   1104, 1184, 0, 768, 771, 775, 813, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1152x864@75Hz */
 	{ DRM_MODE("1152x864", DRM_MODE_TYPE_DRIVER, 108000, 1152, 1216,
 		   1344, 1600, 0, 864, 865, 868, 900, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x768@60Hz RB */
+	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 68250, 1280, 1328,
+		   1360, 1440, 0, 768, 771, 778, 790, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x768@60Hz */
 	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 79500, 1280, 1344,
 		   1472, 1664, 0, 768, 771, 778, 798, 0,
@@ -122,6 +133,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 117500, 1280, 1360,
 		   1496, 1712, 0, 768, 771, 778, 809, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x768@120Hz RB */
+	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 140250, 1280, 1328,
+		   1360, 1440, 0, 768, 771, 778, 813, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1280x800@60Hz RB */
+	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 71000, 1280, 1328,
+		   1360, 1440, 0, 800, 803, 809, 823, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x800@60Hz */
 	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 83500, 1280, 1352,
 		   1480, 1680, 0, 800, 803, 809, 831, 0,
@@ -134,6 +153,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 122500, 1280, 1360,
 		   1496, 1712, 0, 800, 803, 809, 843, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x800@120Hz RB */
+	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 146250, 1280, 1328,
+		   1360, 1440, 0, 800, 803, 809, 847, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x960@60Hz */
 	{ DRM_MODE("1280x960", DRM_MODE_TYPE_DRIVER, 108000, 1280, 1376,
 		   1488, 1800, 0, 960, 961, 964, 1000, 0,
@@ -142,6 +165,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x960", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1344,
 		   1504, 1728, 0, 960, 961, 964, 1011, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x960@120Hz RB */
+	{ DRM_MODE("1280x960", DRM_MODE_TYPE_DRIVER, 175500, 1280, 1328,
+		   1360, 1440, 0, 960, 963, 967, 1017, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x1024@60Hz */
 	{ DRM_MODE("1280x1024", DRM_MODE_TYPE_DRIVER, 108000, 1280, 1328,
 		   1440, 1688, 0, 1024, 1025, 1028, 1066, 0,
@@ -154,22 +181,42 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x1024", DRM_MODE_TYPE_DRIVER, 157500, 1280, 1344,
 		   1504, 1728, 0, 1024, 1025, 1028, 1072, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x1024@120Hz RB */
+	{ DRM_MODE("1280x1024", DRM_MODE_TYPE_DRIVER, 187250, 1280, 1328,
+		   1360, 1440, 0, 1024, 1027, 1034, 1084, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1360x768@60Hz */
 	{ DRM_MODE("1360x768", DRM_MODE_TYPE_DRIVER, 85500, 1360, 1424,
 		   1536, 1792, 0, 768, 771, 777, 795, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1440x1050@60Hz */
+	/* 1360x768@120Hz RB */
+	{ DRM_MODE("1360x768", DRM_MODE_TYPE_DRIVER, 148250, 1360, 1408,
+		   1440, 1520, 0, 768, 771, 776, 813, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1400x1050@60Hz RB */
+	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 101000, 1400, 1448,
+		   1480, 1560, 0, 1050, 1053, 1057, 1080, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1400x1050@60Hz */
 	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 121750, 1400, 1488,
 		   1632, 1864, 0, 1050, 1053, 1057, 1089, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1440x1050@75Hz */
+	/* 1400x1050@75Hz */
 	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 156000, 1400, 1504,
 		   1648, 1896, 0, 1050, 1053, 1057, 1099, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1440x1050@85Hz */
+	/* 1400x1050@85Hz */
 	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 179500, 1400, 1504,
 		   1656, 1912, 0, 1050, 1053, 1057, 1105, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1400x1050@120Hz RB */
+	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 208000, 1400, 1448,
+		   1480, 1560, 0, 1050, 1053, 1057, 1112, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x900@60Hz RB */
+	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 88750, 1440, 1488,
+		   1520, 1600, 0, 900, 903, 909, 926, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1440x900@60Hz */
 	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 106500, 1440, 1520,
 		   1672, 1904, 0, 900, 903, 909, 934, 0,
@@ -182,6 +229,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 157000, 1440, 1544,
 		   1696, 1952, 0, 900, 903, 909, 948, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1440x900@120Hz RB */
+	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 182750, 1440, 1488,
+		   1520, 1600, 0, 900, 903, 909, 953, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1600x1200@60Hz */
 	{ DRM_MODE("1600x1200", DRM_MODE_TYPE_DRIVER, 162000, 1600, 1664,
 		   1856, 2160, 0, 1200, 1201, 1204, 1250, 0,
@@ -202,6 +253,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1600x1200", DRM_MODE_TYPE_DRIVER, 229500, 1600, 1664,
 		   1856, 2160, 0, 1200, 1201, 1204, 1250, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1600x1200@120Hz RB */
+	{ DRM_MODE("1600x1200", DRM_MODE_TYPE_DRIVER, 268250, 1600, 1648,
+		   1680, 1760, 0, 1200, 1203, 1207, 1271, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1680x1050@60Hz RB */
+	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 119000, 1680, 1728,
+		   1760, 1840, 0, 1050, 1053, 1059, 1080, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1680x1050@60Hz */
 	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 146250, 1680, 1784,
 		   1960, 2240, 0, 1050, 1053, 1059, 1089, 0,
@@ -214,15 +273,23 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 214750, 1680, 1808,
 		   1984, 2288, 0, 1050, 1053, 1059, 1105, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1680x1050@120Hz RB */
+	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 245500, 1680, 1728,
+		   1760, 1840, 0, 1050, 1053, 1059, 1112, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1792x1344@60Hz */
 	{ DRM_MODE("1792x1344", DRM_MODE_TYPE_DRIVER, 204750, 1792, 1920,
 		   2120, 2448, 0, 1344, 1345, 1348, 1394, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1729x1344@75Hz */
+	/* 1792x1344@75Hz */
 	{ DRM_MODE("1792x1344", DRM_MODE_TYPE_DRIVER, 261000, 1792, 1888,
 		   2104, 2456, 0, 1344, 1345, 1348, 1417, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1853x1392@60Hz */
+	/* 1792x1344@120Hz RB */
+	{ DRM_MODE("1792x1344", DRM_MODE_TYPE_DRIVER, 333250, 1792, 1840,
+		   1872, 1952, 0, 1344, 1347, 1351, 1423, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1856x1392@60Hz */
 	{ DRM_MODE("1856x1392", DRM_MODE_TYPE_DRIVER, 218250, 1856, 1952,
 		   2176, 2528, 0, 1392, 1393, 1396, 1439, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
@@ -230,6 +297,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1856x1392", DRM_MODE_TYPE_DRIVER, 288000, 1856, 1984,
 		   2208, 2560, 0, 1392, 1395, 1399, 1500, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1856x1392@120Hz RB */
+	{ DRM_MODE("1856x1392", DRM_MODE_TYPE_DRIVER, 356500, 1856, 1904,
+		   1936, 2016, 0, 1392, 1395, 1399, 1474, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1920x1200@60Hz RB */
+	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 154000, 1920, 1968,
+		   2000, 2080, 0, 1200, 1203, 1209, 1235, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1920x1200@60Hz */
 	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 193250, 1920, 2056,
 		   2256, 2592, 0, 1200, 1203, 1209, 1245, 0,
@@ -242,6 +317,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 281250, 1920, 2064,
 		   2272, 2624, 0, 1200, 1203, 1209, 1262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1200@120Hz RB */
+	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 317000, 1920, 1968,
+		   2000, 2080, 0, 1200, 1203, 1209, 1271, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1920x1440@60Hz */
 	{ DRM_MODE("1920x1440", DRM_MODE_TYPE_DRIVER, 234000, 1920, 2048,
 		   2256, 2600, 0, 1440, 1441, 1444, 1500, 0,
@@ -250,6 +329,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1920x1440", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2064,
 		   2288, 2640, 0, 1440, 1441, 1444, 1500, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1440@120Hz RB */
+	{ DRM_MODE("1920x1440", DRM_MODE_TYPE_DRIVER, 380500, 1920, 1968,
+		   2000, 2080, 0, 1440, 1443, 1447, 1525, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2560x1600@60Hz RB */
+	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 268500, 2560, 2608,
+		   2640, 2720, 0, 1600, 1603, 1609, 1646, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 2560x1600@60Hz */
 	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 348500, 2560, 2752,
 		   3032, 3504, 0, 1600, 1603, 1609, 1658, 0,
@@ -262,6 +349,11 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 505250, 2560, 2768,
 		   3048, 3536, 0, 1600, 1603, 1609, 1682, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 2560x1600@120Hz RB */
+	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 552750, 2560, 2608,
+		   2640, 2720, 0, 1600, 1603, 1609, 1694, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+
 };
 static const int drm_num_dmt_modes =
 	sizeof(drm_dmt_modes) / sizeof(struct drm_display_mode);
@@ -320,12 +412,14 @@ static const struct drm_display_mode edid_est_modes[] = {
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) }, /* 1152x864@75Hz */
 };
 
-static const struct {
+struct minimode {
 	short w;
 	short h;
 	short r;
 	short rb;
-} est3_modes[] = {
+};
+
+static const struct minimode est3_modes[] = {
 	/* byte 6 */
 	{ 640, 350, 85, 0 },
 	{ 640, 400, 85, 0 },
@@ -377,288 +471,304 @@ static const struct {
 	{ 1920, 1440, 60, 0 },
 	{ 1920, 1440, 75, 0 },
 };
-static const int num_est3_modes = sizeof(est3_modes) / sizeof(est3_modes[0]);
+static const int num_est3_modes = ARRAY_SIZE(est3_modes);
+
+static const struct minimode extra_modes[] = {
+	{ 1024, 576,  60, 0 },
+	{ 1366, 768,  60, 0 },
+	{ 1600, 900,  60, 0 },
+	{ 1680, 945,  60, 0 },
+	{ 1920, 1080, 60, 0 },
+	{ 2048, 1152, 60, 0 },
+	{ 2048, 1536, 60, 0 },
+};
+static const int num_extra_modes = ARRAY_SIZE(extra_modes);
 
 /*
  * Probably taken from CEA-861 spec.
  * This table is converted from xorg's hw/xfree86/modes/xf86EdidModes.c.
  */
 static const struct drm_display_mode edid_cea_modes[] = {
-	/* 640x480@60Hz */
+	/* 1 - 640x480@60Hz */
 	{ DRM_MODE("640x480", DRM_MODE_TYPE_DRIVER, 25175, 640, 656,
 		   752, 800, 0, 480, 490, 492, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 720x480@60Hz */
+	/* 2 - 720x480@60Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 27000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 720x480@60Hz */
+	/* 3 - 720x480@60Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 27000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1280x720@60Hz */
+	/* 4 - 1280x720@60Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 1390,
 		   1430, 1650, 0, 720, 725, 730, 750, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1920x1080i@60Hz */
+	/* 5 - 1920x1080i@60Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x480i@60Hz */
+	/* 6 - 1440x480i@60Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x480i@60Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 7 - 1440x480i@60Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x240@60Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 8 - 1440x240@60Hz */
 	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 240, 244, 247, 262, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x240@60Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 9 - 1440x240@60Hz */
 	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 240, 244, 247, 262, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 2880x480i@60Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 10 - 2880x480i@60Hz */
 	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 2880x480i@60Hz */
+	/* 11 - 2880x480i@60Hz */
 	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 2880x240@60Hz */
+	/* 12 - 2880x240@60Hz */
 	{ DRM_MODE("2880x240", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 240, 244, 247, 262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 2880x240@60Hz */
+	/* 13 - 2880x240@60Hz */
 	{ DRM_MODE("2880x240", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 240, 244, 247, 262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x480@60Hz */
+	/* 14 - 1440x480@60Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1472,
 		   1596, 1716, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x480@60Hz */
+	/* 15 - 1440x480@60Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1472,
 		   1596, 1716, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1920x1080@60Hz */
+	/* 16 - 1920x1080@60Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 720x576@50Hz */
+	/* 17 - 720x576@50Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 720x576@50Hz */
+	/* 18 - 720x576@50Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1280x720@50Hz */
+	/* 19 - 1280x720@50Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 1720,
 		   1760, 1980, 0, 720, 725, 730, 750, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1920x1080i@50Hz */
+	/* 20 - 1920x1080i@50Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x576i@50Hz */
+	/* 21 - 1440x576i@50Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x576i@50Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 22 - 1440x576i@50Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x288@50Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 23 - 1440x288@50Hz */
 	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 288, 290, 293, 312, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x288@50Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 24 - 1440x288@50Hz */
 	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 288, 290, 293, 312, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 2880x576i@50Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 25 - 2880x576i@50Hz */
 	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 2880x576i@50Hz */
+	/* 26 - 2880x576i@50Hz */
 	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 2880x288@50Hz */
+	/* 27 - 2880x288@50Hz */
 	{ DRM_MODE("2880x288", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 288, 290, 293, 312, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 2880x288@50Hz */
+	/* 28 - 2880x288@50Hz */
 	{ DRM_MODE("2880x288", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 288, 290, 293, 312, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x576@50Hz */
+	/* 29 - 1440x576@50Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1592, 1728, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x576@50Hz */
+	/* 30 - 1440x576@50Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1592, 1728, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1920x1080@50Hz */
+	/* 31 - 1920x1080@50Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1089, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1920x1080@24Hz */
+	/* 32 - 1920x1080@24Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2558,
 		   2602, 2750, 0, 1080, 1084, 1089, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1920x1080@25Hz */
+	/* 33 - 1920x1080@25Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1089, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1920x1080@30Hz */
+	/* 34 - 1920x1080@30Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 2880x480@60Hz */
+	/* 35 - 2880x480@60Hz */
 	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2944,
 		   3192, 3432, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 2880x480@60Hz */
+	/* 36 - 2880x480@60Hz */
 	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2944,
 		   3192, 3432, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 2880x576@50Hz */
+	/* 37 - 2880x576@50Hz */
 	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2928,
 		   3184, 3456, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 2880x576@50Hz */
+	/* 38 - 2880x576@50Hz */
 	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2928,
 		   3184, 3456, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1920x1080i@50Hz */
+	/* 39 - 1920x1080i@50Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 72000, 1920, 1952,
 		   2120, 2304, 0, 1080, 1126, 1136, 1250, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 1920x1080i@100Hz */
+	/* 40 - 1920x1080i@100Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 1280x720@100Hz */
+	/* 41 - 1280x720@100Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1720,
 		   1760, 1980, 0, 720, 725, 730, 750, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 720x576@100Hz */
+	/* 42 - 720x576@100Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 720x576@100Hz */
+	/* 43 - 720x576@100Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x576i@100Hz */
+	/* 44 - 1440x576i@100Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x576i@100Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 45 - 1440x576i@100Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1920x1080i@120Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 46 - 1920x1080i@120Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
 			DRM_MODE_FLAG_INTERLACE) },
-	/* 1280x720@120Hz */
+	/* 47 - 1280x720@120Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1390,
 		   1430, 1650, 0, 720, 725, 730, 750, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 720x480@120Hz */
+	/* 48 - 720x480@120Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 54000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 720x480@120Hz */
+	/* 49 - 720x480@120Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 54000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x480i@120Hz */
+	/* 50 - 1440x480i@120Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x480i@120Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 51 - 1440x480i@120Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 720x576@200Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 52 - 720x576@200Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 108000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 720x576@200Hz */
+	/* 53 - 720x576@200Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 108000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x576i@200Hz */
+	/* 54 - 1440x576i@200Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x576i@200Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 55 - 1440x576i@200Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 720x480@240Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 56 - 720x480@240Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 108000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 720x480@240Hz */
+	/* 57 - 720x480@240Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 108000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
-	/* 1440x480i@240 */
+	/* 58 - 1440x480i@240 */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1440x480i@240 */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 59 - 1440x480i@240 */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
-	/* 1280x720@24Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 60 - 1280x720@24Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 59400, 1280, 3040,
 		   3080, 3300, 0, 720, 725, 730, 750, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1280x720@25Hz */
+	/* 61 - 1280x720@25Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 3700,
 		   3740, 3960, 0, 720, 725, 730, 750, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1280x720@30Hz */
+	/* 62 - 1280x720@30Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 3040,
 		   3080, 3300, 0, 720, 725, 730, 750, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1920x1080@120Hz */
+	/* 63 - 1920x1080@120Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1920x1080@100Hz */
+	/* 64 - 1920x1080@100Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
 };
-static const int drm_num_cea_modes =
-	sizeof (edid_cea_modes) / sizeof (edid_cea_modes[0]);
+static const int drm_num_cea_modes = ARRAY_SIZE(edid_cea_modes);
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index a0d6e894d97c..5683b7fdd746 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -136,6 +136,9 @@ static void drm_fb_helper_restore_lut_atomic(struct drm_crtc *crtc)
 {
 	uint16_t *r_base, *g_base, *b_base;
 
+	if (crtc->funcs->gamma_set == NULL)
+		return;
+
 	r_base = crtc->gamma_store;
 	g_base = r_base + crtc->gamma_size;
 	b_base = g_base + crtc->gamma_size;
@@ -383,7 +386,6 @@ int drm_fb_helper_init(struct drm_device *dev,
 		       int crtc_count, int max_conn_count)
 {
 	struct drm_crtc *crtc;
-	int ret = 0;
 	int i;
 
 	fb_helper->dev = dev;
@@ -408,10 +410,8 @@ int drm_fb_helper_init(struct drm_device *dev,
 				sizeof(struct drm_connector *),
 				GFP_KERNEL);
 
-		if (!fb_helper->crtc_info[i].mode_set.connectors) {
-			ret = -ENOMEM;
+		if (!fb_helper->crtc_info[i].mode_set.connectors)
 			goto out_free;
-		}
 		fb_helper->crtc_info[i].mode_set.num_connectors = 0;
 	}
 
@@ -1083,7 +1083,7 @@ static bool drm_target_cloned(struct drm_fb_helper *fb_helper,
 
 	/* try and find a 1024x768 mode on each connector */
 	can_clone = true;
-	dmt_mode = drm_mode_find_dmt(fb_helper->dev, 1024, 768, 60);
+	dmt_mode = drm_mode_find_dmt(fb_helper->dev, 1024, 768, 60, false);
 
 	for (i = 0; i < fb_helper->connector_count; i++) {
 
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index 83114b5e3cee..d58e69da1fb5 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -201,6 +201,19 @@ free:
 }
 EXPORT_SYMBOL(drm_gem_object_alloc);
 
+static void
+drm_gem_remove_prime_handles(struct drm_gem_object *obj, struct drm_file *filp)
+{
+	if (obj->import_attach) {
+		drm_prime_remove_imported_buf_handle(&filp->prime,
+				obj->import_attach->dmabuf);
+	}
+	if (obj->export_dma_buf) {
+		drm_prime_remove_imported_buf_handle(&filp->prime,
+				obj->export_dma_buf);
+	}
+}
+
 /**
  * Removes the mapping from handle to filp for this object.
  */
@@ -233,9 +246,7 @@ drm_gem_handle_delete(struct drm_file *filp, u32 handle)
 	idr_remove(&filp->object_idr, handle);
 	spin_unlock(&filp->table_lock);
 
-	if (obj->import_attach)
-		drm_prime_remove_imported_buf_handle(&filp->prime,
-				obj->import_attach->dmabuf);
+	drm_gem_remove_prime_handles(obj, filp);
 
 	if (dev->driver->gem_close_object)
 		dev->driver->gem_close_object(obj, filp);
@@ -272,8 +283,7 @@ again:
 	spin_unlock(&file_priv->table_lock);
 	if (ret == -EAGAIN)
 		goto again;
-
-	if (ret != 0)
+	else if (ret)
 		return ret;
 
 	drm_gem_object_handle_reference(obj);
@@ -329,7 +339,7 @@ drm_gem_create_mmap_offset(struct drm_gem_object *obj)
 	struct drm_gem_mm *mm = dev->mm_private;
 	struct drm_map_list *list;
 	struct drm_local_map *map;
-	int ret = 0;
+	int ret;
 
 	/* Set the object up for mmap'ing */
 	list = &obj->map_list;
@@ -456,8 +466,7 @@ again:
 
 		if (ret == -EAGAIN)
 			goto again;
-
-		if (ret != 0)
+		else if (ret)
 			goto err;
 
 		/* Allocate a reference for the name table.  */
@@ -532,9 +541,7 @@ drm_gem_object_release_handle(int id, void *ptr, void *data)
 	struct drm_gem_object *obj = ptr;
 	struct drm_device *dev = obj->dev;
 
-	if (obj->import_attach)
-		drm_prime_remove_imported_buf_handle(&file_priv->prime,
-				obj->import_attach->dmabuf);
+	drm_gem_remove_prime_handles(obj, file_priv);
 
 	if (dev->driver->gem_close_object)
 		dev->driver->gem_close_object(obj, file_priv);
@@ -628,7 +635,7 @@ void drm_gem_vm_open(struct vm_area_struct *vma)
 	drm_gem_object_reference(obj);
 
 	mutex_lock(&obj->dev->struct_mutex);
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(obj->dev, vma);
 	mutex_unlock(&obj->dev->struct_mutex);
 }
 EXPORT_SYMBOL(drm_gem_vm_open);
@@ -639,7 +646,7 @@ void drm_gem_vm_close(struct vm_area_struct *vma)
 	struct drm_device *dev = obj->dev;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_vm_close_locked(vma);
+	drm_vm_close_locked(obj->dev, vma);
 	drm_gem_object_unreference(obj);
 	mutex_unlock(&dev->struct_mutex);
 }
@@ -712,7 +719,7 @@ int drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 	 */
 	drm_gem_object_reference(obj);
 
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 
 out_unlock:
 	mutex_unlock(&dev->struct_mutex);
diff --git a/drivers/gpu/drm/drm_ioctl.c b/drivers/gpu/drm/drm_ioctl.c
index cf85155da2a0..64a62c697313 100644
--- a/drivers/gpu/drm/drm_ioctl.c
+++ b/drivers/gpu/drm/drm_ioctl.c
@@ -283,6 +283,10 @@ int drm_getcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
 	case DRM_CAP_DUMB_PREFER_SHADOW:
 		req->value = dev->mode_config.prefer_shadow;
 		break;
+	case DRM_CAP_PRIME:
+		req->value |= dev->driver->prime_fd_to_handle ? DRM_PRIME_CAP_IMPORT : 0;
+		req->value |= dev->driver->prime_handle_to_fd ? DRM_PRIME_CAP_EXPORT : 0;
+		break;
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index c869436e238a..c798eeae0a03 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -189,7 +189,7 @@ void drm_vblank_cleanup(struct drm_device *dev)
 	if (dev->num_crtcs == 0)
 		return;
 
-	del_timer(&dev->vblank_disable_timer);
+	del_timer_sync(&dev->vblank_disable_timer);
 
 	vblank_disable_fn((unsigned long)dev);
 
@@ -310,7 +310,7 @@ static void drm_irq_vgaarb_nokms(void *cookie, bool state)
  */
 int drm_irq_install(struct drm_device *dev)
 {
-	int ret = 0;
+	int ret;
 	unsigned long sh_flags = 0;
 	char *irqname;
 
@@ -731,7 +731,7 @@ EXPORT_SYMBOL(drm_calc_vbltimestamp_from_scanoutpos);
 u32 drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
 			      struct timeval *tvblank, unsigned flags)
 {
-	int ret = 0;
+	int ret;
 
 	/* Define requested maximum error on timestamps (nanoseconds). */
 	int max_error = (int) drm_timestamp_precision * 1000;
@@ -1031,18 +1031,15 @@ int drm_modeset_ctl(struct drm_device *dev, void *data,
 		    struct drm_file *file_priv)
 {
 	struct drm_modeset_ctl *modeset = data;
-	int ret = 0;
 	unsigned int crtc;
 
 	/* If drm_vblank_init() hasn't been called yet, just no-op */
 	if (!dev->num_crtcs)
-		goto out;
+		return 0;
 
 	crtc = modeset->crtc;
-	if (crtc >= dev->num_crtcs) {
-		ret = -EINVAL;
-		goto out;
-	}
+	if (crtc >= dev->num_crtcs)
+		return -EINVAL;
 
 	switch (modeset->cmd) {
 	case _DRM_PRE_MODESET:
@@ -1052,12 +1049,10 @@ int drm_modeset_ctl(struct drm_device *dev, void *data,
 		drm_vblank_post_modeset(dev, crtc);
 		break;
 	default:
-		ret = -EINVAL;
-		break;
+		return -EINVAL;
 	}
 
-out:
-	return ret;
+	return 0;
 }
 
 static int drm_queue_vblank_event(struct drm_device *dev, int pipe,
@@ -1154,7 +1149,7 @@ int drm_wait_vblank(struct drm_device *dev, void *data,
 		    struct drm_file *file_priv)
 {
 	union drm_wait_vblank *vblwait = data;
-	int ret = 0;
+	int ret;
 	unsigned int flags, seq, crtc, high_crtc;
 
 	if ((!drm_dev_to_irq(dev)) || (!dev->irq_enabled))
diff --git a/drivers/gpu/drm/drm_lock.c b/drivers/gpu/drm/drm_lock.c
index c79c713eeba0..521152041691 100644
--- a/drivers/gpu/drm/drm_lock.c
+++ b/drivers/gpu/drm/drm_lock.c
@@ -331,7 +331,7 @@ static int drm_notifier(void *priv)
 
 void drm_idlelock_take(struct drm_lock_data *lock_data)
 {
-	int ret = 0;
+	int ret;
 
 	spin_lock_bh(&lock_data->spinlock);
 	lock_data->kernel_waiters++;
diff --git a/drivers/gpu/drm/drm_prime.c b/drivers/gpu/drm/drm_prime.c
index 1bdf2b54eaf6..f546ff98a114 100644
--- a/drivers/gpu/drm/drm_prime.c
+++ b/drivers/gpu/drm/drm_prime.c
@@ -68,6 +68,7 @@ int drm_gem_prime_handle_to_fd(struct drm_device *dev,
 {
 	struct drm_gem_object *obj;
 	void *buf;
+	int ret;
 
 	obj = drm_gem_object_lookup(dev, file_priv, handle);
 	if (!obj)
@@ -100,6 +101,17 @@ int drm_gem_prime_handle_to_fd(struct drm_device *dev,
 		obj->export_dma_buf = buf;
 		*prime_fd = dma_buf_fd(buf, flags);
 	}
+	/* if we've exported this buffer the cheat and add it to the import list
+	 * so we get the correct handle back
+	 */
+	ret = drm_prime_add_imported_buf_handle(&file_priv->prime,
+			obj->export_dma_buf, handle);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(obj);
+		mutex_unlock(&file_priv->prime.lock);
+		return ret;
+	}
+
 	mutex_unlock(&file_priv->prime.lock);
 	return 0;
 }
@@ -227,6 +239,42 @@ out:
 }
 EXPORT_SYMBOL(drm_prime_pages_to_sg);
 
+/* export an sg table into an array of pages and addresses
+   this is currently required by the TTM driver in order to do correct fault
+   handling */
+int drm_prime_sg_to_page_addr_arrays(struct sg_table *sgt, struct page **pages,
+				     dma_addr_t *addrs, int max_pages)
+{
+	unsigned count;
+	struct scatterlist *sg;
+	struct page *page;
+	u32 len, offset;
+	int pg_index;
+	dma_addr_t addr;
+
+	pg_index = 0;
+	for_each_sg(sgt->sgl, sg, sgt->nents, count) {
+		len = sg->length;
+		offset = sg->offset;
+		page = sg_page(sg);
+		addr = sg_dma_address(sg);
+
+		while (len > 0) {
+			if (WARN_ON(pg_index >= max_pages))
+				return -1;
+			pages[pg_index] = page;
+			if (addrs)
+				addrs[pg_index] = addr;
+
+			page++;
+			addr += PAGE_SIZE;
+			len -= PAGE_SIZE;
+			pg_index++;
+		}
+	}
+	return 0;
+}
+EXPORT_SYMBOL(drm_prime_sg_to_page_addr_arrays);
 /* helper function to cleanup a GEM/prime object */
 void drm_prime_gem_destroy(struct drm_gem_object *obj, struct sg_table *sg)
 {
diff --git a/drivers/gpu/drm/drm_stub.c b/drivers/gpu/drm/drm_stub.c
index aa454f80e109..21bcd4a555d8 100644
--- a/drivers/gpu/drm/drm_stub.c
+++ b/drivers/gpu/drm/drm_stub.c
@@ -122,11 +122,10 @@ again:
 	ret = idr_get_new_above(&drm_minors_idr, NULL,
 				base, &new_id);
 	mutex_unlock(&dev->struct_mutex);
-	if (ret == -EAGAIN) {
+	if (ret == -EAGAIN)
 		goto again;
-	} else if (ret) {
+	else if (ret)
 		return ret;
-	}
 
 	if (new_id >= limit) {
 		idr_remove(&drm_minors_idr, new_id);
@@ -211,7 +210,7 @@ EXPORT_SYMBOL(drm_master_put);
 int drm_setmaster_ioctl(struct drm_device *dev, void *data,
 			struct drm_file *file_priv)
 {
-	int ret = 0;
+	int ret;
 
 	if (file_priv->is_master)
 		return 0;
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index 5a7bd51fc3d8..45cf1dd3eb9c 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -347,17 +347,17 @@ static struct bin_attribute edid_attr = {
 };
 
 /**
- * drm_sysfs_connector_add - add an connector to sysfs
+ * drm_sysfs_connector_add - add a connector to sysfs
  * @connector: connector to add
  *
- * Create an connector device in sysfs, along with its associated connector
+ * Create a connector device in sysfs, along with its associated connector
  * properties (so far, connection status, dpms, mode list & edid) and
  * generate a hotplug event so userspace knows there's a new connector
  * available.
  *
  * Note:
- * This routine should only be called *once* for each DRM minor registered.
- * A second call for an already registered device will trigger the BUG_ON
+ * This routine should only be called *once* for each registered connector.
+ * A second call for an already registered connector will trigger the BUG_ON
  * below.
  */
 int drm_sysfs_connector_add(struct drm_connector *connector)
@@ -366,7 +366,7 @@ int drm_sysfs_connector_add(struct drm_connector *connector)
 	int attr_cnt = 0;
 	int opt_cnt = 0;
 	int i;
-	int ret = 0;
+	int ret;
 
 	/* We shouldn't get called more than once for the same connector */
 	BUG_ON(device_is_registered(&connector->kdev));
diff --git a/drivers/gpu/drm/drm_vm.c b/drivers/gpu/drm/drm_vm.c
index 149561818349..961ee08927fe 100644
--- a/drivers/gpu/drm/drm_vm.c
+++ b/drivers/gpu/drm/drm_vm.c
@@ -406,10 +406,9 @@ static const struct vm_operations_struct drm_vm_sg_ops = {
  * Create a new drm_vma_entry structure as the \p vma private data entry and
  * add it to drm_device::vmalist.
  */
-void drm_vm_open_locked(struct vm_area_struct *vma)
+void drm_vm_open_locked(struct drm_device *dev,
+		struct vm_area_struct *vma)
 {
-	struct drm_file *priv = vma->vm_file->private_data;
-	struct drm_device *dev = priv->minor->dev;
 	struct drm_vma_entry *vma_entry;
 
 	DRM_DEBUG("0x%08lx,0x%08lx\n",
@@ -430,14 +429,13 @@ static void drm_vm_open(struct vm_area_struct *vma)
 	struct drm_device *dev = priv->minor->dev;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 	mutex_unlock(&dev->struct_mutex);
 }
 
-void drm_vm_close_locked(struct vm_area_struct *vma)
+void drm_vm_close_locked(struct drm_device *dev,
+		struct vm_area_struct *vma)
 {
-	struct drm_file *priv = vma->vm_file->private_data;
-	struct drm_device *dev = priv->minor->dev;
 	struct drm_vma_entry *pt, *temp;
 
 	DRM_DEBUG("0x%08lx,0x%08lx\n",
@@ -467,7 +465,7 @@ static void drm_vm_close(struct vm_area_struct *vma)
 	struct drm_device *dev = priv->minor->dev;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_vm_close_locked(vma);
+	drm_vm_close_locked(dev, vma);
 	mutex_unlock(&dev->struct_mutex);
 }
 
@@ -519,7 +517,7 @@ static int drm_mmap_dma(struct file *filp, struct vm_area_struct *vma)
 	vma->vm_flags |= VM_RESERVED;	/* Don't swap */
 	vma->vm_flags |= VM_DONTEXPAND;
 
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 	return 0;
 }
 
@@ -670,7 +668,7 @@ int drm_mmap_locked(struct file *filp, struct vm_area_struct *vma)
 	vma->vm_flags |= VM_RESERVED;	/* Don't swap */
 	vma->vm_flags |= VM_DONTEXPAND;
 
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index 3343ac437fe5..7f5096763b7d 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -10,6 +10,12 @@ config DRM_EXYNOS
 	  Choose this option if you have a Samsung SoC EXYNOS chipset.
 	  If M is selected the module will be called exynosdrm.
 
+config DRM_EXYNOS_DMABUF
+	bool "EXYNOS DRM DMABUF"
+	depends on DRM_EXYNOS
+	help
+	  Choose this option if you want to use DMABUF feature for DRM.
+
 config DRM_EXYNOS_FIMD
 	bool "Exynos DRM FIMD"
 	depends on DRM_EXYNOS && !FB_S3C
@@ -27,3 +33,9 @@ config DRM_EXYNOS_VIDI
 	depends on DRM_EXYNOS
 	help
 	  Choose this option if you want to use Exynos VIDI for DRM.
+
+config DRM_EXYNOS_G2D
+	bool "Exynos DRM G2D"
+	depends on DRM_EXYNOS
+	help
+	  Choose this option if you want to use Exynos G2D for DRM.
diff --git a/drivers/gpu/drm/exynos/Makefile b/drivers/gpu/drm/exynos/Makefile
index 9e0bff8badf9..eb651ca8e2a8 100644
--- a/drivers/gpu/drm/exynos/Makefile
+++ b/drivers/gpu/drm/exynos/Makefile
@@ -8,10 +8,12 @@ exynosdrm-y := exynos_drm_drv.o exynos_drm_encoder.o exynos_drm_connector.o \
 		exynos_drm_buf.o exynos_drm_gem.o exynos_drm_core.o \
 		exynos_drm_plane.o
 
+exynosdrm-$(CONFIG_DRM_EXYNOS_DMABUF) += exynos_drm_dmabuf.o
 exynosdrm-$(CONFIG_DRM_EXYNOS_FIMD)	+= exynos_drm_fimd.o
 exynosdrm-$(CONFIG_DRM_EXYNOS_HDMI)	+= exynos_hdmi.o exynos_mixer.o \
 					   exynos_ddc.o exynos_hdmiphy.o \
 					   exynos_drm_hdmi.o
 exynosdrm-$(CONFIG_DRM_EXYNOS_VIDI)	+= exynos_drm_vidi.o
+exynosdrm-$(CONFIG_DRM_EXYNOS_G2D)	+= exynos_drm_g2d.o
 
 obj-$(CONFIG_DRM_EXYNOS)		+= exynosdrm.o
diff --git a/drivers/gpu/drm/exynos/exynos_drm_buf.c b/drivers/gpu/drm/exynos/exynos_drm_buf.c
index de8d2090bce3..b3cb0a69fbf2 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_buf.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_buf.c
@@ -35,7 +35,7 @@ static int lowlevel_buffer_allocate(struct drm_device *dev,
 		unsigned int flags, struct exynos_drm_gem_buf *buf)
 {
 	dma_addr_t start_addr;
-	unsigned int npages, page_size, i = 0;
+	unsigned int npages, i = 0;
 	struct scatterlist *sgl;
 	int ret = 0;
 
@@ -53,13 +53,13 @@ static int lowlevel_buffer_allocate(struct drm_device *dev,
 
 	if (buf->size >= SZ_1M) {
 		npages = buf->size >> SECTION_SHIFT;
-		page_size = SECTION_SIZE;
+		buf->page_size = SECTION_SIZE;
 	} else if (buf->size >= SZ_64K) {
 		npages = buf->size >> 16;
-		page_size = SZ_64K;
+		buf->page_size = SZ_64K;
 	} else {
 		npages = buf->size >> PAGE_SHIFT;
-		page_size = PAGE_SIZE;
+		buf->page_size = PAGE_SIZE;
 	}
 
 	buf->sgt = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
@@ -96,9 +96,9 @@ static int lowlevel_buffer_allocate(struct drm_device *dev,
 
 	while (i < npages) {
 		buf->pages[i] = phys_to_page(start_addr);
-		sg_set_page(sgl, buf->pages[i], page_size, 0);
+		sg_set_page(sgl, buf->pages[i], buf->page_size, 0);
 		sg_dma_address(sgl) = start_addr;
-		start_addr += page_size;
+		start_addr += buf->page_size;
 		sgl = sg_next(sgl);
 		i++;
 	}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index 3486ffed0bf0..4afb625128d7 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -105,6 +105,8 @@ int exynos_drm_overlay_update(struct exynos_drm_overlay *overlay,
 	overlay->fb_y = pos->fb_y;
 	overlay->fb_width = fb->width;
 	overlay->fb_height = fb->height;
+	overlay->src_width = pos->src_w;
+	overlay->src_height = pos->src_h;
 	overlay->bpp = fb->bits_per_pixel;
 	overlay->pitch = fb->pitches[0];
 	overlay->pixel_format = fb->pixel_format;
@@ -153,6 +155,8 @@ static int exynos_drm_crtc_update(struct drm_crtc *crtc)
 	pos.crtc_y = 0;
 	pos.crtc_w = fb->width - crtc->x;
 	pos.crtc_h = fb->height - crtc->y;
+	pos.src_w = pos.crtc_w;
+	pos.src_h = pos.crtc_h;
 
 	return exynos_drm_overlay_update(overlay, crtc->fb, mode, &pos);
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.h b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
index 25f72a62cb88..16b8e2195a0d 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
@@ -42,6 +42,8 @@ void exynos_drm_crtc_disable_vblank(struct drm_device *dev, int crtc);
  *	- the unit is screen coordinates.
  * @fb_y: offset y on a framebuffer to be displayed
  *	- the unit is screen coordinates.
+ * @src_w: width of source area to be displayed from a framebuffer.
+ * @src_h: height of source area to be displayed from a framebuffer.
  * @crtc_x: offset x on hardware screen.
  * @crtc_y: offset y on hardware screen.
  * @crtc_w: width of hardware screen.
@@ -50,6 +52,8 @@ void exynos_drm_crtc_disable_vblank(struct drm_device *dev, int crtc);
 struct exynos_drm_crtc_pos {
 	unsigned int fb_x;
 	unsigned int fb_y;
+	unsigned int src_w;
+	unsigned int src_h;
 	unsigned int crtc_x;
 	unsigned int crtc_y;
 	unsigned int crtc_w;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
new file mode 100644
index 000000000000..274909271c36
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
@@ -0,0 +1,272 @@
+/* exynos_drm_dmabuf.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ * Author: Inki Dae <inki.dae@samsung.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * VA LINUX SYSTEMS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "exynos_drm_drv.h"
+#include "exynos_drm_gem.h"
+
+#include <linux/dma-buf.h>
+
+static struct sg_table *exynos_pages_to_sg(struct page **pages, int nr_pages,
+		unsigned int page_size)
+{
+	struct sg_table *sgt = NULL;
+	struct scatterlist *sgl;
+	int i, ret;
+
+	sgt = kzalloc(sizeof(*sgt), GFP_KERNEL);
+	if (!sgt)
+		goto out;
+
+	ret = sg_alloc_table(sgt, nr_pages, GFP_KERNEL);
+	if (ret)
+		goto err_free_sgt;
+
+	if (page_size < PAGE_SIZE)
+		page_size = PAGE_SIZE;
+
+	for_each_sg(sgt->sgl, sgl, nr_pages, i)
+		sg_set_page(sgl, pages[i], page_size, 0);
+
+	return sgt;
+
+err_free_sgt:
+	kfree(sgt);
+	sgt = NULL;
+out:
+	return NULL;
+}
+
+static struct sg_table *
+		exynos_gem_map_dma_buf(struct dma_buf_attachment *attach,
+					enum dma_data_direction dir)
+{
+	struct exynos_drm_gem_obj *gem_obj = attach->dmabuf->priv;
+	struct drm_device *dev = gem_obj->base.dev;
+	struct exynos_drm_gem_buf *buf;
+	struct sg_table *sgt = NULL;
+	unsigned int npages;
+	int nents;
+
+	DRM_DEBUG_PRIME("%s\n", __FILE__);
+
+	mutex_lock(&dev->struct_mutex);
+
+	buf = gem_obj->buffer;
+
+	/* there should always be pages allocated. */
+	if (!buf->pages) {
+		DRM_ERROR("pages is null.\n");
+		goto err_unlock;
+	}
+
+	npages = buf->size / buf->page_size;
+
+	sgt = exynos_pages_to_sg(buf->pages, npages, buf->page_size);
+	nents = dma_map_sg(attach->dev, sgt->sgl, sgt->nents, dir);
+
+	DRM_DEBUG_PRIME("npages = %d buffer size = 0x%lx page_size = 0x%lx\n",
+			npages, buf->size, buf->page_size);
+
+err_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return sgt;
+}
+
+static void exynos_gem_unmap_dma_buf(struct dma_buf_attachment *attach,
+						struct sg_table *sgt,
+						enum dma_data_direction dir)
+{
+	dma_unmap_sg(attach->dev, sgt->sgl, sgt->nents, dir);
+	sg_free_table(sgt);
+	kfree(sgt);
+	sgt = NULL;
+}
+
+static void exynos_dmabuf_release(struct dma_buf *dmabuf)
+{
+	struct exynos_drm_gem_obj *exynos_gem_obj = dmabuf->priv;
+
+	DRM_DEBUG_PRIME("%s\n", __FILE__);
+
+	/*
+	 * exynos_dmabuf_release() call means that file object's
+	 * f_count is 0 and it calls drm_gem_object_handle_unreference()
+	 * to drop the references that these values had been increased
+	 * at drm_prime_handle_to_fd()
+	 */
+	if (exynos_gem_obj->base.export_dma_buf == dmabuf) {
+		exynos_gem_obj->base.export_dma_buf = NULL;
+
+		/*
+		 * drop this gem object refcount to release allocated buffer
+		 * and resources.
+		 */
+		drm_gem_object_unreference_unlocked(&exynos_gem_obj->base);
+	}
+}
+
+static void *exynos_gem_dmabuf_kmap_atomic(struct dma_buf *dma_buf,
+						unsigned long page_num)
+{
+	/* TODO */
+
+	return NULL;
+}
+
+static void exynos_gem_dmabuf_kunmap_atomic(struct dma_buf *dma_buf,
+						unsigned long page_num,
+						void *addr)
+{
+	/* TODO */
+}
+
+static void *exynos_gem_dmabuf_kmap(struct dma_buf *dma_buf,
+					unsigned long page_num)
+{
+	/* TODO */
+
+	return NULL;
+}
+
+static void exynos_gem_dmabuf_kunmap(struct dma_buf *dma_buf,
+					unsigned long page_num, void *addr)
+{
+	/* TODO */
+}
+
+static struct dma_buf_ops exynos_dmabuf_ops = {
+	.map_dma_buf		= exynos_gem_map_dma_buf,
+	.unmap_dma_buf		= exynos_gem_unmap_dma_buf,
+	.kmap			= exynos_gem_dmabuf_kmap,
+	.kmap_atomic		= exynos_gem_dmabuf_kmap_atomic,
+	.kunmap			= exynos_gem_dmabuf_kunmap,
+	.kunmap_atomic		= exynos_gem_dmabuf_kunmap_atomic,
+	.release		= exynos_dmabuf_release,
+};
+
+struct dma_buf *exynos_dmabuf_prime_export(struct drm_device *drm_dev,
+				struct drm_gem_object *obj, int flags)
+{
+	struct exynos_drm_gem_obj *exynos_gem_obj = to_exynos_gem_obj(obj);
+
+	return dma_buf_export(exynos_gem_obj, &exynos_dmabuf_ops,
+				exynos_gem_obj->base.size, 0600);
+}
+
+struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sgt;
+	struct scatterlist *sgl;
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct exynos_drm_gem_buf *buffer;
+	struct page *page;
+	int ret, i = 0;
+
+	DRM_DEBUG_PRIME("%s\n", __FILE__);
+
+	/* is this one of own objects? */
+	if (dma_buf->ops == &exynos_dmabuf_ops) {
+		struct drm_gem_object *obj;
+
+		exynos_gem_obj = dma_buf->priv;
+		obj = &exynos_gem_obj->base;
+
+		/* is it from our device? */
+		if (obj->dev == drm_dev) {
+			drm_gem_object_reference(obj);
+			return obj;
+		}
+	}
+
+	attach = dma_buf_attach(dma_buf, drm_dev->dev);
+	if (IS_ERR(attach))
+		return ERR_PTR(-EINVAL);
+
+
+	sgt = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sgt)) {
+		ret = PTR_ERR(sgt);
+		goto err_buf_detach;
+	}
+
+	buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
+	if (!buffer) {
+		DRM_ERROR("failed to allocate exynos_drm_gem_buf.\n");
+		ret = -ENOMEM;
+		goto err_unmap_attach;
+	}
+
+	buffer->pages = kzalloc(sizeof(*page) * sgt->nents, GFP_KERNEL);
+	if (!buffer->pages) {
+		DRM_ERROR("failed to allocate pages.\n");
+		ret = -ENOMEM;
+		goto err_free_buffer;
+	}
+
+	exynos_gem_obj = exynos_drm_gem_init(drm_dev, dma_buf->size);
+	if (!exynos_gem_obj) {
+		ret = -ENOMEM;
+		goto err_free_pages;
+	}
+
+	sgl = sgt->sgl;
+	buffer->dma_addr = sg_dma_address(sgl);
+
+	while (i < sgt->nents) {
+		buffer->pages[i] = sg_page(sgl);
+		buffer->size += sg_dma_len(sgl);
+		sgl = sg_next(sgl);
+		i++;
+	}
+
+	exynos_gem_obj->buffer = buffer;
+	buffer->sgt = sgt;
+	exynos_gem_obj->base.import_attach = attach;
+
+	DRM_DEBUG_PRIME("dma_addr = 0x%x, size = 0x%lx\n", buffer->dma_addr,
+								buffer->size);
+
+	return &exynos_gem_obj->base;
+
+err_free_pages:
+	kfree(buffer->pages);
+	buffer->pages = NULL;
+err_free_buffer:
+	kfree(buffer);
+	buffer = NULL;
+err_unmap_attach:
+	dma_buf_unmap_attachment(attach, sgt, DMA_BIDIRECTIONAL);
+err_buf_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
+
+MODULE_AUTHOR("Inki Dae <inki.dae@samsung.com>");
+MODULE_DESCRIPTION("Samsung SoC DRM DMABUF Module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.h b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.h
new file mode 100644
index 000000000000..662a8f98ccdb
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.h
@@ -0,0 +1,39 @@
+/* exynos_drm_dmabuf.h
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ * Author: Inki Dae <inki.dae@samsung.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * VA LINUX SYSTEMS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _EXYNOS_DRM_DMABUF_H_
+#define _EXYNOS_DRM_DMABUF_H_
+
+#ifdef CONFIG_DRM_EXYNOS_DMABUF
+struct dma_buf *exynos_dmabuf_prime_export(struct drm_device *drm_dev,
+				struct drm_gem_object *obj, int flags);
+
+struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
+						struct dma_buf *dma_buf);
+#else
+#define exynos_dmabuf_prime_export		NULL
+#define exynos_dmabuf_prime_import		NULL
+#endif
+#endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index a6819b5f8428..420953197d0a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -39,6 +39,8 @@
 #include "exynos_drm_gem.h"
 #include "exynos_drm_plane.h"
 #include "exynos_drm_vidi.h"
+#include "exynos_drm_dmabuf.h"
+#include "exynos_drm_g2d.h"
 
 #define DRIVER_NAME	"exynos"
 #define DRIVER_DESC	"Samsung SoC DRM"
@@ -147,8 +149,17 @@ static int exynos_drm_unload(struct drm_device *dev)
 
 static int exynos_drm_open(struct drm_device *dev, struct drm_file *file)
 {
+	struct drm_exynos_file_private *file_priv;
+
 	DRM_DEBUG_DRIVER("%s\n", __FILE__);
 
+	file_priv = kzalloc(sizeof(*file_priv), GFP_KERNEL);
+	if (!file_priv)
+		return -ENOMEM;
+
+	drm_prime_init_file_private(&file->prime);
+	file->driver_priv = file_priv;
+
 	return exynos_drm_subdrv_open(dev, file);
 }
 
@@ -170,6 +181,7 @@ static void exynos_drm_preclose(struct drm_device *dev,
 			e->base.destroy(&e->base);
 		}
 	}
+	drm_prime_destroy_file_private(&file->prime);
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
 	exynos_drm_subdrv_close(dev, file);
@@ -193,7 +205,7 @@ static void exynos_drm_lastclose(struct drm_device *dev)
 	exynos_drm_fbdev_restore_mode(dev);
 }
 
-static struct vm_operations_struct exynos_drm_gem_vm_ops = {
+static const struct vm_operations_struct exynos_drm_gem_vm_ops = {
 	.fault = exynos_drm_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
@@ -207,10 +219,18 @@ static struct drm_ioctl_desc exynos_ioctls[] = {
 			DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_MMAP,
 			exynos_drm_gem_mmap_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_GET,
+			exynos_drm_gem_get_ioctl, DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(EXYNOS_PLANE_SET_ZPOS, exynos_plane_set_zpos_ioctl,
 			DRM_UNLOCKED | DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_VIDI_CONNECTION,
 			vidi_connection_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_G2D_GET_VER,
+			exynos_g2d_get_ver_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_G2D_SET_CMDLIST,
+			exynos_g2d_set_cmdlist_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_G2D_EXEC,
+			exynos_g2d_exec_ioctl, DRM_UNLOCKED | DRM_AUTH),
 };
 
 static const struct file_operations exynos_drm_driver_fops = {
@@ -225,7 +245,7 @@ static const struct file_operations exynos_drm_driver_fops = {
 
 static struct drm_driver exynos_drm_driver = {
 	.driver_features	= DRIVER_HAVE_IRQ | DRIVER_BUS_PLATFORM |
-				  DRIVER_MODESET | DRIVER_GEM,
+				  DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
 	.load			= exynos_drm_load,
 	.unload			= exynos_drm_unload,
 	.open			= exynos_drm_open,
@@ -241,6 +261,10 @@ static struct drm_driver exynos_drm_driver = {
 	.dumb_create		= exynos_drm_gem_dumb_create,
 	.dumb_map_offset	= exynos_drm_gem_dumb_map_offset,
 	.dumb_destroy		= exynos_drm_gem_dumb_destroy,
+	.prime_handle_to_fd	= drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle	= drm_gem_prime_fd_to_handle,
+	.gem_prime_export	= exynos_dmabuf_prime_export,
+	.gem_prime_import	= exynos_dmabuf_prime_import,
 	.ioctls			= exynos_ioctls,
 	.fops			= &exynos_drm_driver_fops,
 	.name	= DRIVER_NAME,
@@ -307,6 +331,12 @@ static int __init exynos_drm_init(void)
 		goto out_vidi;
 #endif
 
+#ifdef CONFIG_DRM_EXYNOS_G2D
+	ret = platform_driver_register(&g2d_driver);
+	if (ret < 0)
+		goto out_g2d;
+#endif
+
 	ret = platform_driver_register(&exynos_drm_platform_driver);
 	if (ret < 0)
 		goto out;
@@ -314,6 +344,11 @@ static int __init exynos_drm_init(void)
 	return 0;
 
 out:
+#ifdef CONFIG_DRM_EXYNOS_G2D
+	platform_driver_unregister(&g2d_driver);
+out_g2d:
+#endif
+
 #ifdef CONFIG_DRM_EXYNOS_VIDI
 out_vidi:
 	platform_driver_unregister(&vidi_driver);
@@ -341,6 +376,10 @@ static void __exit exynos_drm_exit(void)
 
 	platform_driver_unregister(&exynos_drm_platform_driver);
 
+#ifdef CONFIG_DRM_EXYNOS_G2D
+	platform_driver_unregister(&g2d_driver);
+#endif
+
 #ifdef CONFIG_DRM_EXYNOS_HDMI
 	platform_driver_unregister(&exynos_drm_common_hdmi_driver);
 	platform_driver_unregister(&mixer_driver);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index 1d814175cd49..c82c90c443e7 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -77,6 +77,8 @@ struct exynos_drm_overlay_ops {
  *	- the unit is screen coordinates.
  * @fb_width: width of a framebuffer.
  * @fb_height: height of a framebuffer.
+ * @src_width: width of a partial image to be displayed from framebuffer.
+ * @src_height: height of a partial image to be displayed from framebuffer.
  * @crtc_x: offset x on hardware screen.
  * @crtc_y: offset y on hardware screen.
  * @crtc_width: window width to be displayed (hardware screen).
@@ -108,6 +110,8 @@ struct exynos_drm_overlay {
 	unsigned int fb_y;
 	unsigned int fb_width;
 	unsigned int fb_height;
+	unsigned int src_width;
+	unsigned int src_height;
 	unsigned int crtc_x;
 	unsigned int crtc_y;
 	unsigned int crtc_width;
@@ -205,6 +209,18 @@ struct exynos_drm_manager {
 	struct exynos_drm_display_ops *display_ops;
 };
 
+struct exynos_drm_g2d_private {
+	struct device		*dev;
+	struct list_head	inuse_cmdlist;
+	struct list_head	event_list;
+	struct list_head	gem_list;
+	unsigned int		gem_nr;
+};
+
+struct drm_exynos_file_private {
+	struct exynos_drm_g2d_private	*g2d_priv;
+};
+
 /*
  * Exynos drm private structure.
  */
@@ -287,4 +303,5 @@ extern struct platform_driver hdmi_driver;
 extern struct platform_driver mixer_driver;
 extern struct platform_driver exynos_drm_common_hdmi_driver;
 extern struct platform_driver vidi_driver;
+extern struct platform_driver g2d_driver;
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.c b/drivers/gpu/drm/exynos/exynos_drm_fb.c
index c38c8f468fa3..f82a299553fb 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.c
@@ -191,7 +191,7 @@ static void exynos_drm_output_poll_changed(struct drm_device *dev)
 		drm_fb_helper_hotplug_event(fb_helper);
 }
 
-static struct drm_mode_config_funcs exynos_drm_mode_config_funcs = {
+static const struct drm_mode_config_funcs exynos_drm_mode_config_funcs = {
 	.fb_create = exynos_user_fb_create,
 	.output_poll_changed = exynos_drm_output_poll_changed,
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.c b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
new file mode 100644
index 000000000000..d2d88f22a037
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
@@ -0,0 +1,937 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co.Ltd
+ * Authors: Joonyoung Shim <jy0922.shim@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 Foundationr
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#include "drmP.h"
+#include "exynos_drm.h"
+#include "exynos_drm_drv.h"
+#include "exynos_drm_gem.h"
+
+#define G2D_HW_MAJOR_VER		4
+#define G2D_HW_MINOR_VER		1
+
+/* vaild register range set from user: 0x0104 ~ 0x0880 */
+#define G2D_VALID_START			0x0104
+#define G2D_VALID_END			0x0880
+
+/* general registers */
+#define G2D_SOFT_RESET			0x0000
+#define G2D_INTEN			0x0004
+#define G2D_INTC_PEND			0x000C
+#define G2D_DMA_SFR_BASE_ADDR		0x0080
+#define G2D_DMA_COMMAND			0x0084
+#define G2D_DMA_STATUS			0x008C
+#define G2D_DMA_HOLD_CMD		0x0090
+
+/* command registers */
+#define G2D_BITBLT_START		0x0100
+
+/* registers for base address */
+#define G2D_SRC_BASE_ADDR		0x0304
+#define G2D_SRC_PLANE2_BASE_ADDR	0x0318
+#define G2D_DST_BASE_ADDR		0x0404
+#define G2D_DST_PLANE2_BASE_ADDR	0x0418
+#define G2D_PAT_BASE_ADDR		0x0500
+#define G2D_MSK_BASE_ADDR		0x0520
+
+/* G2D_SOFT_RESET */
+#define G2D_SFRCLEAR			(1 << 1)
+#define G2D_R				(1 << 0)
+
+/* G2D_INTEN */
+#define G2D_INTEN_ACF			(1 << 3)
+#define G2D_INTEN_UCF			(1 << 2)
+#define G2D_INTEN_GCF			(1 << 1)
+#define G2D_INTEN_SCF			(1 << 0)
+
+/* G2D_INTC_PEND */
+#define G2D_INTP_ACMD_FIN		(1 << 3)
+#define G2D_INTP_UCMD_FIN		(1 << 2)
+#define G2D_INTP_GCMD_FIN		(1 << 1)
+#define G2D_INTP_SCMD_FIN		(1 << 0)
+
+/* G2D_DMA_COMMAND */
+#define G2D_DMA_HALT			(1 << 2)
+#define G2D_DMA_CONTINUE		(1 << 1)
+#define G2D_DMA_START			(1 << 0)
+
+/* G2D_DMA_STATUS */
+#define G2D_DMA_LIST_DONE_COUNT		(0xFF << 17)
+#define G2D_DMA_BITBLT_DONE_COUNT	(0xFFFF << 1)
+#define G2D_DMA_DONE			(1 << 0)
+#define G2D_DMA_LIST_DONE_COUNT_OFFSET	17
+
+/* G2D_DMA_HOLD_CMD */
+#define G2D_USET_HOLD			(1 << 2)
+#define G2D_LIST_HOLD			(1 << 1)
+#define G2D_BITBLT_HOLD			(1 << 0)
+
+/* G2D_BITBLT_START */
+#define G2D_START_CASESEL		(1 << 2)
+#define G2D_START_NHOLT			(1 << 1)
+#define G2D_START_BITBLT		(1 << 0)
+
+#define G2D_CMDLIST_SIZE		(PAGE_SIZE / 4)
+#define G2D_CMDLIST_NUM			64
+#define G2D_CMDLIST_POOL_SIZE		(G2D_CMDLIST_SIZE * G2D_CMDLIST_NUM)
+#define G2D_CMDLIST_DATA_NUM		(G2D_CMDLIST_SIZE / sizeof(u32) - 2)
+
+/* cmdlist data structure */
+struct g2d_cmdlist {
+	u32	head;
+	u32	data[G2D_CMDLIST_DATA_NUM];
+	u32	last;	/* last data offset */
+};
+
+struct drm_exynos_pending_g2d_event {
+	struct drm_pending_event	base;
+	struct drm_exynos_g2d_event	event;
+};
+
+struct g2d_gem_node {
+	struct list_head	list;
+	unsigned int		handle;
+};
+
+struct g2d_cmdlist_node {
+	struct list_head	list;
+	struct g2d_cmdlist	*cmdlist;
+	unsigned int		gem_nr;
+	dma_addr_t		dma_addr;
+
+	struct drm_exynos_pending_g2d_event	*event;
+};
+
+struct g2d_runqueue_node {
+	struct list_head	list;
+	struct list_head	run_cmdlist;
+	struct list_head	event_list;
+	struct completion	complete;
+	int			async;
+};
+
+struct g2d_data {
+	struct device			*dev;
+	struct clk			*gate_clk;
+	struct resource			*regs_res;
+	void __iomem			*regs;
+	int				irq;
+	struct workqueue_struct		*g2d_workq;
+	struct work_struct		runqueue_work;
+	struct exynos_drm_subdrv	subdrv;
+	bool				suspended;
+
+	/* cmdlist */
+	struct g2d_cmdlist_node		*cmdlist_node;
+	struct list_head		free_cmdlist;
+	struct mutex			cmdlist_mutex;
+	dma_addr_t			cmdlist_pool;
+	void				*cmdlist_pool_virt;
+
+	/* runqueue*/
+	struct g2d_runqueue_node	*runqueue_node;
+	struct list_head		runqueue;
+	struct mutex			runqueue_mutex;
+	struct kmem_cache		*runqueue_slab;
+};
+
+static int g2d_init_cmdlist(struct g2d_data *g2d)
+{
+	struct device *dev = g2d->dev;
+	struct g2d_cmdlist_node *node = g2d->cmdlist_node;
+	int nr;
+	int ret;
+
+	g2d->cmdlist_pool_virt = dma_alloc_coherent(dev, G2D_CMDLIST_POOL_SIZE,
+						&g2d->cmdlist_pool, GFP_KERNEL);
+	if (!g2d->cmdlist_pool_virt) {
+		dev_err(dev, "failed to allocate dma memory\n");
+		return -ENOMEM;
+	}
+
+	node = kcalloc(G2D_CMDLIST_NUM, G2D_CMDLIST_NUM * sizeof(*node),
+			GFP_KERNEL);
+	if (!node) {
+		dev_err(dev, "failed to allocate memory\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	for (nr = 0; nr < G2D_CMDLIST_NUM; nr++) {
+		node[nr].cmdlist =
+			g2d->cmdlist_pool_virt + nr * G2D_CMDLIST_SIZE;
+		node[nr].dma_addr =
+			g2d->cmdlist_pool + nr * G2D_CMDLIST_SIZE;
+
+		list_add_tail(&node[nr].list, &g2d->free_cmdlist);
+	}
+
+	return 0;
+
+err:
+	dma_free_coherent(dev, G2D_CMDLIST_POOL_SIZE, g2d->cmdlist_pool_virt,
+			g2d->cmdlist_pool);
+	return ret;
+}
+
+static void g2d_fini_cmdlist(struct g2d_data *g2d)
+{
+	struct device *dev = g2d->dev;
+
+	kfree(g2d->cmdlist_node);
+	dma_free_coherent(dev, G2D_CMDLIST_POOL_SIZE, g2d->cmdlist_pool_virt,
+			g2d->cmdlist_pool);
+}
+
+static struct g2d_cmdlist_node *g2d_get_cmdlist(struct g2d_data *g2d)
+{
+	struct device *dev = g2d->dev;
+	struct g2d_cmdlist_node *node;
+
+	mutex_lock(&g2d->cmdlist_mutex);
+	if (list_empty(&g2d->free_cmdlist)) {
+		dev_err(dev, "there is no free cmdlist\n");
+		mutex_unlock(&g2d->cmdlist_mutex);
+		return NULL;
+	}
+
+	node = list_first_entry(&g2d->free_cmdlist, struct g2d_cmdlist_node,
+				list);
+	list_del_init(&node->list);
+	mutex_unlock(&g2d->cmdlist_mutex);
+
+	return node;
+}
+
+static void g2d_put_cmdlist(struct g2d_data *g2d, struct g2d_cmdlist_node *node)
+{
+	mutex_lock(&g2d->cmdlist_mutex);
+	list_move_tail(&node->list, &g2d->free_cmdlist);
+	mutex_unlock(&g2d->cmdlist_mutex);
+}
+
+static void g2d_add_cmdlist_to_inuse(struct exynos_drm_g2d_private *g2d_priv,
+				     struct g2d_cmdlist_node *node)
+{
+	struct g2d_cmdlist_node *lnode;
+
+	if (list_empty(&g2d_priv->inuse_cmdlist))
+		goto add_to_list;
+
+	/* this links to base address of new cmdlist */
+	lnode = list_entry(g2d_priv->inuse_cmdlist.prev,
+				struct g2d_cmdlist_node, list);
+	lnode->cmdlist->data[lnode->cmdlist->last] = node->dma_addr;
+
+add_to_list:
+	list_add_tail(&node->list, &g2d_priv->inuse_cmdlist);
+
+	if (node->event)
+		list_add_tail(&node->event->base.link, &g2d_priv->event_list);
+}
+
+static int g2d_get_cmdlist_gem(struct drm_device *drm_dev,
+			       struct drm_file *file,
+			       struct g2d_cmdlist_node *node)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct g2d_cmdlist *cmdlist = node->cmdlist;
+	dma_addr_t *addr;
+	int offset;
+	int i;
+
+	for (i = 0; i < node->gem_nr; i++) {
+		struct g2d_gem_node *gem_node;
+
+		gem_node = kzalloc(sizeof(*gem_node), GFP_KERNEL);
+		if (!gem_node) {
+			dev_err(g2d_priv->dev, "failed to allocate gem node\n");
+			return -ENOMEM;
+		}
+
+		offset = cmdlist->last - (i * 2 + 1);
+		gem_node->handle = cmdlist->data[offset];
+
+		addr = exynos_drm_gem_get_dma_addr(drm_dev, gem_node->handle,
+						   file);
+		if (IS_ERR(addr)) {
+			node->gem_nr = i;
+			kfree(gem_node);
+			return PTR_ERR(addr);
+		}
+
+		cmdlist->data[offset] = *addr;
+		list_add_tail(&gem_node->list, &g2d_priv->gem_list);
+		g2d_priv->gem_nr++;
+	}
+
+	return 0;
+}
+
+static void g2d_put_cmdlist_gem(struct drm_device *drm_dev,
+				struct drm_file *file,
+				unsigned int nr)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct g2d_gem_node *node, *n;
+
+	list_for_each_entry_safe_reverse(node, n, &g2d_priv->gem_list, list) {
+		if (!nr)
+			break;
+
+		exynos_drm_gem_put_dma_addr(drm_dev, node->handle, file);
+		list_del_init(&node->list);
+		kfree(node);
+		nr--;
+	}
+}
+
+static void g2d_dma_start(struct g2d_data *g2d,
+			  struct g2d_runqueue_node *runqueue_node)
+{
+	struct g2d_cmdlist_node *node =
+				list_first_entry(&runqueue_node->run_cmdlist,
+						struct g2d_cmdlist_node, list);
+
+	pm_runtime_get_sync(g2d->dev);
+	clk_enable(g2d->gate_clk);
+
+	/* interrupt enable */
+	writel_relaxed(G2D_INTEN_ACF | G2D_INTEN_UCF | G2D_INTEN_GCF,
+			g2d->regs + G2D_INTEN);
+
+	writel_relaxed(node->dma_addr, g2d->regs + G2D_DMA_SFR_BASE_ADDR);
+	writel_relaxed(G2D_DMA_START, g2d->regs + G2D_DMA_COMMAND);
+}
+
+static struct g2d_runqueue_node *g2d_get_runqueue_node(struct g2d_data *g2d)
+{
+	struct g2d_runqueue_node *runqueue_node;
+
+	if (list_empty(&g2d->runqueue))
+		return NULL;
+
+	runqueue_node = list_first_entry(&g2d->runqueue,
+					 struct g2d_runqueue_node, list);
+	list_del_init(&runqueue_node->list);
+	return runqueue_node;
+}
+
+static void g2d_free_runqueue_node(struct g2d_data *g2d,
+				   struct g2d_runqueue_node *runqueue_node)
+{
+	if (!runqueue_node)
+		return;
+
+	mutex_lock(&g2d->cmdlist_mutex);
+	list_splice_tail_init(&runqueue_node->run_cmdlist, &g2d->free_cmdlist);
+	mutex_unlock(&g2d->cmdlist_mutex);
+
+	kmem_cache_free(g2d->runqueue_slab, runqueue_node);
+}
+
+static void g2d_exec_runqueue(struct g2d_data *g2d)
+{
+	g2d->runqueue_node = g2d_get_runqueue_node(g2d);
+	if (g2d->runqueue_node)
+		g2d_dma_start(g2d, g2d->runqueue_node);
+}
+
+static void g2d_runqueue_worker(struct work_struct *work)
+{
+	struct g2d_data *g2d = container_of(work, struct g2d_data,
+					    runqueue_work);
+
+
+	mutex_lock(&g2d->runqueue_mutex);
+	clk_disable(g2d->gate_clk);
+	pm_runtime_put_sync(g2d->dev);
+
+	complete(&g2d->runqueue_node->complete);
+	if (g2d->runqueue_node->async)
+		g2d_free_runqueue_node(g2d, g2d->runqueue_node);
+
+	if (g2d->suspended)
+		g2d->runqueue_node = NULL;
+	else
+		g2d_exec_runqueue(g2d);
+	mutex_unlock(&g2d->runqueue_mutex);
+}
+
+static void g2d_finish_event(struct g2d_data *g2d, u32 cmdlist_no)
+{
+	struct drm_device *drm_dev = g2d->subdrv.drm_dev;
+	struct g2d_runqueue_node *runqueue_node = g2d->runqueue_node;
+	struct drm_exynos_pending_g2d_event *e;
+	struct timeval now;
+	unsigned long flags;
+
+	if (list_empty(&runqueue_node->event_list))
+		return;
+
+	e = list_first_entry(&runqueue_node->event_list,
+			     struct drm_exynos_pending_g2d_event, base.link);
+
+	do_gettimeofday(&now);
+	e->event.tv_sec = now.tv_sec;
+	e->event.tv_usec = now.tv_usec;
+	e->event.cmdlist_no = cmdlist_no;
+
+	spin_lock_irqsave(&drm_dev->event_lock, flags);
+	list_move_tail(&e->base.link, &e->base.file_priv->event_list);
+	wake_up_interruptible(&e->base.file_priv->event_wait);
+	spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+}
+
+static irqreturn_t g2d_irq_handler(int irq, void *dev_id)
+{
+	struct g2d_data *g2d = dev_id;
+	u32 pending;
+
+	pending = readl_relaxed(g2d->regs + G2D_INTC_PEND);
+	if (pending)
+		writel_relaxed(pending, g2d->regs + G2D_INTC_PEND);
+
+	if (pending & G2D_INTP_GCMD_FIN) {
+		u32 cmdlist_no = readl_relaxed(g2d->regs + G2D_DMA_STATUS);
+
+		cmdlist_no = (cmdlist_no & G2D_DMA_LIST_DONE_COUNT) >>
+						G2D_DMA_LIST_DONE_COUNT_OFFSET;
+
+		g2d_finish_event(g2d, cmdlist_no);
+
+		writel_relaxed(0, g2d->regs + G2D_DMA_HOLD_CMD);
+		if (!(pending & G2D_INTP_ACMD_FIN)) {
+			writel_relaxed(G2D_DMA_CONTINUE,
+					g2d->regs + G2D_DMA_COMMAND);
+		}
+	}
+
+	if (pending & G2D_INTP_ACMD_FIN)
+		queue_work(g2d->g2d_workq, &g2d->runqueue_work);
+
+	return IRQ_HANDLED;
+}
+
+static int g2d_check_reg_offset(struct device *dev, struct g2d_cmdlist *cmdlist,
+				int nr, bool for_addr)
+{
+	int reg_offset;
+	int index;
+	int i;
+
+	for (i = 0; i < nr; i++) {
+		index = cmdlist->last - 2 * (i + 1);
+		reg_offset = cmdlist->data[index] & ~0xfffff000;
+
+		if (reg_offset < G2D_VALID_START || reg_offset > G2D_VALID_END)
+			goto err;
+		if (reg_offset % 4)
+			goto err;
+
+		switch (reg_offset) {
+		case G2D_SRC_BASE_ADDR:
+		case G2D_SRC_PLANE2_BASE_ADDR:
+		case G2D_DST_BASE_ADDR:
+		case G2D_DST_PLANE2_BASE_ADDR:
+		case G2D_PAT_BASE_ADDR:
+		case G2D_MSK_BASE_ADDR:
+			if (!for_addr)
+				goto err;
+			break;
+		default:
+			if (for_addr)
+				goto err;
+			break;
+		}
+	}
+
+	return 0;
+
+err:
+	dev_err(dev, "Bad register offset: 0x%x\n", cmdlist->data[index]);
+	return -EINVAL;
+}
+
+/* ioctl functions */
+int exynos_g2d_get_ver_ioctl(struct drm_device *drm_dev, void *data,
+			     struct drm_file *file)
+{
+	struct drm_exynos_g2d_get_ver *ver = data;
+
+	ver->major = G2D_HW_MAJOR_VER;
+	ver->minor = G2D_HW_MINOR_VER;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(exynos_g2d_get_ver_ioctl);
+
+int exynos_g2d_set_cmdlist_ioctl(struct drm_device *drm_dev, void *data,
+				 struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct device *dev = g2d_priv->dev;
+	struct g2d_data *g2d;
+	struct drm_exynos_g2d_set_cmdlist *req = data;
+	struct drm_exynos_g2d_cmd *cmd;
+	struct drm_exynos_pending_g2d_event *e;
+	struct g2d_cmdlist_node *node;
+	struct g2d_cmdlist *cmdlist;
+	unsigned long flags;
+	int size;
+	int ret;
+
+	if (!dev)
+		return -ENODEV;
+
+	g2d = dev_get_drvdata(dev);
+	if (!g2d)
+		return -EFAULT;
+
+	node = g2d_get_cmdlist(g2d);
+	if (!node)
+		return -ENOMEM;
+
+	node->event = NULL;
+
+	if (req->event_type != G2D_EVENT_NOT) {
+		spin_lock_irqsave(&drm_dev->event_lock, flags);
+		if (file->event_space < sizeof(e->event)) {
+			spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+			ret = -ENOMEM;
+			goto err;
+		}
+		file->event_space -= sizeof(e->event);
+		spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+
+		e = kzalloc(sizeof(*node->event), GFP_KERNEL);
+		if (!e) {
+			dev_err(dev, "failed to allocate event\n");
+
+			spin_lock_irqsave(&drm_dev->event_lock, flags);
+			file->event_space += sizeof(e->event);
+			spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		e->event.base.type = DRM_EXYNOS_G2D_EVENT;
+		e->event.base.length = sizeof(e->event);
+		e->event.user_data = req->user_data;
+		e->base.event = &e->event.base;
+		e->base.file_priv = file;
+		e->base.destroy = (void (*) (struct drm_pending_event *)) kfree;
+
+		node->event = e;
+	}
+
+	cmdlist = node->cmdlist;
+
+	cmdlist->last = 0;
+
+	/*
+	 * If don't clear SFR registers, the cmdlist is affected by register
+	 * values of previous cmdlist. G2D hw executes SFR clear command and
+	 * a next command at the same time then the next command is ignored and
+	 * is executed rightly from next next command, so needs a dummy command
+	 * to next command of SFR clear command.
+	 */
+	cmdlist->data[cmdlist->last++] = G2D_SOFT_RESET;
+	cmdlist->data[cmdlist->last++] = G2D_SFRCLEAR;
+	cmdlist->data[cmdlist->last++] = G2D_SRC_BASE_ADDR;
+	cmdlist->data[cmdlist->last++] = 0;
+
+	if (node->event) {
+		cmdlist->data[cmdlist->last++] = G2D_DMA_HOLD_CMD;
+		cmdlist->data[cmdlist->last++] = G2D_LIST_HOLD;
+	}
+
+	/* Check size of cmdlist: last 2 is about G2D_BITBLT_START */
+	size = cmdlist->last + req->cmd_nr * 2 + req->cmd_gem_nr * 2 + 2;
+	if (size > G2D_CMDLIST_DATA_NUM) {
+		dev_err(dev, "cmdlist size is too big\n");
+		ret = -EINVAL;
+		goto err_free_event;
+	}
+
+	cmd = (struct drm_exynos_g2d_cmd *)(uint32_t)req->cmd;
+
+	if (copy_from_user(cmdlist->data + cmdlist->last,
+				(void __user *)cmd,
+				sizeof(*cmd) * req->cmd_nr)) {
+		ret = -EFAULT;
+		goto err_free_event;
+	}
+	cmdlist->last += req->cmd_nr * 2;
+
+	ret = g2d_check_reg_offset(dev, cmdlist, req->cmd_nr, false);
+	if (ret < 0)
+		goto err_free_event;
+
+	node->gem_nr = req->cmd_gem_nr;
+	if (req->cmd_gem_nr) {
+		struct drm_exynos_g2d_cmd *cmd_gem;
+
+		cmd_gem = (struct drm_exynos_g2d_cmd *)(uint32_t)req->cmd_gem;
+
+		if (copy_from_user(cmdlist->data + cmdlist->last,
+					(void __user *)cmd_gem,
+					sizeof(*cmd_gem) * req->cmd_gem_nr)) {
+			ret = -EFAULT;
+			goto err_free_event;
+		}
+		cmdlist->last += req->cmd_gem_nr * 2;
+
+		ret = g2d_check_reg_offset(dev, cmdlist, req->cmd_gem_nr, true);
+		if (ret < 0)
+			goto err_free_event;
+
+		ret = g2d_get_cmdlist_gem(drm_dev, file, node);
+		if (ret < 0)
+			goto err_unmap;
+	}
+
+	cmdlist->data[cmdlist->last++] = G2D_BITBLT_START;
+	cmdlist->data[cmdlist->last++] = G2D_START_BITBLT;
+
+	/* head */
+	cmdlist->head = cmdlist->last / 2;
+
+	/* tail */
+	cmdlist->data[cmdlist->last] = 0;
+
+	g2d_add_cmdlist_to_inuse(g2d_priv, node);
+
+	return 0;
+
+err_unmap:
+	g2d_put_cmdlist_gem(drm_dev, file, node->gem_nr);
+err_free_event:
+	if (node->event) {
+		spin_lock_irqsave(&drm_dev->event_lock, flags);
+		file->event_space += sizeof(e->event);
+		spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+		kfree(node->event);
+	}
+err:
+	g2d_put_cmdlist(g2d, node);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(exynos_g2d_set_cmdlist_ioctl);
+
+int exynos_g2d_exec_ioctl(struct drm_device *drm_dev, void *data,
+			  struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct device *dev = g2d_priv->dev;
+	struct g2d_data *g2d;
+	struct drm_exynos_g2d_exec *req = data;
+	struct g2d_runqueue_node *runqueue_node;
+	struct list_head *run_cmdlist;
+	struct list_head *event_list;
+
+	if (!dev)
+		return -ENODEV;
+
+	g2d = dev_get_drvdata(dev);
+	if (!g2d)
+		return -EFAULT;
+
+	runqueue_node = kmem_cache_alloc(g2d->runqueue_slab, GFP_KERNEL);
+	if (!runqueue_node) {
+		dev_err(dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+	run_cmdlist = &runqueue_node->run_cmdlist;
+	event_list = &runqueue_node->event_list;
+	INIT_LIST_HEAD(run_cmdlist);
+	INIT_LIST_HEAD(event_list);
+	init_completion(&runqueue_node->complete);
+	runqueue_node->async = req->async;
+
+	list_splice_init(&g2d_priv->inuse_cmdlist, run_cmdlist);
+	list_splice_init(&g2d_priv->event_list, event_list);
+
+	if (list_empty(run_cmdlist)) {
+		dev_err(dev, "there is no inuse cmdlist\n");
+		kmem_cache_free(g2d->runqueue_slab, runqueue_node);
+		return -EPERM;
+	}
+
+	mutex_lock(&g2d->runqueue_mutex);
+	list_add_tail(&runqueue_node->list, &g2d->runqueue);
+	if (!g2d->runqueue_node)
+		g2d_exec_runqueue(g2d);
+	mutex_unlock(&g2d->runqueue_mutex);
+
+	if (runqueue_node->async)
+		goto out;
+
+	wait_for_completion(&runqueue_node->complete);
+	g2d_free_runqueue_node(g2d, runqueue_node);
+
+out:
+	return 0;
+}
+EXPORT_SYMBOL_GPL(exynos_g2d_exec_ioctl);
+
+static int g2d_open(struct drm_device *drm_dev, struct device *dev,
+			struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv;
+
+	g2d_priv = kzalloc(sizeof(*g2d_priv), GFP_KERNEL);
+	if (!g2d_priv) {
+		dev_err(dev, "failed to allocate g2d private data\n");
+		return -ENOMEM;
+	}
+
+	g2d_priv->dev = dev;
+	file_priv->g2d_priv = g2d_priv;
+
+	INIT_LIST_HEAD(&g2d_priv->inuse_cmdlist);
+	INIT_LIST_HEAD(&g2d_priv->event_list);
+	INIT_LIST_HEAD(&g2d_priv->gem_list);
+
+	return 0;
+}
+
+static void g2d_close(struct drm_device *drm_dev, struct device *dev,
+			struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct g2d_data *g2d;
+	struct g2d_cmdlist_node *node, *n;
+
+	if (!dev)
+		return;
+
+	g2d = dev_get_drvdata(dev);
+	if (!g2d)
+		return;
+
+	mutex_lock(&g2d->cmdlist_mutex);
+	list_for_each_entry_safe(node, n, &g2d_priv->inuse_cmdlist, list)
+		list_move_tail(&node->list, &g2d->free_cmdlist);
+	mutex_unlock(&g2d->cmdlist_mutex);
+
+	g2d_put_cmdlist_gem(drm_dev, file, g2d_priv->gem_nr);
+
+	kfree(file_priv->g2d_priv);
+}
+
+static int __devinit g2d_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct g2d_data *g2d;
+	struct exynos_drm_subdrv *subdrv;
+	int ret;
+
+	g2d = kzalloc(sizeof(*g2d), GFP_KERNEL);
+	if (!g2d) {
+		dev_err(dev, "failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	g2d->runqueue_slab = kmem_cache_create("g2d_runqueue_slab",
+			sizeof(struct g2d_runqueue_node), 0, 0, NULL);
+	if (!g2d->runqueue_slab) {
+		ret = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	g2d->dev = dev;
+
+	g2d->g2d_workq = create_singlethread_workqueue("g2d");
+	if (!g2d->g2d_workq) {
+		dev_err(dev, "failed to create workqueue\n");
+		ret = -EINVAL;
+		goto err_destroy_slab;
+	}
+
+	INIT_WORK(&g2d->runqueue_work, g2d_runqueue_worker);
+	INIT_LIST_HEAD(&g2d->free_cmdlist);
+	INIT_LIST_HEAD(&g2d->runqueue);
+
+	mutex_init(&g2d->cmdlist_mutex);
+	mutex_init(&g2d->runqueue_mutex);
+
+	ret = g2d_init_cmdlist(g2d);
+	if (ret < 0)
+		goto err_destroy_workqueue;
+
+	g2d->gate_clk = clk_get(dev, "fimg2d");
+	if (IS_ERR(g2d->gate_clk)) {
+		dev_err(dev, "failed to get gate clock\n");
+		ret = PTR_ERR(g2d->gate_clk);
+		goto err_fini_cmdlist;
+	}
+
+	pm_runtime_enable(dev);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "failed to get I/O memory\n");
+		ret = -ENOENT;
+		goto err_put_clk;
+	}
+
+	g2d->regs_res = request_mem_region(res->start, resource_size(res),
+					   dev_name(dev));
+	if (!g2d->regs_res) {
+		dev_err(dev, "failed to request I/O memory\n");
+		ret = -ENOENT;
+		goto err_put_clk;
+	}
+
+	g2d->regs = ioremap(res->start, resource_size(res));
+	if (!g2d->regs) {
+		dev_err(dev, "failed to remap I/O memory\n");
+		ret = -ENXIO;
+		goto err_release_res;
+	}
+
+	g2d->irq = platform_get_irq(pdev, 0);
+	if (g2d->irq < 0) {
+		dev_err(dev, "failed to get irq\n");
+		ret = g2d->irq;
+		goto err_unmap_base;
+	}
+
+	ret = request_irq(g2d->irq, g2d_irq_handler, 0, "drm_g2d", g2d);
+	if (ret < 0) {
+		dev_err(dev, "irq request failed\n");
+		goto err_unmap_base;
+	}
+
+	platform_set_drvdata(pdev, g2d);
+
+	subdrv = &g2d->subdrv;
+	subdrv->dev = dev;
+	subdrv->open = g2d_open;
+	subdrv->close = g2d_close;
+
+	ret = exynos_drm_subdrv_register(subdrv);
+	if (ret < 0) {
+		dev_err(dev, "failed to register drm g2d device\n");
+		goto err_free_irq;
+	}
+
+	dev_info(dev, "The exynos g2d(ver %d.%d) successfully probed\n",
+			G2D_HW_MAJOR_VER, G2D_HW_MINOR_VER);
+
+	return 0;
+
+err_free_irq:
+	free_irq(g2d->irq, g2d);
+err_unmap_base:
+	iounmap(g2d->regs);
+err_release_res:
+	release_resource(g2d->regs_res);
+	kfree(g2d->regs_res);
+err_put_clk:
+	pm_runtime_disable(dev);
+	clk_put(g2d->gate_clk);
+err_fini_cmdlist:
+	g2d_fini_cmdlist(g2d);
+err_destroy_workqueue:
+	destroy_workqueue(g2d->g2d_workq);
+err_destroy_slab:
+	kmem_cache_destroy(g2d->runqueue_slab);
+err_free_mem:
+	kfree(g2d);
+	return ret;
+}
+
+static int __devexit g2d_remove(struct platform_device *pdev)
+{
+	struct g2d_data *g2d = platform_get_drvdata(pdev);
+
+	cancel_work_sync(&g2d->runqueue_work);
+	exynos_drm_subdrv_unregister(&g2d->subdrv);
+	free_irq(g2d->irq, g2d);
+
+	while (g2d->runqueue_node) {
+		g2d_free_runqueue_node(g2d, g2d->runqueue_node);
+		g2d->runqueue_node = g2d_get_runqueue_node(g2d);
+	}
+
+	iounmap(g2d->regs);
+	release_resource(g2d->regs_res);
+	kfree(g2d->regs_res);
+
+	pm_runtime_disable(&pdev->dev);
+	clk_put(g2d->gate_clk);
+
+	g2d_fini_cmdlist(g2d);
+	destroy_workqueue(g2d->g2d_workq);
+	kmem_cache_destroy(g2d->runqueue_slab);
+	kfree(g2d);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int g2d_suspend(struct device *dev)
+{
+	struct g2d_data *g2d = dev_get_drvdata(dev);
+
+	mutex_lock(&g2d->runqueue_mutex);
+	g2d->suspended = true;
+	mutex_unlock(&g2d->runqueue_mutex);
+
+	while (g2d->runqueue_node)
+		/* FIXME: good range? */
+		usleep_range(500, 1000);
+
+	flush_work_sync(&g2d->runqueue_work);
+
+	return 0;
+}
+
+static int g2d_resume(struct device *dev)
+{
+	struct g2d_data *g2d = dev_get_drvdata(dev);
+
+	g2d->suspended = false;
+	g2d_exec_runqueue(g2d);
+
+	return 0;
+}
+#endif
+
+SIMPLE_DEV_PM_OPS(g2d_pm_ops, g2d_suspend, g2d_resume);
+
+struct platform_driver g2d_driver = {
+	.probe		= g2d_probe,
+	.remove		= __devexit_p(g2d_remove),
+	.driver		= {
+		.name	= "s5p-g2d",
+		.owner	= THIS_MODULE,
+		.pm	= &g2d_pm_ops,
+	},
+};
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.h b/drivers/gpu/drm/exynos/exynos_drm_g2d.h
new file mode 100644
index 000000000000..1a9c7ca8c15b
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co.Ltd
+ * Authors: Joonyoung Shim <jy0922.shim@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 Foundationr
+ */
+
+#ifdef CONFIG_DRM_EXYNOS_G2D
+extern int exynos_g2d_get_ver_ioctl(struct drm_device *dev, void *data,
+				    struct drm_file *file_priv);
+extern int exynos_g2d_set_cmdlist_ioctl(struct drm_device *dev, void *data,
+					struct drm_file *file_priv);
+extern int exynos_g2d_exec_ioctl(struct drm_device *dev, void *data,
+				 struct drm_file *file_priv);
+#else
+static inline int exynos_g2d_get_ver_ioctl(struct drm_device *dev, void *data,
+					   struct drm_file *file_priv)
+{
+	return -ENODEV;
+}
+
+static inline int exynos_g2d_set_cmdlist_ioctl(struct drm_device *dev,
+					       void *data,
+					       struct drm_file *file_priv)
+{
+	return -ENODEV;
+}
+
+static inline int exynos_g2d_exec_ioctl(struct drm_device *dev, void *data,
+					struct drm_file *file_priv)
+{
+	return -ENODEV;
+}
+#endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index 1dffa8359f88..fc91293c4560 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -66,6 +66,22 @@ static int check_gem_flags(unsigned int flags)
 	return 0;
 }
 
+static void update_vm_cache_attr(struct exynos_drm_gem_obj *obj,
+					struct vm_area_struct *vma)
+{
+	DRM_DEBUG_KMS("flags = 0x%x\n", obj->flags);
+
+	/* non-cachable as default. */
+	if (obj->flags & EXYNOS_BO_CACHABLE)
+		vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
+	else if (obj->flags & EXYNOS_BO_WC)
+		vma->vm_page_prot =
+			pgprot_writecombine(vm_get_page_prot(vma->vm_flags));
+	else
+		vma->vm_page_prot =
+			pgprot_noncached(vm_get_page_prot(vma->vm_flags));
+}
+
 static unsigned long roundup_gem_size(unsigned long size, unsigned int flags)
 {
 	if (!IS_NONCONTIG_BUFFER(flags)) {
@@ -80,7 +96,7 @@ out:
 	return roundup(size, PAGE_SIZE);
 }
 
-static struct page **exynos_gem_get_pages(struct drm_gem_object *obj,
+struct page **exynos_gem_get_pages(struct drm_gem_object *obj,
 						gfp_t gfpmask)
 {
 	struct inode *inode;
@@ -180,6 +196,7 @@ static int exynos_drm_gem_get_pages(struct drm_gem_object *obj)
 	}
 
 	npages = obj->size >> PAGE_SHIFT;
+	buf->page_size = PAGE_SIZE;
 
 	buf->sgt = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!buf->sgt) {
@@ -262,24 +279,24 @@ static int exynos_drm_gem_handle_create(struct drm_gem_object *obj,
 void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj)
 {
 	struct drm_gem_object *obj;
+	struct exynos_drm_gem_buf *buf;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	if (!exynos_gem_obj)
-		return;
-
 	obj = &exynos_gem_obj->base;
+	buf = exynos_gem_obj->buffer;
 
 	DRM_DEBUG_KMS("handle count = %d\n", atomic_read(&obj->handle_count));
 
-	if ((exynos_gem_obj->flags & EXYNOS_BO_NONCONTIG) &&
-			exynos_gem_obj->buffer->pages)
+	if (!buf->pages)
+		return;
+
+	if (exynos_gem_obj->flags & EXYNOS_BO_NONCONTIG)
 		exynos_drm_gem_put_pages(obj);
 	else
-		exynos_drm_free_buf(obj->dev, exynos_gem_obj->flags,
-					exynos_gem_obj->buffer);
+		exynos_drm_free_buf(obj->dev, exynos_gem_obj->flags, buf);
 
-	exynos_drm_fini_buf(obj->dev, exynos_gem_obj->buffer);
+	exynos_drm_fini_buf(obj->dev, buf);
 	exynos_gem_obj->buffer = NULL;
 
 	if (obj->map_list.map)
@@ -292,7 +309,7 @@ void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj)
 	exynos_gem_obj = NULL;
 }
 
-static struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
+struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
 						      unsigned long size)
 {
 	struct exynos_drm_gem_obj *exynos_gem_obj;
@@ -493,8 +510,7 @@ static int exynos_drm_gem_mmap_buffer(struct file *filp,
 
 	vma->vm_flags |= (VM_IO | VM_RESERVED);
 
-	/* in case of direct mapping, always having non-cachable attribute */
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	update_vm_cache_attr(exynos_gem_obj, vma);
 
 	vm_size = usize = vma->vm_end - vma->vm_start;
 
@@ -588,6 +604,32 @@ int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
+int exynos_drm_gem_get_ioctl(struct drm_device *dev, void *data,
+				      struct drm_file *file_priv)
+{	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct drm_exynos_gem_info *args = data;
+	struct drm_gem_object *obj;
+
+	mutex_lock(&dev->struct_mutex);
+
+	obj = drm_gem_object_lookup(dev, file_priv, args->handle);
+	if (!obj) {
+		DRM_ERROR("failed to lookup gem object.\n");
+		mutex_unlock(&dev->struct_mutex);
+		return -EINVAL;
+	}
+
+	exynos_gem_obj = to_exynos_gem_obj(obj);
+
+	args->flags = exynos_gem_obj->flags;
+	args->size = exynos_gem_obj->size;
+
+	drm_gem_object_unreference(obj);
+	mutex_unlock(&dev->struct_mutex);
+
+	return 0;
+}
+
 int exynos_drm_gem_init_object(struct drm_gem_object *obj)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -597,8 +639,17 @@ int exynos_drm_gem_init_object(struct drm_gem_object *obj)
 
 void exynos_drm_gem_free_object(struct drm_gem_object *obj)
 {
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct exynos_drm_gem_buf *buf;
+
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	exynos_gem_obj = to_exynos_gem_obj(obj);
+	buf = exynos_gem_obj->buffer;
+
+	if (obj->import_attach)
+		drm_prime_gem_destroy(obj, buf->sgt);
+
 	exynos_drm_gem_destroy(to_exynos_gem_obj(obj));
 }
 
@@ -724,6 +775,8 @@ int exynos_drm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 
 int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 {
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct drm_gem_object *obj;
 	int ret;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -735,8 +788,20 @@ int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 		return ret;
 	}
 
+	obj = vma->vm_private_data;
+	exynos_gem_obj = to_exynos_gem_obj(obj);
+
+	ret = check_gem_flags(exynos_gem_obj->flags);
+	if (ret) {
+		drm_gem_vm_close(vma);
+		drm_gem_free_mmap_offset(obj);
+		return ret;
+	}
+
 	vma->vm_flags &= ~VM_PFNMAP;
 	vma->vm_flags |= VM_MIXEDMAP;
 
+	update_vm_cache_attr(exynos_gem_obj, vma);
+
 	return ret;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.h b/drivers/gpu/drm/exynos/exynos_drm_gem.h
index 4ed842039505..14d038b6cb02 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.h
@@ -40,6 +40,7 @@
  *	device address with IOMMU.
  * @sgt: sg table to transfer page data.
  * @pages: contain all pages to allocated memory region.
+ * @page_size: could be 4K, 64K or 1MB.
  * @size: size of allocated memory region.
  */
 struct exynos_drm_gem_buf {
@@ -47,6 +48,7 @@ struct exynos_drm_gem_buf {
 	dma_addr_t		dma_addr;
 	struct sg_table		*sgt;
 	struct page		**pages;
+	unsigned long		page_size;
 	unsigned long		size;
 };
 
@@ -74,9 +76,15 @@ struct exynos_drm_gem_obj {
 	unsigned int			flags;
 };
 
+struct page **exynos_gem_get_pages(struct drm_gem_object *obj, gfp_t gfpmask);
+
 /* destroy a buffer with gem object */
 void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj);
 
+/* create a private gem object and initialize it. */
+struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
+						      unsigned long size);
+
 /* create a new buffer with gem object */
 struct exynos_drm_gem_obj *exynos_drm_gem_create(struct drm_device *dev,
 						unsigned int flags,
@@ -119,6 +127,10 @@ int exynos_drm_gem_map_offset_ioctl(struct drm_device *dev, void *data,
 int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv);
 
+/* get buffer information to memory region allocated by gem. */
+int exynos_drm_gem_get_ioctl(struct drm_device *dev, void *data,
+				      struct drm_file *file_priv);
+
 /* initialize gem object. */
 int exynos_drm_gem_init_object(struct drm_gem_object *obj);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
index 3424463676e0..5d9d2c2f8f3f 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
@@ -37,6 +37,8 @@ struct drm_hdmi_context {
 	struct exynos_drm_subdrv	subdrv;
 	struct exynos_drm_hdmi_context	*hdmi_ctx;
 	struct exynos_drm_hdmi_context	*mixer_ctx;
+
+	bool	enabled[MIXER_WIN_NR];
 };
 
 void exynos_hdmi_ops_register(struct exynos_hdmi_ops *ops)
@@ -189,23 +191,34 @@ static void drm_hdmi_dpms(struct device *subdrv_dev, int mode)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	switch (mode) {
-	case DRM_MODE_DPMS_ON:
-		break;
-	case DRM_MODE_DPMS_STANDBY:
-	case DRM_MODE_DPMS_SUSPEND:
-	case DRM_MODE_DPMS_OFF:
-		if (hdmi_ops && hdmi_ops->disable)
-			hdmi_ops->disable(ctx->hdmi_ctx->ctx);
-		break;
-	default:
-		DRM_DEBUG_KMS("unkown dps mode: %d\n", mode);
-		break;
+	if (mixer_ops && mixer_ops->dpms)
+		mixer_ops->dpms(ctx->mixer_ctx->ctx, mode);
+
+	if (hdmi_ops && hdmi_ops->dpms)
+		hdmi_ops->dpms(ctx->hdmi_ctx->ctx, mode);
+}
+
+static void drm_hdmi_apply(struct device *subdrv_dev)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	int i;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	for (i = 0; i < MIXER_WIN_NR; i++) {
+		if (!ctx->enabled[i])
+			continue;
+		if (mixer_ops && mixer_ops->win_commit)
+			mixer_ops->win_commit(ctx->mixer_ctx->ctx, i);
 	}
+
+	if (hdmi_ops && hdmi_ops->commit)
+		hdmi_ops->commit(ctx->hdmi_ctx->ctx);
 }
 
 static struct exynos_drm_manager_ops drm_hdmi_manager_ops = {
 	.dpms = drm_hdmi_dpms,
+	.apply = drm_hdmi_apply,
 	.enable_vblank = drm_hdmi_enable_vblank,
 	.disable_vblank = drm_hdmi_disable_vblank,
 	.mode_fixup = drm_hdmi_mode_fixup,
@@ -228,21 +241,37 @@ static void drm_mixer_mode_set(struct device *subdrv_dev,
 static void drm_mixer_commit(struct device *subdrv_dev, int zpos)
 {
 	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	int win = (zpos == DEFAULT_ZPOS) ? MIXER_DEFAULT_WIN : zpos;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (win < 0 || win > MIXER_WIN_NR) {
+		DRM_ERROR("mixer window[%d] is wrong\n", win);
+		return;
+	}
+
 	if (mixer_ops && mixer_ops->win_commit)
-		mixer_ops->win_commit(ctx->mixer_ctx->ctx, zpos);
+		mixer_ops->win_commit(ctx->mixer_ctx->ctx, win);
+
+	ctx->enabled[win] = true;
 }
 
 static void drm_mixer_disable(struct device *subdrv_dev, int zpos)
 {
 	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	int win = (zpos == DEFAULT_ZPOS) ? MIXER_DEFAULT_WIN : zpos;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (win < 0 || win > MIXER_WIN_NR) {
+		DRM_ERROR("mixer window[%d] is wrong\n", win);
+		return;
+	}
+
 	if (mixer_ops && mixer_ops->win_disable)
-		mixer_ops->win_disable(ctx->mixer_ctx->ctx, zpos);
+		mixer_ops->win_disable(ctx->mixer_ctx->ctx, win);
+
+	ctx->enabled[win] = false;
 }
 
 static struct exynos_drm_overlay_ops drm_hdmi_overlay_ops = {
@@ -335,25 +364,6 @@ static int __devinit exynos_drm_hdmi_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int hdmi_runtime_suspend(struct device *dev)
-{
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	return 0;
-}
-
-static int hdmi_runtime_resume(struct device *dev)
-{
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	return 0;
-}
-
-static const struct dev_pm_ops hdmi_pm_ops = {
-	.runtime_suspend = hdmi_runtime_suspend,
-	.runtime_resume	 = hdmi_runtime_resume,
-};
-
 static int __devexit exynos_drm_hdmi_remove(struct platform_device *pdev)
 {
 	struct drm_hdmi_context *ctx = platform_get_drvdata(pdev);
@@ -372,6 +382,5 @@ struct platform_driver exynos_drm_common_hdmi_driver = {
 	.driver		= {
 		.name	= "exynos-drm-hdmi",
 		.owner	= THIS_MODULE,
-		.pm = &hdmi_pm_ops,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
index f3ae192c8dcf..bd8126996e52 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
@@ -26,6 +26,9 @@
 #ifndef _EXYNOS_DRM_HDMI_H_
 #define _EXYNOS_DRM_HDMI_H_
 
+#define MIXER_WIN_NR		3
+#define MIXER_DEFAULT_WIN	0
+
 /*
  * exynos hdmi common context structure.
  *
@@ -54,13 +57,14 @@ struct exynos_hdmi_ops {
 	void (*get_max_resol)(void *ctx, unsigned int *width,
 				unsigned int *height);
 	void (*commit)(void *ctx);
-	void (*disable)(void *ctx);
+	void (*dpms)(void *ctx, int mode);
 };
 
 struct exynos_mixer_ops {
 	/* manager */
 	int (*enable_vblank)(void *ctx, int pipe);
 	void (*disable_vblank)(void *ctx);
+	void (*dpms)(void *ctx, int mode);
 
 	/* overlay */
 	void (*win_mode_set)(void *ctx, struct exynos_drm_overlay *overlay);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
index f92fe4c6174a..c4c6525d4653 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -41,8 +41,6 @@ exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		container_of(plane, struct exynos_plane, base);
 	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
 	struct exynos_drm_crtc_pos pos;
-	unsigned int x = src_x >> 16;
-	unsigned int y = src_y >> 16;
 	int ret;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
@@ -53,10 +51,12 @@ exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	pos.crtc_w = crtc_w;
 	pos.crtc_h = crtc_h;
 
-	pos.fb_x = x;
-	pos.fb_y = y;
+	/* considering 16.16 fixed point of source values */
+	pos.fb_x = src_x >> 16;
+	pos.fb_y = src_y >> 16;
+	pos.src_w = src_w >> 16;
+	pos.src_h = src_h >> 16;
 
-	/* TODO: scale feature */
 	ret = exynos_drm_overlay_update(overlay, fb, &crtc->mode, &pos);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index b00353876458..a137e9e39a33 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -57,18 +57,16 @@ struct hdmi_resources {
 struct hdmi_context {
 	struct device			*dev;
 	struct drm_device		*drm_dev;
-	struct fb_videomode		*default_timing;
-	unsigned int			is_v13:1;
-	unsigned int			default_win;
-	unsigned int			default_bpp;
-	bool				hpd_handle;
-	bool				enabled;
+	bool				hpd;
+	bool				powered;
+	bool				is_v13;
+	bool				dvi_mode;
+	struct mutex			hdmi_mutex;
 
 	struct resource			*regs_res;
 	void __iomem			*regs;
-	unsigned int			irq;
-	struct workqueue_struct		*wq;
-	struct work_struct		hotplug_work;
+	unsigned int			external_irq;
+	unsigned int			internal_irq;
 
 	struct i2c_client		*ddc_port;
 	struct i2c_client		*hdmiphy_port;
@@ -78,6 +76,9 @@ struct hdmi_context {
 
 	struct hdmi_resources		res;
 	void				*parent_ctx;
+
+	void				(*cfg_hpd)(bool external);
+	int				(*get_hpd)(void);
 };
 
 /* HDMI Version 1.3 */
@@ -361,6 +362,13 @@ static const u8 hdmiphy_conf27_027[32] = {
 	0x54, 0xe3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00,
 };
 
+static const u8 hdmiphy_conf74_176[32] = {
+	0x01, 0xd1, 0x1f, 0x10, 0x40, 0x5b, 0xef, 0x08,
+	0x81, 0xa0, 0xb9, 0xd8, 0x45, 0xa0, 0xac, 0x80,
+	0x5a, 0x80, 0x11, 0x04, 0x02, 0x22, 0x44, 0x86,
+	0x54, 0xa6, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00,
+};
+
 static const u8 hdmiphy_conf74_25[32] = {
 	0x01, 0xd1, 0x1f, 0x10, 0x40, 0x40, 0xf8, 0x08,
 	0x81, 0xa0, 0xba, 0xd8, 0x45, 0xa0, 0xac, 0x80,
@@ -750,6 +758,63 @@ static const struct hdmi_preset_conf hdmi_conf_1080i60 = {
 	},
 };
 
+static const struct hdmi_preset_conf hdmi_conf_1080p30 = {
+	.core = {
+		.h_blank = {0x18, 0x01},
+		.v2_blank = {0x65, 0x04},
+		.v1_blank = {0x2d, 0x00},
+		.v_line = {0x65, 0x04},
+		.h_line = {0x98, 0x08},
+		.hsync_pol = {0x00},
+		.vsync_pol = {0x00},
+		.int_pro_mode = {0x00},
+		.v_blank_f0 = {0xff, 0xff},
+		.v_blank_f1 = {0xff, 0xff},
+		.h_sync_start = {0x56, 0x00},
+		.h_sync_end = {0x82, 0x00},
+		.v_sync_line_bef_2 = {0x09, 0x00},
+		.v_sync_line_bef_1 = {0x04, 0x00},
+		.v_sync_line_aft_2 = {0xff, 0xff},
+		.v_sync_line_aft_1 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_2 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_1 = {0xff, 0xff},
+		.v_blank_f2 = {0xff, 0xff},
+		.v_blank_f3 = {0xff, 0xff},
+		.v_blank_f4 = {0xff, 0xff},
+		.v_blank_f5 = {0xff, 0xff},
+		.v_sync_line_aft_3 = {0xff, 0xff},
+		.v_sync_line_aft_4 = {0xff, 0xff},
+		.v_sync_line_aft_5 = {0xff, 0xff},
+		.v_sync_line_aft_6 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_3 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_4 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_5 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_6 = {0xff, 0xff},
+		.vact_space_1 = {0xff, 0xff},
+		.vact_space_2 = {0xff, 0xff},
+		.vact_space_3 = {0xff, 0xff},
+		.vact_space_4 = {0xff, 0xff},
+		.vact_space_5 = {0xff, 0xff},
+		.vact_space_6 = {0xff, 0xff},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x98, 0x08, /* h_fsz */
+		0x18, 0x01, 0x80, 0x07, /* hact */
+		0x65, 0x04, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x2d, 0x00, 0x38, 0x04, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x48, 0x02, /* vact_st2 */
+		0x00, 0x00, /* vact_st3 */
+		0x00, 0x00, /* vact_st4 */
+		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+		0x00, /* 3d FP */
+	},
+};
+
 static const struct hdmi_preset_conf hdmi_conf_1080p50 = {
 	.core = {
 		.h_blank = {0xd0, 0x02},
@@ -864,6 +929,7 @@ static const struct hdmi_conf hdmi_confs[] = {
 	{ 1280, 720, 60, false, hdmiphy_conf74_25, &hdmi_conf_720p60 },
 	{ 1920, 1080, 50, true, hdmiphy_conf74_25, &hdmi_conf_1080i50 },
 	{ 1920, 1080, 60, true, hdmiphy_conf74_25, &hdmi_conf_1080i60 },
+	{ 1920, 1080, 30, false, hdmiphy_conf74_176, &hdmi_conf_1080p30 },
 	{ 1920, 1080, 50, false, hdmiphy_conf148_5, &hdmi_conf_1080p50 },
 	{ 1920, 1080, 60, false, hdmiphy_conf148_5, &hdmi_conf_1080p60 },
 };
@@ -1194,12 +1260,8 @@ static int hdmi_conf_index(struct hdmi_context *hdata,
 static bool hdmi_is_connected(void *ctx)
 {
 	struct hdmi_context *hdata = ctx;
-	u32 val = hdmi_reg_read(hdata, HDMI_HPD_STATUS);
-
-	if (val)
-		return true;
 
-	return false;
+	return hdata->hpd;
 }
 
 static int hdmi_get_edid(void *ctx, struct drm_connector *connector,
@@ -1215,10 +1277,12 @@ static int hdmi_get_edid(void *ctx, struct drm_connector *connector,
 
 	raw_edid = drm_get_edid(connector, hdata->ddc_port->adapter);
 	if (raw_edid) {
+		hdata->dvi_mode = !drm_detect_hdmi_monitor(raw_edid);
 		memcpy(edid, raw_edid, min((1 + raw_edid->extensions)
 					* EDID_LENGTH, len));
-		DRM_DEBUG_KMS("width[%d] x height[%d]\n",
-				raw_edid->width_cm, raw_edid->height_cm);
+		DRM_DEBUG_KMS("%s : width[%d] x height[%d]\n",
+			(hdata->dvi_mode ? "dvi monitor" : "hdmi monitor"),
+			raw_edid->width_cm, raw_edid->height_cm);
 	} else {
 		return -ENODEV;
 	}
@@ -1289,28 +1353,6 @@ static int hdmi_check_timing(void *ctx, void *timing)
 		return hdmi_v14_check_timing(check_timing);
 }
 
-static int hdmi_display_power_on(void *ctx, int mode)
-{
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	switch (mode) {
-	case DRM_MODE_DPMS_ON:
-		DRM_DEBUG_KMS("hdmi [on]\n");
-		break;
-	case DRM_MODE_DPMS_STANDBY:
-		break;
-	case DRM_MODE_DPMS_SUSPEND:
-		break;
-	case DRM_MODE_DPMS_OFF:
-		DRM_DEBUG_KMS("hdmi [off]\n");
-		break;
-	default:
-		break;
-	}
-
-	return 0;
-}
-
 static void hdmi_set_acr(u32 freq, u8 *acr)
 {
 	u32 n, cts;
@@ -1463,10 +1505,7 @@ static void hdmi_audio_init(struct hdmi_context *hdata)
 
 static void hdmi_audio_control(struct hdmi_context *hdata, bool onoff)
 {
-	u32 mod;
-
-	mod = hdmi_reg_read(hdata, HDMI_MODE_SEL);
-	if (mod & HDMI_DVI_MODE_EN)
+	if (hdata->dvi_mode)
 		return;
 
 	hdmi_reg_writeb(hdata, HDMI_AUI_CON, onoff ? 2 : 0);
@@ -1478,9 +1517,6 @@ static void hdmi_conf_reset(struct hdmi_context *hdata)
 {
 	u32 reg;
 
-	/* disable hpd handle for drm */
-	hdata->hpd_handle = false;
-
 	if (hdata->is_v13)
 		reg = HDMI_V13_CORE_RSTOUT;
 	else
@@ -1491,16 +1527,10 @@ static void hdmi_conf_reset(struct hdmi_context *hdata)
 	mdelay(10);
 	hdmi_reg_writemask(hdata, reg, ~0, HDMI_CORE_SW_RSTOUT);
 	mdelay(10);
-
-	/* enable hpd handle for drm */
-	hdata->hpd_handle = true;
 }
 
 static void hdmi_conf_init(struct hdmi_context *hdata)
 {
-	/* disable hpd handle for drm */
-	hdata->hpd_handle = false;
-
 	/* enable HPD interrupts */
 	hdmi_reg_writemask(hdata, HDMI_INTC_CON, 0, HDMI_INTC_EN_GLOBAL |
 		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
@@ -1514,6 +1544,14 @@ static void hdmi_conf_init(struct hdmi_context *hdata)
 	/* disable bluescreen */
 	hdmi_reg_writemask(hdata, HDMI_CON_0, 0, HDMI_BLUE_SCR_EN);
 
+	if (hdata->dvi_mode) {
+		/* choose DVI mode */
+		hdmi_reg_writemask(hdata, HDMI_MODE_SEL,
+				HDMI_MODE_DVI_EN, HDMI_MODE_MASK);
+		hdmi_reg_writeb(hdata, HDMI_CON_2,
+				HDMI_VID_PREAMBLE_DIS | HDMI_GUARD_BAND_DIS);
+	}
+
 	if (hdata->is_v13) {
 		/* choose bluescreen (fecal) color */
 		hdmi_reg_writeb(hdata, HDMI_V13_BLUE_SCREEN_0, 0x12);
@@ -1535,9 +1573,6 @@ static void hdmi_conf_init(struct hdmi_context *hdata)
 		hdmi_reg_writeb(hdata, HDMI_AVI_BYTE(1), 2 << 5);
 		hdmi_reg_writemask(hdata, HDMI_CON_1, 2, 3 << 5);
 	}
-
-	/* enable hpd handle for drm */
-	hdata->hpd_handle = true;
 }
 
 static void hdmi_v13_timing_apply(struct hdmi_context *hdata)
@@ -1890,8 +1925,11 @@ static void hdmi_conf_apply(struct hdmi_context *hdata)
 	hdmiphy_conf_reset(hdata);
 	hdmiphy_conf_apply(hdata);
 
+	mutex_lock(&hdata->hdmi_mutex);
 	hdmi_conf_reset(hdata);
 	hdmi_conf_init(hdata);
+	mutex_unlock(&hdata->hdmi_mutex);
+
 	hdmi_audio_init(hdata);
 
 	/* setting core registers */
@@ -1971,20 +2009,86 @@ static void hdmi_commit(void *ctx)
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
 	hdmi_conf_apply(hdata);
+}
+
+static void hdmi_poweron(struct hdmi_context *hdata)
+{
+	struct hdmi_resources *res = &hdata->res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&hdata->hdmi_mutex);
+	if (hdata->powered) {
+		mutex_unlock(&hdata->hdmi_mutex);
+		return;
+	}
+
+	hdata->powered = true;
+
+	if (hdata->cfg_hpd)
+		hdata->cfg_hpd(true);
+	mutex_unlock(&hdata->hdmi_mutex);
+
+	pm_runtime_get_sync(hdata->dev);
+
+	regulator_bulk_enable(res->regul_count, res->regul_bulk);
+	clk_enable(res->hdmiphy);
+	clk_enable(res->hdmi);
+	clk_enable(res->sclk_hdmi);
+}
+
+static void hdmi_poweroff(struct hdmi_context *hdata)
+{
+	struct hdmi_resources *res = &hdata->res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&hdata->hdmi_mutex);
+	if (!hdata->powered)
+		goto out;
+	mutex_unlock(&hdata->hdmi_mutex);
+
+	/*
+	 * The TV power domain needs any condition of hdmiphy to turn off and
+	 * its reset state seems to meet the condition.
+	 */
+	hdmiphy_conf_reset(hdata);
+
+	clk_disable(res->sclk_hdmi);
+	clk_disable(res->hdmi);
+	clk_disable(res->hdmiphy);
+	regulator_bulk_disable(res->regul_count, res->regul_bulk);
+
+	pm_runtime_put_sync(hdata->dev);
 
-	hdata->enabled = true;
+	mutex_lock(&hdata->hdmi_mutex);
+	if (hdata->cfg_hpd)
+		hdata->cfg_hpd(false);
+
+	hdata->powered = false;
+
+out:
+	mutex_unlock(&hdata->hdmi_mutex);
 }
 
-static void hdmi_disable(void *ctx)
+static void hdmi_dpms(void *ctx, int mode)
 {
 	struct hdmi_context *hdata = ctx;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	if (hdata->enabled) {
-		hdmi_audio_control(hdata, false);
-		hdmiphy_conf_reset(hdata);
-		hdmi_conf_reset(hdata);
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		hdmi_poweron(hdata);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		hdmi_poweroff(hdata);
+		break;
+	default:
+		DRM_DEBUG_KMS("unknown dpms mode: %d\n", mode);
+		break;
 	}
 }
 
@@ -1993,30 +2097,35 @@ static struct exynos_hdmi_ops hdmi_ops = {
 	.is_connected	= hdmi_is_connected,
 	.get_edid	= hdmi_get_edid,
 	.check_timing	= hdmi_check_timing,
-	.power_on	= hdmi_display_power_on,
 
 	/* manager */
 	.mode_fixup	= hdmi_mode_fixup,
 	.mode_set	= hdmi_mode_set,
 	.get_max_resol	= hdmi_get_max_resol,
 	.commit		= hdmi_commit,
-	.disable	= hdmi_disable,
+	.dpms		= hdmi_dpms,
 };
 
-/*
- * Handle hotplug events outside the interrupt handler proper.
- */
-static void hdmi_hotplug_func(struct work_struct *work)
+static irqreturn_t hdmi_external_irq_thread(int irq, void *arg)
 {
-	struct hdmi_context *hdata =
-		container_of(work, struct hdmi_context, hotplug_work);
-	struct exynos_drm_hdmi_context *ctx =
-		(struct exynos_drm_hdmi_context *)hdata->parent_ctx;
+	struct exynos_drm_hdmi_context *ctx = arg;
+	struct hdmi_context *hdata = ctx->ctx;
+
+	if (!hdata->get_hpd)
+		goto out;
+
+	mutex_lock(&hdata->hdmi_mutex);
+	hdata->hpd = hdata->get_hpd();
+	mutex_unlock(&hdata->hdmi_mutex);
 
-	drm_helper_hpd_irq_event(ctx->drm_dev);
+	if (ctx->drm_dev)
+		drm_helper_hpd_irq_event(ctx->drm_dev);
+
+out:
+	return IRQ_HANDLED;
 }
 
-static irqreturn_t hdmi_irq_handler(int irq, void *arg)
+static irqreturn_t hdmi_internal_irq_thread(int irq, void *arg)
 {
 	struct exynos_drm_hdmi_context *ctx = arg;
 	struct hdmi_context *hdata = ctx->ctx;
@@ -2025,19 +2134,28 @@ static irqreturn_t hdmi_irq_handler(int irq, void *arg)
 	intc_flag = hdmi_reg_read(hdata, HDMI_INTC_FLAG);
 	/* clearing flags for HPD plug/unplug */
 	if (intc_flag & HDMI_INTC_FLAG_HPD_UNPLUG) {
-		DRM_DEBUG_KMS("unplugged, handling:%d\n", hdata->hpd_handle);
+		DRM_DEBUG_KMS("unplugged\n");
 		hdmi_reg_writemask(hdata, HDMI_INTC_FLAG, ~0,
 			HDMI_INTC_FLAG_HPD_UNPLUG);
 	}
 	if (intc_flag & HDMI_INTC_FLAG_HPD_PLUG) {
-		DRM_DEBUG_KMS("plugged, handling:%d\n", hdata->hpd_handle);
+		DRM_DEBUG_KMS("plugged\n");
 		hdmi_reg_writemask(hdata, HDMI_INTC_FLAG, ~0,
 			HDMI_INTC_FLAG_HPD_PLUG);
 	}
 
-	if (ctx->drm_dev && hdata->hpd_handle)
-		queue_work(hdata->wq, &hdata->hotplug_work);
+	mutex_lock(&hdata->hdmi_mutex);
+	hdata->hpd = hdmi_reg_read(hdata, HDMI_HPD_STATUS);
+	if (hdata->powered && hdata->hpd) {
+		mutex_unlock(&hdata->hdmi_mutex);
+		goto out;
+	}
+	mutex_unlock(&hdata->hdmi_mutex);
+
+	if (ctx->drm_dev)
+		drm_helper_hpd_irq_event(ctx->drm_dev);
 
+out:
 	return IRQ_HANDLED;
 }
 
@@ -2131,68 +2249,6 @@ static int hdmi_resources_cleanup(struct hdmi_context *hdata)
 	return 0;
 }
 
-static void hdmi_resource_poweron(struct hdmi_context *hdata)
-{
-	struct hdmi_resources *res = &hdata->res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	/* turn HDMI power on */
-	regulator_bulk_enable(res->regul_count, res->regul_bulk);
-	/* power-on hdmi physical interface */
-	clk_enable(res->hdmiphy);
-	/* turn clocks on */
-	clk_enable(res->hdmi);
-	clk_enable(res->sclk_hdmi);
-
-	hdmiphy_conf_reset(hdata);
-	hdmi_conf_reset(hdata);
-	hdmi_conf_init(hdata);
-	hdmi_audio_init(hdata);
-}
-
-static void hdmi_resource_poweroff(struct hdmi_context *hdata)
-{
-	struct hdmi_resources *res = &hdata->res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	/* turn clocks off */
-	clk_disable(res->sclk_hdmi);
-	clk_disable(res->hdmi);
-	/* power-off hdmiphy */
-	clk_disable(res->hdmiphy);
-	/* turn HDMI power off */
-	regulator_bulk_disable(res->regul_count, res->regul_bulk);
-}
-
-static int hdmi_runtime_suspend(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
-
-	DRM_DEBUG_KMS("%s\n", __func__);
-
-	hdmi_resource_poweroff(ctx->ctx);
-
-	return 0;
-}
-
-static int hdmi_runtime_resume(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
-
-	DRM_DEBUG_KMS("%s\n", __func__);
-
-	hdmi_resource_poweron(ctx->ctx);
-
-	return 0;
-}
-
-static const struct dev_pm_ops hdmi_pm_ops = {
-	.runtime_suspend = hdmi_runtime_suspend,
-	.runtime_resume	 = hdmi_runtime_resume,
-};
-
 static struct i2c_client *hdmi_ddc, *hdmi_hdmiphy;
 
 void hdmi_attach_ddc_client(struct i2c_client *ddc)
@@ -2237,15 +2293,16 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	mutex_init(&hdata->hdmi_mutex);
+
 	drm_hdmi_ctx->ctx = (void *)hdata;
 	hdata->parent_ctx = (void *)drm_hdmi_ctx;
 
 	platform_set_drvdata(pdev, drm_hdmi_ctx);
 
 	hdata->is_v13 = pdata->is_v13;
-	hdata->default_win = pdata->default_win;
-	hdata->default_timing = &pdata->timing;
-	hdata->default_bpp = pdata->bpp;
+	hdata->cfg_hpd = pdata->cfg_hpd;
+	hdata->get_hpd = pdata->get_hpd;
 	hdata->dev = dev;
 
 	ret = hdmi_resources_init(hdata);
@@ -2294,41 +2351,49 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 
 	hdata->hdmiphy_port = hdmi_hdmiphy;
 
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
-		DRM_ERROR("get interrupt resource failed.\n");
-		ret = -ENXIO;
+	hdata->external_irq = platform_get_irq_byname(pdev, "external_irq");
+	if (hdata->external_irq < 0) {
+		DRM_ERROR("failed to get platform irq\n");
+		ret = hdata->external_irq;
 		goto err_hdmiphy;
 	}
 
-	/* create workqueue and hotplug work */
-	hdata->wq = alloc_workqueue("exynos-drm-hdmi",
-			WQ_UNBOUND | WQ_NON_REENTRANT, 1);
-	if (hdata->wq == NULL) {
-		DRM_ERROR("Failed to create workqueue.\n");
-		ret = -ENOMEM;
+	hdata->internal_irq = platform_get_irq_byname(pdev, "internal_irq");
+	if (hdata->internal_irq < 0) {
+		DRM_ERROR("failed to get platform internal irq\n");
+		ret = hdata->internal_irq;
 		goto err_hdmiphy;
 	}
-	INIT_WORK(&hdata->hotplug_work, hdmi_hotplug_func);
 
-	/* register hpd interrupt */
-	ret = request_irq(res->start, hdmi_irq_handler, 0, "drm_hdmi",
-				drm_hdmi_ctx);
+	ret = request_threaded_irq(hdata->external_irq, NULL,
+			hdmi_external_irq_thread, IRQF_TRIGGER_RISING |
+			IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+			"hdmi_external", drm_hdmi_ctx);
 	if (ret) {
-		DRM_ERROR("request interrupt failed.\n");
-		goto err_workqueue;
+		DRM_ERROR("failed to register hdmi internal interrupt\n");
+		goto err_hdmiphy;
+	}
+
+	if (hdata->cfg_hpd)
+		hdata->cfg_hpd(false);
+
+	ret = request_threaded_irq(hdata->internal_irq, NULL,
+			hdmi_internal_irq_thread, IRQF_ONESHOT,
+			"hdmi_internal", drm_hdmi_ctx);
+	if (ret) {
+		DRM_ERROR("failed to register hdmi internal interrupt\n");
+		goto err_free_irq;
 	}
-	hdata->irq = res->start;
 
 	/* register specific callbacks to common hdmi. */
 	exynos_hdmi_ops_register(&hdmi_ops);
 
-	hdmi_resource_poweron(hdata);
+	pm_runtime_enable(dev);
 
 	return 0;
 
-err_workqueue:
-	destroy_workqueue(hdata->wq);
+err_free_irq:
+	free_irq(hdata->external_irq, drm_hdmi_ctx);
 err_hdmiphy:
 	i2c_del_driver(&hdmiphy_driver);
 err_ddc:
@@ -2348,18 +2413,15 @@ err_data:
 
 static int __devexit hdmi_remove(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct exynos_drm_hdmi_context *ctx = platform_get_drvdata(pdev);
 	struct hdmi_context *hdata = ctx->ctx;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	hdmi_resource_poweroff(hdata);
+	pm_runtime_disable(dev);
 
-	disable_irq(hdata->irq);
-	free_irq(hdata->irq, hdata);
-
-	cancel_work_sync(&hdata->hotplug_work);
-	destroy_workqueue(hdata->wq);
+	free_irq(hdata->internal_irq, hdata);
 
 	hdmi_resources_cleanup(hdata);
 
@@ -2378,12 +2440,43 @@ static int __devexit hdmi_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int hdmi_suspend(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
+	struct hdmi_context *hdata = ctx->ctx;
+
+	disable_irq(hdata->internal_irq);
+	disable_irq(hdata->external_irq);
+
+	hdata->hpd = false;
+	if (ctx->drm_dev)
+		drm_helper_hpd_irq_event(ctx->drm_dev);
+
+	hdmi_poweroff(hdata);
+
+	return 0;
+}
+
+static int hdmi_resume(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
+	struct hdmi_context *hdata = ctx->ctx;
+
+	enable_irq(hdata->external_irq);
+	enable_irq(hdata->internal_irq);
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(hdmi_pm_ops, hdmi_suspend, hdmi_resume);
+
 struct platform_driver hdmi_driver = {
 	.probe		= hdmi_probe,
 	.remove		= __devexit_p(hdmi_remove),
 	.driver		= {
 		.name	= "exynos4-hdmi",
 		.owner	= THIS_MODULE,
-		.pm = &hdmi_pm_ops,
+		.pm	= &hdmi_pm_ops,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index e15438c01129..68ef01028375 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -37,9 +37,6 @@
 #include "exynos_drm_drv.h"
 #include "exynos_drm_hdmi.h"
 
-#define MIXER_WIN_NR		3
-#define MIXER_DEFAULT_WIN	0
-
 #define get_mixer_context(dev)	platform_get_drvdata(to_platform_device(dev))
 
 struct hdmi_win_data {
@@ -57,13 +54,14 @@ struct hdmi_win_data {
 	unsigned int		fb_y;
 	unsigned int		fb_width;
 	unsigned int		fb_height;
+	unsigned int		src_width;
+	unsigned int		src_height;
 	unsigned int		mode_width;
 	unsigned int		mode_height;
 	unsigned int		scan_flags;
 };
 
 struct mixer_resources {
-	struct device		*dev;
 	int			irq;
 	void __iomem		*mixer_regs;
 	void __iomem		*vp_regs;
@@ -76,10 +74,13 @@ struct mixer_resources {
 };
 
 struct mixer_context {
-	unsigned int		irq;
+	struct device		*dev;
 	int			pipe;
 	bool			interlace;
+	bool			powered;
+	u32			int_en;
 
+	struct mutex		mixer_mutex;
 	struct mixer_resources	mixer_res;
 	struct hdmi_win_data	win_data[MIXER_WIN_NR];
 };
@@ -352,10 +353,7 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	struct mixer_resources *res = &ctx->mixer_res;
 	unsigned long flags;
 	struct hdmi_win_data *win_data;
-	unsigned int full_width, full_height, width, height;
 	unsigned int x_ratio, y_ratio;
-	unsigned int src_x_offset, src_y_offset, dst_x_offset, dst_y_offset;
-	unsigned int mode_width, mode_height;
 	unsigned int buf_num;
 	dma_addr_t luma_addr[2], chroma_addr[2];
 	bool tiled_mode = false;
@@ -382,21 +380,9 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 		return;
 	}
 
-	full_width = win_data->fb_width;
-	full_height = win_data->fb_height;
-	width = win_data->crtc_width;
-	height = win_data->crtc_height;
-	mode_width = win_data->mode_width;
-	mode_height = win_data->mode_height;
-
 	/* scaling feature: (src << 16) / dst */
-	x_ratio = (width << 16) / width;
-	y_ratio = (height << 16) / height;
-
-	src_x_offset = win_data->fb_x;
-	src_y_offset = win_data->fb_y;
-	dst_x_offset = win_data->crtc_x;
-	dst_y_offset = win_data->crtc_y;
+	x_ratio = (win_data->src_width << 16) / win_data->crtc_width;
+	y_ratio = (win_data->src_height << 16) / win_data->crtc_height;
 
 	if (buf_num == 2) {
 		luma_addr[0] = win_data->dma_addr;
@@ -404,7 +390,7 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	} else {
 		luma_addr[0] = win_data->dma_addr;
 		chroma_addr[0] = win_data->dma_addr
-			+ (full_width * full_height);
+			+ (win_data->fb_width * win_data->fb_height);
 	}
 
 	if (win_data->scan_flags & DRM_MODE_FLAG_INTERLACE) {
@@ -413,8 +399,8 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 			luma_addr[1] = luma_addr[0] + 0x40;
 			chroma_addr[1] = chroma_addr[0] + 0x40;
 		} else {
-			luma_addr[1] = luma_addr[0] + full_width;
-			chroma_addr[1] = chroma_addr[0] + full_width;
+			luma_addr[1] = luma_addr[0] + win_data->fb_width;
+			chroma_addr[1] = chroma_addr[0] + win_data->fb_width;
 		}
 	} else {
 		ctx->interlace = false;
@@ -435,26 +421,26 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	vp_reg_writemask(res, VP_MODE, val, VP_MODE_FMT_MASK);
 
 	/* setting size of input image */
-	vp_reg_write(res, VP_IMG_SIZE_Y, VP_IMG_HSIZE(full_width) |
-		VP_IMG_VSIZE(full_height));
+	vp_reg_write(res, VP_IMG_SIZE_Y, VP_IMG_HSIZE(win_data->fb_width) |
+		VP_IMG_VSIZE(win_data->fb_height));
 	/* chroma height has to reduced by 2 to avoid chroma distorions */
-	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(full_width) |
-		VP_IMG_VSIZE(full_height / 2));
+	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(win_data->fb_width) |
+		VP_IMG_VSIZE(win_data->fb_height / 2));
 
-	vp_reg_write(res, VP_SRC_WIDTH, width);
-	vp_reg_write(res, VP_SRC_HEIGHT, height);
+	vp_reg_write(res, VP_SRC_WIDTH, win_data->src_width);
+	vp_reg_write(res, VP_SRC_HEIGHT, win_data->src_height);
 	vp_reg_write(res, VP_SRC_H_POSITION,
-			VP_SRC_H_POSITION_VAL(src_x_offset));
-	vp_reg_write(res, VP_SRC_V_POSITION, src_y_offset);
+			VP_SRC_H_POSITION_VAL(win_data->fb_x));
+	vp_reg_write(res, VP_SRC_V_POSITION, win_data->fb_y);
 
-	vp_reg_write(res, VP_DST_WIDTH, width);
-	vp_reg_write(res, VP_DST_H_POSITION, dst_x_offset);
+	vp_reg_write(res, VP_DST_WIDTH, win_data->crtc_width);
+	vp_reg_write(res, VP_DST_H_POSITION, win_data->crtc_x);
 	if (ctx->interlace) {
-		vp_reg_write(res, VP_DST_HEIGHT, height / 2);
-		vp_reg_write(res, VP_DST_V_POSITION, dst_y_offset / 2);
+		vp_reg_write(res, VP_DST_HEIGHT, win_data->crtc_height / 2);
+		vp_reg_write(res, VP_DST_V_POSITION, win_data->crtc_y / 2);
 	} else {
-		vp_reg_write(res, VP_DST_HEIGHT, height);
-		vp_reg_write(res, VP_DST_V_POSITION, dst_y_offset);
+		vp_reg_write(res, VP_DST_HEIGHT, win_data->crtc_height);
+		vp_reg_write(res, VP_DST_V_POSITION, win_data->crtc_y);
 	}
 
 	vp_reg_write(res, VP_H_RATIO, x_ratio);
@@ -468,8 +454,8 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	vp_reg_write(res, VP_TOP_C_PTR, chroma_addr[0]);
 	vp_reg_write(res, VP_BOT_C_PTR, chroma_addr[1]);
 
-	mixer_cfg_scan(ctx, mode_height);
-	mixer_cfg_rgb_fmt(ctx, mode_height);
+	mixer_cfg_scan(ctx, win_data->mode_height);
+	mixer_cfg_rgb_fmt(ctx, win_data->mode_height);
 	mixer_cfg_layer(ctx, win, true);
 	mixer_run(ctx);
 
@@ -484,10 +470,8 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 	struct mixer_resources *res = &ctx->mixer_res;
 	unsigned long flags;
 	struct hdmi_win_data *win_data;
-	unsigned int full_width, width, height;
 	unsigned int x_ratio, y_ratio;
 	unsigned int src_x_offset, src_y_offset, dst_x_offset, dst_y_offset;
-	unsigned int mode_width, mode_height;
 	dma_addr_t dma_addr;
 	unsigned int fmt;
 	u32 val;
@@ -510,26 +494,17 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 		fmt = ARGB8888;
 	}
 
-	dma_addr = win_data->dma_addr;
-	full_width = win_data->fb_width;
-	width = win_data->crtc_width;
-	height = win_data->crtc_height;
-	mode_width = win_data->mode_width;
-	mode_height = win_data->mode_height;
-
 	/* 2x scaling feature */
 	x_ratio = 0;
 	y_ratio = 0;
 
-	src_x_offset = win_data->fb_x;
-	src_y_offset = win_data->fb_y;
 	dst_x_offset = win_data->crtc_x;
 	dst_y_offset = win_data->crtc_y;
 
 	/* converting dma address base and source offset */
-	dma_addr = dma_addr
-		+ (src_x_offset * win_data->bpp >> 3)
-		+ (src_y_offset * full_width * win_data->bpp >> 3);
+	dma_addr = win_data->dma_addr
+		+ (win_data->fb_x * win_data->bpp >> 3)
+		+ (win_data->fb_y * win_data->fb_width * win_data->bpp >> 3);
 	src_x_offset = 0;
 	src_y_offset = 0;
 
@@ -546,10 +521,10 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 		MXR_GRP_CFG_FORMAT_VAL(fmt), MXR_GRP_CFG_FORMAT_MASK);
 
 	/* setup geometry */
-	mixer_reg_write(res, MXR_GRAPHIC_SPAN(win), full_width);
+	mixer_reg_write(res, MXR_GRAPHIC_SPAN(win), win_data->fb_width);
 
-	val  = MXR_GRP_WH_WIDTH(width);
-	val |= MXR_GRP_WH_HEIGHT(height);
+	val  = MXR_GRP_WH_WIDTH(win_data->crtc_width);
+	val |= MXR_GRP_WH_HEIGHT(win_data->crtc_height);
 	val |= MXR_GRP_WH_H_SCALE(x_ratio);
 	val |= MXR_GRP_WH_V_SCALE(y_ratio);
 	mixer_reg_write(res, MXR_GRAPHIC_WH(win), val);
@@ -567,8 +542,8 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 	/* set buffer address to mixer */
 	mixer_reg_write(res, MXR_GRAPHIC_BASE(win), dma_addr);
 
-	mixer_cfg_scan(ctx, mode_height);
-	mixer_cfg_rgb_fmt(ctx, mode_height);
+	mixer_cfg_scan(ctx, win_data->mode_height);
+	mixer_cfg_rgb_fmt(ctx, win_data->mode_height);
 	mixer_cfg_layer(ctx, win, true);
 	mixer_run(ctx);
 
@@ -591,6 +566,116 @@ static void vp_win_reset(struct mixer_context *ctx)
 	WARN(tries == 0, "failed to reset Video Processor\n");
 }
 
+static void mixer_win_reset(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	unsigned long flags;
+	u32 val; /* value stored to register */
+
+	spin_lock_irqsave(&res->reg_slock, flags);
+	mixer_vsync_set_update(ctx, false);
+
+	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_DST_HDMI, MXR_CFG_DST_MASK);
+
+	/* set output in RGB888 mode */
+	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_OUT_RGB888, MXR_CFG_OUT_MASK);
+
+	/* 16 beat burst in DMA */
+	mixer_reg_writemask(res, MXR_STATUS, MXR_STATUS_16_BURST,
+		MXR_STATUS_BURST_MASK);
+
+	/* setting default layer priority: layer1 > layer0 > video
+	 * because typical usage scenario would be
+	 * layer1 - OSD
+	 * layer0 - framebuffer
+	 * video - video overlay
+	 */
+	val = MXR_LAYER_CFG_GRP1_VAL(3);
+	val |= MXR_LAYER_CFG_GRP0_VAL(2);
+	val |= MXR_LAYER_CFG_VP_VAL(1);
+	mixer_reg_write(res, MXR_LAYER_CFG, val);
+
+	/* setting background color */
+	mixer_reg_write(res, MXR_BG_COLOR0, 0x008080);
+	mixer_reg_write(res, MXR_BG_COLOR1, 0x008080);
+	mixer_reg_write(res, MXR_BG_COLOR2, 0x008080);
+
+	/* setting graphical layers */
+
+	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
+	val |= MXR_GRP_CFG_WIN_BLEND_EN;
+	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
+
+	/* the same configuration for both layers */
+	mixer_reg_write(res, MXR_GRAPHIC_CFG(0), val);
+
+	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
+	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
+	mixer_reg_write(res, MXR_GRAPHIC_CFG(1), val);
+
+	/* configuration of Video Processor Registers */
+	vp_win_reset(ctx);
+	vp_default_filter(res);
+
+	/* disable all layers */
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP0_ENABLE);
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP1_ENABLE);
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
+
+	mixer_vsync_set_update(ctx, true);
+	spin_unlock_irqrestore(&res->reg_slock, flags);
+}
+
+static void mixer_poweron(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&ctx->mixer_mutex);
+	if (ctx->powered) {
+		mutex_unlock(&ctx->mixer_mutex);
+		return;
+	}
+	ctx->powered = true;
+	mutex_unlock(&ctx->mixer_mutex);
+
+	pm_runtime_get_sync(ctx->dev);
+
+	clk_enable(res->mixer);
+	clk_enable(res->vp);
+	clk_enable(res->sclk_mixer);
+
+	mixer_reg_write(res, MXR_INT_EN, ctx->int_en);
+	mixer_win_reset(ctx);
+}
+
+static void mixer_poweroff(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&ctx->mixer_mutex);
+	if (!ctx->powered)
+		goto out;
+	mutex_unlock(&ctx->mixer_mutex);
+
+	ctx->int_en = mixer_reg_read(res, MXR_INT_EN);
+
+	clk_disable(res->mixer);
+	clk_disable(res->vp);
+	clk_disable(res->sclk_mixer);
+
+	pm_runtime_put_sync(ctx->dev);
+
+	mutex_lock(&ctx->mixer_mutex);
+	ctx->powered = false;
+
+out:
+	mutex_unlock(&ctx->mixer_mutex);
+}
+
 static int mixer_enable_vblank(void *ctx, int pipe)
 {
 	struct mixer_context *mixer_ctx = ctx;
@@ -618,6 +703,27 @@ static void mixer_disable_vblank(void *ctx)
 	mixer_reg_writemask(res, MXR_INT_EN, 0, MXR_INT_EN_VSYNC);
 }
 
+static void mixer_dpms(void *ctx, int mode)
+{
+	struct mixer_context *mixer_ctx = ctx;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		mixer_poweron(mixer_ctx);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		mixer_poweroff(mixer_ctx);
+		break;
+	default:
+		DRM_DEBUG_KMS("unknown dpms mode: %d\n", mode);
+		break;
+	}
+}
+
 static void mixer_win_mode_set(void *ctx,
 			      struct exynos_drm_overlay *overlay)
 {
@@ -643,7 +749,7 @@ static void mixer_win_mode_set(void *ctx,
 		win = MIXER_DEFAULT_WIN;
 
 	if (win < 0 || win > MIXER_WIN_NR) {
-		DRM_ERROR("overlay plane[%d] is wrong\n", win);
+		DRM_ERROR("mixer window[%d] is wrong\n", win);
 		return;
 	}
 
@@ -665,6 +771,8 @@ static void mixer_win_mode_set(void *ctx,
 	win_data->fb_y = overlay->fb_y;
 	win_data->fb_width = overlay->fb_width;
 	win_data->fb_height = overlay->fb_height;
+	win_data->src_width = overlay->src_width;
+	win_data->src_height = overlay->src_height;
 
 	win_data->mode_width = overlay->mode_width;
 	win_data->mode_height = overlay->mode_height;
@@ -672,44 +780,26 @@ static void mixer_win_mode_set(void *ctx,
 	win_data->scan_flags = overlay->scan_flag;
 }
 
-static void mixer_win_commit(void *ctx, int zpos)
+static void mixer_win_commit(void *ctx, int win)
 {
 	struct mixer_context *mixer_ctx = ctx;
-	int win = zpos;
 
 	DRM_DEBUG_KMS("[%d] %s, win: %d\n", __LINE__, __func__, win);
 
-	if (win == DEFAULT_ZPOS)
-		win = MIXER_DEFAULT_WIN;
-
-	if (win < 0 || win > MIXER_WIN_NR) {
-		DRM_ERROR("overlay plane[%d] is wrong\n", win);
-		return;
-	}
-
 	if (win > 1)
 		vp_video_buffer(mixer_ctx, win);
 	else
 		mixer_graph_buffer(mixer_ctx, win);
 }
 
-static void mixer_win_disable(void *ctx, int zpos)
+static void mixer_win_disable(void *ctx, int win)
 {
 	struct mixer_context *mixer_ctx = ctx;
 	struct mixer_resources *res = &mixer_ctx->mixer_res;
 	unsigned long flags;
-	int win = zpos;
 
 	DRM_DEBUG_KMS("[%d] %s, win: %d\n", __LINE__, __func__, win);
 
-	if (win == DEFAULT_ZPOS)
-		win = MIXER_DEFAULT_WIN;
-
-	if (win < 0 || win > MIXER_WIN_NR) {
-		DRM_ERROR("overlay plane[%d] is wrong\n", win);
-		return;
-	}
-
 	spin_lock_irqsave(&res->reg_slock, flags);
 	mixer_vsync_set_update(mixer_ctx, false);
 
@@ -723,6 +813,7 @@ static struct exynos_mixer_ops mixer_ops = {
 	/* manager */
 	.enable_vblank		= mixer_enable_vblank,
 	.disable_vblank		= mixer_disable_vblank,
+	.dpms			= mixer_dpms,
 
 	/* overlay */
 	.win_mode_set		= mixer_win_mode_set,
@@ -773,7 +864,7 @@ static irqreturn_t mixer_irq_handler(int irq, void *arg)
 	struct exynos_drm_hdmi_context *drm_hdmi_ctx = arg;
 	struct mixer_context *ctx = drm_hdmi_ctx->ctx;
 	struct mixer_resources *res = &ctx->mixer_res;
-	u32 val, val_base;
+	u32 val, base, shadow;
 
 	spin_lock(&res->reg_slock);
 
@@ -784,12 +875,14 @@ static irqreturn_t mixer_irq_handler(int irq, void *arg)
 	if (val & MXR_INT_STATUS_VSYNC) {
 		/* interlace scan need to check shadow register */
 		if (ctx->interlace) {
-			val_base = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(0));
-			if (ctx->win_data[0].dma_addr != val_base)
+			base = mixer_reg_read(res, MXR_GRAPHIC_BASE(0));
+			shadow = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(0));
+			if (base != shadow)
 				goto out;
 
-			val_base = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(1));
-			if (ctx->win_data[1].dma_addr != val_base)
+			base = mixer_reg_read(res, MXR_GRAPHIC_BASE(1));
+			shadow = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(1));
+			if (base != shadow)
 				goto out;
 		}
 
@@ -811,117 +904,6 @@ out:
 	return IRQ_HANDLED;
 }
 
-static void mixer_win_reset(struct mixer_context *ctx)
-{
-	struct mixer_resources *res = &ctx->mixer_res;
-	unsigned long flags;
-	u32 val; /* value stored to register */
-
-	spin_lock_irqsave(&res->reg_slock, flags);
-	mixer_vsync_set_update(ctx, false);
-
-	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_DST_HDMI, MXR_CFG_DST_MASK);
-
-	/* set output in RGB888 mode */
-	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_OUT_RGB888, MXR_CFG_OUT_MASK);
-
-	/* 16 beat burst in DMA */
-	mixer_reg_writemask(res, MXR_STATUS, MXR_STATUS_16_BURST,
-		MXR_STATUS_BURST_MASK);
-
-	/* setting default layer priority: layer1 > layer0 > video
-	 * because typical usage scenario would be
-	 * layer1 - OSD
-	 * layer0 - framebuffer
-	 * video - video overlay
-	 */
-	val = MXR_LAYER_CFG_GRP1_VAL(3);
-	val |= MXR_LAYER_CFG_GRP0_VAL(2);
-	val |= MXR_LAYER_CFG_VP_VAL(1);
-	mixer_reg_write(res, MXR_LAYER_CFG, val);
-
-	/* setting background color */
-	mixer_reg_write(res, MXR_BG_COLOR0, 0x008080);
-	mixer_reg_write(res, MXR_BG_COLOR1, 0x008080);
-	mixer_reg_write(res, MXR_BG_COLOR2, 0x008080);
-
-	/* setting graphical layers */
-
-	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
-	val |= MXR_GRP_CFG_WIN_BLEND_EN;
-	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
-
-	/* the same configuration for both layers */
-	mixer_reg_write(res, MXR_GRAPHIC_CFG(0), val);
-
-	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
-	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
-	mixer_reg_write(res, MXR_GRAPHIC_CFG(1), val);
-
-	/* configuration of Video Processor Registers */
-	vp_win_reset(ctx);
-	vp_default_filter(res);
-
-	/* disable all layers */
-	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP0_ENABLE);
-	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP1_ENABLE);
-	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
-
-	mixer_vsync_set_update(ctx, true);
-	spin_unlock_irqrestore(&res->reg_slock, flags);
-}
-
-static void mixer_resource_poweron(struct mixer_context *ctx)
-{
-	struct mixer_resources *res = &ctx->mixer_res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	clk_enable(res->mixer);
-	clk_enable(res->vp);
-	clk_enable(res->sclk_mixer);
-
-	mixer_win_reset(ctx);
-}
-
-static void mixer_resource_poweroff(struct mixer_context *ctx)
-{
-	struct mixer_resources *res = &ctx->mixer_res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	clk_disable(res->mixer);
-	clk_disable(res->vp);
-	clk_disable(res->sclk_mixer);
-}
-
-static int mixer_runtime_resume(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_mixer_context(dev);
-
-	DRM_DEBUG_KMS("resume - start\n");
-
-	mixer_resource_poweron(ctx->ctx);
-
-	return 0;
-}
-
-static int mixer_runtime_suspend(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_mixer_context(dev);
-
-	DRM_DEBUG_KMS("suspend - start\n");
-
-	mixer_resource_poweroff(ctx->ctx);
-
-	return 0;
-}
-
-static const struct dev_pm_ops mixer_pm_ops = {
-	.runtime_suspend = mixer_runtime_suspend,
-	.runtime_resume	 = mixer_runtime_resume,
-};
-
 static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 				 struct platform_device *pdev)
 {
@@ -931,7 +913,6 @@ static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 	struct resource *res;
 	int ret;
 
-	mixer_res->dev = dev;
 	spin_lock_init(&mixer_res->reg_slock);
 
 	mixer_res->mixer = clk_get(dev, "mixer");
@@ -1027,7 +1008,6 @@ fail:
 		clk_put(mixer_res->vp);
 	if (!IS_ERR_OR_NULL(mixer_res->mixer))
 		clk_put(mixer_res->mixer);
-	mixer_res->dev = NULL;
 	return ret;
 }
 
@@ -1035,7 +1015,6 @@ static void mixer_resources_cleanup(struct mixer_context *ctx)
 {
 	struct mixer_resources *res = &ctx->mixer_res;
 
-	disable_irq(res->irq);
 	free_irq(res->irq, ctx);
 
 	iounmap(res->vp_regs);
@@ -1064,6 +1043,9 @@ static int __devinit mixer_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	mutex_init(&ctx->mixer_mutex);
+
+	ctx->dev = &pdev->dev;
 	drm_hdmi_ctx->ctx = (void *)ctx;
 
 	platform_set_drvdata(pdev, drm_hdmi_ctx);
@@ -1076,7 +1058,7 @@ static int __devinit mixer_probe(struct platform_device *pdev)
 	/* register specific callback point to common hdmi. */
 	exynos_mixer_ops_register(&mixer_ops);
 
-	mixer_resource_poweron(ctx);
+	pm_runtime_enable(dev);
 
 	return 0;
 
@@ -1095,12 +1077,27 @@ static int mixer_remove(struct platform_device *pdev)
 
 	dev_info(dev, "remove successful\n");
 
-	mixer_resource_poweroff(ctx);
+	pm_runtime_disable(&pdev->dev);
+
 	mixer_resources_cleanup(ctx);
 
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int mixer_suspend(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *drm_hdmi_ctx = get_mixer_context(dev);
+	struct mixer_context *ctx = drm_hdmi_ctx->ctx;
+
+	mixer_poweroff(ctx);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(mixer_pm_ops, mixer_suspend, NULL);
+
 struct platform_driver mixer_driver = {
 	.driver = {
 		.name = "s5p-mixer",
diff --git a/drivers/gpu/drm/exynos/regs-hdmi.h b/drivers/gpu/drm/exynos/regs-hdmi.h
index 3c04bea842ce..9cc7c5e9718c 100644
--- a/drivers/gpu/drm/exynos/regs-hdmi.h
+++ b/drivers/gpu/drm/exynos/regs-hdmi.h
@@ -138,14 +138,16 @@
 #define HDMI_ASP_MASK			(1 << 2)
 #define HDMI_EN				(1 << 0)
 
+/* HDMI_CON_2 */
+#define HDMI_VID_PREAMBLE_DIS		(1 << 5)
+#define HDMI_GUARD_BAND_DIS		(1 << 1)
+
 /* HDMI_PHY_STATUS */
 #define HDMI_PHY_STATUS_READY		(1 << 0)
 
 /* HDMI_MODE_SEL */
 #define HDMI_MODE_HDMI_EN		(1 << 1)
 #define HDMI_MODE_DVI_EN		(1 << 0)
-#define HDMI_DVI_MODE_EN		(1)
-#define HDMI_DVI_MODE_DIS		(0)
 #define HDMI_MODE_MASK			(3 << 0)
 
 /* HDMI_TG_CMD */
diff --git a/drivers/gpu/drm/gma500/Makefile b/drivers/gpu/drm/gma500/Makefile
index 1583982917ce..abfa2a93f0d0 100644
--- a/drivers/gpu/drm/gma500/Makefile
+++ b/drivers/gpu/drm/gma500/Makefile
@@ -1,7 +1,7 @@
 #
 #	KMS driver for the GMA500
 #
-ccflags-y += -Iinclude/drm
+ccflags-y += -I$(srctree)/include/drm
 
 gma500_gfx-y += gem_glue.o \
 	  accel_2d.o \
@@ -12,7 +12,6 @@ gma500_gfx-y += gem_glue.o \
 	  intel_bios.o \
 	  intel_i2c.o \
 	  intel_gmbus.o \
-	  intel_opregion.o \
 	  mmu.o \
 	  power.o \
 	  psb_drv.o \
@@ -25,6 +24,8 @@ gma500_gfx-y += gem_glue.o \
 	  psb_device.o \
 	  mid_bios.o
 
+gma500_gfx-$(CONFIG_ACPI) +=  opregion.o \
+
 gma500_gfx-$(CONFIG_DRM_GMA3600) +=  cdv_device.o \
 	  cdv_intel_crt.o \
 	  cdv_intel_display.o \
diff --git a/drivers/gpu/drm/gma500/cdv_device.c b/drivers/gpu/drm/gma500/cdv_device.c
index a54cc738926a..9764045428ce 100644
--- a/drivers/gpu/drm/gma500/cdv_device.c
+++ b/drivers/gpu/drm/gma500/cdv_device.c
@@ -49,13 +49,15 @@ static void cdv_disable_vga(struct drm_device *dev)
 static int cdv_output_init(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	drm_mode_create_scaling_mode_property(dev);
+
 	cdv_disable_vga(dev);
 
 	cdv_intel_crt_init(dev, &dev_priv->mode_dev);
 	cdv_intel_lvds_init(dev, &dev_priv->mode_dev);
 
-	/* These bits indicate HDMI not SDVO on CDV, but we don't yet support
-	   the HDMI interface */
+	/* These bits indicate HDMI not SDVO on CDV */
 	if (REG_READ(SDVOB) & SDVO_DETECTED)
 		cdv_hdmi_init(dev, &dev_priv->mode_dev, SDVOB);
 	if (REG_READ(SDVOC) & SDVO_DETECTED)
@@ -66,76 +68,71 @@ static int cdv_output_init(struct drm_device *dev)
 #ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
 
 /*
- *	Poulsbo Backlight Interfaces
+ *	Cedartrail Backlght Interfaces
  */
 
-#define BLC_PWM_PRECISION_FACTOR 100	/* 10000000 */
-#define BLC_PWM_FREQ_CALC_CONSTANT 32
-#define MHz 1000000
-
-#define PSB_BLC_PWM_PRECISION_FACTOR    10
-#define PSB_BLC_MAX_PWM_REG_FREQ        0xFFFE
-#define PSB_BLC_MIN_PWM_REG_FREQ        0x2
-
-#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
-#define PSB_BACKLIGHT_PWM_CTL_SHIFT	(16)
-
-static int cdv_brightness;
 static struct backlight_device *cdv_backlight_device;
 
-static int cdv_get_brightness(struct backlight_device *bd)
+static int cdv_backlight_combination_mode(struct drm_device *dev)
 {
-	/* return locally cached var instead of HW read (due to DPST etc.) */
-	/* FIXME: ideally return actual value in case firmware fiddled with
-	   it */
-	return cdv_brightness;
+	return REG_READ(BLC_PWM_CTL2) & PWM_LEGACY_MODE;
 }
 
-
-static int cdv_backlight_setup(struct drm_device *dev)
+static int cdv_get_brightness(struct backlight_device *bd)
 {
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	unsigned long core_clock;
-	/* u32 bl_max_freq; */
-	/* unsigned long value; */
-	u16 bl_max_freq;
-	uint32_t value;
-	uint32_t blc_pwm_precision_factor;
-
-	/* get bl_max_freq and pol from dev_priv*/
-	if (!dev_priv->lvds_bl) {
-		dev_err(dev->dev, "Has no valid LVDS backlight info\n");
-		return -ENOENT;
-	}
-	bl_max_freq = dev_priv->lvds_bl->freq;
-	blc_pwm_precision_factor = PSB_BLC_PWM_PRECISION_FACTOR;
+	struct drm_device *dev = bl_get_data(bd);
+	u32 val = REG_READ(BLC_PWM_CTL) & BACKLIGHT_DUTY_CYCLE_MASK;
 
-	core_clock = dev_priv->core_freq;
+	if (cdv_backlight_combination_mode(dev)) {
+		u8 lbpc;
 
-	value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
-	value *= blc_pwm_precision_factor;
-	value /= bl_max_freq;
-	value /= blc_pwm_precision_factor;
+		val &= ~1;
+		pci_read_config_byte(dev->pdev, 0xF4, &lbpc);
+		val *= lbpc;
+	}
+	return val;
+}
 
-	if (value > (unsigned long long)PSB_BLC_MAX_PWM_REG_FREQ ||
-		 value < (unsigned long long)PSB_BLC_MIN_PWM_REG_FREQ)
-				return -ERANGE;
-	else {
-		/* FIXME */
+static u32 cdv_get_max_backlight(struct drm_device *dev)
+{
+	u32 max = REG_READ(BLC_PWM_CTL);
+
+	if (max == 0) {
+		DRM_DEBUG_KMS("LVDS Panel PWM value is 0!\n");
+		/* i915 does this, I believe which means that we should not
+		 * smash PWM control as firmware will take control of it. */
+		return 1;
 	}
-	return 0;
+
+	max >>= 16;
+	if (cdv_backlight_combination_mode(dev))
+		max *= 0xff;
+	return max;
 }
 
 static int cdv_set_brightness(struct backlight_device *bd)
 {
+	struct drm_device *dev = bl_get_data(bd);
 	int level = bd->props.brightness;
+	u32 blc_pwm_ctl;
 
 	/* Percentage 1-100% being valid */
 	if (level < 1)
 		level = 1;
 
-	/*cdv_intel_lvds_set_brightness(dev, level); FIXME */
-	cdv_brightness = level;
+	if (cdv_backlight_combination_mode(dev)) {
+		u32 max = cdv_get_max_backlight(dev);
+		u8 lbpc;
+
+		lbpc = level * 0xfe / max + 1;
+		level /= lbpc;
+
+		pci_write_config_byte(dev->pdev, 0xF4, lbpc);
+	}
+
+	blc_pwm_ctl = REG_READ(BLC_PWM_CTL) & ~BACKLIGHT_DUTY_CYCLE_MASK;
+	REG_WRITE(BLC_PWM_CTL, (blc_pwm_ctl |
+				(level << BACKLIGHT_DUTY_CYCLE_SHIFT)));
 	return 0;
 }
 
@@ -147,7 +144,6 @@ static const struct backlight_ops cdv_ops = {
 static int cdv_backlight_init(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	int ret;
 	struct backlight_properties props;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
@@ -159,14 +155,9 @@ static int cdv_backlight_init(struct drm_device *dev)
 	if (IS_ERR(cdv_backlight_device))
 		return PTR_ERR(cdv_backlight_device);
 
-	ret = cdv_backlight_setup(dev);
-	if (ret < 0) {
-		backlight_device_unregister(cdv_backlight_device);
-		cdv_backlight_device = NULL;
-		return ret;
-	}
-	cdv_backlight_device->props.brightness = 100;
-	cdv_backlight_device->props.max_brightness = 100;
+	cdv_backlight_device->props.brightness =
+			cdv_get_brightness(cdv_backlight_device);
+	cdv_backlight_device->props.max_brightness = cdv_get_max_backlight(dev);
 	backlight_update_status(cdv_backlight_device);
 	dev_priv->backlight_device = cdv_backlight_device;
 	return 0;
@@ -238,6 +229,19 @@ static void cdv_init_pm(struct drm_device *dev)
 	dev_err(dev->dev, "GPU: power management timed out.\n");
 }
 
+static void cdv_errata(struct drm_device *dev)
+{
+	/* Disable bonus launch.
+	 *	CPU and GPU competes for memory and display misses updates and
+	 *	flickers. Worst with dual core, dual displays.
+	 *
+	 *	Fixes were done to Win 7 gfx driver to disable a feature called
+	 *	Bonus Launch to work around the issue, by degrading
+	 *	performance.
+	 */
+	 CDV_MSG_WRITE32(3, 0x30, 0x08027108);
+}
+
 /**
  *	cdv_save_display_registers	-	save registers lost on suspend
  *	@dev: our DRM device
@@ -251,7 +255,7 @@ static int cdv_save_display_registers(struct drm_device *dev)
 	struct psb_save_area *regs = &dev_priv->regs;
 	struct drm_connector *connector;
 
-	dev_info(dev->dev, "Saving GPU registers.\n");
+	dev_dbg(dev->dev, "Saving GPU registers.\n");
 
 	pci_read_config_byte(dev->pdev, 0xF4, &regs->cdv.saveLBB);
 
@@ -355,7 +359,7 @@ static int cdv_restore_display_registers(struct drm_device *dev)
 	REG_WRITE(PSB_INT_MASK_R, regs->cdv.saveIMR);
 
 	/* Fix arbitration bug */
-	CDV_MSG_WRITE32(3, 0x30, 0x08027108);
+	cdv_errata(dev);
 
 	drm_mode_config_reset(dev);
 
@@ -447,13 +451,106 @@ static void cdv_get_core_freq(struct drm_device *dev)
 	}
 }
 
+static void cdv_hotplug_work_func(struct work_struct *work)
+{
+        struct drm_psb_private *dev_priv = container_of(work, struct drm_psb_private,
+							hotplug_work);                 
+        struct drm_device *dev = dev_priv->dev;
+
+        /* Just fire off a uevent and let userspace tell us what to do */
+        drm_helper_hpd_irq_event(dev);
+}                       
+
+/* The core driver has received a hotplug IRQ. We are in IRQ context
+   so extract the needed information and kick off queued processing */
+   
+static int cdv_hotplug_event(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	schedule_work(&dev_priv->hotplug_work);
+	REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	return 1;
+}
+
+static void cdv_hotplug_enable(struct drm_device *dev, bool on)
+{
+	if (on) {
+		u32 hotplug = REG_READ(PORT_HOTPLUG_EN);
+		hotplug |= HDMIB_HOTPLUG_INT_EN | HDMIC_HOTPLUG_INT_EN |
+			   HDMID_HOTPLUG_INT_EN | CRT_HOTPLUG_INT_EN;
+		REG_WRITE(PORT_HOTPLUG_EN, hotplug);
+	}  else {
+		REG_WRITE(PORT_HOTPLUG_EN, 0);
+		REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	}	
+}
+
+/* Cedarview */
+static const struct psb_offset cdv_regmap[2] = {
+	{
+		.fp0 = FPA0,
+		.fp1 = FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = DPLL_A,
+		.dpll_md = DPLL_A_MD,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.base = DSPABASE,
+		.surf = DSPASURF,
+		.addr = DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = FPB0,
+		.fp1 = FPB1,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = DPLL_B,
+		.dpll_md = DPLL_B_MD,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.base = DSPBBASE,
+		.surf = DSPBSURF,
+		.addr = DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	}
+};
+
 static int cdv_chip_setup(struct drm_device *dev)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	INIT_WORK(&dev_priv->hotplug_work, cdv_hotplug_work_func);
+
+	if (pci_enable_msi(dev->pdev))
+		dev_warn(dev->dev, "Enabling MSI failed!\n");
+	dev_priv->regmap = cdv_regmap;
 	cdv_get_core_freq(dev);
-	gma_intel_opregion_init(dev);
+	psb_intel_opregion_init(dev);
 	psb_intel_init_bios(dev);
-	REG_WRITE(PORT_HOTPLUG_EN, 0);
-	REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	cdv_hotplug_enable(dev, false);
 	return 0;
 }
 
@@ -464,13 +561,19 @@ const struct psb_ops cdv_chip_ops = {
 	.accel_2d = 0,
 	.pipes = 2,
 	.crtcs = 2,
+	.hdmi_mask = (1 << 0) | (1 << 1),
+	.lvds_mask = (1 << 1),
+	.cursor_needs_phys = 0,
 	.sgx_offset = MRST_SGX_OFFSET,
 	.chip_setup = cdv_chip_setup,
+	.errata = cdv_errata,
 
 	.crtc_helper = &cdv_intel_helper_funcs,
 	.crtc_funcs = &cdv_intel_crtc_funcs,
 
 	.output_init = cdv_output_init,
+	.hotplug = cdv_hotplug_event,
+	.hotplug_enable = cdv_hotplug_enable,
 
 #ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
 	.backlight_init = cdv_backlight_init,
diff --git a/drivers/gpu/drm/gma500/cdv_intel_crt.c b/drivers/gpu/drm/gma500/cdv_intel_crt.c
index a71a6cd95bdd..187422018601 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_crt.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_crt.c
@@ -67,8 +67,6 @@ static void cdv_intel_crt_dpms(struct drm_encoder *encoder, int mode)
 static int cdv_intel_crt_mode_valid(struct drm_connector *connector,
 				struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
-	int max_clock = 0;
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
 		return MODE_NO_DBLESCAN;
 
@@ -77,18 +75,9 @@ static int cdv_intel_crt_mode_valid(struct drm_connector *connector,
 		return MODE_CLOCK_LOW;
 
 	/* The max clock for CDV is 355 instead of 400 */
-	max_clock = 355000;
-	if (mode->clock > max_clock)
+	if (mode->clock > 355000)
 		return MODE_CLOCK_HIGH;
 
-	if (mode->hdisplay > 1680 || mode->vdisplay > 1050)
-		return MODE_PANEL;
-
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
@@ -156,13 +145,7 @@ static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
 	struct drm_device *dev = connector->dev;
 	u32 hotplug_en;
 	int i, tries = 0, ret = false;
-	u32 adpa_orig;
-
-	/* disable the DAC when doing the hotplug detection */
-
-	adpa_orig = REG_READ(ADPA);
-
-	REG_WRITE(ADPA, adpa_orig & ~(ADPA_DAC_ENABLE));
+	u32 orig;
 
 	/*
 	 * On a CDV thep, CRT detect sequence need to be done twice
@@ -170,7 +153,7 @@ static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
 	 */
 	tries = 2;
 
-	hotplug_en = REG_READ(PORT_HOTPLUG_EN);
+	orig = hotplug_en = REG_READ(PORT_HOTPLUG_EN);
 	hotplug_en &= ~(CRT_HOTPLUG_DETECT_MASK);
 	hotplug_en |= CRT_HOTPLUG_FORCE_DETECT;
 
@@ -195,8 +178,11 @@ static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
 	    CRT_HOTPLUG_MONITOR_NONE)
 		ret = true;
 
-	/* Restore the saved ADPA */
-	REG_WRITE(ADPA, adpa_orig);
+	 /* clear the interrupt we just generated, if any */
+	REG_WRITE(PORT_HOTPLUG_STAT, CRT_HOTPLUG_INT_STATUS);
+
+	/* and put the bits back */
+	REG_WRITE(PORT_HOTPLUG_EN, orig);
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_display.c b/drivers/gpu/drm/gma500/cdv_intel_display.c
index be8455919b33..c3e9a0f701df 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_display.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_display.c
@@ -216,22 +216,22 @@ static void cdv_sb_reset(struct drm_device *dev)
  */
 static int
 cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
-			       struct cdv_intel_clock_t *clock)
+			       struct cdv_intel_clock_t *clock, bool is_lvds)
 {
-	struct psb_intel_crtc *psb_crtc =
-				to_psb_intel_crtc(crtc);
+	struct psb_intel_crtc *psb_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_crtc->pipe;
 	u32 m, n_vco, p;
 	int ret = 0;
 	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+	int ref_sfr = (pipe == 0) ? SB_REF_DPLLA : SB_REF_DPLLB;
 	u32 ref_value;
+	u32 lane_reg, lane_value;
 
 	cdv_sb_reset(dev);
 
-	if ((REG_READ(dpll_reg) & DPLL_SYNCLOCK_ENABLE) == 0) {
-		DRM_ERROR("Attempting to set DPLL with refclk disabled\n");
-		return -EBUSY;
-	}
+	REG_WRITE(dpll_reg, DPLL_SYNCLOCK_ENABLE | DPLL_VGA_MODE_DIS);
+
+	udelay(100);
 
 	/* Follow the BIOS and write the REF/SFR Register. Hardcoded value */
 	ref_value = 0x68A701;
@@ -241,6 +241,35 @@ cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
 	/* We don't know what the other fields of these regs are, so
 	 * leave them in place.
 	 */
+	/* 
+	 * The BIT 14:13 of 0x8010/0x8030 is used to select the ref clk
+	 * for the pipe A/B. Display spec 1.06 has wrong definition.
+	 * Correct definition is like below:
+	 *
+	 * refclka mean use clock from same PLL
+	 *
+	 * if DPLLA sets 01 and DPLLB sets 01, they use clock from their pll
+	 *
+	 * if DPLLA sets 01 and DPLLB sets 02, both use clk from DPLLA
+	 *
+	 */  
+	ret = cdv_sb_read(dev, ref_sfr, &ref_value);
+	if (ret)
+		return ret;
+	ref_value &= ~(REF_CLK_MASK);
+
+	/* use DPLL_A for pipeB on CRT/HDMI */
+	if (pipe == 1 && !is_lvds) {
+		DRM_DEBUG_KMS("use DPLLA for pipe B\n");
+		ref_value |= REF_CLK_DPLLA;
+	} else {
+		DRM_DEBUG_KMS("use their DPLL for pipe A/B\n");
+		ref_value |= REF_CLK_DPLL;
+	}
+	ret = cdv_sb_write(dev, ref_sfr, ref_value);
+	if (ret)
+		return ret;
+
 	ret = cdv_sb_read(dev, SB_M(pipe), &m);
 	if (ret)
 		return ret;
@@ -307,36 +336,29 @@ cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
 	if (ret)
 		return ret;
 
-	/* always Program the Lane Register for the Pipe A*/
-	if (pipe == 0) {
-		/* Program the Lane0/1 for HDMI B */
-		u32 lane_reg, lane_value;
-
-		lane_reg = PSB_LANE0;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-
-		lane_reg = PSB_LANE1;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-
-		/* Program the Lane2/3 for HDMI C */
-		lane_reg = PSB_LANE2;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-
-		lane_reg = PSB_LANE3;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-	}
+	lane_reg = PSB_LANE0;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
+
+	lane_reg = PSB_LANE1;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
+
+	lane_reg = PSB_LANE2;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
+
+	lane_reg = PSB_LANE3;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
 
 	return 0;
 }
@@ -480,14 +502,12 @@ static int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
 			    int x, int y, struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
-	int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
-	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
-	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
 	u32 dspcntr;
 	int ret = 0;
 
@@ -509,9 +529,9 @@ static int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
 	start = psbfb->gtt->offset;
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
 
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -533,15 +553,15 @@ static int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
 		ret = -EINVAL;
 		goto psb_intel_pipe_set_base_exit;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	dev_dbg(dev->dev,
 		"Writing base %08lX %08lX %d %d\n", start, offset, x, y);
 
-	REG_WRITE(dspbase, offset);
-	REG_READ(dspbase);
-	REG_WRITE(dspsurf, start);
-	REG_READ(dspsurf);
+	REG_WRITE(map->base, offset);
+	REG_READ(map->base);
+	REG_WRITE(map->surf, start);
+	REG_READ(map->surf);
 
 psb_intel_pipe_cleaner:
 	/* If there was a previous display we can now unpin it */
@@ -553,6 +573,199 @@ psb_intel_pipe_set_base_exit:
 	return ret;
 }
 
+#define		FIFO_PIPEA		(1 << 0)
+#define		FIFO_PIPEB		(1 << 1)
+
+static bool cdv_intel_pipe_enabled(struct drm_device *dev, int pipe)
+{
+	struct drm_crtc *crtc;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = NULL;
+
+	crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+	psb_intel_crtc = to_psb_intel_crtc(crtc);
+
+	if (crtc->fb == NULL || !psb_intel_crtc->active)
+		return false;
+	return true;
+}
+
+static bool cdv_intel_single_pipe_active (struct drm_device *dev)
+{
+	uint32_t pipe_enabled = 0;
+
+	if (cdv_intel_pipe_enabled(dev, 0))
+		pipe_enabled |= FIFO_PIPEA;
+
+	if (cdv_intel_pipe_enabled(dev, 1))
+		pipe_enabled |= FIFO_PIPEB;
+
+
+	DRM_DEBUG_KMS("pipe enabled %x\n", pipe_enabled);
+
+	if (pipe_enabled == FIFO_PIPEA || pipe_enabled == FIFO_PIPEB)
+		return true;
+	else
+		return false;
+}
+
+static bool is_pipeb_lvds(struct drm_device *dev, struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *connector;
+
+	if (psb_intel_crtc->pipe != 1)
+		return false;
+
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+		if (!connector->encoder
+		    || connector->encoder->crtc != crtc)
+			continue;
+
+		if (psb_intel_encoder->type == INTEL_OUTPUT_LVDS)
+			return true;
+	}
+
+	return false;
+}
+
+static void cdv_intel_disable_self_refresh (struct drm_device *dev)
+{
+	if (REG_READ(FW_BLC_SELF) & FW_BLC_SELF_EN) {
+
+		/* Disable self-refresh before adjust WM */
+		REG_WRITE(FW_BLC_SELF, (REG_READ(FW_BLC_SELF) & ~FW_BLC_SELF_EN));
+		REG_READ(FW_BLC_SELF);
+
+		cdv_intel_wait_for_vblank(dev);
+
+		/* Cedarview workaround to write ovelay plane, which force to leave
+		 * MAX_FIFO state.
+		 */
+		REG_WRITE(OV_OVADD, 0/*dev_priv->ovl_offset*/);
+		REG_READ(OV_OVADD);
+
+		cdv_intel_wait_for_vblank(dev);
+	}
+
+}
+
+static void cdv_intel_update_watermark (struct drm_device *dev, struct drm_crtc *crtc)
+{
+
+	if (cdv_intel_single_pipe_active(dev)) {
+		u32 fw;
+
+		fw = REG_READ(DSPFW1);
+		fw &= ~DSP_FIFO_SR_WM_MASK;
+		fw |= (0x7e << DSP_FIFO_SR_WM_SHIFT);
+		fw &= ~CURSOR_B_FIFO_WM_MASK;
+		fw |= (0x4 << CURSOR_B_FIFO_WM_SHIFT);
+		REG_WRITE(DSPFW1, fw);
+
+		fw = REG_READ(DSPFW2);
+		fw &= ~CURSOR_A_FIFO_WM_MASK;
+		fw |= (0x6 << CURSOR_A_FIFO_WM_SHIFT);
+		fw &= ~DSP_PLANE_C_FIFO_WM_MASK;
+		fw |= (0x8 << DSP_PLANE_C_FIFO_WM_SHIFT);
+		REG_WRITE(DSPFW2, fw);
+
+		REG_WRITE(DSPFW3, 0x36000000);
+
+		/* ignore FW4 */
+
+		if (is_pipeb_lvds(dev, crtc)) {
+			REG_WRITE(DSPFW5, 0x00040330);
+		} else {
+			fw = (3 << DSP_PLANE_B_FIFO_WM1_SHIFT) |
+			     (4 << DSP_PLANE_A_FIFO_WM1_SHIFT) |
+			     (3 << CURSOR_B_FIFO_WM1_SHIFT) |
+			     (4 << CURSOR_FIFO_SR_WM1_SHIFT);
+			REG_WRITE(DSPFW5, fw);
+		}
+
+		REG_WRITE(DSPFW6, 0x10);
+
+		cdv_intel_wait_for_vblank(dev);
+
+		/* enable self-refresh for single pipe active */
+		REG_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
+		REG_READ(FW_BLC_SELF);
+		cdv_intel_wait_for_vblank(dev);
+
+	} else {
+
+		/* HW team suggested values... */
+		REG_WRITE(DSPFW1, 0x3f880808);
+		REG_WRITE(DSPFW2, 0x0b020202);
+		REG_WRITE(DSPFW3, 0x24000000);
+		REG_WRITE(DSPFW4, 0x08030202);
+		REG_WRITE(DSPFW5, 0x01010101);
+		REG_WRITE(DSPFW6, 0x1d0);
+
+		cdv_intel_wait_for_vblank(dev);
+
+		cdv_intel_disable_self_refresh(dev);
+	
+	}
+}
+
+/** Loads the palette/gamma unit for the CRTC with the prepared values */
+static void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int palreg = PALETTE_A;
+	int i;
+
+	/* The clocks have to be on to load the palette. */
+	if (!crtc->enabled)
+		return;
+
+	switch (psb_intel_crtc->pipe) {
+	case 0:
+		break;
+	case 1:
+		palreg = PALETTE_B;
+		break;
+	case 2:
+		palreg = PALETTE_C;
+		break;
+	default:
+		dev_err(dev->dev, "Illegal Pipe Number.\n");
+		return;
+	}
+
+	if (gma_power_begin(dev, false)) {
+		for (i = 0; i < 256; i++) {
+			REG_WRITE(palreg + 4 * i,
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]));
+		}
+		gma_power_end(dev);
+	} else {
+		for (i = 0; i < 256; i++) {
+			dev_priv->regs.pipe[0].palette[i] =
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]);
+		}
+
+	}
+}
+
 /**
  * Sets the power management mode of the pipe and plane.
  *
@@ -562,62 +775,80 @@ psb_intel_pipe_set_base_exit:
 static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	/* XXX: When our outputs are all unaware of DPMS modes other than off
 	 * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
 	 */
+	cdv_intel_disable_self_refresh(dev);
+
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
+		if (psb_intel_crtc->active)
+			return;
+
+		psb_intel_crtc->active = true;
+
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
 		}
 
 		/* Jim Bish - switch plan and pipe per scott */
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		udelay(150);
 
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+			REG_WRITE(map->conf, temp | PIPEACONF_ENABLE);
+
+		temp = REG_READ(map->status);
+		temp &= ~(0xFFFF);
+		temp |= PIPE_FIFO_UNDERRUN;
+		REG_WRITE(map->status, temp);
+		REG_READ(map->status);
 
-		psb_intel_crtc_load_lut(crtc);
+		cdv_intel_update_watermark(dev, crtc);
+		cdv_intel_crtc_load_lut(crtc);
 
 		/* Give the overlay scaler a chance to enable
 		 * if it's on this pipe */
 		/* psb_intel_crtc_dpms_video(crtc, true); TODO */
+		psb_intel_crtc->crtc_enable = true;
 		break;
 	case DRM_MODE_DPMS_OFF:
+		if (!psb_intel_crtc->active)
+			return;
+
+		psb_intel_crtc->active = false;
+
 		/* Give the overlay scaler a chance to disable
 		 * if it's on this pipe */
 		/* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
@@ -627,14 +858,15 @@ static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 
 		/* Jim Bish - changed pipe/plane here as well. */
 
+		drm_vblank_off(dev, pipe);
 		/* Wait for vblank for the disable to take effect */
 		cdv_intel_wait_for_vblank(dev);
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp & ~PIPEACONF_ENABLE);
+			REG_READ(map->conf);
 		}
 
 		/* Wait for vblank for the disable to take effect. */
@@ -643,23 +875,25 @@ static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 		udelay(150);
 
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) != 0) {
-			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 		}
 
 		/* Wait for the clocks to turn off. */
 		udelay(150);
+		cdv_intel_update_watermark(dev, crtc);
+		psb_intel_crtc->crtc_enable = false;
 		break;
 	}
 	/*Set FIFO Watermarks*/
@@ -709,21 +943,10 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 			       struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dpll_md_reg = (psb_intel_crtc->pipe == 0) ? DPLL_A_MD : DPLL_B_MD;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
-	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
-	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
-	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
-	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
-	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
-	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
-	int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
-	int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
-	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk;
 	struct cdv_intel_clock_t clock;
 	u32 dpll = 0, dspcntr, pipeconf;
@@ -757,13 +980,18 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 		}
 	}
 
-	refclk = 96000;
-
-	/* Hack selection about ref clk for CRT */
-	/* Select 27MHz as the reference clk for HDMI */
-	if (is_crt || is_hdmi)
+	if (dev_priv->dplla_96mhz)
+		/* low-end sku, 96/100 mhz */
+		refclk = 96000;
+	else
+		/* high-end sku, 27/100 mhz */
 		refclk = 27000;
 
+	if (is_lvds && dev_priv->lvds_use_ssc) {
+		refclk = dev_priv->lvds_ssc_freq * 1000;
+		DRM_DEBUG_KMS("Use SSC reference clock %d Mhz\n", dev_priv->lvds_ssc_freq);
+	}
+
 	drm_mode_debug_printmodeline(adjusted_mode);
 
 	ok = cdv_intel_find_best_PLL(crtc, adjusted_mode->clock, refclk,
@@ -779,18 +1007,17 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 /*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
 		dpll |= 3;
 	}
-		dpll |= PLL_REF_INPUT_DREFCLK;
+/*		dpll |= PLL_REF_INPUT_DREFCLK; */
 
 	dpll |= DPLL_SYNCLOCK_ENABLE;
-	dpll |= DPLL_VGA_MODE_DIS;
-	if (is_lvds)
+/*	if (is_lvds)
 		dpll |= DPLLB_MODE_LVDS;
 	else
-		dpll |= DPLLB_MODE_DAC_SERIAL;
+		dpll |= DPLLB_MODE_DAC_SERIAL; */
 	/* dpll |= (2 << 11); */
 
 	/* setup pipeconf */
-	pipeconf = REG_READ(pipeconf_reg);
+	pipeconf = REG_READ(map->conf);
 
 	/* Set up the display plane register */
 	dspcntr = DISPPLANE_GAMMA_ENABLE;
@@ -803,10 +1030,10 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 	dspcntr |= DISPLAY_PLANE_ENABLE;
 	pipeconf |= PIPEACONF_ENABLE;
 
-	REG_WRITE(dpll_reg, dpll | DPLL_VGA_MODE_DIS | DPLL_SYNCLOCK_ENABLE);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll, dpll | DPLL_VGA_MODE_DIS | DPLL_SYNCLOCK_ENABLE);
+	REG_READ(map->dpll);
 
-	cdv_dpll_set_clock_cdv(dev, crtc, &clock);
+	cdv_dpll_set_clock_cdv(dev, crtc, &clock, is_lvds);
 
 	udelay(150);
 
@@ -848,48 +1075,48 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 	DRM_DEBUG_KMS("Mode for pipe %c:\n", pipe == 0 ? 'A' : 'B');
 	drm_mode_debug_printmodeline(mode);
 
-	REG_WRITE(dpll_reg,
-		(REG_READ(dpll_reg) & ~DPLL_LOCK) | DPLL_VCO_ENABLE);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll,
+		(REG_READ(map->dpll) & ~DPLL_LOCK) | DPLL_VCO_ENABLE);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150); /* 42 usec w/o calibration, 110 with.  rounded up. */
 
-	if (!(REG_READ(dpll_reg) & DPLL_LOCK)) {
+	if (!(REG_READ(map->dpll) & DPLL_LOCK)) {
 		dev_err(dev->dev, "Failed to get DPLL lock\n");
 		return -EBUSY;
 	}
 
 	{
 		int sdvo_pixel_multiply = adjusted_mode->clock / mode->clock;
-		REG_WRITE(dpll_md_reg, (0 << DPLL_MD_UDI_DIVIDER_SHIFT) | ((sdvo_pixel_multiply - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT));
+		REG_WRITE(map->dpll_md, (0 << DPLL_MD_UDI_DIVIDER_SHIFT) | ((sdvo_pixel_multiply - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT));
 	}
 
-	REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+	REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 		  ((adjusted_mode->crtc_htotal - 1) << 16));
-	REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+	REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 		  ((adjusted_mode->crtc_hblank_end - 1) << 16));
-	REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+	REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 		  ((adjusted_mode->crtc_hsync_end - 1) << 16));
-	REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+	REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 		  ((adjusted_mode->crtc_vtotal - 1) << 16));
-	REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+	REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 		  ((adjusted_mode->crtc_vblank_end - 1) << 16));
-	REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+	REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 		  ((adjusted_mode->crtc_vsync_end - 1) << 16));
 	/* pipesrc and dspsize control the size that is scaled from,
 	 * which should always be the user's requested size.
 	 */
-	REG_WRITE(dspsize_reg,
+	REG_WRITE(map->size,
 		  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
-	REG_WRITE(dsppos_reg, 0);
-	REG_WRITE(pipesrc_reg,
+	REG_WRITE(map->pos, 0);
+	REG_WRITE(map->src,
 		  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
-	REG_WRITE(pipeconf_reg, pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, pipeconf);
+	REG_READ(map->conf);
 
 	cdv_intel_wait_for_vblank(dev);
 
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	/* Flush the plane changes */
 	{
@@ -903,58 +1130,6 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 	return 0;
 }
 
-/** Loads the palette/gamma unit for the CRTC with the prepared values */
-static void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_psb_private *dev_priv =
-				(struct drm_psb_private *)dev->dev_private;
-	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
-	int palreg = PALETTE_A;
-	int i;
-
-	/* The clocks have to be on to load the palette. */
-	if (!crtc->enabled)
-		return;
-
-	switch (psb_intel_crtc->pipe) {
-	case 0:
-		break;
-	case 1:
-		palreg = PALETTE_B;
-		break;
-	case 2:
-		palreg = PALETTE_C;
-		break;
-	default:
-		dev_err(dev->dev, "Illegal Pipe Number.\n");
-		return;
-	}
-
-	if (gma_power_begin(dev, false)) {
-		for (i = 0; i < 256; i++) {
-			REG_WRITE(palreg + 4 * i,
-				  ((psb_intel_crtc->lut_r[i] +
-				  psb_intel_crtc->lut_adj[i]) << 16) |
-				  ((psb_intel_crtc->lut_g[i] +
-				  psb_intel_crtc->lut_adj[i]) << 8) |
-				  (psb_intel_crtc->lut_b[i] +
-				  psb_intel_crtc->lut_adj[i]));
-		}
-		gma_power_end(dev);
-	} else {
-		for (i = 0; i < 256; i++) {
-			dev_priv->regs.psb.save_palette_a[i] =
-				  ((psb_intel_crtc->lut_r[i] +
-				  psb_intel_crtc->lut_adj[i]) << 16) |
-				  ((psb_intel_crtc->lut_g[i] +
-				  psb_intel_crtc->lut_adj[i]) << 8) |
-				  (psb_intel_crtc->lut_b[i] +
-				  psb_intel_crtc->lut_adj[i]);
-		}
-
-	}
-}
 
 /**
  * Save HW states of giving crtc
@@ -962,11 +1137,10 @@ static void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
 static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private *dev_priv =
-			(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -975,25 +1149,25 @@ static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 		return;
 	}
 
-	crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
-	crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
-	crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
-	crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
-	crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
-	crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
-	crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
-	crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
-	crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
-	crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
-	crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
-	crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
-	crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+	crtc_state->saveDSPCNTR = REG_READ(map->cntr);
+	crtc_state->savePIPECONF = REG_READ(map->conf);
+	crtc_state->savePIPESRC = REG_READ(map->src);
+	crtc_state->saveFP0 = REG_READ(map->fp0);
+	crtc_state->saveFP1 = REG_READ(map->fp1);
+	crtc_state->saveDPLL = REG_READ(map->dpll);
+	crtc_state->saveHTOTAL = REG_READ(map->htotal);
+	crtc_state->saveHBLANK = REG_READ(map->hblank);
+	crtc_state->saveHSYNC = REG_READ(map->hsync);
+	crtc_state->saveVTOTAL = REG_READ(map->vtotal);
+	crtc_state->saveVBLANK = REG_READ(map->vblank);
+	crtc_state->saveVSYNC = REG_READ(map->vsync);
+	crtc_state->saveDSPSTRIDE = REG_READ(map->stride);
 
 	/*NOTE: DSPSIZE DSPPOS only for psb*/
-	crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
-	crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+	crtc_state->saveDSPSIZE = REG_READ(map->size);
+	crtc_state->saveDSPPOS = REG_READ(map->pos);
 
-	crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+	crtc_state->saveDSPBASE = REG_READ(map->base);
 
 	DRM_DEBUG("(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
 			crtc_state->saveDSPCNTR,
@@ -1014,7 +1188,7 @@ static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 			crtc_state->saveDSPBASE
 		);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
 }
@@ -1025,12 +1199,10 @@ static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private * dev_priv =
-				(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	/* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -1041,23 +1213,23 @@ static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
 
 	DRM_DEBUG(
 		"current:(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
-		REG_READ(pipeA ? DSPACNTR : DSPBCNTR),
-		REG_READ(pipeA ? PIPEACONF : PIPEBCONF),
-		REG_READ(pipeA ? PIPEASRC : PIPEBSRC),
-		REG_READ(pipeA ? FPA0 : FPB0),
-		REG_READ(pipeA ? FPA1 : FPB1),
-		REG_READ(pipeA ? DPLL_A : DPLL_B),
-		REG_READ(pipeA ? HTOTAL_A : HTOTAL_B),
-		REG_READ(pipeA ? HBLANK_A : HBLANK_B),
-		REG_READ(pipeA ? HSYNC_A : HSYNC_B),
-		REG_READ(pipeA ? VTOTAL_A : VTOTAL_B),
-		REG_READ(pipeA ? VBLANK_A : VBLANK_B),
-		REG_READ(pipeA ? VSYNC_A : VSYNC_B),
-		REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE),
-		REG_READ(pipeA ? DSPASIZE : DSPBSIZE),
-		REG_READ(pipeA ? DSPAPOS : DSPBPOS),
-		REG_READ(pipeA ? DSPABASE : DSPBBASE)
-		);
+		REG_READ(map->cntr),
+		REG_READ(map->conf),
+		REG_READ(map->src),
+		REG_READ(map->fp0),
+		REG_READ(map->fp1),
+		REG_READ(map->dpll),
+		REG_READ(map->htotal),
+		REG_READ(map->hblank),
+		REG_READ(map->hsync),
+		REG_READ(map->vtotal),
+		REG_READ(map->vblank),
+		REG_READ(map->vsync),
+		REG_READ(map->stride),
+		REG_READ(map->size),
+		REG_READ(map->pos),
+		REG_READ(map->base)
+	);
 
 	DRM_DEBUG(
 		"saved: (%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
@@ -1077,51 +1249,51 @@ static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
 		crtc_state->saveDSPSIZE,
 		crtc_state->saveDSPPOS,
 		crtc_state->saveDSPBASE
-		);
+	);
 
 
 	if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
-		REG_WRITE(pipeA ? DPLL_A : DPLL_B,
-			crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
-		REG_READ(pipeA ? DPLL_A : DPLL_B);
+		REG_WRITE(map->dpll,
+				crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
+		REG_READ(map->dpll);
 		DRM_DEBUG("write dpll: %x\n",
-				REG_READ(pipeA ? DPLL_A : DPLL_B));
+				REG_READ(map->dpll));
 		udelay(150);
 	}
 
-	REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
-	REG_READ(pipeA ? FPA0 : FPB0);
+	REG_WRITE(map->fp0, crtc_state->saveFP0);
+	REG_READ(map->fp0);
 
-	REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
-	REG_READ(pipeA ? FPA1 : FPB1);
+	REG_WRITE(map->fp1, crtc_state->saveFP1);
+	REG_READ(map->fp1);
 
-	REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
-	REG_READ(pipeA ? DPLL_A : DPLL_B);
+	REG_WRITE(map->dpll, crtc_state->saveDPLL);
+	REG_READ(map->dpll);
 	udelay(150);
 
-	REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
-	REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
-	REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
-	REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
-	REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
-	REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
-	REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+	REG_WRITE(map->htotal, crtc_state->saveHTOTAL);
+	REG_WRITE(map->hblank, crtc_state->saveHBLANK);
+	REG_WRITE(map->hsync, crtc_state->saveHSYNC);
+	REG_WRITE(map->vtotal, crtc_state->saveVTOTAL);
+	REG_WRITE(map->vblank, crtc_state->saveVBLANK);
+	REG_WRITE(map->vsync, crtc_state->saveVSYNC);
+	REG_WRITE(map->stride, crtc_state->saveDSPSTRIDE);
 
-	REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
-	REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+	REG_WRITE(map->size, crtc_state->saveDSPSIZE);
+	REG_WRITE(map->pos, crtc_state->saveDSPPOS);
 
-	REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
-	REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+	REG_WRITE(map->src, crtc_state->savePIPESRC);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
+	REG_WRITE(map->conf, crtc_state->savePIPECONF);
 
 	cdv_intel_wait_for_vblank(dev);
 
-	REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+	REG_WRITE(map->cntr, crtc_state->saveDSPCNTR);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
 
 	cdv_intel_wait_for_vblank(dev);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
 }
@@ -1296,35 +1468,30 @@ static void i8xx_clock(int refclk, struct cdv_intel_clock_t *clock)
 static int cdv_intel_crtc_clock_get(struct drm_device *dev,
 				struct drm_crtc *crtc)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 dpll;
 	u32 fp;
 	struct cdv_intel_clock_t clock;
 	bool is_lvds;
-	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
 
 	if (gma_power_begin(dev, false)) {
-		dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+		dpll = REG_READ(map->dpll);
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+			fp = REG_READ(map->fp0);
 		else
-			fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+			fp = REG_READ(map->fp1);
 		is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
 		gma_power_end(dev);
 	} else {
-		dpll = (pipe == 0) ?
-			dev_priv->regs.psb.saveDPLL_A :
-			dev_priv->regs.psb.saveDPLL_B;
-
+		dpll = p->dpll;
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA0 :
-				dev_priv->regs.psb.saveFPB0;
+			fp = p->fp0;
 		else
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA1 :
-				dev_priv->regs.psb.saveFPB1;
+			fp = p->fp1;
 
 		is_lvds = (pipe == 1) &&
 				(dev_priv->regs.psb.saveLVDS & LVDS_PORT_EN);
@@ -1382,32 +1549,26 @@ struct drm_display_mode *cdv_intel_crtc_mode_get(struct drm_device *dev,
 {
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	struct drm_display_mode *mode;
 	int htot;
 	int hsync;
 	int vtot;
 	int vsync;
-	struct drm_psb_private *dev_priv = dev->dev_private;
 
 	if (gma_power_begin(dev, false)) {
-		htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
-		hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
-		vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
-		vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+		htot = REG_READ(map->htotal);
+		hsync = REG_READ(map->hsync);
+		vtot = REG_READ(map->vtotal);
+		vsync = REG_READ(map->vsync);
 		gma_power_end(dev);
 	} else {
-		htot = (pipe == 0) ?
-			dev_priv->regs.psb.saveHTOTAL_A :
-			dev_priv->regs.psb.saveHTOTAL_B;
-		hsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveHSYNC_A :
-			dev_priv->regs.psb.saveHSYNC_B;
-		vtot = (pipe == 0) ?
-			dev_priv->regs.psb.saveVTOTAL_A :
-			dev_priv->regs.psb.saveVTOTAL_B;
-		vsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveVSYNC_A :
-			dev_priv->regs.psb.saveVSYNC_B;
+		htot = p->htotal;
+		hsync = p->hsync;
+		vtot = p->vtotal;
+		vsync = p->vsync;
 	}
 
 	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
diff --git a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
index 8d5269555005..88b59d4a7b7f 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
@@ -242,8 +242,6 @@ static int cdv_hdmi_get_modes(struct drm_connector *connector)
 static int cdv_hdmi_mode_valid(struct drm_connector *connector,
 				 struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
-
 	if (mode->clock > 165000)
 		return MODE_CLOCK_HIGH;
 	if (mode->clock < 20000)
@@ -257,11 +255,6 @@ static int cdv_hdmi_mode_valid(struct drm_connector *connector,
 	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
 		return MODE_NO_INTERLACE;
 
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_lvds.c b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
index 8359c1a3f45f..ff5b58eb878c 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_lvds.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
@@ -356,6 +356,8 @@ static void cdv_intel_lvds_mode_set(struct drm_encoder *encoder,
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(
+							encoder->crtc);
 	u32 pfit_control;
 
 	/*
@@ -377,6 +379,8 @@ static void cdv_intel_lvds_mode_set(struct drm_encoder *encoder,
 	else
 		pfit_control = 0;
 
+	pfit_control |= psb_intel_crtc->pipe << PFIT_PIPE_SHIFT;
+
 	if (dev_priv->lvds_dither)
 		pfit_control |= PANEL_8TO6_DITHER_ENABLE;
 
@@ -552,10 +556,60 @@ static void cdv_intel_lvds_enc_destroy(struct drm_encoder *encoder)
 	drm_encoder_cleanup(encoder);
 }
 
-const struct drm_encoder_funcs cdv_intel_lvds_enc_funcs = {
+static const struct drm_encoder_funcs cdv_intel_lvds_enc_funcs = {
 	.destroy = cdv_intel_lvds_enc_destroy,
 };
 
+/*
+ * Enumerate the child dev array parsed from VBT to check whether
+ * the LVDS is present.
+ * If it is present, return 1.
+ * If it is not present, return false.
+ * If no child dev is parsed from VBT, it assumes that the LVDS is present.
+ */
+static bool lvds_is_present_in_vbt(struct drm_device *dev,
+				   u8 *i2c_pin)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int i;
+
+	if (!dev_priv->child_dev_num)
+		return true;
+
+	for (i = 0; i < dev_priv->child_dev_num; i++) {
+		struct child_device_config *child = dev_priv->child_dev + i;
+
+		/* If the device type is not LFP, continue.
+		 * We have to check both the new identifiers as well as the
+		 * old for compatibility with some BIOSes.
+		 */
+		if (child->device_type != DEVICE_TYPE_INT_LFP &&
+		    child->device_type != DEVICE_TYPE_LFP)
+			continue;
+
+		if (child->i2c_pin)
+		    *i2c_pin = child->i2c_pin;
+
+		/* However, we cannot trust the BIOS writers to populate
+		 * the VBT correctly.  Since LVDS requires additional
+		 * information from AIM blocks, a non-zero addin offset is
+		 * a good indicator that the LVDS is actually present.
+		 */
+		if (child->addin_offset)
+			return true;
+
+		/* But even then some BIOS writers perform some black magic
+		 * and instantiate the device without reference to any
+		 * additional data.  Trust that if the VBT was written into
+		 * the OpRegion then they have validated the LVDS's existence.
+		 */
+		if (dev_priv->opregion.vbt)
+			return true;
+	}
+
+	return false;
+}
+
 /**
  * cdv_intel_lvds_init - setup LVDS connectors on this device
  * @dev: drm device
@@ -576,6 +630,13 @@ void cdv_intel_lvds_init(struct drm_device *dev,
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	u32 lvds;
 	int pipe;
+	u8 pin;
+
+	pin = GMBUS_PORT_PANEL;
+	if (!lvds_is_present_in_vbt(dev, &pin)) {
+		DRM_DEBUG_KMS("LVDS is not present in VBT\n");
+		return;
+	}
 
 	psb_intel_encoder = kzalloc(sizeof(struct psb_intel_encoder),
 				    GFP_KERNEL);
@@ -710,6 +771,19 @@ void cdv_intel_lvds_init(struct drm_device *dev,
 		goto failed_find;
 	}
 
+	/* setup PWM */
+	{
+		u32 pwm;
+
+		pwm = REG_READ(BLC_PWM_CTL2);
+		if (pipe == 1)
+			pwm |= PWM_PIPE_B;
+		else
+			pwm &= ~PWM_PIPE_B;
+		pwm |= PWM_ENABLE;
+		REG_WRITE(BLC_PWM_CTL2, pwm);
+	}
+
 out:
 	drm_sysfs_connector_add(connector);
 	return;
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
index 8ea202f1ba50..5732b5702e1c 100644
--- a/drivers/gpu/drm/gma500/framebuffer.c
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -153,7 +153,7 @@ static void psbfb_vm_close(struct vm_area_struct *vma)
 {
 }
 
-static struct vm_operations_struct psbfb_vm_ops = {
+static const struct vm_operations_struct psbfb_vm_ops = {
 	.fault	= psbfb_vm_fault,
 	.open	= psbfb_vm_open,
 	.close	= psbfb_vm_close
@@ -408,6 +408,8 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 			return -ENOMEM;
 	}
 
+	memset(dev_priv->vram_addr + backing->offset, 0, size);
+
 	mutex_lock(&dev->struct_mutex);
 
 	info = framebuffer_alloc(0, device);
@@ -453,8 +455,7 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 	info->fix.ypanstep = 0;
 
 	/* Accessed stolen memory directly */
-	info->screen_base = (char *)dev_priv->vram_addr +
-							backing->offset;
+	info->screen_base = dev_priv->vram_addr + backing->offset;
 	info->screen_size = size;
 
 	if (dev_priv->gtt.stolen_size) {
@@ -475,7 +476,7 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 
 	/* Use default scratch pixmap (info->pixmap.flags = FB_PIXMAP_SYSTEM) */
 
-	dev_info(dev->dev, "allocated %dx%d fb\n",
+	dev_dbg(dev->dev, "allocated %dx%d fb\n",
 					psbfb->base.width, psbfb->base.height);
 
 	mutex_unlock(&dev->struct_mutex);
@@ -543,9 +544,25 @@ static int psbfb_probe(struct drm_fb_helper *helper,
 				struct drm_fb_helper_surface_size *sizes)
 {
 	struct psb_fbdev *psb_fbdev = (struct psb_fbdev *)helper;
+	struct drm_device *dev = psb_fbdev->psb_fb_helper.dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	int new_fb = 0;
+	int bytespp;
 	int ret;
 
+	bytespp = sizes->surface_bpp / 8;
+	if (bytespp == 3)	/* no 24bit packed */
+		bytespp = 4;
+
+	/* If the mode will not fit in 32bit then switch to 16bit to get
+	   a console on full resolution. The X mode setting server will
+	   allocate its own 32bit GEM framebuffer */
+	if (ALIGN(sizes->fb_width * bytespp, 64) * sizes->fb_height >
+	                dev_priv->vram_stolen_size) {
+                sizes->surface_bpp = 16;
+                sizes->surface_depth = 16;
+        }
+
 	if (!helper->fb) {
 		ret = psbfb_create(psb_fbdev, sizes);
 		if (ret)
@@ -555,7 +572,7 @@ static int psbfb_probe(struct drm_fb_helper *helper,
 	return new_fb;
 }
 
-struct drm_fb_helper_funcs psb_fb_helper_funcs = {
+static struct drm_fb_helper_funcs psb_fb_helper_funcs = {
 	.gamma_set = psbfb_gamma_set,
 	.gamma_get = psbfb_gamma_get,
 	.fb_probe = psbfb_probe,
@@ -732,10 +749,7 @@ static void psb_setup_outputs(struct drm_device *dev)
 			clone_mask = (1 << INTEL_OUTPUT_SDVO);
 			break;
 		case INTEL_OUTPUT_LVDS:
-			if (IS_MRST(dev))
-				crtc_mask = (1 << 0);
-			else
-				crtc_mask = (1 << 1);
+		        crtc_mask = dev_priv->ops->lvds_mask;
 			clone_mask = (1 << INTEL_OUTPUT_LVDS);
 			break;
 		case INTEL_OUTPUT_MIPI:
@@ -747,10 +761,7 @@ static void psb_setup_outputs(struct drm_device *dev)
 			clone_mask = (1 << INTEL_OUTPUT_MIPI2);
 			break;
 		case INTEL_OUTPUT_HDMI:
-			if (IS_MFLD(dev))
-				crtc_mask = (1 << 1);
-			else	
-				crtc_mask = (1 << 0);
+		        crtc_mask = dev_priv->ops->hdmi_mask;
 			clone_mask = (1 << INTEL_OUTPUT_HDMI);
 			break;
 		}
@@ -771,7 +782,7 @@ void psb_modeset_init(struct drm_device *dev)
 	dev->mode_config.min_width = 0;
 	dev->mode_config.min_height = 0;
 
-	dev->mode_config.funcs = (void *) &psb_mode_funcs;
+	dev->mode_config.funcs = &psb_mode_funcs;
 
 	/* set memory base */
 	/* Oaktrail and Poulsbo should use BAR 2*/
@@ -786,15 +797,23 @@ void psb_modeset_init(struct drm_device *dev)
 	dev->mode_config.max_height = 2048;
 
 	psb_setup_outputs(dev);
+
+	if (dev_priv->ops->errata)
+	        dev_priv->ops->errata(dev);
+
+        dev_priv->modeset = true;
 }
 
 void psb_modeset_cleanup(struct drm_device *dev)
 {
-	mutex_lock(&dev->struct_mutex);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	if (dev_priv->modeset) {
+		mutex_lock(&dev->struct_mutex);
 
-	drm_kms_helper_poll_fini(dev);
-	psb_fbdev_fini(dev);
-	drm_mode_config_cleanup(dev);
+		drm_kms_helper_poll_fini(dev);
+		psb_fbdev_fini(dev);
+		drm_mode_config_cleanup(dev);
 
-	mutex_unlock(&dev->struct_mutex);
+		mutex_unlock(&dev->struct_mutex);
+	}
 }
diff --git a/drivers/gpu/drm/gma500/gem.c b/drivers/gpu/drm/gma500/gem.c
index 9fbb86868e2e..fc7d144bc2d3 100644
--- a/drivers/gpu/drm/gma500/gem.c
+++ b/drivers/gpu/drm/gma500/gem.c
@@ -124,6 +124,8 @@ static int psb_gem_create(struct drm_file *file,
 		dev_err(dev->dev, "GEM init failed for %lld\n", size);
 		return -ENOMEM;
 	}
+	/* Limit the object to 32bit mappings */
+	mapping_set_gfp_mask(r->gem.filp->f_mapping, GFP_KERNEL | __GFP_DMA32);
 	/* Give the object a handle so we can carry it more easily */
 	ret = drm_gem_handle_create(file, &r->gem, &handle);
 	if (ret) {
diff --git a/drivers/gpu/drm/gma500/gtt.c b/drivers/gpu/drm/gma500/gtt.c
index c6465b40090f..04a371aceb34 100644
--- a/drivers/gpu/drm/gma500/gtt.c
+++ b/drivers/gpu/drm/gma500/gtt.c
@@ -39,6 +39,10 @@ static inline uint32_t psb_gtt_mask_pte(uint32_t pfn, int type)
 {
 	uint32_t mask = PSB_PTE_VALID;
 
+	/* Ensure we explode rather than put an invalid low mapping of
+	   a high mapping page into the gtt */
+	BUG_ON(pfn & ~(0xFFFFFFFF >> PAGE_SHIFT));
+
 	if (type & PSB_MMU_CACHED_MEMORY)
 		mask |= PSB_PTE_CACHED;
 	if (type & PSB_MMU_RO_MEMORY)
@@ -57,7 +61,7 @@ static inline uint32_t psb_gtt_mask_pte(uint32_t pfn, int type)
  *	Given a gtt_range object return the GTT offset of the page table
  *	entries for this gtt_range
  */
-static u32 *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
+static u32 __iomem *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	unsigned long offset;
@@ -78,7 +82,8 @@ static u32 *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
  */
 static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
 {
-	u32 *gtt_slot, pte;
+	u32 __iomem *gtt_slot;
+	u32 pte;
 	struct page **pages;
 	int i;
 
@@ -93,7 +98,7 @@ static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
 	pages = r->pages;
 
 	/* Make sure changes are visible to the GPU */
-	set_pages_array_uc(pages, r->npage);
+	set_pages_array_wc(pages, r->npage);
 
 	/* Write our page entries into the GTT itself */
 	for (i = r->roll; i < r->npage; i++) {
@@ -122,7 +127,8 @@ static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
 static void psb_gtt_remove(struct drm_device *dev, struct gtt_range *r)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	u32 *gtt_slot, pte;
+	u32 __iomem *gtt_slot;
+	u32 pte;
 	int i;
 
 	WARN_ON(r->stolen);
@@ -148,7 +154,8 @@ static void psb_gtt_remove(struct drm_device *dev, struct gtt_range *r)
  */
 void psb_gtt_roll(struct drm_device *dev, struct gtt_range *r, int roll)
 {
-	u32 *gtt_slot, pte;
+	u32 __iomem *gtt_slot;
+	u32 pte;
 	int i;
 
 	if (roll >= r->npage) {
@@ -409,8 +416,6 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 	unsigned long stolen_size, vram_stolen_size;
 	unsigned i, num_pages;
 	unsigned pfn_base;
-	uint32_t vram_pages;
-	uint32_t dvmt_mode = 0;
 	struct psb_gtt *pg;
 
 	int ret = 0;
@@ -483,13 +488,8 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 
 	stolen_size = vram_stolen_size;
 
-	printk(KERN_INFO "Stolen memory information\n");
-	printk(KERN_INFO "       base in RAM: 0x%x\n", dev_priv->stolen_base);
-	printk(KERN_INFO "       size: %luK, calculated by (GTT RAM base) - (Stolen base), seems wrong\n",
-		vram_stolen_size/1024);
-	dvmt_mode = (dev_priv->gmch_ctrl >> 4) & 0x7;
-	printk(KERN_INFO "      the correct size should be: %dM(dvmt mode=%d)\n",
-		(dvmt_mode == 1) ? 1 : (2 << (dvmt_mode - 1)), dvmt_mode);
+	dev_dbg(dev->dev, "Stolen memory base 0x%x, size %luK\n",
+			dev_priv->stolen_base, vram_stolen_size / 1024);
 
 	if (resume && (gtt_pages != pg->gtt_pages) &&
 	    (stolen_size != pg->stolen_size)) {
@@ -525,8 +525,8 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 	 */
 
 	pfn_base = dev_priv->stolen_base >> PAGE_SHIFT;
-	vram_pages = num_pages = vram_stolen_size >> PAGE_SHIFT;
-	printk(KERN_INFO"Set up %d stolen pages starting at 0x%08x, GTT offset %dK\n",
+	num_pages = vram_stolen_size >> PAGE_SHIFT;
+	dev_dbg(dev->dev, "Set up %d stolen pages starting at 0x%08x, GTT offset %dK\n",
 		num_pages, pfn_base << PAGE_SHIFT, 0);
 	for (i = 0; i < num_pages; ++i) {
 		pte = psb_gtt_mask_pte(pfn_base + i, 0);
diff --git a/drivers/gpu/drm/gma500/intel_bios.c b/drivers/gpu/drm/gma500/intel_bios.c
index d4d0c5b8bf91..973d7f6d66b7 100644
--- a/drivers/gpu/drm/gma500/intel_bios.c
+++ b/drivers/gpu/drm/gma500/intel_bios.c
@@ -26,6 +26,8 @@
 #include "psb_intel_reg.h"
 #include "intel_bios.h"
 
+#define	SLAVE_ADDR1	0x70
+#define	SLAVE_ADDR2	0x72
 
 static void *find_section(struct bdb_header *bdb, int section_id)
 {
@@ -52,6 +54,16 @@ static void *find_section(struct bdb_header *bdb, int section_id)
 	return NULL;
 }
 
+static u16
+get_blocksize(void *p)
+{
+	u16 *block_ptr, block_size;
+
+	block_ptr = (u16 *)((char *)p - 2);
+	block_size = *block_ptr;
+	return block_size;
+}
+
 static void fill_detail_timing_data(struct drm_display_mode *panel_fixed_mode,
 			struct lvds_dvo_timing *dvo_timing)
 {
@@ -75,6 +87,16 @@ static void fill_detail_timing_data(struct drm_display_mode *panel_fixed_mode,
 	panel_fixed_mode->clock = dvo_timing->clock * 10;
 	panel_fixed_mode->type = DRM_MODE_TYPE_PREFERRED;
 
+	if (dvo_timing->hsync_positive)
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_PHSYNC;
+	else
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_NHSYNC;
+
+	if (dvo_timing->vsync_positive)
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_PVSYNC;
+	else
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_NVSYNC;
+
 	/* Some VBTs have bogus h/vtotal values */
 	if (panel_fixed_mode->hsync_end > panel_fixed_mode->htotal)
 		panel_fixed_mode->htotal = panel_fixed_mode->hsync_end + 1;
@@ -217,6 +239,180 @@ static void parse_general_features(struct drm_psb_private *dev_priv,
 	}
 }
 
+static void
+parse_sdvo_device_mapping(struct drm_psb_private *dev_priv,
+			  struct bdb_header *bdb)
+{
+	struct sdvo_device_mapping *p_mapping;
+	struct bdb_general_definitions *p_defs;
+	struct child_device_config *p_child;
+	int i, child_device_num, count;
+	u16	block_size;
+
+	p_defs = find_section(bdb, BDB_GENERAL_DEFINITIONS);
+	if (!p_defs) {
+		DRM_DEBUG_KMS("No general definition block is found, unable to construct sdvo mapping.\n");
+		return;
+	}
+	/* judge whether the size of child device meets the requirements.
+	 * If the child device size obtained from general definition block
+	 * is different with sizeof(struct child_device_config), skip the
+	 * parsing of sdvo device info
+	 */
+	if (p_defs->child_dev_size != sizeof(*p_child)) {
+		/* different child dev size . Ignore it */
+		DRM_DEBUG_KMS("different child size is found. Invalid.\n");
+		return;
+	}
+	/* get the block size of general definitions */
+	block_size = get_blocksize(p_defs);
+	/* get the number of child device */
+	child_device_num = (block_size - sizeof(*p_defs)) /
+				sizeof(*p_child);
+	count = 0;
+	for (i = 0; i < child_device_num; i++) {
+		p_child = &(p_defs->devices[i]);
+		if (!p_child->device_type) {
+			/* skip the device block if device type is invalid */
+			continue;
+		}
+		if (p_child->slave_addr != SLAVE_ADDR1 &&
+			p_child->slave_addr != SLAVE_ADDR2) {
+			/*
+			 * If the slave address is neither 0x70 nor 0x72,
+			 * it is not a SDVO device. Skip it.
+			 */
+			continue;
+		}
+		if (p_child->dvo_port != DEVICE_PORT_DVOB &&
+			p_child->dvo_port != DEVICE_PORT_DVOC) {
+			/* skip the incorrect SDVO port */
+			DRM_DEBUG_KMS("Incorrect SDVO port. Skip it\n");
+			continue;
+		}
+		DRM_DEBUG_KMS("the SDVO device with slave addr %2x is found on"
+				" %s port\n",
+				p_child->slave_addr,
+				(p_child->dvo_port == DEVICE_PORT_DVOB) ?
+					"SDVOB" : "SDVOC");
+		p_mapping = &(dev_priv->sdvo_mappings[p_child->dvo_port - 1]);
+		if (!p_mapping->initialized) {
+			p_mapping->dvo_port = p_child->dvo_port;
+			p_mapping->slave_addr = p_child->slave_addr;
+			p_mapping->dvo_wiring = p_child->dvo_wiring;
+			p_mapping->ddc_pin = p_child->ddc_pin;
+			p_mapping->i2c_pin = p_child->i2c_pin;
+			p_mapping->initialized = 1;
+			DRM_DEBUG_KMS("SDVO device: dvo=%x, addr=%x, wiring=%d, ddc_pin=%d, i2c_pin=%d\n",
+				      p_mapping->dvo_port,
+				      p_mapping->slave_addr,
+				      p_mapping->dvo_wiring,
+				      p_mapping->ddc_pin,
+				      p_mapping->i2c_pin);
+		} else {
+			DRM_DEBUG_KMS("Maybe one SDVO port is shared by "
+					 "two SDVO device.\n");
+		}
+		if (p_child->slave2_addr) {
+			/* Maybe this is a SDVO device with multiple inputs */
+			/* And the mapping info is not added */
+			DRM_DEBUG_KMS("there exists the slave2_addr. Maybe this"
+				" is a SDVO device with multiple inputs.\n");
+		}
+		count++;
+	}
+
+	if (!count) {
+		/* No SDVO device info is found */
+		DRM_DEBUG_KMS("No SDVO device info is found in VBT\n");
+	}
+	return;
+}
+
+
+static void
+parse_driver_features(struct drm_psb_private *dev_priv,
+		      struct bdb_header *bdb)
+{
+	struct bdb_driver_features *driver;
+
+	driver = find_section(bdb, BDB_DRIVER_FEATURES);
+	if (!driver)
+		return;
+
+	/* This bit means to use 96Mhz for DPLL_A or not */
+	if (driver->primary_lfp_id)
+		dev_priv->dplla_96mhz = true;
+	else
+		dev_priv->dplla_96mhz = false;
+}
+
+static void
+parse_device_mapping(struct drm_psb_private *dev_priv,
+		       struct bdb_header *bdb)
+{
+	struct bdb_general_definitions *p_defs;
+	struct child_device_config *p_child, *child_dev_ptr;
+	int i, child_device_num, count;
+	u16	block_size;
+
+	p_defs = find_section(bdb, BDB_GENERAL_DEFINITIONS);
+	if (!p_defs) {
+		DRM_DEBUG_KMS("No general definition block is found, no devices defined.\n");
+		return;
+	}
+	/* judge whether the size of child device meets the requirements.
+	 * If the child device size obtained from general definition block
+	 * is different with sizeof(struct child_device_config), skip the
+	 * parsing of sdvo device info
+	 */
+	if (p_defs->child_dev_size != sizeof(*p_child)) {
+		/* different child dev size . Ignore it */
+		DRM_DEBUG_KMS("different child size is found. Invalid.\n");
+		return;
+	}
+	/* get the block size of general definitions */
+	block_size = get_blocksize(p_defs);
+	/* get the number of child device */
+	child_device_num = (block_size - sizeof(*p_defs)) /
+				sizeof(*p_child);
+	count = 0;
+	/* get the number of child devices that are present */
+	for (i = 0; i < child_device_num; i++) {
+		p_child = &(p_defs->devices[i]);
+		if (!p_child->device_type) {
+			/* skip the device block if device type is invalid */
+			continue;
+		}
+		count++;
+	}
+	if (!count) {
+		DRM_DEBUG_KMS("no child dev is parsed from VBT\n");
+		return;
+	}
+	dev_priv->child_dev = kcalloc(count, sizeof(*p_child), GFP_KERNEL);
+	if (!dev_priv->child_dev) {
+		DRM_DEBUG_KMS("No memory space for child devices\n");
+		return;
+	}
+
+	dev_priv->child_dev_num = count;
+	count = 0;
+	for (i = 0; i < child_device_num; i++) {
+		p_child = &(p_defs->devices[i]);
+		if (!p_child->device_type) {
+			/* skip the device block if device type is invalid */
+			continue;
+		}
+		child_dev_ptr = dev_priv->child_dev + count;
+		count++;
+		memcpy((void *)child_dev_ptr, (void *)p_child,
+					sizeof(*p_child));
+	}
+	return;
+}
+
+
 /**
  * psb_intel_init_bios - initialize VBIOS settings & find VBT
  * @dev: DRM device
@@ -236,38 +432,54 @@ bool psb_intel_init_bios(struct drm_device *dev)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct pci_dev *pdev = dev->pdev;
 	struct vbt_header *vbt = NULL;
-	struct bdb_header *bdb;
-	u8 __iomem *bios;
+	struct bdb_header *bdb = NULL;
+	u8 __iomem *bios = NULL;
 	size_t size;
 	int i;
 
-	bios = pci_map_rom(pdev, &size);
-	if (!bios)
-		return -1;
+	/* XXX Should this validation be moved to intel_opregion.c? */
+	if (dev_priv->opregion.vbt) {
+		struct vbt_header *vbt = dev_priv->opregion.vbt;
+		if (memcmp(vbt->signature, "$VBT", 4) == 0) {
+			DRM_DEBUG_KMS("Using VBT from OpRegion: %20s\n",
+					 vbt->signature);
+			bdb = (struct bdb_header *)((char *)vbt + vbt->bdb_offset);
+		} else
+			dev_priv->opregion.vbt = NULL;
+	}
 
-	/* Scour memory looking for the VBT signature */
-	for (i = 0; i + 4 < size; i++) {
-		if (!memcmp(bios + i, "$VBT", 4)) {
-			vbt = (struct vbt_header *)(bios + i);
-			break;
+	if (bdb == NULL) {
+		bios = pci_map_rom(pdev, &size);
+		if (!bios)
+			return -1;
+
+		/* Scour memory looking for the VBT signature */
+		for (i = 0; i + 4 < size; i++) {
+			if (!memcmp(bios + i, "$VBT", 4)) {
+				vbt = (struct vbt_header *)(bios + i);
+				break;
+			}
 		}
-	}
 
-	if (!vbt) {
-		dev_err(dev->dev, "VBT signature missing\n");
-		pci_unmap_rom(pdev, bios);
-		return -1;
+		if (!vbt) {
+			dev_err(dev->dev, "VBT signature missing\n");
+			pci_unmap_rom(pdev, bios);
+			return -1;
+		}
+		bdb = (struct bdb_header *)(bios + i + vbt->bdb_offset);
 	}
 
-	bdb = (struct bdb_header *)(bios + i + vbt->bdb_offset);
-
-	/* Grab useful general definitions */
+	/* Grab useful general dxefinitions */
 	parse_general_features(dev_priv, bdb);
+	parse_driver_features(dev_priv, bdb);
 	parse_lfp_panel_data(dev_priv, bdb);
 	parse_sdvo_panel_data(dev_priv, bdb);
+	parse_sdvo_device_mapping(dev_priv, bdb);
+	parse_device_mapping(dev_priv, bdb);
 	parse_backlight_data(dev_priv, bdb);
 
-	pci_unmap_rom(pdev, bios);
+	if (bios)
+		pci_unmap_rom(pdev, bios);
 
 	return 0;
 }
@@ -278,26 +490,8 @@ bool psb_intel_init_bios(struct drm_device *dev)
 void psb_intel_destroy_bios(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct drm_display_mode *sdvo_lvds_vbt_mode =
-				dev_priv->sdvo_lvds_vbt_mode;
-	struct drm_display_mode *lfp_lvds_vbt_mode =
-				dev_priv->lfp_lvds_vbt_mode;
-	struct bdb_lvds_backlight *lvds_bl =
-				dev_priv->lvds_bl;
-
-	/*free sdvo panel mode*/
-	if (sdvo_lvds_vbt_mode) {
-		dev_priv->sdvo_lvds_vbt_mode = NULL;
-		kfree(sdvo_lvds_vbt_mode);
-	}
 
-	if (lfp_lvds_vbt_mode) {
-		dev_priv->lfp_lvds_vbt_mode = NULL;
-		kfree(lfp_lvds_vbt_mode);
-	}
-
-	if (lvds_bl) {
-		dev_priv->lvds_bl = NULL;
-		kfree(lvds_bl);
-	}
+	kfree(dev_priv->sdvo_lvds_vbt_mode);
+	kfree(dev_priv->lfp_lvds_vbt_mode);
+	kfree(dev_priv->lvds_bl);
 }
diff --git a/drivers/gpu/drm/gma500/intel_bios.h b/drivers/gpu/drm/gma500/intel_bios.h
index 70f1bf018183..0a738663eb5a 100644
--- a/drivers/gpu/drm/gma500/intel_bios.h
+++ b/drivers/gpu/drm/gma500/intel_bios.h
@@ -127,9 +127,93 @@ struct bdb_general_features {
 	/* bits 5 */
 	u8 int_crt_support:1;
 	u8 int_tv_support:1;
-	u8 rsvd11:6; /* finish byte */
+	u8 int_efp_support:1;
+	u8 dp_ssc_enb:1;	/* PCH attached eDP supports SSC */
+	u8 dp_ssc_freq:1;	/* SSC freq for PCH attached eDP */
+	u8 rsvd11:3; /* finish byte */
 } __attribute__((packed));
 
+/* pre-915 */
+#define GPIO_PIN_DVI_LVDS	0x03 /* "DVI/LVDS DDC GPIO pins" */
+#define GPIO_PIN_ADD_I2C	0x05 /* "ADDCARD I2C GPIO pins" */
+#define GPIO_PIN_ADD_DDC	0x04 /* "ADDCARD DDC GPIO pins" */
+#define GPIO_PIN_ADD_DDC_I2C	0x06 /* "ADDCARD DDC/I2C GPIO pins" */
+
+/* Pre 915 */
+#define DEVICE_TYPE_NONE	0x00
+#define DEVICE_TYPE_CRT		0x01
+#define DEVICE_TYPE_TV		0x09
+#define DEVICE_TYPE_EFP		0x12
+#define DEVICE_TYPE_LFP		0x22
+/* On 915+ */
+#define DEVICE_TYPE_CRT_DPMS		0x6001
+#define DEVICE_TYPE_CRT_DPMS_HOTPLUG	0x4001
+#define DEVICE_TYPE_TV_COMPOSITE	0x0209
+#define DEVICE_TYPE_TV_MACROVISION	0x0289
+#define DEVICE_TYPE_TV_RF_COMPOSITE	0x020c
+#define DEVICE_TYPE_TV_SVIDEO_COMPOSITE	0x0609
+#define DEVICE_TYPE_TV_SCART		0x0209
+#define DEVICE_TYPE_TV_CODEC_HOTPLUG_PWR 0x6009
+#define DEVICE_TYPE_EFP_HOTPLUG_PWR	0x6012
+#define DEVICE_TYPE_EFP_DVI_HOTPLUG_PWR	0x6052
+#define DEVICE_TYPE_EFP_DVI_I		0x6053
+#define DEVICE_TYPE_EFP_DVI_D_DUAL	0x6152
+#define DEVICE_TYPE_EFP_DVI_D_HDCP	0x60d2
+#define DEVICE_TYPE_OPENLDI_HOTPLUG_PWR	0x6062
+#define DEVICE_TYPE_OPENLDI_DUALPIX	0x6162
+#define DEVICE_TYPE_LFP_PANELLINK	0x5012
+#define DEVICE_TYPE_LFP_CMOS_PWR	0x5042
+#define DEVICE_TYPE_LFP_LVDS_PWR	0x5062
+#define DEVICE_TYPE_LFP_LVDS_DUAL	0x5162
+#define DEVICE_TYPE_LFP_LVDS_DUAL_HDCP	0x51e2
+
+#define DEVICE_CFG_NONE		0x00
+#define DEVICE_CFG_12BIT_DVOB	0x01
+#define DEVICE_CFG_12BIT_DVOC	0x02
+#define DEVICE_CFG_24BIT_DVOBC	0x09
+#define DEVICE_CFG_24BIT_DVOCB	0x0a
+#define DEVICE_CFG_DUAL_DVOB	0x11
+#define DEVICE_CFG_DUAL_DVOC	0x12
+#define DEVICE_CFG_DUAL_DVOBC	0x13
+#define DEVICE_CFG_DUAL_LINK_DVOBC	0x19
+#define DEVICE_CFG_DUAL_LINK_DVOCB	0x1a
+
+#define DEVICE_WIRE_NONE	0x00
+#define DEVICE_WIRE_DVOB	0x01
+#define DEVICE_WIRE_DVOC	0x02
+#define DEVICE_WIRE_DVOBC	0x03
+#define DEVICE_WIRE_DVOBB	0x05
+#define DEVICE_WIRE_DVOCC	0x06
+#define DEVICE_WIRE_DVOB_MASTER 0x0d
+#define DEVICE_WIRE_DVOC_MASTER 0x0e
+
+#define DEVICE_PORT_DVOA	0x00 /* none on 845+ */
+#define DEVICE_PORT_DVOB	0x01
+#define DEVICE_PORT_DVOC	0x02
+
+struct child_device_config {
+	u16 handle;
+	u16 device_type;
+	u8  device_id[10]; /* ascii string */
+	u16 addin_offset;
+	u8  dvo_port; /* See Device_PORT_* above */
+	u8  i2c_pin;
+	u8  slave_addr;
+	u8  ddc_pin;
+	u16 edid_ptr;
+	u8  dvo_cfg; /* See DEVICE_CFG_* above */
+	u8  dvo2_port;
+	u8  i2c2_pin;
+	u8  slave2_addr;
+	u8  ddc2_pin;
+	u8  capabilities;
+	u8  dvo_wiring;/* See DEVICE_WIRE_* above */
+	u8  dvo2_wiring;
+	u16 extended_type;
+	u8  dvo_function;
+} __attribute__((packed));
+
+
 struct bdb_general_definitions {
 	/* DDC GPIO */
 	u8 crt_ddc_gmbus_pin;
@@ -144,13 +228,18 @@ struct bdb_general_definitions {
 	u8 boot_display[2];
 	u8 child_dev_size;
 
-	/* device info */
-	u8 tv_or_lvds_info[33];
-	u8 dev1[33];
-	u8 dev2[33];
-	u8 dev3[33];
-	u8 dev4[33];
-	/* may be another device block here on some platforms */
+	/*
+	 * Device info:
+	 * If TV is present, it'll be at devices[0].
+	 * LVDS will be next, either devices[0] or [1], if present.
+	 * On some platforms the number of device is 6. But could be as few as
+	 * 4 if both TV and LVDS are missing.
+	 * And the device num is related with the size of general definition
+	 * block. It is obtained by using the following formula:
+	 * number = (block_size - sizeof(bdb_general_definitions))/
+	 *	     sizeof(child_device_config);
+	 */
+	struct child_device_config devices[0];
 };
 
 struct bdb_lvds_options {
@@ -302,6 +391,45 @@ struct bdb_sdvo_lvds_options {
 	u8 panel_misc_bits_4;
 } __attribute__((packed));
 
+struct bdb_driver_features {
+	u8 boot_dev_algorithm:1;
+	u8 block_display_switch:1;
+	u8 allow_display_switch:1;
+	u8 hotplug_dvo:1;
+	u8 dual_view_zoom:1;
+	u8 int15h_hook:1;
+	u8 sprite_in_clone:1;
+	u8 primary_lfp_id:1;
+
+	u16 boot_mode_x;
+	u16 boot_mode_y;
+	u8 boot_mode_bpp;
+	u8 boot_mode_refresh;
+
+	u16 enable_lfp_primary:1;
+	u16 selective_mode_pruning:1;
+	u16 dual_frequency:1;
+	u16 render_clock_freq:1; /* 0: high freq; 1: low freq */
+	u16 nt_clone_support:1;
+	u16 power_scheme_ui:1; /* 0: CUI; 1: 3rd party */
+	u16 sprite_display_assign:1; /* 0: secondary; 1: primary */
+	u16 cui_aspect_scaling:1;
+	u16 preserve_aspect_ratio:1;
+	u16 sdvo_device_power_down:1;
+	u16 crt_hotplug:1;
+	u16 lvds_config:2;
+	u16 tv_hotplug:1;
+	u16 hdmi_config:2;
+
+	u8 static_display:1;
+	u8 reserved2:7;
+	u16 legacy_crt_max_x;
+	u16 legacy_crt_max_y;
+	u8 legacy_crt_max_refresh;
+
+	u8 hdmi_termination;
+	u8 custom_vbt_version;
+} __attribute__((packed));
 
 extern bool psb_intel_init_bios(struct drm_device *dev);
 extern void psb_intel_destroy_bios(struct drm_device *dev);
@@ -427,4 +555,21 @@ extern void psb_intel_destroy_bios(struct drm_device *dev);
 #define   SWF14_APM_STANDBY	0x1
 #define   SWF14_APM_RESTORE	0x0
 
+/* Add the device class for LFP, TV, HDMI */
+#define	 DEVICE_TYPE_INT_LFP	0x1022
+#define	 DEVICE_TYPE_INT_TV	0x1009
+#define	 DEVICE_TYPE_HDMI	0x60D2
+#define	 DEVICE_TYPE_DP		0x68C6
+#define	 DEVICE_TYPE_eDP	0x78C6
+
+/* define the DVO port for HDMI output type */
+#define		DVO_B		1
+#define		DVO_C		2
+#define		DVO_D		3
+
+/* define the PORT for DP output type */
+#define		PORT_IDPB	7
+#define		PORT_IDPC	8
+#define		PORT_IDPD	9
+
 #endif /* _I830_BIOS_H_ */
diff --git a/drivers/gpu/drm/gma500/mdfld_device.c b/drivers/gpu/drm/gma500/mdfld_device.c
index af656787db0f..265ad0de44a6 100644
--- a/drivers/gpu/drm/gma500/mdfld_device.c
+++ b/drivers/gpu/drm/gma500/mdfld_device.c
@@ -163,142 +163,30 @@ struct backlight_device *mdfld_get_backlight_device(void)
  *
  * Notes: FIXME_JLIU7 need to add the support for DPI MIPI & HDMI audio
  */
-static int mdfld_save_display_registers(struct drm_device *dev, int pipe)
+static int mdfld_save_display_registers(struct drm_device *dev, int pipenum)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct medfield_state *regs = &dev_priv->regs.mdfld;
+	struct psb_pipe *pipe = &dev_priv->regs.pipe[pipenum];
+	const struct psb_offset *map = &dev_priv->regmap[pipenum];
 	int i;
+	u32 *mipi_val;
 
 	/* register */
-	u32 dpll_reg = MRST_DPLL_A;
-	u32 fp_reg = MRST_FPA0;
-	u32 pipeconf_reg = PIPEACONF;
-	u32 htot_reg = HTOTAL_A;
-	u32 hblank_reg = HBLANK_A;
-	u32 hsync_reg = HSYNC_A;
-	u32 vtot_reg = VTOTAL_A;
-	u32 vblank_reg = VBLANK_A;
-	u32 vsync_reg = VSYNC_A;
-	u32 pipesrc_reg = PIPEASRC;
-	u32 dspstride_reg = DSPASTRIDE;
-	u32 dsplinoff_reg = DSPALINOFF;
-	u32 dsptileoff_reg = DSPATILEOFF;
-	u32 dspsize_reg = DSPASIZE;
-	u32 dsppos_reg = DSPAPOS;
-	u32 dspsurf_reg = DSPASURF;
 	u32 mipi_reg = MIPI;
-	u32 dspcntr_reg = DSPACNTR;
-	u32 dspstatus_reg = PIPEASTAT;
-	u32 palette_reg = PALETTE_A;
-
-	/* pointer to values */
-	u32 *dpll_val = &regs->saveDPLL_A;
-	u32 *fp_val = &regs->saveFPA0;
-	u32 *pipeconf_val = &regs->savePIPEACONF;
-	u32 *htot_val = &regs->saveHTOTAL_A;
-	u32 *hblank_val = &regs->saveHBLANK_A;
-	u32 *hsync_val = &regs->saveHSYNC_A;
-	u32 *vtot_val = &regs->saveVTOTAL_A;
-	u32 *vblank_val = &regs->saveVBLANK_A;
-	u32 *vsync_val = &regs->saveVSYNC_A;
-	u32 *pipesrc_val = &regs->savePIPEASRC;
-	u32 *dspstride_val = &regs->saveDSPASTRIDE;
-	u32 *dsplinoff_val = &regs->saveDSPALINOFF;
-	u32 *dsptileoff_val = &regs->saveDSPATILEOFF;
-	u32 *dspsize_val = &regs->saveDSPASIZE;
-	u32 *dsppos_val = &regs->saveDSPAPOS;
-	u32 *dspsurf_val = &regs->saveDSPASURF;
-	u32 *mipi_val = &regs->saveMIPI;
-	u32 *dspcntr_val = &regs->saveDSPACNTR;
-	u32 *dspstatus_val = &regs->saveDSPASTATUS;
-	u32 *palette_val = regs->save_palette_a;
-
-	switch (pipe) {
+
+	switch (pipenum) {
 	case 0:
+		mipi_val = &regs->saveMIPI;
 		break;
 	case 1:
-		/* regester */
-		dpll_reg = MDFLD_DPLL_B;
-		fp_reg = MDFLD_DPLL_DIV0;
-		pipeconf_reg = PIPEBCONF;
-		htot_reg = HTOTAL_B;
-		hblank_reg = HBLANK_B;
-		hsync_reg = HSYNC_B;
-		vtot_reg = VTOTAL_B;
-		vblank_reg = VBLANK_B;
-		vsync_reg = VSYNC_B;
-		pipesrc_reg = PIPEBSRC;
-		dspstride_reg = DSPBSTRIDE;
-		dsplinoff_reg = DSPBLINOFF;
-		dsptileoff_reg = DSPBTILEOFF;
-		dspsize_reg = DSPBSIZE;
-		dsppos_reg = DSPBPOS;
-		dspsurf_reg = DSPBSURF;
-		dspcntr_reg = DSPBCNTR;
-		dspstatus_reg = PIPEBSTAT;
-		palette_reg = PALETTE_B;
-
-		/* values */
-		dpll_val = &regs->saveDPLL_B;
-		fp_val = &regs->saveFPB0;
-		pipeconf_val = &regs->savePIPEBCONF;
-		htot_val = &regs->saveHTOTAL_B;
-		hblank_val = &regs->saveHBLANK_B;
-		hsync_val = &regs->saveHSYNC_B;
-		vtot_val = &regs->saveVTOTAL_B;
-		vblank_val = &regs->saveVBLANK_B;
-		vsync_val = &regs->saveVSYNC_B;
-		pipesrc_val = &regs->savePIPEBSRC;
-		dspstride_val = &regs->saveDSPBSTRIDE;
-		dsplinoff_val = &regs->saveDSPBLINOFF;
-		dsptileoff_val = &regs->saveDSPBTILEOFF;
-		dspsize_val = &regs->saveDSPBSIZE;
-		dsppos_val = &regs->saveDSPBPOS;
-		dspsurf_val = &regs->saveDSPBSURF;
-		dspcntr_val = &regs->saveDSPBCNTR;
-		dspstatus_val = &regs->saveDSPBSTATUS;
-		palette_val = regs->save_palette_b;
+		mipi_val = &regs->saveMIPI;
 		break;
 	case 2:
 		/* register */
-		pipeconf_reg = PIPECCONF;
-		htot_reg = HTOTAL_C;
-		hblank_reg = HBLANK_C;
-		hsync_reg = HSYNC_C;
-		vtot_reg = VTOTAL_C;
-		vblank_reg = VBLANK_C;
-		vsync_reg = VSYNC_C;
-		pipesrc_reg = PIPECSRC;
-		dspstride_reg = DSPCSTRIDE;
-		dsplinoff_reg = DSPCLINOFF;
-		dsptileoff_reg = DSPCTILEOFF;
-		dspsize_reg = DSPCSIZE;
-		dsppos_reg = DSPCPOS;
-		dspsurf_reg = DSPCSURF;
 		mipi_reg = MIPI_C;
-		dspcntr_reg = DSPCCNTR;
-		dspstatus_reg = PIPECSTAT;
-		palette_reg = PALETTE_C;
-
 		/* pointer to values */
-		pipeconf_val = &regs->savePIPECCONF;
-		htot_val = &regs->saveHTOTAL_C;
-		hblank_val = &regs->saveHBLANK_C;
-		hsync_val = &regs->saveHSYNC_C;
-		vtot_val = &regs->saveVTOTAL_C;
-		vblank_val = &regs->saveVBLANK_C;
-		vsync_val = &regs->saveVSYNC_C;
-		pipesrc_val = &regs->savePIPECSRC;
-		dspstride_val = &regs->saveDSPCSTRIDE;
-		dsplinoff_val = &regs->saveDSPCLINOFF;
-		dsptileoff_val = &regs->saveDSPCTILEOFF;
-		dspsize_val = &regs->saveDSPCSIZE;
-		dsppos_val = &regs->saveDSPCPOS;
-		dspsurf_val = &regs->saveDSPCSURF;
 		mipi_val = &regs->saveMIPI_C;
-		dspcntr_val = &regs->saveDSPCCNTR;
-		dspstatus_val = &regs->saveDSPCSTATUS;
-		palette_val = regs->save_palette_c;
 		break;
 	default:
 		DRM_ERROR("%s, invalid pipe number.\n", __func__);
@@ -306,30 +194,30 @@ static int mdfld_save_display_registers(struct drm_device *dev, int pipe)
 	}
 
 	/* Pipe & plane A info */
-	*dpll_val = PSB_RVDC32(dpll_reg);
-	*fp_val = PSB_RVDC32(fp_reg);
-	*pipeconf_val = PSB_RVDC32(pipeconf_reg);
-	*htot_val = PSB_RVDC32(htot_reg);
-	*hblank_val = PSB_RVDC32(hblank_reg);
-	*hsync_val = PSB_RVDC32(hsync_reg);
-	*vtot_val = PSB_RVDC32(vtot_reg);
-	*vblank_val = PSB_RVDC32(vblank_reg);
-	*vsync_val = PSB_RVDC32(vsync_reg);
-	*pipesrc_val = PSB_RVDC32(pipesrc_reg);
-	*dspstride_val = PSB_RVDC32(dspstride_reg);
-	*dsplinoff_val = PSB_RVDC32(dsplinoff_reg);
-	*dsptileoff_val = PSB_RVDC32(dsptileoff_reg);
-	*dspsize_val = PSB_RVDC32(dspsize_reg);
-	*dsppos_val = PSB_RVDC32(dsppos_reg);
-	*dspsurf_val = PSB_RVDC32(dspsurf_reg);
-	*dspcntr_val = PSB_RVDC32(dspcntr_reg);
-	*dspstatus_val = PSB_RVDC32(dspstatus_reg);
+	pipe->dpll = PSB_RVDC32(map->dpll);
+	pipe->fp0 = PSB_RVDC32(map->fp0);
+	pipe->conf = PSB_RVDC32(map->conf);
+	pipe->htotal = PSB_RVDC32(map->htotal);
+	pipe->hblank = PSB_RVDC32(map->hblank);
+	pipe->hsync = PSB_RVDC32(map->hsync);
+	pipe->vtotal = PSB_RVDC32(map->vtotal);
+	pipe->vblank = PSB_RVDC32(map->vblank);
+	pipe->vsync = PSB_RVDC32(map->vsync);
+	pipe->src = PSB_RVDC32(map->src);
+	pipe->stride = PSB_RVDC32(map->stride);
+	pipe->linoff = PSB_RVDC32(map->linoff);
+	pipe->tileoff = PSB_RVDC32(map->tileoff);
+	pipe->size = PSB_RVDC32(map->size);
+	pipe->pos = PSB_RVDC32(map->pos);
+	pipe->surf = PSB_RVDC32(map->surf);
+	pipe->cntr = PSB_RVDC32(map->cntr);
+	pipe->status = PSB_RVDC32(map->status);
 
 	/*save palette (gamma) */
 	for (i = 0; i < 256; i++)
-		palette_val[i] = PSB_RVDC32(palette_reg + (i << 2));
+		pipe->palette[i] = PSB_RVDC32(map->palette + (i << 2));
 
-	if (pipe == 1) {
+	if (pipenum == 1) {
 		regs->savePFIT_CONTROL = PSB_RVDC32(PFIT_CONTROL);
 		regs->savePFIT_PGM_RATIOS = PSB_RVDC32(PFIT_PGM_RATIOS);
 
@@ -349,7 +237,7 @@ static int mdfld_save_display_registers(struct drm_device *dev, int pipe)
  *
  * Notes: FIXME_JLIU7 need to add the support for DPI MIPI & HDMI audio
  */
-static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
+static int mdfld_restore_display_registers(struct drm_device *dev, int pipenum)
 {
 	/* To get  panel out of ULPS mode. */
 	u32 temp = 0;
@@ -357,142 +245,30 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct mdfld_dsi_config *dsi_config = NULL;
 	struct medfield_state *regs = &dev_priv->regs.mdfld;
-	u32 i = 0;
-	u32 dpll = 0;
+	struct psb_pipe *pipe = &dev_priv->regs.pipe[pipenum];
+	const struct psb_offset *map = &dev_priv->regmap[pipenum];
+	u32 i;
+	u32 dpll;
 	u32 timeout = 0;
 
-	/* regester */
-	u32 dpll_reg = MRST_DPLL_A;
-	u32 fp_reg = MRST_FPA0;
-	u32 pipeconf_reg = PIPEACONF;
-	u32 htot_reg = HTOTAL_A;
-	u32 hblank_reg = HBLANK_A;
-	u32 hsync_reg = HSYNC_A;
-	u32 vtot_reg = VTOTAL_A;
-	u32 vblank_reg = VBLANK_A;
-	u32 vsync_reg = VSYNC_A;
-	u32 pipesrc_reg = PIPEASRC;
-	u32 dspstride_reg = DSPASTRIDE;
-	u32 dsplinoff_reg = DSPALINOFF;
-	u32 dsptileoff_reg = DSPATILEOFF;
-	u32 dspsize_reg = DSPASIZE;
-	u32 dsppos_reg = DSPAPOS;
-	u32 dspsurf_reg = DSPASURF;
-	u32 dspstatus_reg = PIPEASTAT;
+	/* register */
 	u32 mipi_reg = MIPI;
-	u32 dspcntr_reg = DSPACNTR;
-	u32 palette_reg = PALETTE_A;
 
 	/* values */
-	u32 dpll_val = regs->saveDPLL_A & ~DPLL_VCO_ENABLE;
-	u32 fp_val = regs->saveFPA0;
-	u32 pipeconf_val = regs->savePIPEACONF;
-	u32 htot_val = regs->saveHTOTAL_A;
-	u32 hblank_val = regs->saveHBLANK_A;
-	u32 hsync_val = regs->saveHSYNC_A;
-	u32 vtot_val = regs->saveVTOTAL_A;
-	u32 vblank_val = regs->saveVBLANK_A;
-	u32 vsync_val = regs->saveVSYNC_A;
-	u32 pipesrc_val = regs->savePIPEASRC;
-	u32 dspstride_val = regs->saveDSPASTRIDE;
-	u32 dsplinoff_val = regs->saveDSPALINOFF;
-	u32 dsptileoff_val = regs->saveDSPATILEOFF;
-	u32 dspsize_val = regs->saveDSPASIZE;
-	u32 dsppos_val = regs->saveDSPAPOS;
-	u32 dspsurf_val = regs->saveDSPASURF;
-	u32 dspstatus_val = regs->saveDSPASTATUS;
+	u32 dpll_val = pipe->dpll;
 	u32 mipi_val = regs->saveMIPI;
-	u32 dspcntr_val = regs->saveDSPACNTR;
-	u32 *palette_val = regs->save_palette_a;
 
-	switch (pipe) {
+	switch (pipenum) {
 	case 0:
+		dpll_val &= ~DPLL_VCO_ENABLE;
 		dsi_config = dev_priv->dsi_configs[0];
 		break;
 	case 1:
-		/* regester */
-		dpll_reg = MDFLD_DPLL_B;
-		fp_reg = MDFLD_DPLL_DIV0;
-		pipeconf_reg = PIPEBCONF;
-		htot_reg = HTOTAL_B;
-		hblank_reg = HBLANK_B;
-		hsync_reg = HSYNC_B;
-		vtot_reg = VTOTAL_B;
-		vblank_reg = VBLANK_B;
-		vsync_reg = VSYNC_B;
-		pipesrc_reg = PIPEBSRC;
-		dspstride_reg = DSPBSTRIDE;
-		dsplinoff_reg = DSPBLINOFF;
-		dsptileoff_reg = DSPBTILEOFF;
-		dspsize_reg = DSPBSIZE;
-		dsppos_reg = DSPBPOS;
-		dspsurf_reg = DSPBSURF;
-		dspcntr_reg = DSPBCNTR;
-		dspstatus_reg = PIPEBSTAT;
-		palette_reg = PALETTE_B;
-
-		/* values */
-		dpll_val = regs->saveDPLL_B & ~DPLL_VCO_ENABLE;
-		fp_val = regs->saveFPB0;
-		pipeconf_val = regs->savePIPEBCONF;
-		htot_val = regs->saveHTOTAL_B;
-		hblank_val = regs->saveHBLANK_B;
-		hsync_val = regs->saveHSYNC_B;
-		vtot_val = regs->saveVTOTAL_B;
-		vblank_val = regs->saveVBLANK_B;
-		vsync_val = regs->saveVSYNC_B;
-		pipesrc_val = regs->savePIPEBSRC;
-		dspstride_val = regs->saveDSPBSTRIDE;
-		dsplinoff_val = regs->saveDSPBLINOFF;
-		dsptileoff_val = regs->saveDSPBTILEOFF;
-		dspsize_val = regs->saveDSPBSIZE;
-		dsppos_val = regs->saveDSPBPOS;
-		dspsurf_val = regs->saveDSPBSURF;
-		dspcntr_val = regs->saveDSPBCNTR;
-		dspstatus_val = regs->saveDSPBSTATUS;
-		palette_val = regs->save_palette_b;
+		dpll_val &= ~DPLL_VCO_ENABLE;
 		break;
 	case 2:
-		/* regester */
-		pipeconf_reg = PIPECCONF;
-		htot_reg = HTOTAL_C;
-		hblank_reg = HBLANK_C;
-		hsync_reg = HSYNC_C;
-		vtot_reg = VTOTAL_C;
-		vblank_reg = VBLANK_C;
-		vsync_reg = VSYNC_C;
-		pipesrc_reg = PIPECSRC;
-		dspstride_reg = DSPCSTRIDE;
-		dsplinoff_reg = DSPCLINOFF;
-		dsptileoff_reg = DSPCTILEOFF;
-		dspsize_reg = DSPCSIZE;
-		dsppos_reg = DSPCPOS;
-		dspsurf_reg = DSPCSURF;
 		mipi_reg = MIPI_C;
-		dspcntr_reg = DSPCCNTR;
-		dspstatus_reg = PIPECSTAT;
-		palette_reg = PALETTE_C;
-
-		/* values */
-		pipeconf_val = regs->savePIPECCONF;
-		htot_val = regs->saveHTOTAL_C;
-		hblank_val = regs->saveHBLANK_C;
-		hsync_val = regs->saveHSYNC_C;
-		vtot_val = regs->saveVTOTAL_C;
-		vblank_val = regs->saveVBLANK_C;
-		vsync_val = regs->saveVSYNC_C;
-		pipesrc_val = regs->savePIPECSRC;
-		dspstride_val = regs->saveDSPCSTRIDE;
-		dsplinoff_val = regs->saveDSPCLINOFF;
-		dsptileoff_val = regs->saveDSPCTILEOFF;
-		dspsize_val = regs->saveDSPCSIZE;
-		dsppos_val = regs->saveDSPCPOS;
-		dspsurf_val = regs->saveDSPCSURF;
 		mipi_val = regs->saveMIPI_C;
-		dspcntr_val = regs->saveDSPCCNTR;
-		dspstatus_val = regs->saveDSPCSTATUS;
-		palette_val = regs->save_palette_c;
-
 		dsi_config = dev_priv->dsi_configs[1];
 		break;
 	default:
@@ -503,14 +279,14 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 	/*make sure VGA plane is off. it initializes to on after reset!*/
 	PSB_WVDC32(0x80000000, VGACNTRL);
 
-	if (pipe == 1) {
-		PSB_WVDC32(dpll_val & ~DPLL_VCO_ENABLE, dpll_reg);
-		PSB_RVDC32(dpll_reg);
+	if (pipenum == 1) {
+		PSB_WVDC32(dpll_val & ~DPLL_VCO_ENABLE, map->dpll);
+		PSB_RVDC32(map->dpll);
 
-		PSB_WVDC32(fp_val, fp_reg);
+		PSB_WVDC32(pipe->fp0, map->fp0);
 	} else {
 
-		dpll = PSB_RVDC32(dpll_reg);
+		dpll = PSB_RVDC32(map->dpll);
 
 		if (!(dpll & DPLL_VCO_ENABLE)) {
 
@@ -518,23 +294,23 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 			   before enable the VCO */
 			if (dpll & MDFLD_PWR_GATE_EN) {
 				dpll &= ~MDFLD_PWR_GATE_EN;
-				PSB_WVDC32(dpll, dpll_reg);
+				PSB_WVDC32(dpll, map->dpll);
 				/* FIXME_MDFLD PO - change 500 to 1 after PO */
 				udelay(500);
 			}
 
-			PSB_WVDC32(fp_val, fp_reg);
-			PSB_WVDC32(dpll_val, dpll_reg);
+			PSB_WVDC32(pipe->fp0, map->fp0);
+			PSB_WVDC32(dpll_val, map->dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 
 			dpll_val |= DPLL_VCO_ENABLE;
-			PSB_WVDC32(dpll_val, dpll_reg);
-			PSB_RVDC32(dpll_reg);
+			PSB_WVDC32(dpll_val, map->dpll);
+			PSB_RVDC32(map->dpll);
 
 			/* wait for DSI PLL to lock */
 			while (timeout < 20000 &&
-			  !(PSB_RVDC32(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+			  !(PSB_RVDC32(map->conf) & PIPECONF_DSIPLL_LOCK)) {
 				udelay(150);
 				timeout++;
 			}
@@ -547,28 +323,28 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 		}
 	}
 	/* Restore mode */
-	PSB_WVDC32(htot_val, htot_reg);
-	PSB_WVDC32(hblank_val, hblank_reg);
-	PSB_WVDC32(hsync_val, hsync_reg);
-	PSB_WVDC32(vtot_val, vtot_reg);
-	PSB_WVDC32(vblank_val, vblank_reg);
-	PSB_WVDC32(vsync_val, vsync_reg);
-	PSB_WVDC32(pipesrc_val, pipesrc_reg);
-	PSB_WVDC32(dspstatus_val, dspstatus_reg);
+	PSB_WVDC32(pipe->htotal, map->htotal);
+	PSB_WVDC32(pipe->hblank, map->hblank);
+	PSB_WVDC32(pipe->hsync, map->hsync);
+	PSB_WVDC32(pipe->vtotal, map->vtotal);
+	PSB_WVDC32(pipe->vblank, map->vblank);
+	PSB_WVDC32(pipe->vsync, map->vsync);
+	PSB_WVDC32(pipe->src, map->src);
+	PSB_WVDC32(pipe->status, map->status);
 
 	/*set up the plane*/
-	PSB_WVDC32(dspstride_val, dspstride_reg);
-	PSB_WVDC32(dsplinoff_val, dsplinoff_reg);
-	PSB_WVDC32(dsptileoff_val, dsptileoff_reg);
-	PSB_WVDC32(dspsize_val, dspsize_reg);
-	PSB_WVDC32(dsppos_val, dsppos_reg);
-	PSB_WVDC32(dspsurf_val, dspsurf_reg);
-
-	if (pipe == 1) {
+	PSB_WVDC32(pipe->stride, map->stride);
+	PSB_WVDC32(pipe->linoff, map->linoff);
+	PSB_WVDC32(pipe->tileoff, map->tileoff);
+	PSB_WVDC32(pipe->size, map->size);
+	PSB_WVDC32(pipe->pos, map->pos);
+	PSB_WVDC32(pipe->surf, map->surf);
+
+	if (pipenum == 1) {
 		/* restore palette (gamma) */
 		/*DRM_UDELAY(50000); */
 		for (i = 0; i < 256; i++)
-			PSB_WVDC32(palette_val[i], palette_reg + (i << 2));
+			PSB_WVDC32(pipe->palette[i], map->palette + (i << 2));
 
 		PSB_WVDC32(regs->savePFIT_CONTROL, PFIT_CONTROL);
 		PSB_WVDC32(regs->savePFIT_PGM_RATIOS, PFIT_PGM_RATIOS);
@@ -578,7 +354,7 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 		/*TODO: resume pipe*/
 
 		/*enable the plane*/
-		PSB_WVDC32(dspcntr_val & ~DISPLAY_PLANE_ENABLE, dspcntr_reg);
+		PSB_WVDC32(pipe->cntr & ~DISPLAY_PLANE_ENABLE, map->cntr);
 
 		return 0;
 	}
@@ -588,7 +364,7 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 
 	/*setup MIPI adapter + MIPI IP registers*/
 	if (dsi_config)
-		mdfld_dsi_controller_init(dsi_config, pipe);
+		mdfld_dsi_controller_init(dsi_config, pipenum);
 
 	if (in_atomic() || in_interrupt())
 		mdelay(20);
@@ -596,7 +372,7 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 		msleep(20);
 
 	/*enable the plane*/
-	PSB_WVDC32(dspcntr_val, dspcntr_reg);
+	PSB_WVDC32(pipe->cntr, map->cntr);
 
 	if (in_atomic() || in_interrupt())
 		mdelay(20);
@@ -625,12 +401,12 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 	mdelay(1);
 
 	/*enable the pipe*/
-	PSB_WVDC32(pipeconf_val, pipeconf_reg);
+	PSB_WVDC32(pipe->conf, map->conf);
 
 	/* restore palette (gamma) */
 	/*DRM_UDELAY(50000); */
 	for (i = 0; i < 256; i++)
-		PSB_WVDC32(palette_val[i], palette_reg + (i << 2));
+		PSB_WVDC32(pipe->palette[i], map->palette + (i << 2));
 
 	return 0;
 }
@@ -667,14 +443,98 @@ static int mdfld_power_up(struct drm_device *dev)
 	return 0;
 }
 
+/* Medfield  */
+static const struct psb_offset mdfld_regmap[3] = {
+	{
+		.fp0 = MRST_FPA0,
+		.fp1 = MRST_FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = MRST_DPLL_A,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.surf = DSPASURF,
+		.addr = MRST_DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = MDFLD_DPLL_DIV0,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = MDFLD_DPLL_B,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.surf = DSPBSURF,
+		.addr = MRST_DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	},
+	{
+		.fp0 = MRST_FPA0,	/* This is what the old code did ?? */
+		.cntr = DSPCCNTR,
+		.conf = PIPECCONF,
+		.src = PIPECSRC,
+		/* No DPLL_C */
+		.dpll = MRST_DPLL_A,
+		.htotal = HTOTAL_C,
+		.hblank = HBLANK_C,
+		.hsync = HSYNC_C,
+		.vtotal = VTOTAL_C,
+		.vblank = VBLANK_C,
+		.vsync = VSYNC_C,
+		.stride = DSPCSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPCPOS,
+		.surf = DSPCSURF,
+		.addr = MDFLD_DSPCBASE,
+		.status = PIPECSTAT,
+		.linoff = DSPCLINOFF,
+		.tileoff = DSPCTILEOFF,
+		.palette = PALETTE_C,
+	},
+};
+
+static int mdfld_chip_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	if (pci_enable_msi(dev->pdev))
+		dev_warn(dev->dev, "Enabling MSI failed!\n");
+	dev_priv->regmap = mdfld_regmap;
+	return mid_chip_setup(dev);
+}
+
 const struct psb_ops mdfld_chip_ops = {
 	.name = "mdfld",
 	.accel_2d = 0,
 	.pipes = 3,
 	.crtcs = 3,
+	.lvds_mask = (1 << 1),
+	.hdmi_mask = (1 << 1),
+	.cursor_needs_phys = 0,
 	.sgx_offset = MRST_SGX_OFFSET,
 
-	.chip_setup = mid_chip_setup,
+	.chip_setup = mdfld_chip_setup,
 	.crtc_helper = &mdfld_helper_funcs,
 	.crtc_funcs = &psb_intel_crtc_funcs,
 
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
index d52358b744a0..b34ff097b979 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
@@ -869,7 +869,6 @@ void mdfld_dsi_dpi_mode_set(struct drm_encoder *encoder,
 		mdfld_set_pipe_timing(dsi_config, pipe);
 
 		REG_WRITE(DSPABASE, 0x00);
-		REG_WRITE(DSPASTRIDE, (mode->hdisplay * 4));
 		REG_WRITE(DSPASIZE,
 			((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
 
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
index baa0e14165e0..489ffd2c66e5 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
@@ -605,6 +605,8 @@ int mdfld_dsi_pkg_sender_init(struct mdfld_dsi_connector *dsi_connector,
 	struct mdfld_dsi_config *dsi_config =
 				mdfld_dsi_get_config(dsi_connector);
 	struct drm_device *dev = dsi_config->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 mipi_val = 0;
 
 	if (!dsi_connector) {
@@ -632,21 +634,13 @@ int mdfld_dsi_pkg_sender_init(struct mdfld_dsi_connector *dsi_connector,
 	pkg_sender->status = MDFLD_DSI_PKG_SENDER_FREE;
 
 	/*init regs*/
-	if (pipe == 0) {
-		pkg_sender->dpll_reg = MRST_DPLL_A;
-		pkg_sender->dspcntr_reg = DSPACNTR;
-		pkg_sender->pipeconf_reg = PIPEACONF;
-		pkg_sender->dsplinoff_reg = DSPALINOFF;
-		pkg_sender->dspsurf_reg = DSPASURF;
-		pkg_sender->pipestat_reg = PIPEASTAT;
-	} else if (pipe == 2) {
-		pkg_sender->dpll_reg = MRST_DPLL_A;
-		pkg_sender->dspcntr_reg = DSPCCNTR;
-		pkg_sender->pipeconf_reg = PIPECCONF;
-		pkg_sender->dsplinoff_reg = DSPCLINOFF;
-		pkg_sender->dspsurf_reg = DSPCSURF;
-		pkg_sender->pipestat_reg = PIPECSTAT;
-	}
+	/* FIXME: should just copy the regmap ptr ? */
+	pkg_sender->dpll_reg = map->dpll;
+	pkg_sender->dspcntr_reg = map->cntr;
+	pkg_sender->pipeconf_reg = map->conf;
+	pkg_sender->dsplinoff_reg = map->linoff;
+	pkg_sender->dspsurf_reg = map->surf;
+	pkg_sender->pipestat_reg = map->status;
 
 	pkg_sender->mipi_intr_stat_reg = MIPI_INTR_STAT_REG(pipe);
 	pkg_sender->mipi_lp_gen_data_reg = MIPI_LP_GEN_DATA_REG(pipe);
diff --git a/drivers/gpu/drm/gma500/mdfld_intel_display.c b/drivers/gpu/drm/gma500/mdfld_intel_display.c
index a35a2921bdf7..3f3cd619c79f 100644
--- a/drivers/gpu/drm/gma500/mdfld_intel_display.c
+++ b/drivers/gpu/drm/gma500/mdfld_intel_display.c
@@ -50,17 +50,14 @@ struct mrst_clock_t {
 
 void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int count, temp;
-	u32 pipeconf_reg = PIPEACONF;
 
 	switch (pipe) {
 	case 0:
-		break;
 	case 1:
-		pipeconf_reg = PIPEBCONF;
-		break;
 	case 2:
-		pipeconf_reg = PIPECCONF;
 		break;
 	default:
 		DRM_ERROR("Illegal Pipe Number.\n");
@@ -73,7 +70,7 @@ void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe)
 
 	/* Wait for for the pipe disable to take effect. */
 	for (count = 0; count < COUNT_MAX; count++) {
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_PIPE_STATE) == 0)
 			break;
 	}
@@ -81,17 +78,14 @@ void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe)
 
 void mdfldWaitForPipeEnable(struct drm_device *dev, int pipe)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int count, temp;
-	u32 pipeconf_reg = PIPEACONF;
 
 	switch (pipe) {
 	case 0:
-		break;
 	case 1:
-		pipeconf_reg = PIPEBCONF;
-		break;
 	case 2:
-		pipeconf_reg = PIPECCONF;
 		break;
 	default:
 		DRM_ERROR("Illegal Pipe Number.\n");
@@ -104,7 +98,7 @@ void mdfldWaitForPipeEnable(struct drm_device *dev, int pipe)
 
 	/* Wait for for the pipe enable to take effect. */
 	for (count = 0; count < COUNT_MAX; count++) {
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_PIPE_STATE) == 1)
 			break;
 	}
@@ -189,15 +183,12 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 				struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_i915_master_private *master_priv; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
-	int dsplinoff = DSPALINOFF;
-	int dspsurf = DSPASURF;
-	int dspstride = DSPASTRIDE;
-	int dspcntr_reg = DSPACNTR;
 	u32 dspcntr;
 	int ret;
 
@@ -215,23 +206,7 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	if (ret)
 		return ret;
 
-	switch (pipe) {
-	case 0:
-		dsplinoff = DSPALINOFF;
-		break;
-	case 1:
-		dsplinoff = DSPBLINOFF;
-		dspsurf = DSPBSURF;
-		dspstride = DSPBSTRIDE;
-		dspcntr_reg = DSPBCNTR;
-		break;
-	case 2:
-		dsplinoff = DSPCLINOFF;
-		dspsurf = DSPCSURF;
-		dspstride = DSPCSTRIDE;
-		dspcntr_reg = DSPCCNTR;
-		break;
-	default:
+	if (pipe > 2) {
 		DRM_ERROR("Illegal Pipe Number.\n");
 		return -EINVAL;
 	}
@@ -242,8 +217,8 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	start = psbfb->gtt->offset;
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
-	dspcntr = REG_READ(dspcntr_reg);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -261,14 +236,14 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
 		break;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	dev_dbg(dev->dev, "Writing base %08lX %08lX %d %d\n",
 						start, offset, x, y);
-	REG_WRITE(dsplinoff, offset);
-	REG_READ(dsplinoff);
-	REG_WRITE(dspsurf, start);
-	REG_READ(dspsurf);
+	REG_WRITE(map->linoff, offset);
+	REG_READ(map->linoff);
+	REG_WRITE(map->surf, start);
+	REG_READ(map->surf);
 
 	gma_power_end(dev);
 
@@ -281,78 +256,56 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
  */
 void mdfld_disable_crtc(struct drm_device *dev, int pipe)
 {
-	int dpll_reg = MRST_DPLL_A;
-	int dspcntr_reg = DSPACNTR;
-	int dspbase_reg = MRST_DSPABASE;
-	int pipeconf_reg = PIPEACONF;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	dev_dbg(dev->dev, "pipe = %d\n", pipe);
 
 
-	switch (pipe) {
-	case 0:
-		break;
-	case 1:
-		dpll_reg = MDFLD_DPLL_B;
-		dspcntr_reg = DSPBCNTR;
-		dspbase_reg = DSPBSURF;
-		pipeconf_reg = PIPEBCONF;
-		break;
-	case 2:
-		dpll_reg = MRST_DPLL_A;
-		dspcntr_reg = DSPCCNTR;
-		dspbase_reg = MDFLD_DSPCBASE;
-		pipeconf_reg = PIPECCONF;
-		break;
-	default:
-		DRM_ERROR("Illegal Pipe Number.\n");
-		return;
-	}
-
 	if (pipe != 1)
 		mdfld_dsi_gen_fifo_ready(dev, MIPI_GEN_FIFO_STAT_REG(pipe),
 				HS_CTRL_FIFO_EMPTY | HS_DATA_FIFO_EMPTY);
 
 	/* Disable display plane */
-	temp = REG_READ(dspcntr_reg);
+	temp = REG_READ(map->cntr);
 	if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-		REG_WRITE(dspcntr_reg,
+		REG_WRITE(map->cntr,
 			  temp & ~DISPLAY_PLANE_ENABLE);
 		/* Flush the plane changes */
-		REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-		REG_READ(dspbase_reg);
+		REG_WRITE(map->base, REG_READ(map->base));
+		REG_READ(map->base);
 	}
 
 	/* FIXME_JLIU7 MDFLD_PO revisit */
 
 	/* Next, disable display pipes */
-	temp = REG_READ(pipeconf_reg);
+	temp = REG_READ(map->conf);
 	if ((temp & PIPEACONF_ENABLE) != 0) {
 		temp &= ~PIPEACONF_ENABLE;
 		temp |= PIPECONF_PLANE_OFF | PIPECONF_CURSOR_OFF;
-		REG_WRITE(pipeconf_reg, temp);
-		REG_READ(pipeconf_reg);
+		REG_WRITE(map->conf, temp);
+		REG_READ(map->conf);
 
 		/* Wait for for the pipe disable to take effect. */
 		mdfldWaitForPipeDisable(dev, pipe);
 	}
 
-	temp = REG_READ(dpll_reg);
+	temp = REG_READ(map->dpll);
 	if (temp & DPLL_VCO_ENABLE) {
 		if ((pipe != 1 &&
 			!((REG_READ(PIPEACONF) | REG_READ(PIPECCONF))
 				& PIPEACONF_ENABLE)) || pipe == 1) {
 			temp &= ~(DPLL_VCO_ENABLE);
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to turn off. */
 			/* FIXME_MDFLD PO may need more delay */
 			udelay(500);
 
 			if (!(temp & MDFLD_PWR_GATE_EN)) {
 				/* gating power of DPLL */
-				REG_WRITE(dpll_reg, temp | MDFLD_PWR_GATE_EN);
+				REG_WRITE(map->dpll, temp | MDFLD_PWR_GATE_EN);
 				/* FIXME_MDFLD PO - change 500 to 1 after PO */
 				udelay(5000);
 			}
@@ -373,41 +326,15 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = MRST_DPLL_A;
-	int dspcntr_reg = DSPACNTR;
-	int dspbase_reg = MRST_DSPABASE;
-	int pipeconf_reg = PIPEACONF;
-	u32 pipestat_reg = PIPEASTAT;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 pipeconf = dev_priv->pipeconf[pipe];
 	u32 temp;
 	int timeout = 0;
 
 	dev_dbg(dev->dev, "mode = %d, pipe = %d\n", mode, pipe);
 
-/* FIXME_JLIU7 MDFLD_PO replaced w/ the following function */
-/* mdfld_dbi_dpms (struct drm_device *dev, int pipe, bool enabled) */
-
-	switch (pipe) {
-	case 0:
-		break;
-	case 1:
-		dpll_reg = DPLL_B;
-		dspcntr_reg = DSPBCNTR;
-		dspbase_reg = MRST_DSPBBASE;
-		pipeconf_reg = PIPEBCONF;
-		dpll_reg = MDFLD_DPLL_B;
-		break;
-	case 2:
-		dpll_reg = MRST_DPLL_A;
-		dspcntr_reg = DSPCCNTR;
-		dspbase_reg = MDFLD_DSPCBASE;
-		pipeconf_reg = PIPECCONF;
-		pipestat_reg = PIPECSTAT;
-		break;
-	default:
-		DRM_ERROR("Illegal Pipe Number.\n");
-		return;
-	}
+	/* Note: Old code uses pipe a stat for pipe b but that appears
+	   to be a bug */
 
 	if (!gma_power_begin(dev, true))
 		return;
@@ -420,25 +347,25 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
 			/* When ungating power of DPLL, needs to wait 0.5us
 			   before enable the VCO */
 			if (temp & MDFLD_PWR_GATE_EN) {
 				temp &= ~MDFLD_PWR_GATE_EN;
-				REG_WRITE(dpll_reg, temp);
+				REG_WRITE(map->dpll, temp);
 				/* FIXME_MDFLD PO - change 500 to 1 after PO */
 				udelay(500);
 			}
 
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 
 			/**
 			 * wait for DSI PLL to lock
@@ -446,25 +373,25 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 			 * since both MIPI pipes share the same PLL.
 			 */
 			while ((pipe != 2) && (timeout < 20000) &&
-			  !(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+			  !(REG_READ(map->conf) & PIPECONF_DSIPLL_LOCK)) {
 				udelay(150);
 				timeout++;
 			}
 		}
 
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0) {
-			REG_WRITE(pipeconf_reg, pipeconf);
+			REG_WRITE(map->conf, pipeconf);
 
 			/* Wait for for the pipe enable to take effect. */
 			mdfldWaitForPipeEnable(dev, pipe);
@@ -473,39 +400,39 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 		/*workaround for sighting 3741701 Random X blank display*/
 		/*perform w/a in video mode only on pipe A or C*/
 		if (pipe == 0 || pipe == 2) {
-			REG_WRITE(pipestat_reg, REG_READ(pipestat_reg));
+			REG_WRITE(map->status, REG_READ(map->status));
 			msleep(100);
-			if (PIPE_VBLANK_STATUS & REG_READ(pipestat_reg))
+			if (PIPE_VBLANK_STATUS & REG_READ(map->status))
 				dev_dbg(dev->dev, "OK");
 			else {
 				dev_dbg(dev->dev, "STUCK!!!!");
 				/*shutdown controller*/
-				temp = REG_READ(dspcntr_reg);
-				REG_WRITE(dspcntr_reg,
+				temp = REG_READ(map->cntr);
+				REG_WRITE(map->cntr,
 						temp & ~DISPLAY_PLANE_ENABLE);
-				REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+				REG_WRITE(map->base, REG_READ(map->base));
 				/*mdfld_dsi_dpi_shut_down(dev, pipe);*/
 				REG_WRITE(0xb048, 1);
 				msleep(100);
-				temp = REG_READ(pipeconf_reg);
+				temp = REG_READ(map->conf);
 				temp &= ~PIPEACONF_ENABLE;
-				REG_WRITE(pipeconf_reg, temp);
+				REG_WRITE(map->conf, temp);
 				msleep(100); /*wait for pipe disable*/
 				REG_WRITE(MIPI_DEVICE_READY_REG(pipe), 0);
 				msleep(100);
 				REG_WRITE(0xb004, REG_READ(0xb004));
 				/* try to bring the controller back up again*/
 				REG_WRITE(MIPI_DEVICE_READY_REG(pipe), 1);
-				temp = REG_READ(dspcntr_reg);
-				REG_WRITE(dspcntr_reg,
+				temp = REG_READ(map->cntr);
+				REG_WRITE(map->cntr,
 						temp | DISPLAY_PLANE_ENABLE);
-				REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+				REG_WRITE(map->base, REG_READ(map->base));
 				/*mdfld_dsi_dpi_turn_on(dev, pipe);*/
 				REG_WRITE(0xb048, 2);
 				msleep(100);
-				temp = REG_READ(pipeconf_reg);
+				temp = REG_READ(map->conf);
 				temp |= PIPEACONF_ENABLE;
-				REG_WRITE(pipeconf_reg, temp);
+				REG_WRITE(map->conf, temp);
 			}
 		}
 
@@ -529,35 +456,35 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
 
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
 			temp &= ~PIPEACONF_ENABLE;
 			temp |= PIPECONF_PLANE_OFF | PIPECONF_CURSOR_OFF;
-			REG_WRITE(pipeconf_reg, temp);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp);
+			REG_READ(map->conf);
 
 			/* Wait for for the pipe disable to take effect. */
 			mdfldWaitForPipeDisable(dev, pipe);
 		}
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if (temp & DPLL_VCO_ENABLE) {
 			if ((pipe != 1 && !((REG_READ(PIPEACONF)
 				| REG_READ(PIPECCONF)) & PIPEACONF_ENABLE))
 					|| pipe == 1) {
 				temp &= ~(DPLL_VCO_ENABLE);
-				REG_WRITE(dpll_reg, temp);
-				REG_READ(dpll_reg);
+				REG_WRITE(map->dpll, temp);
+				REG_READ(map->dpll);
 				/* Wait for the clocks to turn off. */
 				/* FIXME_MDFLD PO may need more delay */
 				udelay(500);
@@ -764,21 +691,7 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = psb_intel_crtc->pipe;
-	int fp_reg = MRST_FPA0;
-	int dpll_reg = MRST_DPLL_A;
-	int dspcntr_reg = DSPACNTR;
-	int pipeconf_reg = PIPEACONF;
-	int htot_reg = HTOTAL_A;
-	int hblank_reg = HBLANK_A;
-	int hsync_reg = HSYNC_A;
-	int vtot_reg = VTOTAL_A;
-	int vblank_reg = VBLANK_A;
-	int vsync_reg = VSYNC_A;
-	int dspsize_reg = DSPASIZE;
-	int dsppos_reg = DSPAPOS;
-	int pipesrc_reg = PIPEASRC;
-	u32 *pipeconf = &dev_priv->pipeconf[pipe];
-	u32 *dspcntr = &dev_priv->dspcntr[pipe];
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk = 0;
 	int clk_n = 0, clk_p2 = 0, clk_byte = 1, clk = 0, m_conv = 0,
 								clk_tmp = 0;
@@ -806,45 +719,6 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 	}
 #endif
 
-	switch (pipe) {
-	case 0:
-		break;
-	case 1:
-		fp_reg = FPB0;
-		dpll_reg = DPLL_B;
-		dspcntr_reg = DSPBCNTR;
-		pipeconf_reg = PIPEBCONF;
-		htot_reg = HTOTAL_B;
-		hblank_reg = HBLANK_B;
-		hsync_reg = HSYNC_B;
-		vtot_reg = VTOTAL_B;
-		vblank_reg = VBLANK_B;
-		vsync_reg = VSYNC_B;
-		dspsize_reg = DSPBSIZE;
-		dsppos_reg = DSPBPOS;
-		pipesrc_reg = PIPEBSRC;
-		fp_reg = MDFLD_DPLL_DIV0;
-		dpll_reg = MDFLD_DPLL_B;
-		break;
-	case 2:
-		dpll_reg = MRST_DPLL_A;
-		dspcntr_reg = DSPCCNTR;
-		pipeconf_reg = PIPECCONF;
-		htot_reg = HTOTAL_C;
-		hblank_reg = HBLANK_C;
-		hsync_reg = HSYNC_C;
-		vtot_reg = VTOTAL_C;
-		vblank_reg = VBLANK_C;
-		vsync_reg = VSYNC_C;
-		dspsize_reg = DSPCSIZE;
-		dsppos_reg = DSPCPOS;
-		pipesrc_reg = PIPECSRC;
-		break;
-	default:
-		DRM_ERROR("Illegal Pipe Number.\n");
-		return 0;
-	}
-
 	ret = check_fb(crtc->fb);
 	if (ret)
 		return ret;
@@ -929,21 +803,21 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		 * contained within the displayable area of the screen image
 		 * (frame buffer).
 		 */
-		REG_WRITE(dspsize_reg, ((min(mode->crtc_vdisplay, adjusted_mode->crtc_vdisplay) - 1) << 16)
+		REG_WRITE(map->size, ((min(mode->crtc_vdisplay, adjusted_mode->crtc_vdisplay) - 1) << 16)
 				| (min(mode->crtc_hdisplay, adjusted_mode->crtc_hdisplay) - 1));
 		/* Set the CRTC with encoder mode. */
-		REG_WRITE(pipesrc_reg, ((mode->crtc_hdisplay - 1) << 16)
+		REG_WRITE(map->src, ((mode->crtc_hdisplay - 1) << 16)
 				 | (mode->crtc_vdisplay - 1));
 	} else {
-		REG_WRITE(dspsize_reg,
+		REG_WRITE(map->size,
 				((mode->crtc_vdisplay - 1) << 16) |
 						(mode->crtc_hdisplay - 1));
-		REG_WRITE(pipesrc_reg,
+		REG_WRITE(map->src,
 				((mode->crtc_hdisplay - 1) << 16) |
 						(mode->crtc_vdisplay - 1));
 	}
 
-	REG_WRITE(dsppos_reg, 0);
+	REG_WRITE(map->pos, 0);
 
 	if (psb_intel_encoder)
 		drm_connector_property_get_value(connector,
@@ -961,34 +835,34 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		offsetY = (adjusted_mode->crtc_vdisplay -
 					mode->crtc_vdisplay) / 2;
 
-		REG_WRITE(htot_reg, (mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start -
+		REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start -
 								offsetX - 1) |
 			((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
-		REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start -
+		REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start -
 								offsetX - 1) |
 			((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
-		REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start -
+		REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start -
 								offsetY - 1) |
 			((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
-		REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start -
+		REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start -
 								offsetY - 1) |
 			((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
 	} else {
-		REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+		REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 			((adjusted_mode->crtc_hblank_end - 1) << 16));
-		REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+		REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 			((adjusted_mode->crtc_hsync_end - 1) << 16));
-		REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+		REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 			((adjusted_mode->crtc_vblank_end - 1) << 16));
-		REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+		REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 			((adjusted_mode->crtc_vsync_end - 1) << 16));
 	}
 
@@ -1000,12 +874,12 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 	}
 
 	/* setup pipeconf */
-	*pipeconf = PIPEACONF_ENABLE; /* FIXME_JLIU7 REG_READ(pipeconf_reg); */
+	dev_priv->pipeconf[pipe] = PIPEACONF_ENABLE; /* FIXME_JLIU7 REG_READ(pipeconf_reg); */
 
 	/* Set up the display plane register */
-	*dspcntr = REG_READ(dspcntr_reg);
-	*dspcntr |= pipe << DISPPLANE_SEL_PIPE_POS;
-	*dspcntr |= DISPLAY_PLANE_ENABLE;
+	dev_priv->dspcntr[pipe] = REG_READ(map->cntr);
+	dev_priv->dspcntr[pipe] |= pipe << DISPPLANE_SEL_PIPE_POS;
+	dev_priv->dspcntr[pipe] |= DISPLAY_PLANE_ENABLE;
 
 	if (is_mipi2)
 		goto mrst_crtc_mode_set_exit;
@@ -1070,21 +944,21 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 					clock.p1, m_conv);
 		}
 
-		dpll = REG_READ(dpll_reg);
+		dpll = REG_READ(map->dpll);
 
 		if (dpll & DPLL_VCO_ENABLE) {
 			dpll &= ~DPLL_VCO_ENABLE;
-			REG_WRITE(dpll_reg, dpll);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, dpll);
+			REG_READ(map->dpll);
 
 			/* FIXME jliu7 check the DPLL lock bit PIPEACONF[29] */
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 
 			/* reset M1, N1 & P1 */
-			REG_WRITE(fp_reg, 0);
+			REG_WRITE(map->fp0, 0);
 			dpll &= ~MDFLD_P1_MASK;
-			REG_WRITE(dpll_reg, dpll);
+			REG_WRITE(map->dpll, dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 		}
@@ -1093,7 +967,7 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		 * enable the VCO */
 		if (dpll & MDFLD_PWR_GATE_EN) {
 			dpll &= ~MDFLD_PWR_GATE_EN;
-			REG_WRITE(dpll_reg, dpll);
+			REG_WRITE(map->dpll, dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 		}
@@ -1134,18 +1008,18 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		fp = 0x000000c1;
 	}
 
-	REG_WRITE(fp_reg, fp);
-	REG_WRITE(dpll_reg, dpll);
+	REG_WRITE(map->fp0, fp);
+	REG_WRITE(map->dpll, dpll);
 	/* FIXME_MDFLD PO - change 500 to 1 after PO */
 	udelay(500);
 
 	dpll |= DPLL_VCO_ENABLE;
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 
 	/* wait for DSI PLL to lock */
 	while (timeout < 20000 &&
-			!(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+			!(REG_READ(map->conf) & PIPECONF_DSIPLL_LOCK)) {
 		udelay(150);
 		timeout++;
 	}
@@ -1155,11 +1029,11 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 
 	dev_dbg(dev->dev, "is_mipi = 0x%x\n", is_mipi);
 
-	REG_WRITE(pipeconf_reg, *pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, dev_priv->pipeconf[pipe]);
+	REG_READ(map->conf);
 
 	/* Wait for for the pipe enable to take effect. */
-	REG_WRITE(dspcntr_reg, *dspcntr);
+	REG_WRITE(map->cntr, dev_priv->dspcntr[pipe]);
 	psb_intel_wait_for_vblank(dev);
 
 mrst_crtc_mode_set_exit:
diff --git a/drivers/gpu/drm/gma500/mid_bios.c b/drivers/gpu/drm/gma500/mid_bios.c
index 5eee9ad80da4..b2a790bd9899 100644
--- a/drivers/gpu/drm/gma500/mid_bios.c
+++ b/drivers/gpu/drm/gma500/mid_bios.c
@@ -118,139 +118,214 @@ static void mid_get_pci_revID(struct drm_psb_private *dev_priv)
 					dev_priv->platform_rev_id);
 }
 
+struct vbt_header {
+	u32 signature;
+	u8 revision;
+} __packed;
+
+/* The same for r0 and r1 */
+struct vbt_r0 {
+	struct vbt_header vbt_header;
+	u8 size;
+	u8 checksum;
+} __packed;
+
+struct vbt_r10 {
+	struct vbt_header vbt_header;
+	u8 checksum;
+	u16 size;
+	u8 panel_count;
+	u8 primary_panel_idx;
+	u8 secondary_panel_idx;
+	u8 __reserved[5];
+} __packed;
+
+static int read_vbt_r0(u32 addr, struct vbt_r0 *vbt)
+{
+	void __iomem *vbt_virtual;
+
+	vbt_virtual = ioremap(addr, sizeof(*vbt));
+	if (vbt_virtual == NULL)
+		return -1;
+
+	memcpy_fromio(vbt, vbt_virtual, sizeof(*vbt));
+	iounmap(vbt_virtual);
+
+	return 0;
+}
+
+static int read_vbt_r10(u32 addr, struct vbt_r10 *vbt)
+{
+	void __iomem *vbt_virtual;
+
+	vbt_virtual = ioremap(addr, sizeof(*vbt));
+	if (!vbt_virtual)
+		return -1;
+
+	memcpy_fromio(vbt, vbt_virtual, sizeof(*vbt));
+	iounmap(vbt_virtual);
+
+	return 0;
+}
+
+static int mid_get_vbt_data_r0(struct drm_psb_private *dev_priv, u32 addr)
+{
+	struct vbt_r0 vbt;
+	void __iomem *gct_virtual;
+	struct gct_r0 gct;
+	u8 bpi;
+
+	if (read_vbt_r0(addr, &vbt))
+		return -1;
+
+	gct_virtual = ioremap(addr + sizeof(vbt), vbt.size - sizeof(vbt));
+	if (!gct_virtual)
+		return -1;
+	memcpy_fromio(&gct, gct_virtual, sizeof(gct));
+	iounmap(gct_virtual);
+
+	bpi = gct.PD.BootPanelIndex;
+	dev_priv->gct_data.bpi = bpi;
+	dev_priv->gct_data.pt = gct.PD.PanelType;
+	dev_priv->gct_data.DTD = gct.panel[bpi].DTD;
+	dev_priv->gct_data.Panel_Port_Control =
+		gct.panel[bpi].Panel_Port_Control;
+	dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+		gct.panel[bpi].Panel_MIPI_Display_Descriptor;
+
+	return 0;
+}
+
+static int mid_get_vbt_data_r1(struct drm_psb_private *dev_priv, u32 addr)
+{
+	struct vbt_r0 vbt;
+	void __iomem *gct_virtual;
+	struct gct_r1 gct;
+	u8 bpi;
+
+	if (read_vbt_r0(addr, &vbt))
+		return -1;
+
+	gct_virtual = ioremap(addr + sizeof(vbt), vbt.size - sizeof(vbt));
+	if (!gct_virtual)
+		return -1;
+	memcpy_fromio(&gct, gct_virtual, sizeof(gct));
+	iounmap(gct_virtual);
+
+	bpi = gct.PD.BootPanelIndex;
+	dev_priv->gct_data.bpi = bpi;
+	dev_priv->gct_data.pt = gct.PD.PanelType;
+	dev_priv->gct_data.DTD = gct.panel[bpi].DTD;
+	dev_priv->gct_data.Panel_Port_Control =
+		gct.panel[bpi].Panel_Port_Control;
+	dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+		gct.panel[bpi].Panel_MIPI_Display_Descriptor;
+
+	return 0;
+}
+
+static int mid_get_vbt_data_r10(struct drm_psb_private *dev_priv, u32 addr)
+{
+	struct vbt_r10 vbt;
+	void __iomem *gct_virtual;
+	struct gct_r10 *gct;
+	struct oaktrail_timing_info *dp_ti = &dev_priv->gct_data.DTD;
+	struct gct_r10_timing_info *ti;
+	int ret = -1;
+
+	if (read_vbt_r10(addr, &vbt))
+		return -1;
+
+	gct = kmalloc(sizeof(*gct) * vbt.panel_count, GFP_KERNEL);
+	if (!gct)
+		return -1;
+
+	gct_virtual = ioremap(addr + sizeof(vbt),
+			sizeof(*gct) * vbt.panel_count);
+	if (!gct_virtual)
+		goto out;
+	memcpy_fromio(gct, gct_virtual, sizeof(*gct));
+	iounmap(gct_virtual);
+
+	dev_priv->gct_data.bpi = vbt.primary_panel_idx;
+	dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+		gct[vbt.primary_panel_idx].Panel_MIPI_Display_Descriptor;
+
+	ti = &gct[vbt.primary_panel_idx].DTD;
+	dp_ti->pixel_clock = ti->pixel_clock;
+	dp_ti->hactive_hi = ti->hactive_hi;
+	dp_ti->hactive_lo = ti->hactive_lo;
+	dp_ti->hblank_hi = ti->hblank_hi;
+	dp_ti->hblank_lo = ti->hblank_lo;
+	dp_ti->hsync_offset_hi = ti->hsync_offset_hi;
+	dp_ti->hsync_offset_lo = ti->hsync_offset_lo;
+	dp_ti->hsync_pulse_width_hi = ti->hsync_pulse_width_hi;
+	dp_ti->hsync_pulse_width_lo = ti->hsync_pulse_width_lo;
+	dp_ti->vactive_hi = ti->vactive_hi;
+	dp_ti->vactive_lo = ti->vactive_lo;
+	dp_ti->vblank_hi = ti->vblank_hi;
+	dp_ti->vblank_lo = ti->vblank_lo;
+	dp_ti->vsync_offset_hi = ti->vsync_offset_hi;
+	dp_ti->vsync_offset_lo = ti->vsync_offset_lo;
+	dp_ti->vsync_pulse_width_hi = ti->vsync_pulse_width_hi;
+	dp_ti->vsync_pulse_width_lo = ti->vsync_pulse_width_lo;
+
+	ret = 0;
+out:
+	kfree(gct);
+	return ret;
+}
+
 static void mid_get_vbt_data(struct drm_psb_private *dev_priv)
 {
 	struct drm_device *dev = dev_priv->dev;
-	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
 	u32 addr;
-	u16 new_size;
-	u8 *vbt_virtual;
-	u8 bpi;
-	u8 number_desc = 0;
-	struct oaktrail_timing_info *dp_ti = &dev_priv->gct_data.DTD;
-	struct gct_r10_timing_info ti;
-	void *pGCT;
+	u8 __iomem *vbt_virtual;
+	struct vbt_header vbt_header;
 	struct pci_dev *pci_gfx_root = pci_get_bus_and_slot(0, PCI_DEVFN(2, 0));
+	int ret = -1;
 
-	/* Get the address of the platform config vbt, B0:D2:F0;0xFC */
+	/* Get the address of the platform config vbt */
 	pci_read_config_dword(pci_gfx_root, 0xFC, &addr);
 	pci_dev_put(pci_gfx_root);
 
 	dev_dbg(dev->dev, "drm platform config address is %x\n", addr);
 
-	/* check for platform config address == 0. */
-	/* this means fw doesn't support vbt */
-
-	if (addr == 0) {
-		vbt->size = 0;
-		return;
-	}
+	if (!addr)
+		goto out;
 
 	/* get the virtual address of the vbt */
-	vbt_virtual = ioremap(addr, sizeof(*vbt));
-	if (vbt_virtual == NULL) {
-		vbt->size = 0;
-		return;
-	}
+	vbt_virtual = ioremap(addr, sizeof(vbt_header));
+	if (!vbt_virtual)
+		goto out;
 
-	memcpy(vbt, vbt_virtual, sizeof(*vbt));
-	iounmap(vbt_virtual); /* Free virtual address space */
+	memcpy_fromio(&vbt_header, vbt_virtual, sizeof(vbt_header));
+	iounmap(vbt_virtual);
 
-	/* No matching signature don't process the data */
-	if (memcmp(vbt->signature, "$GCT", 4)) {
-		vbt->size = 0;
-		return;
-	}
+	if (memcmp(&vbt_header.signature, "$GCT", 4))
+		goto out;
+
+	dev_dbg(dev->dev, "GCT revision is %02x\n", vbt_header.revision);
 
-	dev_dbg(dev->dev, "GCT revision is %x\n", vbt->revision);
-
-	switch (vbt->revision) {
-	case 0:
-		vbt->oaktrail_gct = ioremap(addr + sizeof(*vbt) - 4,
-					vbt->size - sizeof(*vbt) + 4);
-		pGCT = vbt->oaktrail_gct;
-		bpi = ((struct oaktrail_gct_v1 *)pGCT)->PD.BootPanelIndex;
-		dev_priv->gct_data.bpi = bpi;
-		dev_priv->gct_data.pt =
-			((struct oaktrail_gct_v1 *)pGCT)->PD.PanelType;
-		memcpy(&dev_priv->gct_data.DTD,
-			&((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].DTD,
-				sizeof(struct oaktrail_timing_info));
-		dev_priv->gct_data.Panel_Port_Control =
-		  ((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].Panel_Port_Control;
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
-			((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+	switch (vbt_header.revision) {
+	case 0x00:
+		ret = mid_get_vbt_data_r0(dev_priv, addr);
 		break;
-	case 1:
-		vbt->oaktrail_gct = ioremap(addr + sizeof(*vbt) - 4,
-					vbt->size - sizeof(*vbt) + 4);
-		pGCT = vbt->oaktrail_gct;
-		bpi = ((struct oaktrail_gct_v2 *)pGCT)->PD.BootPanelIndex;
-		dev_priv->gct_data.bpi = bpi;
-		dev_priv->gct_data.pt =
-			((struct oaktrail_gct_v2 *)pGCT)->PD.PanelType;
-		memcpy(&dev_priv->gct_data.DTD,
-			&((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].DTD,
-				sizeof(struct oaktrail_timing_info));
-		dev_priv->gct_data.Panel_Port_Control =
-		  ((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].Panel_Port_Control;
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
-			((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+	case 0x01:
+		ret = mid_get_vbt_data_r1(dev_priv, addr);
 		break;
 	case 0x10:
-		/*header definition changed from rev 01 (v2) to rev 10h. */
-		/*so, some values have changed location*/
-		new_size = vbt->checksum; /*checksum contains lo size byte*/
-		/*LSB of oaktrail_gct contains hi size byte*/
-		new_size |= ((0xff & (unsigned int)(long)vbt->oaktrail_gct)) << 8;
-
-		vbt->checksum = vbt->size; /*size contains the checksum*/
-		if (new_size > 0xff)
-			vbt->size = 0xff; /*restrict size to 255*/
-		else
-			vbt->size = new_size;
-
-		/* number of descriptors defined in the GCT */
-		number_desc = ((0xff00 & (unsigned int)(long)vbt->oaktrail_gct)) >> 8;
-		bpi = ((0xff0000 & (unsigned int)(long)vbt->oaktrail_gct)) >> 16;
-		vbt->oaktrail_gct = ioremap(addr + GCT_R10_HEADER_SIZE,
-				GCT_R10_DISPLAY_DESC_SIZE * number_desc);
-		pGCT = vbt->oaktrail_gct;
-		pGCT = (u8 *)pGCT + (bpi*GCT_R10_DISPLAY_DESC_SIZE);
-		dev_priv->gct_data.bpi = bpi; /*save boot panel id*/
-
-		/*copy the GCT display timings into a temp structure*/
-		memcpy(&ti, pGCT, sizeof(struct gct_r10_timing_info));
-
-		/*now copy the temp struct into the dev_priv->gct_data*/
-		dp_ti->pixel_clock = ti.pixel_clock;
-		dp_ti->hactive_hi = ti.hactive_hi;
-		dp_ti->hactive_lo = ti.hactive_lo;
-		dp_ti->hblank_hi = ti.hblank_hi;
-		dp_ti->hblank_lo = ti.hblank_lo;
-		dp_ti->hsync_offset_hi = ti.hsync_offset_hi;
-		dp_ti->hsync_offset_lo = ti.hsync_offset_lo;
-		dp_ti->hsync_pulse_width_hi = ti.hsync_pulse_width_hi;
-		dp_ti->hsync_pulse_width_lo = ti.hsync_pulse_width_lo;
-		dp_ti->vactive_hi = ti.vactive_hi;
-		dp_ti->vactive_lo = ti.vactive_lo;
-		dp_ti->vblank_hi = ti.vblank_hi;
-		dp_ti->vblank_lo = ti.vblank_lo;
-		dp_ti->vsync_offset_hi = ti.vsync_offset_hi;
-		dp_ti->vsync_offset_lo = ti.vsync_offset_lo;
-		dp_ti->vsync_pulse_width_hi = ti.vsync_pulse_width_hi;
-		dp_ti->vsync_pulse_width_lo = ti.vsync_pulse_width_lo;
-
-		/* Move the MIPI_Display_Descriptor data from GCT to dev priv */
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
-							*((u8 *)pGCT + 0x0d);
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor |=
-						(*((u8 *)pGCT + 0x0e)) << 8;
+		ret = mid_get_vbt_data_r10(dev_priv, addr);
 		break;
 	default:
 		dev_err(dev->dev, "Unknown revision of GCT!\n");
-		vbt->size = 0;
 	}
+
+out:
+	if (ret)
+		dev_err(dev->dev, "Unable to read GCT!");
+	else
+		dev_priv->has_gct = true;
 }
 
 int mid_chip_setup(struct drm_device *dev)
diff --git a/drivers/gpu/drm/gma500/oaktrail.h b/drivers/gpu/drm/gma500/oaktrail.h
index 2da1f368f14e..f2f9f38a5362 100644
--- a/drivers/gpu/drm/gma500/oaktrail.h
+++ b/drivers/gpu/drm/gma500/oaktrail.h
@@ -19,14 +19,6 @@
 
 /* MID device specific descriptors */
 
-struct oaktrail_vbt {
-	s8 signature[4];	/*4 bytes,"$GCT" */
-	u8 revision;
-	u8 size;
-	u8 checksum;
-	void *oaktrail_gct;
-} __packed;
-
 struct oaktrail_timing_info {
 	u16 pixel_clock;
 	u8 hactive_lo;
@@ -161,7 +153,7 @@ union oaktrail_panel_rx {
 	u16 panel_receiver;
 } __packed;
 
-struct oaktrail_gct_v1 {
+struct gct_r0 {
 	union { /*8 bits,Defined as follows: */
 		struct {
 			u8 PanelType:4; /*4 bits, Bit field for panels*/
@@ -178,7 +170,7 @@ struct oaktrail_gct_v1 {
 	union oaktrail_panel_rx panelrx[4]; /* panel receivers*/
 } __packed;
 
-struct oaktrail_gct_v2 {
+struct gct_r1 {
 	union { /*8 bits,Defined as follows: */
 		struct {
 			u8 PanelType:4; /*4 bits, Bit field for panels*/
@@ -195,6 +187,16 @@ struct oaktrail_gct_v2 {
 	union oaktrail_panel_rx panelrx[4]; /* panel receivers*/
 } __packed;
 
+struct gct_r10 {
+	struct gct_r10_timing_info DTD;
+	u16 Panel_MIPI_Display_Descriptor;
+	u16 Panel_MIPI_Receiver_Descriptor;
+	u16 Panel_Backlight_Inverter_Descriptor;
+	u8 Panel_Initial_Brightness;
+	u32 MIPI_Ctlr_Init_ptr;
+	u32 MIPI_Panel_Init_ptr;
+} __packed;
+
 struct oaktrail_gct_data {
 	u8 bpi; /* boot panel index, number of panel used during boot */
 	u8 pt; /* panel type, 4 bit field, 0=lvds, 1=mipi */
@@ -213,9 +215,6 @@ struct oaktrail_gct_data {
 #define MODE_SETTING_IN_DSR		0x4
 #define MODE_SETTING_ENCODER_DONE	0x8
 
-#define GCT_R10_HEADER_SIZE		16
-#define GCT_R10_DISPLAY_DESC_SIZE	28
-
 /*
  *	Moorestown HDMI interfaces
  */
diff --git a/drivers/gpu/drm/gma500/oaktrail_crtc.c b/drivers/gpu/drm/gma500/oaktrail_crtc.c
index a39b0d0d680f..f821c835ca90 100644
--- a/drivers/gpu/drm/gma500/oaktrail_crtc.c
+++ b/drivers/gpu/drm/gma500/oaktrail_crtc.c
@@ -162,12 +162,10 @@ mrstFindBestPLL(struct drm_crtc *crtc, int target, int refclk,
 static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int dspbase_reg = (pipe == 0) ? MRST_DSPABASE : DSPBBASE;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	if (!gma_power_begin(dev, true))
@@ -181,32 +179,32 @@ static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
 		}
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+			REG_WRITE(map->conf, temp | PIPEACONF_ENABLE);
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		psb_intel_crtc_load_lut(crtc);
@@ -223,28 +221,28 @@ static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 		/* Disable the VGA plane that we never use */
 		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp & ~PIPEACONF_ENABLE);
+			REG_READ(map->conf);
 		}
 		/* Wait for for the pipe disable to take effect. */
 		psb_intel_wait_for_vblank(dev);
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) != 0) {
-			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 		}
 
 		/* Wait for the clocks to turn off. */
@@ -292,17 +290,7 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = psb_intel_crtc->pipe;
-	int fp_reg = (pipe == 0) ? MRST_FPA0 : FPB0;
-	int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
-	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
-	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
-	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
-	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
-	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
-	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
-	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk = 0;
 	struct oaktrail_clock_t clock;
 	u32 dpll = 0, fp = 0, dspcntr, pipeconf;
@@ -350,7 +338,7 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	if (oaktrail_panel_fitter_pipe(dev) == pipe)
 		REG_WRITE(PFIT_CONTROL, 0);
 
-	REG_WRITE(pipesrc_reg,
+	REG_WRITE(map->src,
 		  ((mode->crtc_hdisplay - 1) << 16) |
 		  (mode->crtc_vdisplay - 1));
 
@@ -369,34 +357,34 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 		offsetY = (adjusted_mode->crtc_vdisplay -
 			   mode->crtc_vdisplay) / 2;
 
-		REG_WRITE(htot_reg, (mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg,
+		REG_WRITE(map->hblank,
 			(adjusted_mode->crtc_hblank_start - offsetX - 1) |
 			((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
-		REG_WRITE(hsync_reg,
+		REG_WRITE(map->hsync,
 			(adjusted_mode->crtc_hsync_start - offsetX - 1) |
 			((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
-		REG_WRITE(vblank_reg,
+		REG_WRITE(map->vblank,
 			(adjusted_mode->crtc_vblank_start - offsetY - 1) |
 			((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
-		REG_WRITE(vsync_reg,
+		REG_WRITE(map->vsync,
 			(adjusted_mode->crtc_vsync_start - offsetY - 1) |
 			((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
 	} else {
-		REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+		REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 			((adjusted_mode->crtc_hblank_end - 1) << 16));
-		REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+		REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 			((adjusted_mode->crtc_hsync_end - 1) << 16));
-		REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+		REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 			((adjusted_mode->crtc_vblank_end - 1) << 16));
-		REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+		REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 			((adjusted_mode->crtc_vsync_end - 1) << 16));
 	}
 
@@ -408,10 +396,10 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	}
 
 	/* setup pipeconf */
-	pipeconf = REG_READ(pipeconf_reg);
+	pipeconf = REG_READ(map->conf);
 
 	/* Set up the display plane register */
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr |= DISPPLANE_GAMMA_ENABLE;
 
 	if (pipe == 0)
@@ -467,30 +455,30 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	mrstPrintPll("chosen", &clock);
 
 	if (dpll & DPLL_VCO_ENABLE) {
-		REG_WRITE(fp_reg, fp);
-		REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
-		REG_READ(dpll_reg);
+		REG_WRITE(map->fp0, fp);
+		REG_WRITE(map->dpll, dpll & ~DPLL_VCO_ENABLE);
+		REG_READ(map->dpll);
 		/* Check the DPLLA lock bit PIPEACONF[29] */
 		udelay(150);
 	}
 
-	REG_WRITE(fp_reg, fp);
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->fp0, fp);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
 	/* write it again -- the BIOS does, after all */
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
-	REG_WRITE(pipeconf_reg, pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, pipeconf);
+	REG_READ(map->conf);
 	psb_intel_wait_for_vblank(dev);
 
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 	psb_intel_wait_for_vblank(dev);
 
 oaktrail_crtc_mode_set_exit:
@@ -509,15 +497,13 @@ static int oaktrail_pipe_set_base(struct drm_crtc *crtc,
 			    int x, int y, struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
 
-	int dspbase = (pipe == 0 ? DSPALINOFF : DSPBBASE);
-	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
-	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
 	u32 dspcntr;
 	int ret = 0;
 
@@ -533,9 +519,9 @@ static int oaktrail_pipe_set_base(struct drm_crtc *crtc,
 	start = psbfb->gtt->offset;
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
 
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -557,12 +543,12 @@ static int oaktrail_pipe_set_base(struct drm_crtc *crtc,
 		ret = -EINVAL;
 		goto pipe_set_base_exit;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
-	REG_WRITE(dspbase, offset);
-	REG_READ(dspbase);
-	REG_WRITE(dspsurf, start);
-	REG_READ(dspsurf);
+	REG_WRITE(map->base, offset);
+	REG_READ(map->base);
+	REG_WRITE(map->surf, start);
+	REG_READ(map->surf);
 
 pipe_set_base_exit:
 	gma_power_end(dev);
diff --git a/drivers/gpu/drm/gma500/oaktrail_device.c b/drivers/gpu/drm/gma500/oaktrail_device.c
index 41d1924ea31e..0f9b7db80f6b 100644
--- a/drivers/gpu/drm/gma500/oaktrail_device.c
+++ b/drivers/gpu/drm/gma500/oaktrail_device.c
@@ -187,6 +187,7 @@ static int oaktrail_save_display_registers(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_save_area *regs = &dev_priv->regs;
+	struct psb_pipe *p = &regs->pipe[0];
 	int i;
 	u32 pp_stat;
 
@@ -201,24 +202,24 @@ static int oaktrail_save_display_registers(struct drm_device *dev)
 	regs->psb.saveCHICKENBIT = PSB_RVDC32(DSPCHICKENBIT);
 
 	/* Pipe & plane A info */
-	regs->psb.savePIPEACONF = PSB_RVDC32(PIPEACONF);
-	regs->psb.savePIPEASRC = PSB_RVDC32(PIPEASRC);
-	regs->psb.saveFPA0 = PSB_RVDC32(MRST_FPA0);
-	regs->psb.saveFPA1 = PSB_RVDC32(MRST_FPA1);
-	regs->psb.saveDPLL_A = PSB_RVDC32(MRST_DPLL_A);
-	regs->psb.saveHTOTAL_A = PSB_RVDC32(HTOTAL_A);
-	regs->psb.saveHBLANK_A = PSB_RVDC32(HBLANK_A);
-	regs->psb.saveHSYNC_A = PSB_RVDC32(HSYNC_A);
-	regs->psb.saveVTOTAL_A = PSB_RVDC32(VTOTAL_A);
-	regs->psb.saveVBLANK_A = PSB_RVDC32(VBLANK_A);
-	regs->psb.saveVSYNC_A = PSB_RVDC32(VSYNC_A);
+	p->conf = PSB_RVDC32(PIPEACONF);
+	p->src = PSB_RVDC32(PIPEASRC);
+	p->fp0 = PSB_RVDC32(MRST_FPA0);
+	p->fp1 = PSB_RVDC32(MRST_FPA1);
+	p->dpll = PSB_RVDC32(MRST_DPLL_A);
+	p->htotal = PSB_RVDC32(HTOTAL_A);
+	p->hblank = PSB_RVDC32(HBLANK_A);
+	p->hsync = PSB_RVDC32(HSYNC_A);
+	p->vtotal = PSB_RVDC32(VTOTAL_A);
+	p->vblank = PSB_RVDC32(VBLANK_A);
+	p->vsync = PSB_RVDC32(VSYNC_A);
 	regs->psb.saveBCLRPAT_A = PSB_RVDC32(BCLRPAT_A);
-	regs->psb.saveDSPACNTR = PSB_RVDC32(DSPACNTR);
-	regs->psb.saveDSPASTRIDE = PSB_RVDC32(DSPASTRIDE);
-	regs->psb.saveDSPAADDR = PSB_RVDC32(DSPABASE);
-	regs->psb.saveDSPASURF = PSB_RVDC32(DSPASURF);
-	regs->psb.saveDSPALINOFF = PSB_RVDC32(DSPALINOFF);
-	regs->psb.saveDSPATILEOFF = PSB_RVDC32(DSPATILEOFF);
+	p->cntr = PSB_RVDC32(DSPACNTR);
+	p->stride = PSB_RVDC32(DSPASTRIDE);
+	p->addr = PSB_RVDC32(DSPABASE);
+	p->surf = PSB_RVDC32(DSPASURF);
+	p->linoff = PSB_RVDC32(DSPALINOFF);
+	p->tileoff = PSB_RVDC32(DSPATILEOFF);
 
 	/* Save cursor regs */
 	regs->psb.saveDSPACURSOR_CTRL = PSB_RVDC32(CURACNTR);
@@ -227,7 +228,7 @@ static int oaktrail_save_display_registers(struct drm_device *dev)
 
 	/* Save palette (gamma) */
 	for (i = 0; i < 256; i++)
-		regs->psb.save_palette_a[i] = PSB_RVDC32(PALETTE_A + (i << 2));
+		p->palette[i] = PSB_RVDC32(PALETTE_A + (i << 2));
 
 	if (dev_priv->hdmi_priv)
 		oaktrail_hdmi_save(dev);
@@ -300,6 +301,7 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_save_area *regs = &dev_priv->regs;
+	struct psb_pipe *p = &regs->pipe[0];
 	u32 pp_stat;
 	int i;
 
@@ -317,21 +319,21 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 	PSB_WVDC32(0x80000000, VGACNTRL);
 
 	/* set the plls */
-	PSB_WVDC32(regs->psb.saveFPA0, MRST_FPA0);
-	PSB_WVDC32(regs->psb.saveFPA1, MRST_FPA1);
+	PSB_WVDC32(p->fp0, MRST_FPA0);
+	PSB_WVDC32(p->fp1, MRST_FPA1);
 
 	/* Actually enable it */
-	PSB_WVDC32(regs->psb.saveDPLL_A, MRST_DPLL_A);
+	PSB_WVDC32(p->dpll, MRST_DPLL_A);
 	DRM_UDELAY(150);
 
 	/* Restore mode */
-	PSB_WVDC32(regs->psb.saveHTOTAL_A, HTOTAL_A);
-	PSB_WVDC32(regs->psb.saveHBLANK_A, HBLANK_A);
-	PSB_WVDC32(regs->psb.saveHSYNC_A, HSYNC_A);
-	PSB_WVDC32(regs->psb.saveVTOTAL_A, VTOTAL_A);
-	PSB_WVDC32(regs->psb.saveVBLANK_A, VBLANK_A);
-	PSB_WVDC32(regs->psb.saveVSYNC_A, VSYNC_A);
-	PSB_WVDC32(regs->psb.savePIPEASRC, PIPEASRC);
+	PSB_WVDC32(p->htotal, HTOTAL_A);
+	PSB_WVDC32(p->hblank, HBLANK_A);
+	PSB_WVDC32(p->hsync, HSYNC_A);
+	PSB_WVDC32(p->vtotal, VTOTAL_A);
+	PSB_WVDC32(p->vblank, VBLANK_A);
+	PSB_WVDC32(p->vsync, VSYNC_A);
+	PSB_WVDC32(p->src, PIPEASRC);
 	PSB_WVDC32(regs->psb.saveBCLRPAT_A, BCLRPAT_A);
 
 	/* Restore performance mode*/
@@ -339,16 +341,16 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 
 	/* Enable the pipe*/
 	if (dev_priv->iLVDS_enable)
-		PSB_WVDC32(regs->psb.savePIPEACONF, PIPEACONF);
+		PSB_WVDC32(p->conf, PIPEACONF);
 
 	/* Set up the plane*/
-	PSB_WVDC32(regs->psb.saveDSPALINOFF, DSPALINOFF);
-	PSB_WVDC32(regs->psb.saveDSPASTRIDE, DSPASTRIDE);
-	PSB_WVDC32(regs->psb.saveDSPATILEOFF, DSPATILEOFF);
+	PSB_WVDC32(p->linoff, DSPALINOFF);
+	PSB_WVDC32(p->stride, DSPASTRIDE);
+	PSB_WVDC32(p->tileoff, DSPATILEOFF);
 
 	/* Enable the plane */
-	PSB_WVDC32(regs->psb.saveDSPACNTR, DSPACNTR);
-	PSB_WVDC32(regs->psb.saveDSPASURF, DSPASURF);
+	PSB_WVDC32(p->cntr, DSPACNTR);
+	PSB_WVDC32(p->surf, DSPASURF);
 
 	/* Enable Cursor A */
 	PSB_WVDC32(regs->psb.saveDSPACURSOR_CTRL, CURACNTR);
@@ -357,7 +359,7 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 
 	/* Restore palette (gamma) */
 	for (i = 0; i < 256; i++)
-		PSB_WVDC32(regs->psb.save_palette_a[i], PALETTE_A + (i << 2));
+		PSB_WVDC32(p->palette[i], PALETTE_A + (i << 2));
 
 	if (dev_priv->hdmi_priv)
 		oaktrail_hdmi_restore(dev);
@@ -454,31 +456,84 @@ static int oaktrail_power_up(struct drm_device *dev)
 	return 0;
 }
 
+/* Oaktrail */
+static const struct psb_offset oaktrail_regmap[2] = {
+	{
+		.fp0 = MRST_FPA0,
+		.fp1 = MRST_FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = MRST_DPLL_A,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.surf = DSPASURF,
+		.addr = MRST_DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = FPB0,
+		.fp1 = FPB1,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = DPLL_B,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.surf = DSPBSURF,
+		.addr = DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	},
+};
 
 static int oaktrail_chip_setup(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
 	int ret;
 	
+	if (pci_enable_msi(dev->pdev))
+		dev_warn(dev->dev, "Enabling MSI failed!\n");
+
+	dev_priv->regmap = oaktrail_regmap;
+
 	ret = mid_chip_setup(dev);
 	if (ret < 0)
 		return ret;
-	if (vbt->size == 0) {
+	if (!dev_priv->has_gct) {
 		/* Now pull the BIOS data */
-		gma_intel_opregion_init(dev);
+		psb_intel_opregion_init(dev);
 		psb_intel_init_bios(dev);
 	}
+	oaktrail_hdmi_setup(dev);
 	return 0;
 }
 
 static void oaktrail_teardown(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
 
 	oaktrail_hdmi_teardown(dev);
-	if (vbt->size == 0)
+	if (!dev_priv->has_gct)
 		psb_intel_destroy_bios(dev);
 }
 
@@ -487,6 +542,9 @@ const struct psb_ops oaktrail_chip_ops = {
 	.accel_2d = 1,
 	.pipes = 2,
 	.crtcs = 2,
+	.hdmi_mask = (1 << 0),
+	.lvds_mask = (1 << 0),
+	.cursor_needs_phys = 0,
 	.sgx_offset = MRST_SGX_OFFSET,
 
 	.chip_setup = oaktrail_chip_setup,
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi.c b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
index f8b367b45f66..c10899c953b9 100644
--- a/drivers/gpu/drm/gma500/oaktrail_hdmi.c
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
@@ -179,7 +179,6 @@ static void oaktrail_hdmi_dpms(struct drm_encoder *encoder, int mode)
 static int oaktrail_hdmi_mode_valid(struct drm_connector *connector,
 				struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
 	if (mode->clock > 165000)
 		return MODE_CLOCK_HIGH;
 	if (mode->clock < 20000)
@@ -188,11 +187,6 @@ static int oaktrail_hdmi_mode_valid(struct drm_connector *connector,
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
 		return MODE_NO_DBLESCAN;
 
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
@@ -440,6 +434,7 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
 	struct psb_state *regs = &dev_priv->regs.psb;
+	struct psb_pipe *pipeb = &dev_priv->regs.pipe[1];
 	int i;
 
 	/* dpll */
@@ -450,14 +445,14 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 	hdmi_dev->saveDPLL_CLK_ENABLE = PSB_RVDC32(DPLL_CLK_ENABLE);
 
 	/* pipe B */
-	regs->savePIPEBCONF = PSB_RVDC32(PIPEBCONF);
-	regs->savePIPEBSRC  = PSB_RVDC32(PIPEBSRC);
-	regs->saveHTOTAL_B  = PSB_RVDC32(HTOTAL_B);
-	regs->saveHBLANK_B  = PSB_RVDC32(HBLANK_B);
-	regs->saveHSYNC_B   = PSB_RVDC32(HSYNC_B);
-	regs->saveVTOTAL_B  = PSB_RVDC32(VTOTAL_B);
-	regs->saveVBLANK_B  = PSB_RVDC32(VBLANK_B);
-	regs->saveVSYNC_B   = PSB_RVDC32(VSYNC_B);
+	pipeb->conf = PSB_RVDC32(PIPEBCONF);
+	pipeb->src = PSB_RVDC32(PIPEBSRC);
+	pipeb->htotal = PSB_RVDC32(HTOTAL_B);
+	pipeb->hblank = PSB_RVDC32(HBLANK_B);
+	pipeb->hsync = PSB_RVDC32(HSYNC_B);
+	pipeb->vtotal = PSB_RVDC32(VTOTAL_B);
+	pipeb->vblank = PSB_RVDC32(VBLANK_B);
+	pipeb->vsync = PSB_RVDC32(VSYNC_B);
 
 	hdmi_dev->savePCH_PIPEBCONF = PSB_RVDC32(PCH_PIPEBCONF);
 	hdmi_dev->savePCH_PIPEBSRC = PSB_RVDC32(PCH_PIPEBSRC);
@@ -469,12 +464,12 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 	hdmi_dev->savePCH_VSYNC_B  = PSB_RVDC32(PCH_VSYNC_B);
 
 	/* plane */
-	regs->saveDSPBCNTR = PSB_RVDC32(DSPBCNTR);
-	regs->saveDSPBSTRIDE = PSB_RVDC32(DSPBSTRIDE);
-	regs->saveDSPBADDR = PSB_RVDC32(DSPBBASE);
-	regs->saveDSPBSURF = PSB_RVDC32(DSPBSURF);
-	regs->saveDSPBLINOFF = PSB_RVDC32(DSPBLINOFF);
-	regs->saveDSPBTILEOFF = PSB_RVDC32(DSPBTILEOFF);
+	pipeb->cntr = PSB_RVDC32(DSPBCNTR);
+	pipeb->stride = PSB_RVDC32(DSPBSTRIDE);
+	pipeb->addr = PSB_RVDC32(DSPBBASE);
+	pipeb->surf = PSB_RVDC32(DSPBSURF);
+	pipeb->linoff = PSB_RVDC32(DSPBLINOFF);
+	pipeb->tileoff = PSB_RVDC32(DSPBTILEOFF);
 
 	/* cursor B */
 	regs->saveDSPBCURSOR_CTRL = PSB_RVDC32(CURBCNTR);
@@ -483,7 +478,7 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 
 	/* save palette */
 	for (i = 0; i < 256; i++)
-		regs->save_palette_b[i] = PSB_RVDC32(PALETTE_B + (i << 2));
+		pipeb->palette[i] = PSB_RVDC32(PALETTE_B + (i << 2));
 }
 
 /* restore HDMI register state */
@@ -492,6 +487,7 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
 	struct psb_state *regs = &dev_priv->regs.psb;
+	struct psb_pipe *pipeb = &dev_priv->regs.pipe[1];
 	int i;
 
 	/* dpll */
@@ -503,13 +499,13 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 	DRM_UDELAY(150);
 
 	/* pipe */
-	PSB_WVDC32(regs->savePIPEBSRC, PIPEBSRC);
-	PSB_WVDC32(regs->saveHTOTAL_B, HTOTAL_B);
-	PSB_WVDC32(regs->saveHBLANK_B, HBLANK_B);
-	PSB_WVDC32(regs->saveHSYNC_B,  HSYNC_B);
-	PSB_WVDC32(regs->saveVTOTAL_B, VTOTAL_B);
-	PSB_WVDC32(regs->saveVBLANK_B, VBLANK_B);
-	PSB_WVDC32(regs->saveVSYNC_B,  VSYNC_B);
+	PSB_WVDC32(pipeb->src, PIPEBSRC);
+	PSB_WVDC32(pipeb->htotal, HTOTAL_B);
+	PSB_WVDC32(pipeb->hblank, HBLANK_B);
+	PSB_WVDC32(pipeb->hsync,  HSYNC_B);
+	PSB_WVDC32(pipeb->vtotal, VTOTAL_B);
+	PSB_WVDC32(pipeb->vblank, VBLANK_B);
+	PSB_WVDC32(pipeb->vsync,  VSYNC_B);
 
 	PSB_WVDC32(hdmi_dev->savePCH_PIPEBSRC, PCH_PIPEBSRC);
 	PSB_WVDC32(hdmi_dev->savePCH_HTOTAL_B, PCH_HTOTAL_B);
@@ -519,15 +515,15 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 	PSB_WVDC32(hdmi_dev->savePCH_VBLANK_B, PCH_VBLANK_B);
 	PSB_WVDC32(hdmi_dev->savePCH_VSYNC_B,  PCH_VSYNC_B);
 
-	PSB_WVDC32(regs->savePIPEBCONF, PIPEBCONF);
+	PSB_WVDC32(pipeb->conf, PIPEBCONF);
 	PSB_WVDC32(hdmi_dev->savePCH_PIPEBCONF, PCH_PIPEBCONF);
 
 	/* plane */
-	PSB_WVDC32(regs->saveDSPBLINOFF, DSPBLINOFF);
-	PSB_WVDC32(regs->saveDSPBSTRIDE, DSPBSTRIDE);
-	PSB_WVDC32(regs->saveDSPBTILEOFF, DSPBTILEOFF);
-	PSB_WVDC32(regs->saveDSPBCNTR, DSPBCNTR);
-	PSB_WVDC32(regs->saveDSPBSURF, DSPBSURF);
+	PSB_WVDC32(pipeb->linoff, DSPBLINOFF);
+	PSB_WVDC32(pipeb->stride, DSPBSTRIDE);
+	PSB_WVDC32(pipeb->tileoff, DSPBTILEOFF);
+	PSB_WVDC32(pipeb->cntr, DSPBCNTR);
+	PSB_WVDC32(pipeb->surf, DSPBSURF);
 
 	/* cursor B */
 	PSB_WVDC32(regs->saveDSPBCURSOR_CTRL, CURBCNTR);
@@ -536,5 +532,5 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 
 	/* restore palette */
 	for (i = 0; i < 256; i++)
-		PSB_WVDC32(regs->save_palette_b[i], PALETTE_B + (i << 2));
+		PSB_WVDC32(pipeb->palette[i], PALETTE_B + (i << 2));
 }
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
index 5e84fbde749b..88627e3ba1e3 100644
--- a/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
@@ -250,7 +250,7 @@ static irqreturn_t oaktrail_hdmi_i2c_handler(int this_irq, void *dev)
  */
 static void oaktrail_hdmi_i2c_gpio_fix(void)
 {
-	void *base;
+	void __iomem *base;
 	unsigned int gpio_base = 0xff12c000;
 	int gpio_len = 0x1000;
 	u32 temp;
diff --git a/drivers/gpu/drm/gma500/oaktrail_lvds.c b/drivers/gpu/drm/gma500/oaktrail_lvds.c
index 654f32b22b21..558c77fb55ec 100644
--- a/drivers/gpu/drm/gma500/oaktrail_lvds.c
+++ b/drivers/gpu/drm/gma500/oaktrail_lvds.c
@@ -257,7 +257,7 @@ static void oaktrail_lvds_get_configuration_mode(struct drm_device *dev,
 	mode_dev->panel_fixed_mode = NULL;
 
 	/* Use the firmware provided data on Moorestown */
-	if (dev_priv->vbt_data.size != 0x00) { /*if non-zero, then use vbt*/
+	if (dev_priv->has_gct) {
 		mode = kzalloc(sizeof(*mode), GFP_KERNEL);
 		if (!mode)
 			return;
@@ -371,7 +371,7 @@ void oaktrail_lvds_init(struct drm_device *dev,
 					BRIGHTNESS_MAX_LEVEL);
 
 	mode_dev->panel_wants_dither = false;
-	if (dev_priv->vbt_data.size != 0x00)
+	if (dev_priv->has_gct)
 		mode_dev->panel_wants_dither = (dev_priv->gct_data.
 			Panel_Port_Control & MRST_PANEL_8TO6_DITHER_ENABLE);
         if (dev_priv->lvds_dither)
diff --git a/drivers/gpu/drm/gma500/opregion.c b/drivers/gpu/drm/gma500/opregion.c
new file mode 100644
index 000000000000..4f186eca3a30
--- /dev/null
+++ b/drivers/gpu/drm/gma500/opregion.c
@@ -0,0 +1,344 @@
+/*
+ * Copyright 2011 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/acpi.h>
+#include <linux/acpi_io.h>
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+
+#define PCI_ASLE 0xe4
+#define PCI_ASLS 0xfc
+
+#define OPREGION_HEADER_OFFSET 0
+#define OPREGION_ACPI_OFFSET   0x100
+#define   ACPI_CLID 0x01ac /* current lid state indicator */
+#define   ACPI_CDCK 0x01b0 /* current docking state indicator */
+#define OPREGION_SWSCI_OFFSET  0x200
+#define OPREGION_ASLE_OFFSET   0x300
+#define OPREGION_VBT_OFFSET    0x400
+
+#define OPREGION_SIGNATURE "IntelGraphicsMem"
+#define MBOX_ACPI      (1<<0)
+#define MBOX_SWSCI     (1<<1)
+#define MBOX_ASLE      (1<<2)
+
+struct opregion_header {
+	u8 signature[16];
+	u32 size;
+	u32 opregion_ver;
+	u8 bios_ver[32];
+	u8 vbios_ver[16];
+	u8 driver_ver[16];
+	u32 mboxes;
+	u8 reserved[164];
+} __packed;
+
+/* OpRegion mailbox #1: public ACPI methods */
+struct opregion_acpi {
+	u32 drdy;	/* driver readiness */
+	u32 csts;	/* notification status */
+	u32 cevt;	/* current event */
+	u8 rsvd1[20];
+	u32 didl[8];	/* supported display devices ID list */
+	u32 cpdl[8];	/* currently presented display list */
+	u32 cadl[8];	/* currently active display list */
+	u32 nadl[8];	/* next active devices list */
+	u32 aslp;	/* ASL sleep time-out */
+	u32 tidx;	/* toggle table index */
+	u32 chpd;	/* current hotplug enable indicator */
+	u32 clid;	/* current lid state*/
+	u32 cdck;	/* current docking state */
+	u32 sxsw;	/* Sx state resume */
+	u32 evts;	/* ASL supported events */
+	u32 cnot;	/* current OS notification */
+	u32 nrdy;	/* driver status */
+	u8 rsvd2[60];
+} __packed;
+
+/* OpRegion mailbox #2: SWSCI */
+struct opregion_swsci {
+	/*FIXME: add it later*/
+} __packed;
+
+/* OpRegion mailbox #3: ASLE */
+struct opregion_asle {
+	u32 ardy;	/* driver readiness */
+	u32 aslc;	/* ASLE interrupt command */
+	u32 tche;	/* technology enabled indicator */
+	u32 alsi;	/* current ALS illuminance reading */
+	u32 bclp;	/* backlight brightness to set */
+	u32 pfit;	/* panel fitting state */
+	u32 cblv;	/* current brightness level */
+	u16 bclm[20];	/* backlight level duty cycle mapping table */
+	u32 cpfm;	/* current panel fitting mode */
+	u32 epfm;	/* enabled panel fitting modes */
+	u8 plut[74];	/* panel LUT and identifier */
+	u32 pfmb;	/* PWM freq and min brightness */
+	u8 rsvd[102];
+} __packed;
+
+/* ASLE irq request bits */
+#define ASLE_SET_ALS_ILLUM     (1 << 0)
+#define ASLE_SET_BACKLIGHT     (1 << 1)
+#define ASLE_SET_PFIT          (1 << 2)
+#define ASLE_SET_PWM_FREQ      (1 << 3)
+#define ASLE_REQ_MSK           0xf
+
+/* response bits of ASLE irq request */
+#define ASLE_ALS_ILLUM_FAILED   (1<<10)
+#define ASLE_BACKLIGHT_FAILED   (1<<12)
+#define ASLE_PFIT_FAILED        (1<<14)
+#define ASLE_PWM_FREQ_FAILED    (1<<16)
+
+/* ASLE backlight brightness to set */
+#define ASLE_BCLP_VALID                (1<<31)
+#define ASLE_BCLP_MSK          (~(1<<31))
+
+/* ASLE panel fitting request */
+#define ASLE_PFIT_VALID         (1<<31)
+#define ASLE_PFIT_CENTER (1<<0)
+#define ASLE_PFIT_STRETCH_TEXT (1<<1)
+#define ASLE_PFIT_STRETCH_GFX (1<<2)
+
+/* response bits of ASLE irq request */
+#define ASLE_ALS_ILLUM_FAILED	(1<<10)
+#define ASLE_BACKLIGHT_FAILED	(1<<12)
+#define ASLE_PFIT_FAILED	(1<<14)
+#define ASLE_PWM_FREQ_FAILED	(1<<16)
+
+/* ASLE backlight brightness to set */
+#define ASLE_BCLP_VALID                (1<<31)
+#define ASLE_BCLP_MSK          (~(1<<31))
+
+/* ASLE panel fitting request */
+#define ASLE_PFIT_VALID         (1<<31)
+#define ASLE_PFIT_CENTER (1<<0)
+#define ASLE_PFIT_STRETCH_TEXT (1<<1)
+#define ASLE_PFIT_STRETCH_GFX (1<<2)
+
+/* PWM frequency and minimum brightness */
+#define ASLE_PFMB_BRIGHTNESS_MASK (0xff)
+#define ASLE_PFMB_BRIGHTNESS_VALID (1<<8)
+#define ASLE_PFMB_PWM_MASK (0x7ffffe00)
+#define ASLE_PFMB_PWM_VALID (1<<31)
+
+#define ASLE_CBLV_VALID         (1<<31)
+
+static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct backlight_device *bd = dev_priv->backlight_device;
+
+	DRM_DEBUG_DRIVER("asle set backlight %x\n", bclp);
+
+	if (!(bclp & ASLE_BCLP_VALID))
+		return ASLE_BACKLIGHT_FAILED;
+
+	if (bd == NULL)
+		return ASLE_BACKLIGHT_FAILED;
+
+	bclp &= ASLE_BCLP_MSK;
+	if (bclp > 255)
+		return ASLE_BACKLIGHT_FAILED;
+
+	if (config_enabled(CONFIG_BACKLIGHT_CLASS_DEVICE)) {
+		int max = bd->props.max_brightness;
+		bd->props.brightness = bclp * max / 255;
+		backlight_update_status(bd);
+	}
+
+	asle->cblv = (bclp * 0x64) / 0xff | ASLE_CBLV_VALID;
+
+	return 0;
+}
+
+void psb_intel_opregion_asle_intr(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct opregion_asle *asle = dev_priv->opregion.asle;
+	u32 asle_stat = 0;
+	u32 asle_req;
+
+	if (!asle)
+		return;
+
+	asle_req = asle->aslc & ASLE_REQ_MSK;
+	if (!asle_req) {
+		DRM_DEBUG_DRIVER("non asle set request??\n");
+		return;
+	}
+
+	if (asle_req & ASLE_SET_BACKLIGHT)
+		asle_stat |= asle_set_backlight(dev, asle->bclp);
+
+	asle->aslc = asle_stat;
+}
+
+#define ASLE_ALS_EN    (1<<0)
+#define ASLE_BLC_EN    (1<<1)
+#define ASLE_PFIT_EN   (1<<2)
+#define ASLE_PFMB_EN   (1<<3)
+
+void psb_intel_opregion_enable_asle(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct opregion_asle *asle = dev_priv->opregion.asle;
+
+	if (asle) {
+		/* Don't do this on Medfield or other non PC like devices, they
+		   use the bit for something different altogether */
+		psb_enable_pipestat(dev_priv, 0, PIPE_LEGACY_BLC_EVENT_ENABLE);
+		psb_enable_pipestat(dev_priv, 1, PIPE_LEGACY_BLC_EVENT_ENABLE);
+
+		asle->tche = ASLE_ALS_EN | ASLE_BLC_EN | ASLE_PFIT_EN
+								| ASLE_PFMB_EN;
+		asle->ardy = 1;
+	}
+}
+
+#define ACPI_EV_DISPLAY_SWITCH (1<<0)
+#define ACPI_EV_LID            (1<<1)
+#define ACPI_EV_DOCK           (1<<2)
+
+static struct psb_intel_opregion *system_opregion;
+
+static int psb_intel_opregion_video_event(struct notifier_block *nb,
+					  unsigned long val, void *data)
+{
+	/* The only video events relevant to opregion are 0x80. These indicate
+	   either a docking event, lid switch or display switch request. In
+	   Linux, these are handled by the dock, button and video drivers.
+	   We might want to fix the video driver to be opregion-aware in
+	   future, but right now we just indicate to the firmware that the
+	   request has been handled */
+
+	struct opregion_acpi *acpi;
+
+	if (!system_opregion)
+		return NOTIFY_DONE;
+
+	acpi = system_opregion->acpi;
+	acpi->csts = 0;
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block psb_intel_opregion_notifier = {
+	.notifier_call = psb_intel_opregion_video_event,
+};
+
+void psb_intel_opregion_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_opregion *opregion = &dev_priv->opregion;
+
+	if (!opregion->header)
+		return;
+
+	if (opregion->acpi) {
+		/* Notify BIOS we are ready to handle ACPI video ext notifs.
+		 * Right now, all the events are handled by the ACPI video
+		 * module. We don't actually need to do anything with them. */
+		opregion->acpi->csts = 0;
+		opregion->acpi->drdy = 1;
+
+		system_opregion = opregion;
+		register_acpi_notifier(&psb_intel_opregion_notifier);
+	}
+
+	if (opregion->asle)
+		psb_intel_opregion_enable_asle(dev);
+}
+
+void psb_intel_opregion_fini(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_opregion *opregion = &dev_priv->opregion;
+
+	if (!opregion->header)
+		return;
+
+	if (opregion->acpi) {
+		opregion->acpi->drdy = 0;
+
+		system_opregion = NULL;
+		unregister_acpi_notifier(&psb_intel_opregion_notifier);
+	}
+
+	/* just clear all opregion memory pointers now */
+	iounmap(opregion->header);
+	opregion->header = NULL;
+	opregion->acpi = NULL;
+	opregion->swsci = NULL;
+	opregion->asle = NULL;
+	opregion->vbt = NULL;
+}
+
+int psb_intel_opregion_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_opregion *opregion = &dev_priv->opregion;
+	u32 opregion_phy, mboxes;
+	void __iomem *base;
+	int err = 0;
+
+	pci_read_config_dword(dev->pdev, PCI_ASLS, &opregion_phy);
+	if (opregion_phy == 0) {
+		DRM_DEBUG_DRIVER("ACPI Opregion not supported\n");
+		return -ENOTSUPP;
+	}
+	DRM_DEBUG("OpRegion detected at 0x%8x\n", opregion_phy);
+	base = acpi_os_ioremap(opregion_phy, 8*1024);
+	if (!base)
+		return -ENOMEM;
+
+	if (memcmp(base, OPREGION_SIGNATURE, 16)) {
+		DRM_DEBUG_DRIVER("opregion signature mismatch\n");
+		err = -EINVAL;
+		goto err_out;
+	}
+
+	opregion->header = base;
+	opregion->vbt = base + OPREGION_VBT_OFFSET;
+
+	opregion->lid_state = base + ACPI_CLID;
+
+	mboxes = opregion->header->mboxes;
+	if (mboxes & MBOX_ACPI) {
+		DRM_DEBUG_DRIVER("Public ACPI methods supported\n");
+		opregion->acpi = base + OPREGION_ACPI_OFFSET;
+	}
+
+	if (mboxes & MBOX_ASLE) {
+		DRM_DEBUG_DRIVER("ASLE supported\n");
+		opregion->asle = base + OPREGION_ASLE_OFFSET;
+	}
+
+	return 0;
+
+err_out:
+	iounmap(base);
+	return err;
+}
+
diff --git a/drivers/gpu/drm/gma500/intel_opregion.c b/drivers/gpu/drm/gma500/opregion.h
index d946bc1b17bf..72dc6b921265 100644
--- a/drivers/gpu/drm/gma500/intel_opregion.c
+++ b/drivers/gpu/drm/gma500/opregion.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2010 Intel Corporation
+ * Copyright 2012 Intel Corporation
  *
  * Permission is hereby granted, free of charge, to any person obtaining a
  * copy of this software and associated documentation files (the "Software"),
@@ -20,62 +20,30 @@
  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
  * DEALINGS IN THE SOFTWARE.
  *
- * FIXME: resolve with the i915 version
  */
 
-#include "psb_drv.h"
+#if defined(CONFIG_ACPI)
+extern void psb_intel_opregion_asle_intr(struct drm_device *dev);
+extern void psb_intel_opregion_init(struct drm_device *dev);
+extern void psb_intel_opregion_fini(struct drm_device *dev);
+extern int psb_intel_opregion_setup(struct drm_device *dev);
 
-struct opregion_header {
-	u8 signature[16];
-	u32 size;
-	u32 opregion_ver;
-	u8 bios_ver[32];
-	u8 vbios_ver[16];
-	u8 driver_ver[16];
-	u32 mboxes;
-	u8 reserved[164];
-} __packed;
+#else
 
-struct opregion_apci {
-	/*FIXME: add it later*/
-} __packed;
-
-struct opregion_swsci {
-	/*FIXME: add it later*/
-} __packed;
-
-struct opregion_acpi {
-	/*FIXME: add it later*/
-} __packed;
-
-int gma_intel_opregion_init(struct drm_device *dev)
+extern inline void psb_intel_opregion_asle_intr(struct drm_device *dev)
 {
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	u32 opregion_phy;
-	void *base;
-	u32 *lid_state;
-
-	dev_priv->lid_state = NULL;
-
-	pci_read_config_dword(dev->pdev, 0xfc, &opregion_phy);
-	if (opregion_phy == 0)
-		return -ENOTSUPP;
-
-	base = ioremap(opregion_phy, 8*1024);
-	if (!base)
-		return -ENOMEM;
+}
 
-	lid_state = base + 0x01ac;
+extern inline void psb_intel_opregion_init(struct drm_device *dev)
+{
+}
 
-	dev_priv->lid_state = lid_state;
-	dev_priv->lid_last_state = readl(lid_state);
-	return 0;
+extern inline void psb_intel_opregion_fini(struct drm_device *dev)
+{
 }
 
-int gma_intel_opregion_exit(struct drm_device *dev)
+extern inline int psb_intel_opregion_setup(struct drm_device *dev)
 {
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	if (dev_priv->lid_state)
-		iounmap(dev_priv->lid_state);
 	return 0;
 }
+#endif
diff --git a/drivers/gpu/drm/gma500/psb_device.c b/drivers/gpu/drm/gma500/psb_device.c
index 95d163e4f1f4..eff039bf92d4 100644
--- a/drivers/gpu/drm/gma500/psb_device.c
+++ b/drivers/gpu/drm/gma500/psb_device.c
@@ -197,7 +197,8 @@ static int psb_save_display_registers(struct drm_device *dev)
 	}
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
-		connector->funcs->save(connector);
+		if (connector->funcs->save)
+			connector->funcs->save(connector);
 
 	mutex_unlock(&dev->mode_config.mutex);
 	return 0;
@@ -235,7 +236,8 @@ static int psb_restore_display_registers(struct drm_device *dev)
 			crtc->funcs->restore(crtc);
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
-		connector->funcs->restore(connector);
+		if (connector->funcs->restore)
+			connector->funcs->restore(connector);
 
 	mutex_unlock(&dev->mode_config.mutex);
 	return 0;
@@ -289,17 +291,80 @@ static void psb_get_core_freq(struct drm_device *dev)
 	}
 }
 
+/* Poulsbo */
+static const struct psb_offset psb_regmap[2] = {
+	{
+		.fp0 = FPA0,
+		.fp1 = FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = DPLL_A,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.base = DSPABASE,
+		.surf = DSPASURF,
+		.addr = DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = FPB0,
+		.fp1 = FPB1,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = DPLL_B,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.base = DSPBBASE,
+		.surf = DSPBSURF,
+		.addr = DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	}
+};
+
 static int psb_chip_setup(struct drm_device *dev)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	dev_priv->regmap = psb_regmap;
 	psb_get_core_freq(dev);
 	gma_intel_setup_gmbus(dev);
-	gma_intel_opregion_init(dev);
+	psb_intel_opregion_init(dev);
 	psb_intel_init_bios(dev);
 	return 0;
 }
 
+/* Not exactly an erratum more an irritation */
+static void psb_chip_errata(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	psb_lid_timer_init(dev_priv);
+}
+
 static void psb_chip_teardown(struct drm_device *dev)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	psb_lid_timer_takedown(dev_priv);
 	gma_intel_teardown_gmbus(dev);
 }
 
@@ -308,9 +373,13 @@ const struct psb_ops psb_chip_ops = {
 	.accel_2d = 1,
 	.pipes = 2,
 	.crtcs = 2,
+	.hdmi_mask = (1 << 0),
+	.lvds_mask = (1 << 1),
+	.cursor_needs_phys = 1,
 	.sgx_offset = PSB_SGX_OFFSET,
 	.chip_setup = psb_chip_setup,
 	.chip_teardown = psb_chip_teardown,
+	.errata = psb_chip_errata,
 
 	.crtc_helper = &psb_intel_helper_funcs,
 	.crtc_funcs = &psb_intel_crtc_funcs,
diff --git a/drivers/gpu/drm/gma500/psb_drv.c b/drivers/gpu/drm/gma500/psb_drv.c
index c34adf9d910a..caba6e08693c 100644
--- a/drivers/gpu/drm/gma500/psb_drv.c
+++ b/drivers/gpu/drm/gma500/psb_drv.c
@@ -79,6 +79,14 @@ static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
 	{ 0x8086, 0x0be5, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
 	{ 0x8086, 0x0be6, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
 	{ 0x8086, 0x0be7, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be8, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be9, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bea, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0beb, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bec, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bed, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bee, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bef, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
 #endif
 	{ 0, }
 };
@@ -144,10 +152,6 @@ static void psb_lastclose(struct drm_device *dev)
 	return;
 }
 
-static void psb_do_takedown(struct drm_device *dev)
-{
-}
-
 static int psb_do_init(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
@@ -172,24 +176,6 @@ static int psb_do_init(struct drm_device *dev)
 	dev_priv->gatt_free_offset = pg->mmu_gatt_start +
 	    (stolen_gtt << PAGE_SHIFT) * 1024;
 
-	if (1 || drm_debug) {
-		uint32_t core_id = PSB_RSGX32(PSB_CR_CORE_ID);
-		uint32_t core_rev = PSB_RSGX32(PSB_CR_CORE_REVISION);
-		DRM_INFO("SGX core id = 0x%08x\n", core_id);
-		DRM_INFO("SGX core rev major = 0x%02x, minor = 0x%02x\n",
-			 (core_rev & _PSB_CC_REVISION_MAJOR_MASK) >>
-			 _PSB_CC_REVISION_MAJOR_SHIFT,
-			 (core_rev & _PSB_CC_REVISION_MINOR_MASK) >>
-			 _PSB_CC_REVISION_MINOR_SHIFT);
-		DRM_INFO
-		    ("SGX core rev maintenance = 0x%02x, designer = 0x%02x\n",
-		     (core_rev & _PSB_CC_REVISION_MAINTENANCE_MASK) >>
-		     _PSB_CC_REVISION_MAINTENANCE_SHIFT,
-		     (core_rev & _PSB_CC_REVISION_DESIGNER_MASK) >>
-		     _PSB_CC_REVISION_DESIGNER_SHIFT);
-	}
-
-
 	spin_lock_init(&dev_priv->irqmask_lock);
 	spin_lock_init(&dev_priv->lock_2d);
 
@@ -204,7 +190,6 @@ static int psb_do_init(struct drm_device *dev)
 	PSB_WSGX32(pg->gatt_start, PSB_CR_BIF_TWOD_REQ_BASE);
 	return 0;
 out_err:
-	psb_do_takedown(dev);
 	return ret;
 }
 
@@ -214,18 +199,16 @@ static int psb_driver_unload(struct drm_device *dev)
 
 	/* Kill vblank etc here */
 
-	gma_backlight_exit(dev);
-
-	psb_modeset_cleanup(dev);
 
 	if (dev_priv) {
-		psb_lid_timer_takedown(dev_priv);
-		gma_intel_opregion_exit(dev);
+		if (dev_priv->backlight_device)
+			gma_backlight_exit(dev);
+		psb_modeset_cleanup(dev);
 
 		if (dev_priv->ops->chip_teardown)
 			dev_priv->ops->chip_teardown(dev);
-		psb_do_takedown(dev);
 
+		psb_intel_opregion_fini(dev);
 
 		if (dev_priv->pf_pd) {
 			psb_mmu_free_pagedir(dev_priv->pf_pd);
@@ -246,6 +229,7 @@ static int psb_driver_unload(struct drm_device *dev)
 		}
 		psb_gtt_takedown(dev);
 		if (dev_priv->scratch_page) {
+			set_pages_wb(dev_priv->scratch_page, 1);
 			__free_page(dev_priv->scratch_page);
 			dev_priv->scratch_page = NULL;
 		}
@@ -258,15 +242,13 @@ static int psb_driver_unload(struct drm_device *dev)
 			dev_priv->sgx_reg = NULL;
 		}
 
+		/* Destroy VBT data */
+		psb_intel_destroy_bios(dev);
+
 		kfree(dev_priv);
 		dev->dev_private = NULL;
-
-		/*destroy VBT data*/
-		psb_intel_destroy_bios(dev);
 	}
-
 	gma_power_uninit(dev);
-
 	return 0;
 }
 
@@ -290,11 +272,6 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 
 	pci_set_master(dev->pdev);
 
-	if (!IS_PSB(dev)) {
-		if (pci_enable_msi(dev->pdev))
-			dev_warn(dev->dev, "Enabling MSI failed!\n");
-	}
-
 	dev_priv->num_pipe = dev_priv->ops->pipes;
 
 	resource_start = pci_resource_start(dev->pdev, PSB_MMIO_RESOURCE);
@@ -309,6 +286,8 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 	if (!dev_priv->sgx_reg)
 		goto out_err;
 
+	psb_intel_opregion_setup(dev);
+
 	ret = dev_priv->ops->chip_setup(dev);
 	if (ret)
 		goto out_err;
@@ -348,10 +327,7 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 	PSB_WSGX32(0x20000000, PSB_CR_PDS_EXEC_BASE);
 	PSB_WSGX32(0x30000000, PSB_CR_BIF_3D_REQ_BASE);
 
-/*	igd_opregion_init(&dev_priv->opregion_dev); */
 	acpi_video_register();
-	if (dev_priv->lid_state)
-		psb_lid_timer_init(dev_priv);
 
 	ret = drm_vblank_init(dev, dev_priv->num_pipe);
 	if (ret)
@@ -370,8 +346,8 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 	PSB_WVDC32(0x00000000, PSB_INT_ENABLE_R);
 	PSB_WVDC32(0xFFFFFFFF, PSB_INT_MASK_R);
 	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
-	if (IS_PSB(dev) && drm_core_check_feature(dev, DRIVER_MODESET))
-		drm_irq_install(dev);
+
+	drm_irq_install(dev);
 
 	dev->vblank_disable_allowed = 1;
 
@@ -619,7 +595,7 @@ static const struct dev_pm_ops psb_pm_ops = {
 	.runtime_idle = psb_runtime_idle,
 };
 
-static struct vm_operations_struct psb_gem_vm_ops = {
+static const struct vm_operations_struct psb_gem_vm_ops = {
 	.fault = psb_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
diff --git a/drivers/gpu/drm/gma500/psb_drv.h b/drivers/gpu/drm/gma500/psb_drv.h
index 40ce2c9bc2e4..1bd115ecefe1 100644
--- a/drivers/gpu/drm/gma500/psb_drv.h
+++ b/drivers/gpu/drm/gma500/psb_drv.h
@@ -30,6 +30,7 @@
 #include "psb_intel_drv.h"
 #include "gtt.h"
 #include "power.h"
+#include "opregion.h"
 #include "oaktrail.h"
 
 /* Append new drm mode definition here, align with libdrm definition */
@@ -120,6 +121,7 @@ enum {
 #define PSB_HWSTAM		  0x2098
 #define PSB_INSTPM		  0x20C0
 #define PSB_INT_IDENTITY_R        0x20A4
+#define _PSB_IRQ_ASLE		  (1<<0)
 #define _MDFLD_PIPEC_EVENT_FLAG   (1<<2)
 #define _MDFLD_PIPEC_VBLANK_FLAG  (1<<3)
 #define _PSB_DPST_PIPEB_FLAG      (1<<4)
@@ -130,6 +132,7 @@ enum {
 #define _PSB_VSYNC_PIPEA_FLAG	  (1<<7)
 #define _MDFLD_MIPIA_FLAG	  (1<<16)
 #define _MDFLD_MIPIC_FLAG	  (1<<17)
+#define _PSB_IRQ_DISP_HOTSYNC	  (1<<17)
 #define _PSB_IRQ_SGX_FLAG	  (1<<18)
 #define _PSB_IRQ_MSVDX_FLAG	  (1<<19)
 #define _LNC_IRQ_TOPAZ_FLAG	  (1<<20)
@@ -257,7 +260,8 @@ struct psb_intel_opregion {
 	struct opregion_acpi *acpi;
 	struct opregion_swsci *swsci;
 	struct opregion_asle *asle;
-	int enabled;
+	void *vbt;
+	u32 __iomem *lid_state;
 };
 
 struct sdvo_device_mapping {
@@ -277,50 +281,72 @@ struct intel_gmbus {
 };
 
 /*
+ *	Register offset maps
+ */
+
+struct psb_offset {
+	u32	fp0;
+	u32	fp1;
+	u32	cntr;
+	u32	conf;
+	u32	src;
+	u32	dpll;
+	u32	dpll_md;
+	u32	htotal;
+	u32	hblank;
+	u32	hsync;
+	u32	vtotal;
+	u32	vblank;
+	u32	vsync;
+	u32	stride;
+	u32	size;
+	u32	pos;
+	u32	surf;
+	u32	addr;
+	u32	base;
+	u32	status;
+	u32	linoff;
+	u32	tileoff;
+	u32	palette;
+};
+
+/*
  *	Register save state. This is used to hold the context when the
  *	device is powered off. In the case of Oaktrail this can (but does not
  *	yet) include screen blank. Operations occuring during the save
  *	update the register cache instead.
  */
+
+/*
+ *	Common status for pipes.
+ */
+struct psb_pipe {
+	u32	fp0;
+	u32	fp1;
+	u32	cntr;
+	u32	conf;
+	u32	src;
+	u32	dpll;
+	u32	dpll_md;
+	u32	htotal;
+	u32	hblank;
+	u32	hsync;
+	u32	vtotal;
+	u32	vblank;
+	u32	vsync;
+	u32	stride;
+	u32	size;
+	u32	pos;
+	u32	base;
+	u32	surf;
+	u32	addr;
+	u32	status;
+	u32	linoff;
+	u32	tileoff;
+	u32	palette[256];
+};
+
 struct psb_state {
-	uint32_t saveDSPACNTR;
-	uint32_t saveDSPBCNTR;
-	uint32_t savePIPEACONF;
-	uint32_t savePIPEBCONF;
-	uint32_t savePIPEASRC;
-	uint32_t savePIPEBSRC;
-	uint32_t saveFPA0;
-	uint32_t saveFPA1;
-	uint32_t saveDPLL_A;
-	uint32_t saveDPLL_A_MD;
-	uint32_t saveHTOTAL_A;
-	uint32_t saveHBLANK_A;
-	uint32_t saveHSYNC_A;
-	uint32_t saveVTOTAL_A;
-	uint32_t saveVBLANK_A;
-	uint32_t saveVSYNC_A;
-	uint32_t saveDSPASTRIDE;
-	uint32_t saveDSPASIZE;
-	uint32_t saveDSPAPOS;
-	uint32_t saveDSPABASE;
-	uint32_t saveDSPASURF;
-	uint32_t saveDSPASTATUS;
-	uint32_t saveFPB0;
-	uint32_t saveFPB1;
-	uint32_t saveDPLL_B;
-	uint32_t saveDPLL_B_MD;
-	uint32_t saveHTOTAL_B;
-	uint32_t saveHBLANK_B;
-	uint32_t saveHSYNC_B;
-	uint32_t saveVTOTAL_B;
-	uint32_t saveVBLANK_B;
-	uint32_t saveVSYNC_B;
-	uint32_t saveDSPBSTRIDE;
-	uint32_t saveDSPBSIZE;
-	uint32_t saveDSPBPOS;
-	uint32_t saveDSPBBASE;
-	uint32_t saveDSPBSURF;
-	uint32_t saveDSPBSTATUS;
 	uint32_t saveVCLK_DIVISOR_VGA0;
 	uint32_t saveVCLK_DIVISOR_VGA1;
 	uint32_t saveVCLK_POST_DIV;
@@ -335,14 +361,8 @@ struct psb_state {
 	uint32_t savePP_CONTROL;
 	uint32_t savePP_CYCLE;
 	uint32_t savePFIT_CONTROL;
-	uint32_t savePaletteA[256];
-	uint32_t savePaletteB[256];
 	uint32_t saveCLOCKGATING;
 	uint32_t saveDSPARB;
-	uint32_t saveDSPATILEOFF;
-	uint32_t saveDSPBTILEOFF;
-	uint32_t saveDSPAADDR;
-	uint32_t saveDSPBADDR;
 	uint32_t savePFIT_AUTO_RATIOS;
 	uint32_t savePFIT_PGM_RATIOS;
 	uint32_t savePP_ON_DELAYS;
@@ -350,8 +370,6 @@ struct psb_state {
 	uint32_t savePP_DIVISOR;
 	uint32_t saveBCLRPAT_A;
 	uint32_t saveBCLRPAT_B;
-	uint32_t saveDSPALINOFF;
-	uint32_t saveDSPBLINOFF;
 	uint32_t savePERF_MODE;
 	uint32_t saveDSPFW1;
 	uint32_t saveDSPFW2;
@@ -366,8 +384,6 @@ struct psb_state {
 	uint32_t saveDSPBCURSOR_BASE;
 	uint32_t saveDSPACURSOR_POS;
 	uint32_t saveDSPBCURSOR_POS;
-	uint32_t save_palette_a[256];
-	uint32_t save_palette_b[256];
 	uint32_t saveOV_OVADD;
 	uint32_t saveOV_OGAMC0;
 	uint32_t saveOV_OGAMC1;
@@ -390,64 +406,7 @@ struct psb_state {
 };
 
 struct medfield_state {
-	uint32_t saveDPLL_A;
-	uint32_t saveFPA0;
-	uint32_t savePIPEACONF;
-	uint32_t saveHTOTAL_A;
-	uint32_t saveHBLANK_A;
-	uint32_t saveHSYNC_A;
-	uint32_t saveVTOTAL_A;
-	uint32_t saveVBLANK_A;
-	uint32_t saveVSYNC_A;
-	uint32_t savePIPEASRC;
-	uint32_t saveDSPASTRIDE;
-	uint32_t saveDSPALINOFF;
-	uint32_t saveDSPATILEOFF;
-	uint32_t saveDSPASIZE;
-	uint32_t saveDSPAPOS;
-	uint32_t saveDSPASURF;
-	uint32_t saveDSPACNTR;
-	uint32_t saveDSPASTATUS;
-	uint32_t save_palette_a[256];
 	uint32_t saveMIPI;
-
-	uint32_t saveDPLL_B;
-	uint32_t saveFPB0;
-	uint32_t savePIPEBCONF;
-	uint32_t saveHTOTAL_B;
-	uint32_t saveHBLANK_B;
-	uint32_t saveHSYNC_B;
-	uint32_t saveVTOTAL_B;
-	uint32_t saveVBLANK_B;
-	uint32_t saveVSYNC_B;
-	uint32_t savePIPEBSRC;
-	uint32_t saveDSPBSTRIDE;
-	uint32_t saveDSPBLINOFF;
-	uint32_t saveDSPBTILEOFF;
-	uint32_t saveDSPBSIZE;
-	uint32_t saveDSPBPOS;
-	uint32_t saveDSPBSURF;
-	uint32_t saveDSPBCNTR;
-	uint32_t saveDSPBSTATUS;
-	uint32_t save_palette_b[256];
-
-	uint32_t savePIPECCONF;
-	uint32_t saveHTOTAL_C;
-	uint32_t saveHBLANK_C;
-	uint32_t saveHSYNC_C;
-	uint32_t saveVTOTAL_C;
-	uint32_t saveVBLANK_C;
-	uint32_t saveVSYNC_C;
-	uint32_t savePIPECSRC;
-	uint32_t saveDSPCSTRIDE;
-	uint32_t saveDSPCLINOFF;
-	uint32_t saveDSPCTILEOFF;
-	uint32_t saveDSPCSIZE;
-	uint32_t saveDSPCPOS;
-	uint32_t saveDSPCSURF;
-	uint32_t saveDSPCCNTR;
-	uint32_t saveDSPCSTATUS;
-	uint32_t save_palette_c[256];
 	uint32_t saveMIPI_C;
 
 	uint32_t savePFIT_CONTROL;
@@ -476,6 +435,7 @@ struct cdv_state {
 };
 
 struct psb_save_area {
+	struct psb_pipe pipe[3];
 	uint32_t saveBSM;
 	uint32_t saveVBT;
 	union {
@@ -494,15 +454,19 @@ struct psb_ops;
 struct drm_psb_private {
 	struct drm_device *dev;
 	const struct psb_ops *ops;
+	const struct psb_offset *regmap;
+	
+	struct child_device_config *child_dev;
+	int child_dev_num;
 
 	struct psb_gtt gtt;
 
 	/* GTT Memory manager */
 	struct psb_gtt_mm *gtt_mm;
 	struct page *scratch_page;
-	u32 *gtt_map;
+	u32 __iomem *gtt_map;
 	uint32_t stolen_base;
-	void *vram_addr;
+	u8 __iomem *vram_addr;
 	unsigned long vram_stolen_size;
 	int gtt_initialized;
 	u16 gmch_ctrl;		/* Saved GTT setup */
@@ -518,8 +482,8 @@ struct drm_psb_private {
 	 * Register base
 	 */
 
-	uint8_t *sgx_reg;
-	uint8_t *vdc_reg;
+	uint8_t __iomem *sgx_reg;
+	uint8_t __iomem *vdc_reg;
 	uint32_t gatt_free_offset;
 
 	/*
@@ -543,6 +507,7 @@ struct drm_psb_private {
 	 * Modesetting
 	 */
 	struct psb_intel_mode_device mode_dev;
+	bool modeset;	/* true if we have done the mode_device setup */
 
 	struct drm_crtc *plane_to_crtc_mapping[PSB_NUM_PIPE];
 	struct drm_crtc *pipe_to_crtc_mapping[PSB_NUM_PIPE];
@@ -605,7 +570,7 @@ struct drm_psb_private {
 	int rpm_enabled;
 
 	/* MID specific */
-	struct oaktrail_vbt vbt_data;
+	bool has_gct;
 	struct oaktrail_gct_data gct_data;
 
 	/* Oaktrail HDMI state */
@@ -621,6 +586,11 @@ struct drm_psb_private {
 	uint32_t msi_addr;
 	uint32_t msi_data;
 
+	/*
+	 * Hotplug handling
+	 */
+
+	struct work_struct hotplug_work;
 
 	/*
 	 * LID-Switch
@@ -628,7 +598,6 @@ struct drm_psb_private {
 	spinlock_t lid_lock;
 	struct timer_list lid_timer;
 	struct psb_intel_opregion opregion;
-	u32 *lid_state;
 	u32 lid_last_state;
 
 	/*
@@ -669,6 +638,8 @@ struct drm_psb_private {
 	u32 dspcntr[3];
 
 	int mdfld_panel_id;
+
+	bool dplla_96mhz;	/* DPLL data from the VBT */
 };
 
 
@@ -682,6 +653,9 @@ struct psb_ops {
 	int pipes;		/* Number of output pipes */
 	int crtcs;		/* Number of CRTCs */
 	int sgx_offset;		/* Base offset of SGX device */
+	int hdmi_mask;		/* Mask of HDMI CRTCs */
+	int lvds_mask;		/* Mask of LVDS CRTCs */
+	int cursor_needs_phys;  /* If cursor base reg need physical address */
 
 	/* Sub functions */
 	struct drm_crtc_helper_funcs const *crtc_helper;
@@ -690,9 +664,13 @@ struct psb_ops {
 	/* Setup hooks */
 	int (*chip_setup)(struct drm_device *dev);
 	void (*chip_teardown)(struct drm_device *dev);
+	/* Optional helper caller after modeset */
+	void (*errata)(struct drm_device *dev);
 
 	/* Display management hooks */
 	int (*output_init)(struct drm_device *dev);
+	int (*hotplug)(struct drm_device *dev);
+	void (*hotplug_enable)(struct drm_device *dev, bool on);
 	/* Power management hooks */
 	void (*init_pm)(struct drm_device *dev);
 	int (*save_regs)(struct drm_device *dev);
@@ -789,12 +767,6 @@ psb_disable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask);
 extern u32 psb_get_vblank_counter(struct drm_device *dev, int crtc);
 
 /*
- * intel_opregion.c
- */
-extern int gma_intel_opregion_init(struct drm_device *dev);
-extern int gma_intel_opregion_exit(struct drm_device *dev);
-
-/*
  * framebuffer.c
  */
 extern int psbfb_probed(struct drm_device *dev);
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.c b/drivers/gpu/drm/gma500/psb_intel_display.c
index 2616558457c8..36c3c99612f6 100644
--- a/drivers/gpu/drm/gma500/psb_intel_display.c
+++ b/drivers/gpu/drm/gma500/psb_intel_display.c
@@ -337,15 +337,12 @@ static int psb_intel_pipe_set_base(struct drm_crtc *crtc,
 			    int x, int y, struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_i915_master_private *master_priv; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
-	int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
-	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
-	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
 	u32 dspcntr;
 	int ret = 0;
 
@@ -367,9 +364,9 @@ static int psb_intel_pipe_set_base(struct drm_crtc *crtc,
 
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
 
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -392,18 +389,10 @@ static int psb_intel_pipe_set_base(struct drm_crtc *crtc,
 		psb_gtt_unpin(psbfb->gtt);
 		goto psb_intel_pipe_set_base_exit;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
-
+	REG_WRITE(map->cntr, dspcntr);
 
-	if (0 /* FIXMEAC - check what PSB needs */) {
-		REG_WRITE(dspbase, offset);
-		REG_READ(dspbase);
-		REG_WRITE(dspsurf, start);
-		REG_READ(dspsurf);
-	} else {
-		REG_WRITE(dspbase, start + offset);
-		REG_READ(dspbase);
-	}
+	REG_WRITE(map->base, start + offset);
+	REG_READ(map->base);
 
 psb_intel_pipe_cleaner:
 	/* If there was a previous display we can now unpin it */
@@ -424,14 +413,10 @@ psb_intel_pipe_set_base_exit:
 static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_i915_master_private *master_priv; */
-	/* struct drm_i915_private *dev_priv = dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	/* XXX: When our outputs are all unaware of DPMS modes other than off
@@ -442,34 +427,34 @@ static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
 		}
 
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+			REG_WRITE(map->conf, temp | PIPEACONF_ENABLE);
 
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		psb_intel_crtc_load_lut(crtc);
@@ -487,29 +472,29 @@ static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
 
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp & ~PIPEACONF_ENABLE);
+			REG_READ(map->conf);
 		}
 
 		/* Wait for vblank for the disable to take effect. */
 		psb_intel_wait_for_vblank(dev);
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) != 0) {
-			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 		}
 
 		/* Wait for the clocks to turn off. */
@@ -589,22 +574,11 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 			       struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
 	int pipe = psb_intel_crtc->pipe;
-	int fp_reg = (pipe == 0) ? FPA0 : FPB0;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
-	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
-	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
-	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
-	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
-	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
-	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
-	int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
-	int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
-	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk;
 	struct psb_intel_clock_t clock;
 	u32 dpll = 0, fp = 0, dspcntr, pipeconf;
@@ -690,7 +664,7 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 	dpll |= PLL_REF_INPUT_DREFCLK;
 
 	/* setup pipeconf */
-	pipeconf = REG_READ(pipeconf_reg);
+	pipeconf = REG_READ(map->conf);
 
 	/* Set up the display plane register */
 	dspcntr = DISPPLANE_GAMMA_ENABLE;
@@ -712,9 +686,9 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 	drm_mode_debug_printmodeline(mode);
 
 	if (dpll & DPLL_VCO_ENABLE) {
-		REG_WRITE(fp_reg, fp);
-		REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
-		REG_READ(dpll_reg);
+		REG_WRITE(map->fp0, fp);
+		REG_WRITE(map->dpll, dpll & ~DPLL_VCO_ENABLE);
+		REG_READ(map->dpll);
 		udelay(150);
 	}
 
@@ -747,45 +721,45 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 		REG_READ(LVDS);
 	}
 
-	REG_WRITE(fp_reg, fp);
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->fp0, fp);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
 	/* write it again -- the BIOS does, after all */
-	REG_WRITE(dpll_reg, dpll);
+	REG_WRITE(map->dpll, dpll);
 
-	REG_READ(dpll_reg);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
-	REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+	REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 		  ((adjusted_mode->crtc_htotal - 1) << 16));
-	REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+	REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 		  ((adjusted_mode->crtc_hblank_end - 1) << 16));
-	REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+	REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 		  ((adjusted_mode->crtc_hsync_end - 1) << 16));
-	REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+	REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 		  ((adjusted_mode->crtc_vtotal - 1) << 16));
-	REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+	REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 		  ((adjusted_mode->crtc_vblank_end - 1) << 16));
-	REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+	REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 		  ((adjusted_mode->crtc_vsync_end - 1) << 16));
 	/* pipesrc and dspsize control the size that is scaled from,
 	 * which should always be the user's requested size.
 	 */
-	REG_WRITE(dspsize_reg,
+	REG_WRITE(map->size,
 		  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
-	REG_WRITE(dsppos_reg, 0);
-	REG_WRITE(pipesrc_reg,
+	REG_WRITE(map->pos, 0);
+	REG_WRITE(map->src,
 		  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
-	REG_WRITE(pipeconf_reg, pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, pipeconf);
+	REG_READ(map->conf);
 
 	psb_intel_wait_for_vblank(dev);
 
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	/* Flush the plane changes */
 	crtc_funcs->mode_set_base(crtc, x, y, old_fb);
@@ -799,10 +773,10 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_psb_private *dev_priv =
-				(struct drm_psb_private *)dev->dev_private;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
-	int palreg = PALETTE_A;
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
+	int palreg = map->palette;
 	int i;
 
 	/* The clocks have to be on to load the palette. */
@@ -811,12 +785,7 @@ void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 
 	switch (psb_intel_crtc->pipe) {
 	case 0:
-		break;
 	case 1:
-		palreg = PALETTE_B;
-		break;
-	case 2:
-		palreg = PALETTE_C;
 		break;
 	default:
 		dev_err(dev->dev, "Illegal Pipe Number.\n");
@@ -836,7 +805,7 @@ void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 		gma_power_end(dev);
 	} else {
 		for (i = 0; i < 256; i++) {
-			dev_priv->regs.psb.save_palette_a[i] =
+			dev_priv->regs.pipe[0].palette[i] =
 				  ((psb_intel_crtc->lut_r[i] +
 				  psb_intel_crtc->lut_adj[i]) << 16) |
 				  ((psb_intel_crtc->lut_g[i] +
@@ -854,11 +823,10 @@ void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 static void psb_intel_crtc_save(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private *dev_priv =
-			(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -867,27 +835,27 @@ static void psb_intel_crtc_save(struct drm_crtc *crtc)
 		return;
 	}
 
-	crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
-	crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
-	crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
-	crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
-	crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
-	crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
-	crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
-	crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
-	crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
-	crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
-	crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
-	crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
-	crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+	crtc_state->saveDSPCNTR = REG_READ(map->cntr);
+	crtc_state->savePIPECONF = REG_READ(map->conf);
+	crtc_state->savePIPESRC = REG_READ(map->src);
+	crtc_state->saveFP0 = REG_READ(map->fp0);
+	crtc_state->saveFP1 = REG_READ(map->fp1);
+	crtc_state->saveDPLL = REG_READ(map->dpll);
+	crtc_state->saveHTOTAL = REG_READ(map->htotal);
+	crtc_state->saveHBLANK = REG_READ(map->hblank);
+	crtc_state->saveHSYNC = REG_READ(map->hsync);
+	crtc_state->saveVTOTAL = REG_READ(map->vtotal);
+	crtc_state->saveVBLANK = REG_READ(map->vblank);
+	crtc_state->saveVSYNC = REG_READ(map->vsync);
+	crtc_state->saveDSPSTRIDE = REG_READ(map->stride);
 
 	/*NOTE: DSPSIZE DSPPOS only for psb*/
-	crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
-	crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+	crtc_state->saveDSPSIZE = REG_READ(map->size);
+	crtc_state->saveDSPPOS = REG_READ(map->pos);
 
-	crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+	crtc_state->saveDSPBASE = REG_READ(map->base);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
 }
@@ -898,12 +866,10 @@ static void psb_intel_crtc_save(struct drm_crtc *crtc)
 static void psb_intel_crtc_restore(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private * dev_priv =
-				(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	/* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -913,45 +879,45 @@ static void psb_intel_crtc_restore(struct drm_crtc *crtc)
 	}
 
 	if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
-		REG_WRITE(pipeA ? DPLL_A : DPLL_B,
+		REG_WRITE(map->dpll,
 			crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
-		REG_READ(pipeA ? DPLL_A : DPLL_B);
+		REG_READ(map->dpll);
 		udelay(150);
 	}
 
-	REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
-	REG_READ(pipeA ? FPA0 : FPB0);
+	REG_WRITE(map->fp0, crtc_state->saveFP0);
+	REG_READ(map->fp0);
 
-	REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
-	REG_READ(pipeA ? FPA1 : FPB1);
+	REG_WRITE(map->fp1, crtc_state->saveFP1);
+	REG_READ(map->fp1);
 
-	REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
-	REG_READ(pipeA ? DPLL_A : DPLL_B);
+	REG_WRITE(map->dpll, crtc_state->saveDPLL);
+	REG_READ(map->dpll);
 	udelay(150);
 
-	REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
-	REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
-	REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
-	REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
-	REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
-	REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
-	REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+	REG_WRITE(map->htotal, crtc_state->saveHTOTAL);
+	REG_WRITE(map->hblank, crtc_state->saveHBLANK);
+	REG_WRITE(map->hsync, crtc_state->saveHSYNC);
+	REG_WRITE(map->vtotal, crtc_state->saveVTOTAL);
+	REG_WRITE(map->vblank, crtc_state->saveVBLANK);
+	REG_WRITE(map->vsync, crtc_state->saveVSYNC);
+	REG_WRITE(map->stride, crtc_state->saveDSPSTRIDE);
 
-	REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
-	REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+	REG_WRITE(map->size, crtc_state->saveDSPSIZE);
+	REG_WRITE(map->pos, crtc_state->saveDSPPOS);
 
-	REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
-	REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+	REG_WRITE(map->src, crtc_state->savePIPESRC);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
+	REG_WRITE(map->conf, crtc_state->savePIPECONF);
 
 	psb_intel_wait_for_vblank(dev);
 
-	REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+	REG_WRITE(map->cntr, crtc_state->saveDSPCNTR);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
 
 	psb_intel_wait_for_vblank(dev);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
 }
@@ -962,6 +928,7 @@ static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
 				 uint32_t width, uint32_t height)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
 	uint32_t control = (pipe == 0) ? CURACNTR : CURBCNTR;
@@ -969,8 +936,10 @@ static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
 	uint32_t temp;
 	size_t addr = 0;
 	struct gtt_range *gt;
+	struct gtt_range *cursor_gt = psb_intel_crtc->cursor_gt;
 	struct drm_gem_object *obj;
-	int ret;
+	void *tmp_dst, *tmp_src;
+	int ret, i, cursor_pages;
 
 	/* if we want to turn of the cursor ignore width and height */
 	if (!handle) {
@@ -1019,10 +988,32 @@ static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
 		return ret;
 	}
 
+	if (dev_priv->ops->cursor_needs_phys) {
+		if (cursor_gt == NULL) {
+			dev_err(dev->dev, "No hardware cursor mem available");
+			return -ENOMEM;
+		}
 
-	addr = gt->offset;	/* Or resource.start ??? */
+		/* Prevent overflow */
+		if (gt->npage > 4)
+			cursor_pages = 4;
+		else
+			cursor_pages = gt->npage;
+
+		/* Copy the cursor to cursor mem */
+		tmp_dst = dev_priv->vram_addr + cursor_gt->offset;
+		for (i = 0; i < cursor_pages; i++) {
+			tmp_src = kmap(gt->pages[i]);
+			memcpy(tmp_dst, tmp_src, PAGE_SIZE);
+			kunmap(gt->pages[i]);
+			tmp_dst += PAGE_SIZE;
+		}
 
-	psb_intel_crtc->cursor_addr = addr;
+		addr = psb_intel_crtc->cursor_addr;
+	} else {
+		addr = gt->offset;      /* Or resource.start ??? */
+		psb_intel_crtc->cursor_addr = addr;
+	}
 
 	temp = 0;
 	/* set the pipe for the cursor */
@@ -1115,34 +1106,30 @@ static int psb_intel_crtc_clock_get(struct drm_device *dev,
 				struct drm_crtc *crtc)
 {
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 dpll;
 	u32 fp;
 	struct psb_intel_clock_t clock;
 	bool is_lvds;
-	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
 
 	if (gma_power_begin(dev, false)) {
-		dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+		dpll = REG_READ(map->dpll);
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+			fp = REG_READ(map->fp0);
 		else
-			fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+			fp = REG_READ(map->fp1);
 		is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
 		gma_power_end(dev);
 	} else {
-		dpll = (pipe == 0) ?
-			dev_priv->regs.psb.saveDPLL_A :
-			dev_priv->regs.psb.saveDPLL_B;
+		dpll = p->dpll;
 
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA0 :
-				dev_priv->regs.psb.saveFPB0;
+			fp = p->fp0;
 		else
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA1 :
-				dev_priv->regs.psb.saveFPB1;
+		        fp = p->fp1;
 
 		is_lvds = (pipe == 1) && (dev_priv->regs.psb.saveLVDS &
 								LVDS_PORT_EN);
@@ -1202,26 +1189,20 @@ struct drm_display_mode *psb_intel_crtc_mode_get(struct drm_device *dev,
 	int vtot;
 	int vsync;
 	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 
 	if (gma_power_begin(dev, false)) {
-		htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
-		hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
-		vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
-		vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+		htot = REG_READ(map->htotal);
+		hsync = REG_READ(map->hsync);
+		vtot = REG_READ(map->vtotal);
+		vsync = REG_READ(map->vsync);
 		gma_power_end(dev);
 	} else {
-		htot = (pipe == 0) ?
-			dev_priv->regs.psb.saveHTOTAL_A :
-			dev_priv->regs.psb.saveHTOTAL_B;
-		hsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveHSYNC_A :
-			dev_priv->regs.psb.saveHSYNC_B;
-		vtot = (pipe == 0) ?
-			dev_priv->regs.psb.saveVTOTAL_A :
-			dev_priv->regs.psb.saveVTOTAL_B;
-		vsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveVSYNC_A :
-			dev_priv->regs.psb.saveVSYNC_B;
+		htot = p->htotal;
+		hsync = p->hsync;
+		vtot = p->vtotal;
+		vsync = p->vsync;
 	}
 
 	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
@@ -1257,6 +1238,9 @@ void psb_intel_crtc_destroy(struct drm_crtc *crtc)
 		drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
 		psb_intel_crtc->cursor_obj = NULL;
 	}
+
+	if (psb_intel_crtc->cursor_gt != NULL)
+		psb_gtt_free_range(crtc->dev, psb_intel_crtc->cursor_gt);
 	kfree(psb_intel_crtc->crtc_state);
 	drm_crtc_cleanup(crtc);
 	kfree(psb_intel_crtc);
@@ -1285,13 +1269,33 @@ const struct drm_crtc_funcs psb_intel_crtc_funcs = {
  * Set the default value of cursor control and base register
  * to zero. This is a workaround for h/w defect on Oaktrail
  */
-static void psb_intel_cursor_init(struct drm_device *dev, int pipe)
+static void psb_intel_cursor_init(struct drm_device *dev,
+				  struct psb_intel_crtc *psb_intel_crtc)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	u32 control[3] = { CURACNTR, CURBCNTR, CURCCNTR };
 	u32 base[3] = { CURABASE, CURBBASE, CURCBASE };
+	struct gtt_range *cursor_gt;
+
+	if (dev_priv->ops->cursor_needs_phys) {
+		/* Allocate 4 pages of stolen mem for a hardware cursor. That
+		 * is enough for the 64 x 64 ARGB cursors we support.
+		 */
+		cursor_gt = psb_gtt_alloc_range(dev, 4 * PAGE_SIZE, "cursor", 1);
+		if (!cursor_gt) {
+			psb_intel_crtc->cursor_gt = NULL;
+			goto out;
+		}
+		psb_intel_crtc->cursor_gt = cursor_gt;
+		psb_intel_crtc->cursor_addr = dev_priv->stolen_base +
+							cursor_gt->offset;
+	} else {
+		psb_intel_crtc->cursor_gt = NULL;
+	}
 
-	REG_WRITE(control[pipe], 0);
-	REG_WRITE(base[pipe], 0);
+out:
+	REG_WRITE(control[psb_intel_crtc->pipe], 0);
+	REG_WRITE(base[psb_intel_crtc->pipe], 0);
 }
 
 void psb_intel_crtc_init(struct drm_device *dev, int pipe,
@@ -1357,7 +1361,7 @@ void psb_intel_crtc_init(struct drm_device *dev, int pipe,
 	psb_intel_crtc->mode_set.connectors =
 	    (struct drm_connector **) (psb_intel_crtc + 1);
 	psb_intel_crtc->mode_set.num_connectors = 0;
-	psb_intel_cursor_init(dev, pipe);
+	psb_intel_cursor_init(dev, psb_intel_crtc);
 }
 
 int psb_intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
diff --git a/drivers/gpu/drm/gma500/psb_intel_drv.h b/drivers/gpu/drm/gma500/psb_intel_drv.h
index f40535e56689..2515f83248cb 100644
--- a/drivers/gpu/drm/gma500/psb_intel_drv.h
+++ b/drivers/gpu/drm/gma500/psb_intel_drv.h
@@ -106,11 +106,6 @@ struct psb_intel_mode_device {
 	 size_t(*bo_offset) (struct drm_device *dev, void *bo);
 
 	/*
-	 * Cursor (Can go ?)
-	 */
-	int cursor_needs_physical;
-
-	/*
 	 * LVDS info
 	 */
 	int backlight_duty_cycle;	/* restore backlight to this value */
@@ -176,6 +171,7 @@ struct psb_intel_crtc {
 	int pipe;
 	int plane;
 	uint32_t cursor_addr;
+	struct gtt_range *cursor_gt;
 	u8 lut_r[256], lut_g[256], lut_b[256];
 	u8 lut_adj[256];
 	struct psb_intel_framebuffer *fbdev_fb;
@@ -193,6 +189,9 @@ struct psb_intel_crtc {
 	/*crtc mode setting flags*/
 	u32 mode_flags;
 
+	bool active;
+	bool crtc_enable;
+
 	/* Saved Crtc HW states */
 	struct psb_intel_crtc_state *crtc_state;
 };
diff --git a/drivers/gpu/drm/gma500/psb_intel_reg.h b/drivers/gpu/drm/gma500/psb_intel_reg.h
index e89d3a2e8fdc..8e8c8efb0a89 100644
--- a/drivers/gpu/drm/gma500/psb_intel_reg.h
+++ b/drivers/gpu/drm/gma500/psb_intel_reg.h
@@ -91,6 +91,9 @@
 
 #define BLC_PWM_CTL		0x61254
 #define BLC_PWM_CTL2		0x61250
+#define  PWM_ENABLE		(1 << 31)
+#define  PWM_LEGACY_MODE	(1 << 30)
+#define  PWM_PIPE_B		(1 << 29)
 #define BLC_PWM_CTL_C		0x62254
 #define BLC_PWM_CTL2_C		0x62250
 #define BACKLIGHT_MODULATION_FREQ_SHIFT		(17)
@@ -216,7 +219,7 @@
 #define DPLLB_LVDS_P2_CLOCK_DIV_14	(0 << 24)	/* i915 */
 #define DPLLB_LVDS_P2_CLOCK_DIV_7	(1 << 24)	/* i915 */
 #define DPLL_P2_CLOCK_DIV_MASK		0x03000000	/* i915 */
-#define DPLL_FPA01_P1_POST_DIV_MASK	0x00ff0000	/* i915 */
+#define DPLL_FPA0h1_P1_POST_DIV_MASK	0x00ff0000	/* i915 */
 #define DPLL_LOCK			(1 << 15)	/* CDV */
 
 /*
@@ -343,6 +346,9 @@
 #define FP_M2_DIV_SHIFT			0
 
 #define PORT_HOTPLUG_EN		0x61110
+#define HDMIB_HOTPLUG_INT_EN		(1 << 29)
+#define HDMIC_HOTPLUG_INT_EN		(1 << 28)
+#define HDMID_HOTPLUG_INT_EN		(1 << 27)
 #define SDVOB_HOTPLUG_INT_EN		(1 << 26)
 #define SDVOC_HOTPLUG_INT_EN		(1 << 25)
 #define TV_HOTPLUG_INT_EN		(1 << 18)
@@ -501,10 +507,12 @@
 #define PIPE_VBLANK_INTERRUPT_ENABLE		(1UL << 17)
 #define PIPE_START_VBLANK_INTERRUPT_ENABLE	(1UL << 18)
 #define PIPE_TE_ENABLE				(1UL << 22)
+#define PIPE_LEGACY_BLC_EVENT_ENABLE		(1UL << 22)
 #define PIPE_DPST_EVENT_ENABLE			(1UL << 23)
 #define PIPE_VSYNC_ENABL			(1UL << 25)
 #define PIPE_HDMI_AUDIO_UNDERRUN		(1UL << 26)
 #define PIPE_HDMI_AUDIO_BUFFER_DONE		(1UL << 27)
+#define PIPE_FIFO_UNDERRUN			(1UL << 31)
 #define PIPE_HDMI_AUDIO_INT_MASK		(PIPE_HDMI_AUDIO_UNDERRUN | \
 						PIPE_HDMI_AUDIO_BUFFER_DONE)
 #define PIPE_EVENT_MASK ((1 << 29)|(1 << 28)|(1 << 27)|(1 << 26)|(1 << 24)|(1 << 23)|(1 << 22)|(1 << 21)|(1 << 20)|(1 << 16))
@@ -569,12 +577,27 @@ struct dpst_guardband {
 #define PIPE_PIXEL_MASK		0x00ffffff
 #define PIPE_PIXEL_SHIFT	0
 
+#define FW_BLC_SELF		0x20e0 
+#define FW_BLC_SELF_EN          (1<<15)
+
 #define DSPARB			0x70030
 #define DSPFW1			0x70034
+#define DSP_FIFO_SR_WM_MASK		0xFF800000
+#define DSP_FIFO_SR_WM_SHIFT		23
+#define CURSOR_B_FIFO_WM_MASK		0x003F0000
+#define CURSOR_B_FIFO_WM_SHIFT		16
 #define DSPFW2			0x70038
+#define CURSOR_A_FIFO_WM_MASK		0x3F00
+#define CURSOR_A_FIFO_WM_SHIFT		8
+#define DSP_PLANE_C_FIFO_WM_MASK	0x7F
+#define DSP_PLANE_C_FIFO_WM_SHIFT	0
 #define DSPFW3			0x7003c
 #define DSPFW4			0x70050
 #define DSPFW5			0x70054
+#define DSP_PLANE_B_FIFO_WM1_SHIFT	24
+#define DSP_PLANE_A_FIFO_WM1_SHIFT	16
+#define CURSOR_B_FIFO_WM1_SHIFT		8
+#define CURSOR_FIFO_SR_WM1_SHIFT	0
 #define DSPFW6			0x70058
 #define DSPCHICKENBIT		0x70400
 #define DSPACNTR		0x70180
@@ -1290,6 +1313,15 @@ No status bits are changed.
 #define SB_N_CB_TUNE_MASK			PSB_MASK(25, 24)
 #define SB_N_CB_TUNE_SHIFT			24
 
+/* the bit 14:13 is used to select between the different reference clock for Pipe A/B */
+#define SB_REF_DPLLA		0x8010
+#define SB_REF_DPLLB		0x8030
+#define	REF_CLK_MASK		(0x3 << 13)
+#define REF_CLK_CORE		(0 << 13)
+#define REF_CLK_DPLL		(1 << 13)
+#define REF_CLK_DPLLA		(2 << 13)
+/* For the DPLL B, it will use the reference clk from DPLL A when using (2 << 13) */
+
 #define _SB_REF_A		0x8018
 #define _SB_REF_B		0x8038
 #define SB_REF_SFR(pipe)	_PIPE(pipe, _SB_REF_A, _SB_REF_B)
@@ -1313,6 +1345,7 @@ No status bits are changed.
 
 #define LANE_PLL_MASK		(0x7 << 20)
 #define LANE_PLL_ENABLE		(0x3 << 20)
+#define LANE_PLL_PIPE(p)	(((p) == 0) ? (1 << 21) : (0 << 21))
 
 
 #endif
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo.c b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
index 36330cabcea2..d39b15be7649 100644
--- a/drivers/gpu/drm/gma500/psb_intel_sdvo.c
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
@@ -1141,7 +1141,6 @@ static void psb_intel_sdvo_dpms(struct drm_encoder *encoder, int mode)
 static int psb_intel_sdvo_mode_valid(struct drm_connector *connector,
 				 struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
 	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
 
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
@@ -1161,11 +1160,6 @@ static int psb_intel_sdvo_mode_valid(struct drm_connector *connector,
 			return MODE_PANEL;
 	}
 
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
@@ -2044,8 +2038,7 @@ psb_intel_sdvo_add_hdmi_properties(struct psb_intel_sdvo_connector *connector)
 	struct drm_device *dev = connector->base.base.dev;
 
 	intel_attach_force_audio_property(&connector->base.base);
-	if (INTEL_INFO(dev)->gen >= 4 && IS_MOBILE(dev))
-		intel_attach_broadcast_rgb_property(&connector->base.base);
+	intel_attach_broadcast_rgb_property(&connector->base.base);
 	*/
 }
 
diff --git a/drivers/gpu/drm/gma500/psb_irq.c b/drivers/gpu/drm/gma500/psb_irq.c
index 1869586457b1..8652cdf3f03f 100644
--- a/drivers/gpu/drm/gma500/psb_irq.c
+++ b/drivers/gpu/drm/gma500/psb_irq.c
@@ -190,6 +190,9 @@ static void mid_pipe_event_handler(struct drm_device *dev, int pipe)
  */
 static void psb_vdc_interrupt(struct drm_device *dev, uint32_t vdc_stat)
 {
+	if (vdc_stat & _PSB_IRQ_ASLE)
+		psb_intel_opregion_asle_intr(dev);
+
 	if (vdc_stat & _PSB_VSYNC_PIPEA_FLAG)
 		mid_pipe_event_handler(dev, 0);
 
@@ -199,11 +202,9 @@ static void psb_vdc_interrupt(struct drm_device *dev, uint32_t vdc_stat)
 
 irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
 {
-	struct drm_device *dev = (struct drm_device *) arg;
-	struct drm_psb_private *dev_priv =
-	    (struct drm_psb_private *) dev->dev_private;
-
-	uint32_t vdc_stat, dsp_int = 0, sgx_int = 0;
+	struct drm_device *dev = arg;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	uint32_t vdc_stat, dsp_int = 0, sgx_int = 0, hotplug_int = 0;
 	int handled = 0;
 
 	spin_lock(&dev_priv->irqmask_lock);
@@ -220,6 +221,8 @@ irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
 
 	if (vdc_stat & _PSB_IRQ_SGX_FLAG)
 		sgx_int = 1;
+	if (vdc_stat & _PSB_IRQ_DISP_HOTSYNC)
+		hotplug_int = 1;
 
 	vdc_stat &= dev_priv->vdc_irq_mask;
 	spin_unlock(&dev_priv->irqmask_lock);
@@ -241,6 +244,13 @@ irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
 		handled = 1;
 	}
 
+	/* Note: this bit has other meanings on some devices, so we will
+	   need to address that later if it ever matters */
+	if (hotplug_int && dev_priv->ops->hotplug) {
+		handled = dev_priv->ops->hotplug(dev);
+		REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	}
+
 	PSB_WVDC32(vdc_stat, PSB_INT_IDENTITY_R);
 	(void) PSB_RVDC32(PSB_INT_IDENTITY_R);
 	DRM_READMEMORYBARRIER();
@@ -273,6 +283,11 @@ void psb_irq_preinstall(struct drm_device *dev)
 		dev_priv->vdc_irq_mask |= _MDFLD_PIPEC_EVENT_FLAG;
 	*/
 
+	/* Revisit this area - want per device masks ? */
+	if (dev_priv->ops->hotplug)
+		dev_priv->vdc_irq_mask |= _PSB_IRQ_DISP_HOTSYNC;
+	dev_priv->vdc_irq_mask |= _PSB_IRQ_ASLE;
+
 	/* This register is safe even if display island is off */
 	PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
 	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
@@ -305,18 +320,23 @@ int psb_irq_postinstall(struct drm_device *dev)
 	else
 		psb_disable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
 
+	if (dev_priv->ops->hotplug_enable)
+		dev_priv->ops->hotplug_enable(dev, true);
+
 	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
 	return 0;
 }
 
 void psb_irq_uninstall(struct drm_device *dev)
 {
-	struct drm_psb_private *dev_priv =
-	    (struct drm_psb_private *) dev->dev_private;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
 
+	if (dev_priv->ops->hotplug_enable)
+		dev_priv->ops->hotplug_enable(dev, false);
+
 	PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
 
 	if (dev->vblank_enabled[0])
@@ -406,7 +426,7 @@ void psb_irq_turn_off_dpst(struct drm_device *dev)
 		psb_disable_pipestat(dev_priv, 0, PIPE_DPST_EVENT_ENABLE);
 
 		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
-		PSB_WVDC32(pwm_reg & !(PWM_PHASEIN_INT_ENABLE),
+		PSB_WVDC32(pwm_reg & ~PWM_PHASEIN_INT_ENABLE,
 							PWM_CONTROL_LOGIC);
 		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
 
diff --git a/drivers/gpu/drm/gma500/psb_lid.c b/drivers/gpu/drm/gma500/psb_lid.c
index b867aabe6bf3..1d2ebb5e530f 100644
--- a/drivers/gpu/drm/gma500/psb_lid.c
+++ b/drivers/gpu/drm/gma500/psb_lid.c
@@ -29,7 +29,7 @@ static void psb_lid_timer_func(unsigned long data)
 	struct drm_device *dev = (struct drm_device *)dev_priv->dev;
 	struct timer_list *lid_timer = &dev_priv->lid_timer;
 	unsigned long irq_flags;
-	u32 *lid_state = dev_priv->lid_state;
+	u32 __iomem *lid_state = dev_priv->opregion.lid_state;
 	u32 pp_status;
 
 	if (readl(lid_state) == dev_priv->lid_last_state)
@@ -40,10 +40,16 @@ static void psb_lid_timer_func(unsigned long data)
 		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) | POWER_TARGET_ON);
 		do {
 			pp_status = REG_READ(PP_STATUS);
-		} while ((pp_status & PP_ON) == 0);
+		} while ((pp_status & PP_ON) == 0 &&
+			 (pp_status & PP_SEQUENCE_MASK) != 0);
 
-		/*FIXME: should be backlight level before*/
-		psb_intel_lvds_set_brightness(dev, 100);
+		if (REG_READ(PP_STATUS) & PP_ON) {
+			/*FIXME: should be backlight level before*/
+			psb_intel_lvds_set_brightness(dev, 100);
+		} else {
+			DRM_DEBUG("LVDS panel never powered up");
+			return;
+		}
 	} else {
 		psb_intel_lvds_set_brightness(dev, 0);
 
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index ce7fc77678b4..2e9268da58d8 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -11,17 +11,21 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o \
 	  i915_gem_evict.o \
 	  i915_gem_execbuffer.o \
 	  i915_gem_gtt.o \
+	  i915_gem_stolen.o \
 	  i915_gem_tiling.o \
+	  i915_sysfs.o \
 	  i915_trace_points.o \
 	  intel_display.o \
 	  intel_crt.o \
 	  intel_lvds.o \
 	  intel_bios.o \
+	  intel_ddi.o \
 	  intel_dp.o \
 	  intel_hdmi.o \
 	  intel_sdvo.o \
 	  intel_modes.o \
 	  intel_panel.o \
+	  intel_pm.o \
 	  intel_i2c.o \
 	  intel_fb.o \
 	  intel_tv.o \
@@ -34,7 +38,8 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o \
 	  dvo_ch7017.o \
 	  dvo_ivch.o \
 	  dvo_tfp410.o \
-	  dvo_sil164.o
+	  dvo_sil164.o \
+	  i915_gem_dmabuf.o
 
 i915-$(CONFIG_COMPAT)   += i915_ioc32.o
 
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index e6162a1681f0..eb2b3c25b9e1 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -47,7 +47,6 @@ enum {
 	FLUSHING_LIST,
 	INACTIVE_LIST,
 	PINNED_LIST,
-	DEFERRED_FREE_LIST,
 };
 
 static const char *yesno(int v)
@@ -178,18 +177,10 @@ static int i915_gem_object_list_info(struct seq_file *m, void *data)
 		seq_printf(m, "Inactive:\n");
 		head = &dev_priv->mm.inactive_list;
 		break;
-	case PINNED_LIST:
-		seq_printf(m, "Pinned:\n");
-		head = &dev_priv->mm.pinned_list;
-		break;
 	case FLUSHING_LIST:
 		seq_printf(m, "Flushing:\n");
 		head = &dev_priv->mm.flushing_list;
 		break;
-	case DEFERRED_FREE_LIST:
-		seq_printf(m, "Deferred free:\n");
-		head = &dev_priv->mm.deferred_free_list;
-		break;
 	default:
 		mutex_unlock(&dev->struct_mutex);
 		return -EINVAL;
@@ -252,21 +243,11 @@ static int i915_gem_object_info(struct seq_file *m, void* data)
 		   count, mappable_count, size, mappable_size);
 
 	size = count = mappable_size = mappable_count = 0;
-	count_objects(&dev_priv->mm.pinned_list, mm_list);
-	seq_printf(m, "  %u [%u] pinned objects, %zu [%zu] bytes\n",
-		   count, mappable_count, size, mappable_size);
-
-	size = count = mappable_size = mappable_count = 0;
 	count_objects(&dev_priv->mm.inactive_list, mm_list);
 	seq_printf(m, "  %u [%u] inactive objects, %zu [%zu] bytes\n",
 		   count, mappable_count, size, mappable_size);
 
 	size = count = mappable_size = mappable_count = 0;
-	count_objects(&dev_priv->mm.deferred_free_list, mm_list);
-	seq_printf(m, "  %u [%u] freed objects, %zu [%zu] bytes\n",
-		   count, mappable_count, size, mappable_size);
-
-	size = count = mappable_size = mappable_count = 0;
 	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list) {
 		if (obj->fault_mappable) {
 			size += obj->gtt_space->size;
@@ -294,6 +275,7 @@ static int i915_gem_gtt_info(struct seq_file *m, void* data)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
 	struct drm_device *dev = node->minor->dev;
+	uintptr_t list = (uintptr_t) node->info_ent->data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj;
 	size_t total_obj_size, total_gtt_size;
@@ -305,6 +287,9 @@ static int i915_gem_gtt_info(struct seq_file *m, void* data)
 
 	total_obj_size = total_gtt_size = count = 0;
 	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list) {
+		if (list == PINNED_LIST && obj->pin_count == 0)
+			continue;
+
 		seq_printf(m, "   ");
 		describe_obj(m, obj);
 		seq_printf(m, "\n");
@@ -321,7 +306,6 @@ static int i915_gem_gtt_info(struct seq_file *m, void* data)
 	return 0;
 }
 
-
 static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
@@ -430,10 +414,6 @@ static void i915_ring_seqno_info(struct seq_file *m,
 	if (ring->get_seqno) {
 		seq_printf(m, "Current sequence (%s): %d\n",
 			   ring->name, ring->get_seqno(ring));
-		seq_printf(m, "Waiter sequence (%s):  %d\n",
-			   ring->name, ring->waiting_seqno);
-		seq_printf(m, "IRQ sequence (%s):     %d\n",
-			   ring->name, ring->irq_seqno);
 	}
 }
 
@@ -468,7 +448,45 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 	if (ret)
 		return ret;
 
-	if (!HAS_PCH_SPLIT(dev)) {
+	if (IS_VALLEYVIEW(dev)) {
+		seq_printf(m, "Display IER:\t%08x\n",
+			   I915_READ(VLV_IER));
+		seq_printf(m, "Display IIR:\t%08x\n",
+			   I915_READ(VLV_IIR));
+		seq_printf(m, "Display IIR_RW:\t%08x\n",
+			   I915_READ(VLV_IIR_RW));
+		seq_printf(m, "Display IMR:\t%08x\n",
+			   I915_READ(VLV_IMR));
+		for_each_pipe(pipe)
+			seq_printf(m, "Pipe %c stat:\t%08x\n",
+				   pipe_name(pipe),
+				   I915_READ(PIPESTAT(pipe)));
+
+		seq_printf(m, "Master IER:\t%08x\n",
+			   I915_READ(VLV_MASTER_IER));
+
+		seq_printf(m, "Render IER:\t%08x\n",
+			   I915_READ(GTIER));
+		seq_printf(m, "Render IIR:\t%08x\n",
+			   I915_READ(GTIIR));
+		seq_printf(m, "Render IMR:\t%08x\n",
+			   I915_READ(GTIMR));
+
+		seq_printf(m, "PM IER:\t\t%08x\n",
+			   I915_READ(GEN6_PMIER));
+		seq_printf(m, "PM IIR:\t\t%08x\n",
+			   I915_READ(GEN6_PMIIR));
+		seq_printf(m, "PM IMR:\t\t%08x\n",
+			   I915_READ(GEN6_PMIMR));
+
+		seq_printf(m, "Port hotplug:\t%08x\n",
+			   I915_READ(PORT_HOTPLUG_EN));
+		seq_printf(m, "DPFLIPSTAT:\t%08x\n",
+			   I915_READ(VLV_DPFLIPSTAT));
+		seq_printf(m, "DPINVGTT:\t%08x\n",
+			   I915_READ(DPINVGTT));
+
+	} else if (!HAS_PCH_SPLIT(dev)) {
 		seq_printf(m, "Interrupt enable:    %08x\n",
 			   I915_READ(IER));
 		seq_printf(m, "Interrupt identity:  %08x\n",
@@ -564,69 +582,6 @@ static int i915_hws_info(struct seq_file *m, void *data)
 	return 0;
 }
 
-static int i915_ringbuffer_data(struct seq_file *m, void *data)
-{
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
-	int ret;
-
-	ret = mutex_lock_interruptible(&dev->struct_mutex);
-	if (ret)
-		return ret;
-
-	ring = &dev_priv->ring[(uintptr_t)node->info_ent->data];
-	if (!ring->obj) {
-		seq_printf(m, "No ringbuffer setup\n");
-	} else {
-		const u8 __iomem *virt = ring->virtual_start;
-		uint32_t off;
-
-		for (off = 0; off < ring->size; off += 4) {
-			uint32_t *ptr = (uint32_t *)(virt + off);
-			seq_printf(m, "%08x :  %08x\n", off, *ptr);
-		}
-	}
-	mutex_unlock(&dev->struct_mutex);
-
-	return 0;
-}
-
-static int i915_ringbuffer_info(struct seq_file *m, void *data)
-{
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
-	int ret;
-
-	ring = &dev_priv->ring[(uintptr_t)node->info_ent->data];
-	if (ring->size == 0)
-		return 0;
-
-	ret = mutex_lock_interruptible(&dev->struct_mutex);
-	if (ret)
-		return ret;
-
-	seq_printf(m, "Ring %s:\n", ring->name);
-	seq_printf(m, "  Head :    %08x\n", I915_READ_HEAD(ring) & HEAD_ADDR);
-	seq_printf(m, "  Tail :    %08x\n", I915_READ_TAIL(ring) & TAIL_ADDR);
-	seq_printf(m, "  Size :    %08x\n", ring->size);
-	seq_printf(m, "  Active :  %08x\n", intel_ring_get_active_head(ring));
-	seq_printf(m, "  NOPID :   %08x\n", I915_READ_NOPID(ring));
-	if (IS_GEN6(dev) || IS_GEN7(dev)) {
-		seq_printf(m, "  Sync 0 :   %08x\n", I915_READ_SYNC_0(ring));
-		seq_printf(m, "  Sync 1 :   %08x\n", I915_READ_SYNC_1(ring));
-	}
-	seq_printf(m, "  Control : %08x\n", I915_READ_CTL(ring));
-	seq_printf(m, "  Start :   %08x\n", I915_READ_START(ring));
-
-	mutex_unlock(&dev->struct_mutex);
-
-	return 0;
-}
-
 static const char *ring_str(int ring)
 {
 	switch (ring) {
@@ -704,6 +659,7 @@ static void i915_ring_error_state(struct seq_file *m,
 				  struct drm_i915_error_state *error,
 				  unsigned ring)
 {
+	BUG_ON(ring >= I915_NUM_RINGS); /* shut up confused gcc */
 	seq_printf(m, "%s command stream:\n", ring_str(ring));
 	seq_printf(m, "  HEAD: 0x%08x\n", error->head[ring]);
 	seq_printf(m, "  TAIL: 0x%08x\n", error->tail[ring]);
@@ -718,8 +674,8 @@ static void i915_ring_error_state(struct seq_file *m,
 	if (INTEL_INFO(dev)->gen >= 4)
 		seq_printf(m, "  INSTPS: 0x%08x\n", error->instps[ring]);
 	seq_printf(m, "  INSTPM: 0x%08x\n", error->instpm[ring]);
+	seq_printf(m, "  FADDR: 0x%08x\n", error->faddr[ring]);
 	if (INTEL_INFO(dev)->gen >= 6) {
-		seq_printf(m, "  FADDR: 0x%08x\n", error->faddr[ring]);
 		seq_printf(m, "  FAULT_REG: 0x%08x\n", error->fault_reg[ring]);
 		seq_printf(m, "  SYNC_0: 0x%08x\n",
 			   error->semaphore_mboxes[ring][0]);
@@ -727,31 +683,35 @@ static void i915_ring_error_state(struct seq_file *m,
 			   error->semaphore_mboxes[ring][1]);
 	}
 	seq_printf(m, "  seqno: 0x%08x\n", error->seqno[ring]);
+	seq_printf(m, "  waiting: %s\n", yesno(error->waiting[ring]));
 	seq_printf(m, "  ring->head: 0x%08x\n", error->cpu_ring_head[ring]);
 	seq_printf(m, "  ring->tail: 0x%08x\n", error->cpu_ring_tail[ring]);
 }
 
+struct i915_error_state_file_priv {
+	struct drm_device *dev;
+	struct drm_i915_error_state *error;
+};
+
 static int i915_error_state(struct seq_file *m, void *unused)
 {
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
+	struct i915_error_state_file_priv *error_priv = m->private;
+	struct drm_device *dev = error_priv->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct drm_i915_error_state *error;
-	unsigned long flags;
+	struct drm_i915_error_state *error = error_priv->error;
+	struct intel_ring_buffer *ring;
 	int i, j, page, offset, elt;
 
-	spin_lock_irqsave(&dev_priv->error_lock, flags);
-	if (!dev_priv->first_error) {
+	if (!error) {
 		seq_printf(m, "no error state collected\n");
-		goto out;
+		return 0;
 	}
 
-	error = dev_priv->first_error;
-
 	seq_printf(m, "Time: %ld s %ld us\n", error->time.tv_sec,
 		   error->time.tv_usec);
 	seq_printf(m, "PCI ID: 0x%04x\n", dev->pci_device);
 	seq_printf(m, "EIR: 0x%08x\n", error->eir);
+	seq_printf(m, "IER: 0x%08x\n", error->ier);
 	seq_printf(m, "PGTBL_ER: 0x%08x\n", error->pgtbl_er);
 
 	for (i = 0; i < dev_priv->num_fence_regs; i++)
@@ -762,11 +722,8 @@ static int i915_error_state(struct seq_file *m, void *unused)
 		seq_printf(m, "DONE_REG: 0x%08x\n", error->done_reg);
 	}
 
-	i915_ring_error_state(m, dev, error, RCS);
-	if (HAS_BLT(dev))
-		i915_ring_error_state(m, dev, error, BCS);
-	if (HAS_BSD(dev))
-		i915_ring_error_state(m, dev, error, VCS);
+	for_each_ring(ring, dev_priv, i)
+		i915_ring_error_state(m, dev, error, i);
 
 	if (error->active_bo)
 		print_error_buffers(m, "Active",
@@ -828,12 +785,71 @@ static int i915_error_state(struct seq_file *m, void *unused)
 	if (error->display)
 		intel_display_print_error_state(m, dev, error->display);
 
-out:
+	return 0;
+}
+
+static ssize_t
+i915_error_state_write(struct file *filp,
+		       const char __user *ubuf,
+		       size_t cnt,
+		       loff_t *ppos)
+{
+	struct seq_file *m = filp->private_data;
+	struct i915_error_state_file_priv *error_priv = m->private;
+	struct drm_device *dev = error_priv->dev;
+
+	DRM_DEBUG_DRIVER("Resetting error state\n");
+
+	mutex_lock(&dev->struct_mutex);
+	i915_destroy_error_state(dev);
+	mutex_unlock(&dev->struct_mutex);
+
+	return cnt;
+}
+
+static int i915_error_state_open(struct inode *inode, struct file *file)
+{
+	struct drm_device *dev = inode->i_private;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct i915_error_state_file_priv *error_priv;
+	unsigned long flags;
+
+	error_priv = kzalloc(sizeof(*error_priv), GFP_KERNEL);
+	if (!error_priv)
+		return -ENOMEM;
+
+	error_priv->dev = dev;
+
+	spin_lock_irqsave(&dev_priv->error_lock, flags);
+	error_priv->error = dev_priv->first_error;
+	if (error_priv->error)
+		kref_get(&error_priv->error->ref);
 	spin_unlock_irqrestore(&dev_priv->error_lock, flags);
 
-	return 0;
+	return single_open(file, i915_error_state, error_priv);
+}
+
+static int i915_error_state_release(struct inode *inode, struct file *file)
+{
+	struct seq_file *m = file->private_data;
+	struct i915_error_state_file_priv *error_priv = m->private;
+
+	if (error_priv->error)
+		kref_put(&error_priv->error->ref, i915_error_state_free);
+	kfree(error_priv);
+
+	return single_release(inode, file);
 }
 
+static const struct file_operations i915_error_state_fops = {
+	.owner = THIS_MODULE,
+	.open = i915_error_state_open,
+	.read = seq_read,
+	.write = i915_error_state_write,
+	.llseek = default_llseek,
+	.release = i915_error_state_release,
+};
+
 static int i915_rstdby_delays(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
@@ -1132,6 +1148,17 @@ static int gen6_drpc_info(struct seq_file *m)
 
 	seq_printf(m, "Core Power Down: %s\n",
 		   yesno(gt_core_status & GEN6_CORE_CPD_STATE_MASK));
+
+	/* Not exactly sure what this is */
+	seq_printf(m, "RC6 \"Locked to RPn\" residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6_LOCKED));
+	seq_printf(m, "RC6 residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6));
+	seq_printf(m, "RC6+ residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6p));
+	seq_printf(m, "RC6++ residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6pp));
+
 	return 0;
 }
 
@@ -1306,17 +1333,25 @@ static int i915_opregion(struct seq_file *m, void *unused)
 	struct drm_device *dev = node->minor->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_opregion *opregion = &dev_priv->opregion;
+	void *data = kmalloc(OPREGION_SIZE, GFP_KERNEL);
 	int ret;
 
+	if (data == NULL)
+		return -ENOMEM;
+
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
 	if (ret)
-		return ret;
+		goto out;
 
-	if (opregion->header)
-		seq_write(m, opregion->header, OPREGION_SIZE);
+	if (opregion->header) {
+		memcpy_fromio(data, opregion->header, OPREGION_SIZE);
+		seq_write(m, data, OPREGION_SIZE);
+	}
 
 	mutex_unlock(&dev->struct_mutex);
 
+out:
+	kfree(data);
 	return 0;
 }
 
@@ -1505,6 +1540,53 @@ static int i915_ppgtt_info(struct seq_file *m, void *data)
 	return 0;
 }
 
+static int i915_dpio_info(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret;
+
+
+	if (!IS_VALLEYVIEW(dev)) {
+		seq_printf(m, "unsupported\n");
+		return 0;
+	}
+
+	ret = mutex_lock_interruptible(&dev->mode_config.mutex);
+	if (ret)
+		return ret;
+
+	seq_printf(m, "DPIO_CTL: 0x%08x\n", I915_READ(DPIO_CTL));
+
+	seq_printf(m, "DPIO_DIV_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_DIV_A));
+	seq_printf(m, "DPIO_DIV_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_DIV_B));
+
+	seq_printf(m, "DPIO_REFSFR_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_REFSFR_A));
+	seq_printf(m, "DPIO_REFSFR_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_REFSFR_B));
+
+	seq_printf(m, "DPIO_CORE_CLK_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_CORE_CLK_A));
+	seq_printf(m, "DPIO_CORE_CLK_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_CORE_CLK_B));
+
+	seq_printf(m, "DPIO_LFP_COEFF_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_LFP_COEFF_A));
+	seq_printf(m, "DPIO_LFP_COEFF_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_LFP_COEFF_B));
+
+	seq_printf(m, "DPIO_FASTCLK_DISABLE: 0x%08x\n",
+		   intel_dpio_read(dev_priv, DPIO_FASTCLK_DISABLE));
+
+	mutex_unlock(&dev->mode_config.mutex);
+
+	return 0;
+}
+
 static ssize_t
 i915_wedged_read(struct file *filp,
 		 char __user *ubuf,
@@ -1562,6 +1644,65 @@ static const struct file_operations i915_wedged_fops = {
 };
 
 static ssize_t
+i915_ring_stop_read(struct file *filp,
+		    char __user *ubuf,
+		    size_t max,
+		    loff_t *ppos)
+{
+	struct drm_device *dev = filp->private_data;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	char buf[20];
+	int len;
+
+	len = snprintf(buf, sizeof(buf),
+		       "0x%08x\n", dev_priv->stop_rings);
+
+	if (len > sizeof(buf))
+		len = sizeof(buf);
+
+	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+}
+
+static ssize_t
+i915_ring_stop_write(struct file *filp,
+		     const char __user *ubuf,
+		     size_t cnt,
+		     loff_t *ppos)
+{
+	struct drm_device *dev = filp->private_data;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	char buf[20];
+	int val = 0;
+
+	if (cnt > 0) {
+		if (cnt > sizeof(buf) - 1)
+			return -EINVAL;
+
+		if (copy_from_user(buf, ubuf, cnt))
+			return -EFAULT;
+		buf[cnt] = 0;
+
+		val = simple_strtoul(buf, NULL, 0);
+	}
+
+	DRM_DEBUG_DRIVER("Stopping rings 0x%08x\n", val);
+
+	mutex_lock(&dev->struct_mutex);
+	dev_priv->stop_rings = val;
+	mutex_unlock(&dev->struct_mutex);
+
+	return cnt;
+}
+
+static const struct file_operations i915_ring_stop_fops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = i915_ring_stop_read,
+	.write = i915_ring_stop_write,
+	.llseek = default_llseek,
+};
+
+static ssize_t
 i915_max_freq_read(struct file *filp,
 		   char __user *ubuf,
 		   size_t max,
@@ -1738,7 +1879,7 @@ static int i915_forcewake_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
-int i915_forcewake_release(struct inode *inode, struct file *file)
+static int i915_forcewake_release(struct inode *inode, struct file *file)
 {
 	struct drm_device *dev = inode->i_private;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1803,11 +1944,10 @@ static struct drm_info_list i915_debugfs_list[] = {
 	{"i915_capabilities", i915_capabilities, 0},
 	{"i915_gem_objects", i915_gem_object_info, 0},
 	{"i915_gem_gtt", i915_gem_gtt_info, 0},
+	{"i915_gem_pinned", i915_gem_gtt_info, 0, (void *) PINNED_LIST},
 	{"i915_gem_active", i915_gem_object_list_info, 0, (void *) ACTIVE_LIST},
 	{"i915_gem_flushing", i915_gem_object_list_info, 0, (void *) FLUSHING_LIST},
 	{"i915_gem_inactive", i915_gem_object_list_info, 0, (void *) INACTIVE_LIST},
-	{"i915_gem_pinned", i915_gem_object_list_info, 0, (void *) PINNED_LIST},
-	{"i915_gem_deferred_free", i915_gem_object_list_info, 0, (void *) DEFERRED_FREE_LIST},
 	{"i915_gem_pageflip", i915_gem_pageflip_info, 0},
 	{"i915_gem_request", i915_gem_request_info, 0},
 	{"i915_gem_seqno", i915_gem_seqno_info, 0},
@@ -1816,13 +1956,6 @@ static struct drm_info_list i915_debugfs_list[] = {
 	{"i915_gem_hws", i915_hws_info, 0, (void *)RCS},
 	{"i915_gem_hws_blt", i915_hws_info, 0, (void *)BCS},
 	{"i915_gem_hws_bsd", i915_hws_info, 0, (void *)VCS},
-	{"i915_ringbuffer_data", i915_ringbuffer_data, 0, (void *)RCS},
-	{"i915_ringbuffer_info", i915_ringbuffer_info, 0, (void *)RCS},
-	{"i915_bsd_ringbuffer_data", i915_ringbuffer_data, 0, (void *)VCS},
-	{"i915_bsd_ringbuffer_info", i915_ringbuffer_info, 0, (void *)VCS},
-	{"i915_blt_ringbuffer_data", i915_ringbuffer_data, 0, (void *)BCS},
-	{"i915_blt_ringbuffer_info", i915_ringbuffer_info, 0, (void *)BCS},
-	{"i915_error_state", i915_error_state, 0},
 	{"i915_rstdby_delays", i915_rstdby_delays, 0},
 	{"i915_cur_delayinfo", i915_cur_delayinfo, 0},
 	{"i915_delayfreq_table", i915_delayfreq_table, 0},
@@ -1839,6 +1972,7 @@ static struct drm_info_list i915_debugfs_list[] = {
 	{"i915_gen6_forcewake_count", i915_gen6_forcewake_count_info, 0},
 	{"i915_swizzle_info", i915_swizzle_info, 0},
 	{"i915_ppgtt_info", i915_ppgtt_info, 0},
+	{"i915_dpio", i915_dpio_info, 0},
 };
 #define I915_DEBUGFS_ENTRIES ARRAY_SIZE(i915_debugfs_list)
 
@@ -1867,6 +2001,17 @@ int i915_debugfs_init(struct drm_minor *minor)
 				  &i915_cache_sharing_fops);
 	if (ret)
 		return ret;
+	ret = i915_debugfs_create(minor->debugfs_root, minor,
+				  "i915_ring_stop",
+				  &i915_ring_stop_fops);
+	if (ret)
+		return ret;
+
+	ret = i915_debugfs_create(minor->debugfs_root, minor,
+				  "i915_error_state",
+				  &i915_error_state_fops);
+	if (ret)
+		return ret;
 
 	return drm_debugfs_create_files(i915_debugfs_list,
 					I915_DEBUGFS_ENTRIES,
@@ -1885,6 +2030,8 @@ void i915_debugfs_cleanup(struct drm_minor *minor)
 				 1, minor);
 	drm_debugfs_remove_files((struct drm_info_list *) &i915_cache_sharing_fops,
 				 1, minor);
+	drm_debugfs_remove_files((struct drm_info_list *) &i915_ring_stop_fops,
+				 1, minor);
 }
 
 #endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index ba60f3c8f911..f94792626b94 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -26,6 +26,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "drmP.h"
 #include "drm.h"
 #include "drm_crtc_helper.h"
@@ -34,15 +36,62 @@
 #include "i915_drm.h"
 #include "i915_drv.h"
 #include "i915_trace.h"
-#include "../../../platform/x86/intel_ips.h"
 #include <linux/pci.h>
 #include <linux/vgaarb.h>
 #include <linux/acpi.h>
 #include <linux/pnp.h>
 #include <linux/vga_switcheroo.h>
 #include <linux/slab.h>
-#include <linux/module.h>
 #include <acpi/video.h>
+#include <asm/pat.h>
+
+#define LP_RING(d) (&((struct drm_i915_private *)(d))->ring[RCS])
+
+#define BEGIN_LP_RING(n) \
+	intel_ring_begin(LP_RING(dev_priv), (n))
+
+#define OUT_RING(x) \
+	intel_ring_emit(LP_RING(dev_priv), x)
+
+#define ADVANCE_LP_RING() \
+	intel_ring_advance(LP_RING(dev_priv))
+
+/**
+ * Lock test for when it's just for synchronization of ring access.
+ *
+ * In that case, we don't need to do it when GEM is initialized as nobody else
+ * has access to the ring.
+ */
+#define RING_LOCK_TEST_WITH_RETURN(dev, file) do {			\
+	if (LP_RING(dev->dev_private)->obj == NULL)			\
+		LOCK_TEST_WITH_RETURN(dev, file);			\
+} while (0)
+
+static inline u32
+intel_read_legacy_status_page(struct drm_i915_private *dev_priv, int reg)
+{
+	if (I915_NEED_GFX_HWS(dev_priv->dev))
+		return ioread32(dev_priv->dri1.gfx_hws_cpu_addr + reg);
+	else
+		return intel_read_status_page(LP_RING(dev_priv), reg);
+}
+
+#define READ_HWSP(dev_priv, reg) intel_read_legacy_status_page(dev_priv, reg)
+#define READ_BREADCRUMB(dev_priv) READ_HWSP(dev_priv, I915_BREADCRUMB_INDEX)
+#define I915_BREADCRUMB_INDEX		0x21
+
+void i915_update_dri1_breadcrumb(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct drm_i915_master_private *master_priv;
+
+	if (dev->primary->master) {
+		master_priv = dev->primary->master->driver_priv;
+		if (master_priv->sarea_priv)
+			master_priv->sarea_priv->last_dispatch =
+				READ_BREADCRUMB(dev_priv);
+	}
+}
 
 static void i915_write_hws_pga(struct drm_device *dev)
 {
@@ -97,7 +146,7 @@ static void i915_free_hws(struct drm_device *dev)
 
 	if (ring->status_page.gfx_addr) {
 		ring->status_page.gfx_addr = 0;
-		drm_core_ioremapfree(&dev_priv->hws_map, dev);
+		iounmap(dev_priv->dri1.gfx_hws_cpu_addr);
 	}
 
 	/* Need to rewrite hardware status page */
@@ -195,7 +244,7 @@ static int i915_initialize(struct drm_device * dev, drm_i915_init_t * init)
 
 	/* Allow hardware batchbuffers unless told otherwise.
 	 */
-	dev_priv->allow_batchbuffer = 1;
+	dev_priv->dri1.allow_batchbuffer = 1;
 
 	return 0;
 }
@@ -207,7 +256,7 @@ static int i915_dma_resume(struct drm_device * dev)
 
 	DRM_DEBUG_DRIVER("%s\n", __func__);
 
-	if (ring->map.handle == NULL) {
+	if (ring->virtual_start == NULL) {
 		DRM_ERROR("can not ioremap virtual address for"
 			  " ring buffer\n");
 		return -ENOMEM;
@@ -236,6 +285,9 @@ static int i915_dma_init(struct drm_device *dev, void *data,
 	drm_i915_init_t *init = data;
 	int retcode = 0;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	switch (init->func) {
 	case I915_INIT_DMA:
 		retcode = i915_initialize(dev, init);
@@ -578,6 +630,9 @@ static int i915_flush_ioctl(struct drm_device *dev, void *data,
 {
 	int ret;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
 
 	mutex_lock(&dev->struct_mutex);
@@ -598,7 +653,10 @@ static int i915_batchbuffer(struct drm_device *dev, void *data,
 	int ret;
 	struct drm_clip_rect *cliprects = NULL;
 
-	if (!dev_priv->allow_batchbuffer) {
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv->dri1.allow_batchbuffer) {
 		DRM_ERROR("Batchbuffer ioctl disabled\n");
 		return -EINVAL;
 	}
@@ -655,6 +713,9 @@ static int i915_cmdbuffer(struct drm_device *dev, void *data,
 	DRM_DEBUG_DRIVER("i915 cmdbuffer, buf %p sz %d cliprects %d\n",
 			cmdbuf->buf, cmdbuf->sz, cmdbuf->num_cliprects);
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
 
 	if (cmdbuf->num_cliprects < 0)
@@ -706,11 +767,166 @@ fail_batch_free:
 	return ret;
 }
 
+static int i915_emit_irq(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
+
+	i915_kernel_lost_context(dev);
+
+	DRM_DEBUG_DRIVER("\n");
+
+	dev_priv->counter++;
+	if (dev_priv->counter > 0x7FFFFFFFUL)
+		dev_priv->counter = 1;
+	if (master_priv->sarea_priv)
+		master_priv->sarea_priv->last_enqueue = dev_priv->counter;
+
+	if (BEGIN_LP_RING(4) == 0) {
+		OUT_RING(MI_STORE_DWORD_INDEX);
+		OUT_RING(I915_BREADCRUMB_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
+		OUT_RING(dev_priv->counter);
+		OUT_RING(MI_USER_INTERRUPT);
+		ADVANCE_LP_RING();
+	}
+
+	return dev_priv->counter;
+}
+
+static int i915_wait_irq(struct drm_device * dev, int irq_nr)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
+	int ret = 0;
+	struct intel_ring_buffer *ring = LP_RING(dev_priv);
+
+	DRM_DEBUG_DRIVER("irq_nr=%d breadcrumb=%d\n", irq_nr,
+		  READ_BREADCRUMB(dev_priv));
+
+	if (READ_BREADCRUMB(dev_priv) >= irq_nr) {
+		if (master_priv->sarea_priv)
+			master_priv->sarea_priv->last_dispatch = READ_BREADCRUMB(dev_priv);
+		return 0;
+	}
+
+	if (master_priv->sarea_priv)
+		master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
+
+	if (ring->irq_get(ring)) {
+		DRM_WAIT_ON(ret, ring->irq_queue, 3 * DRM_HZ,
+			    READ_BREADCRUMB(dev_priv) >= irq_nr);
+		ring->irq_put(ring);
+	} else if (wait_for(READ_BREADCRUMB(dev_priv) >= irq_nr, 3000))
+		ret = -EBUSY;
+
+	if (ret == -EBUSY) {
+		DRM_ERROR("EBUSY -- rec: %d emitted: %d\n",
+			  READ_BREADCRUMB(dev_priv), (int)dev_priv->counter);
+	}
+
+	return ret;
+}
+
+/* Needs the lock as it touches the ring.
+ */
+static int i915_irq_emit(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	drm_i915_irq_emit_t *emit = data;
+	int result;
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv || !LP_RING(dev_priv)->virtual_start) {
+		DRM_ERROR("called with no initialization\n");
+		return -EINVAL;
+	}
+
+	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
+
+	mutex_lock(&dev->struct_mutex);
+	result = i915_emit_irq(dev);
+	mutex_unlock(&dev->struct_mutex);
+
+	if (DRM_COPY_TO_USER(emit->irq_seq, &result, sizeof(int))) {
+		DRM_ERROR("copy_to_user\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+/* Doesn't need the hardware lock.
+ */
+static int i915_irq_wait(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	drm_i915_irq_wait_t *irqwait = data;
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv) {
+		DRM_ERROR("called with no initialization\n");
+		return -EINVAL;
+	}
+
+	return i915_wait_irq(dev, irqwait->irq_seq);
+}
+
+static int i915_vblank_pipe_get(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	drm_i915_vblank_pipe_t *pipe = data;
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv) {
+		DRM_ERROR("called with no initialization\n");
+		return -EINVAL;
+	}
+
+	pipe->pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
+
+	return 0;
+}
+
+/**
+ * Schedule buffer swap at given vertical blank.
+ */
+static int i915_vblank_swap(struct drm_device *dev, void *data,
+		     struct drm_file *file_priv)
+{
+	/* The delayed swap mechanism was fundamentally racy, and has been
+	 * removed.  The model was that the client requested a delayed flip/swap
+	 * from the kernel, then waited for vblank before continuing to perform
+	 * rendering.  The problem was that the kernel might wake the client
+	 * up before it dispatched the vblank swap (since the lock has to be
+	 * held while touching the ringbuffer), in which case the client would
+	 * clear and start the next frame before the swap occurred, and
+	 * flicker would occur in addition to likely missing the vblank.
+	 *
+	 * In the absence of this ioctl, userland falls back to a correct path
+	 * of waiting for a vblank, then dispatching the swap on its own.
+	 * Context switching to userland and back is plenty fast enough for
+	 * meeting the requirements of vblank swapping.
+	 */
+	return -EINVAL;
+}
+
 static int i915_flip_bufs(struct drm_device *dev, void *data,
 			  struct drm_file *file_priv)
 {
 	int ret;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	DRM_DEBUG_DRIVER("%s\n", __func__);
 
 	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
@@ -739,7 +955,7 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		value = dev->pdev->irq ? 1 : 0;
 		break;
 	case I915_PARAM_ALLOW_BATCHBUFFER:
-		value = dev_priv->allow_batchbuffer ? 1 : 0;
+		value = dev_priv->dri1.allow_batchbuffer ? 1 : 0;
 		break;
 	case I915_PARAM_LAST_DISPATCH:
 		value = READ_BREADCRUMB(dev_priv);
@@ -748,7 +964,7 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		value = dev->pci_device;
 		break;
 	case I915_PARAM_HAS_GEM:
-		value = dev_priv->has_gem;
+		value = 1;
 		break;
 	case I915_PARAM_NUM_FENCES_AVAIL:
 		value = dev_priv->num_fence_regs - dev_priv->fence_reg_start;
@@ -761,13 +977,13 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		break;
 	case I915_PARAM_HAS_EXECBUF2:
 		/* depends on GEM */
-		value = dev_priv->has_gem;
+		value = 1;
 		break;
 	case I915_PARAM_HAS_BSD:
-		value = HAS_BSD(dev);
+		value = intel_ring_initialized(&dev_priv->ring[VCS]);
 		break;
 	case I915_PARAM_HAS_BLT:
-		value = HAS_BLT(dev);
+		value = intel_ring_initialized(&dev_priv->ring[BCS]);
 		break;
 	case I915_PARAM_HAS_RELAXED_FENCING:
 		value = 1;
@@ -787,6 +1003,9 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	case I915_PARAM_HAS_LLC:
 		value = HAS_LLC(dev);
 		break;
+	case I915_PARAM_HAS_ALIASING_PPGTT:
+		value = dev_priv->mm.aliasing_ppgtt ? 1 : 0;
+		break;
 	default:
 		DRM_DEBUG_DRIVER("Unknown parameter %d\n",
 				 param->param);
@@ -816,10 +1035,9 @@ static int i915_setparam(struct drm_device *dev, void *data,
 	case I915_SETPARAM_USE_MI_BATCHBUFFER_START:
 		break;
 	case I915_SETPARAM_TEX_LRU_LOG_GRANULARITY:
-		dev_priv->tex_lru_log_granularity = param->value;
 		break;
 	case I915_SETPARAM_ALLOW_BATCHBUFFER:
-		dev_priv->allow_batchbuffer = param->value;
+		dev_priv->dri1.allow_batchbuffer = param->value ? 1 : 0;
 		break;
 	case I915_SETPARAM_NUM_USED_FENCES:
 		if (param->value > dev_priv->num_fence_regs ||
@@ -844,6 +1062,9 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
 	drm_i915_hws_addr_t *hws = data;
 	struct intel_ring_buffer *ring = LP_RING(dev_priv);
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	if (!I915_NEED_GFX_HWS(dev))
 		return -EINVAL;
 
@@ -861,23 +1082,17 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
 
 	ring->status_page.gfx_addr = hws->addr & (0x1ffff<<12);
 
-	dev_priv->hws_map.offset = dev->agp->base + hws->addr;
-	dev_priv->hws_map.size = 4*1024;
-	dev_priv->hws_map.type = 0;
-	dev_priv->hws_map.flags = 0;
-	dev_priv->hws_map.mtrr = 0;
-
-	drm_core_ioremap_wc(&dev_priv->hws_map, dev);
-	if (dev_priv->hws_map.handle == NULL) {
+	dev_priv->dri1.gfx_hws_cpu_addr = ioremap_wc(dev->agp->base + hws->addr,
+						     4096);
+	if (dev_priv->dri1.gfx_hws_cpu_addr == NULL) {
 		i915_dma_cleanup(dev);
 		ring->status_page.gfx_addr = 0;
 		DRM_ERROR("can not ioremap virtual address for"
 				" G33 hw status page\n");
 		return -ENOMEM;
 	}
-	ring->status_page.page_addr =
-		(void __force __iomem *)dev_priv->hws_map.handle;
-	memset_io(ring->status_page.page_addr, 0, PAGE_SIZE);
+
+	memset_io(dev_priv->dri1.gfx_hws_cpu_addr, 0, PAGE_SIZE);
 	I915_WRITE(HWS_PGA, ring->status_page.gfx_addr);
 
 	DRM_DEBUG_DRIVER("load hws HWS_PGA with gfx mem 0x%x\n",
@@ -1013,133 +1228,6 @@ intel_teardown_mchbar(struct drm_device *dev)
 		release_resource(&dev_priv->mch_res);
 }
 
-#define PTE_ADDRESS_MASK		0xfffff000
-#define PTE_ADDRESS_MASK_HIGH		0x000000f0 /* i915+ */
-#define PTE_MAPPING_TYPE_UNCACHED	(0 << 1)
-#define PTE_MAPPING_TYPE_DCACHE		(1 << 1) /* i830 only */
-#define PTE_MAPPING_TYPE_CACHED		(3 << 1)
-#define PTE_MAPPING_TYPE_MASK		(3 << 1)
-#define PTE_VALID			(1 << 0)
-
-/**
- * i915_stolen_to_phys - take an offset into stolen memory and turn it into
- *                       a physical one
- * @dev: drm device
- * @offset: address to translate
- *
- * Some chip functions require allocations from stolen space and need the
- * physical address of the memory in question.
- */
-static unsigned long i915_stolen_to_phys(struct drm_device *dev, u32 offset)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct pci_dev *pdev = dev_priv->bridge_dev;
-	u32 base;
-
-#if 0
-	/* On the machines I have tested the Graphics Base of Stolen Memory
-	 * is unreliable, so compute the base by subtracting the stolen memory
-	 * from the Top of Low Usable DRAM which is where the BIOS places
-	 * the graphics stolen memory.
-	 */
-	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
-		/* top 32bits are reserved = 0 */
-		pci_read_config_dword(pdev, 0xA4, &base);
-	} else {
-		/* XXX presume 8xx is the same as i915 */
-		pci_bus_read_config_dword(pdev->bus, 2, 0x5C, &base);
-	}
-#else
-	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
-		u16 val;
-		pci_read_config_word(pdev, 0xb0, &val);
-		base = val >> 4 << 20;
-	} else {
-		u8 val;
-		pci_read_config_byte(pdev, 0x9c, &val);
-		base = val >> 3 << 27;
-	}
-	base -= dev_priv->mm.gtt->stolen_size;
-#endif
-
-	return base + offset;
-}
-
-static void i915_warn_stolen(struct drm_device *dev)
-{
-	DRM_ERROR("not enough stolen space for compressed buffer, disabling\n");
-	DRM_ERROR("hint: you may be able to increase stolen memory size in the BIOS to avoid this\n");
-}
-
-static void i915_setup_compression(struct drm_device *dev, int size)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_mm_node *compressed_fb, *uninitialized_var(compressed_llb);
-	unsigned long cfb_base;
-	unsigned long ll_base = 0;
-
-	/* Just in case the BIOS is doing something questionable. */
-	intel_disable_fbc(dev);
-
-	compressed_fb = drm_mm_search_free(&dev_priv->mm.stolen, size, 4096, 0);
-	if (compressed_fb)
-		compressed_fb = drm_mm_get_block(compressed_fb, size, 4096);
-	if (!compressed_fb)
-		goto err;
-
-	cfb_base = i915_stolen_to_phys(dev, compressed_fb->start);
-	if (!cfb_base)
-		goto err_fb;
-
-	if (!(IS_GM45(dev) || HAS_PCH_SPLIT(dev))) {
-		compressed_llb = drm_mm_search_free(&dev_priv->mm.stolen,
-						    4096, 4096, 0);
-		if (compressed_llb)
-			compressed_llb = drm_mm_get_block(compressed_llb,
-							  4096, 4096);
-		if (!compressed_llb)
-			goto err_fb;
-
-		ll_base = i915_stolen_to_phys(dev, compressed_llb->start);
-		if (!ll_base)
-			goto err_llb;
-	}
-
-	dev_priv->cfb_size = size;
-
-	dev_priv->compressed_fb = compressed_fb;
-	if (HAS_PCH_SPLIT(dev))
-		I915_WRITE(ILK_DPFC_CB_BASE, compressed_fb->start);
-	else if (IS_GM45(dev)) {
-		I915_WRITE(DPFC_CB_BASE, compressed_fb->start);
-	} else {
-		I915_WRITE(FBC_CFB_BASE, cfb_base);
-		I915_WRITE(FBC_LL_BASE, ll_base);
-		dev_priv->compressed_llb = compressed_llb;
-	}
-
-	DRM_DEBUG_KMS("FBC base 0x%08lx, ll base 0x%08lx, size %dM\n",
-		      cfb_base, ll_base, size >> 20);
-	return;
-
-err_llb:
-	drm_mm_put_block(compressed_llb);
-err_fb:
-	drm_mm_put_block(compressed_fb);
-err:
-	dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
-	i915_warn_stolen(dev);
-}
-
-static void i915_cleanup_compression(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	drm_mm_put_block(dev_priv->compressed_fb);
-	if (dev_priv->compressed_llb)
-		drm_mm_put_block(dev_priv->compressed_llb);
-}
-
 /* true = enable decode, false = disable decoder */
 static unsigned int i915_vga_set_decode(void *cookie, bool state)
 {
@@ -1158,14 +1246,14 @@ static void i915_switcheroo_set_state(struct pci_dev *pdev, enum vga_switcheroo_
 	struct drm_device *dev = pci_get_drvdata(pdev);
 	pm_message_t pmm = { .event = PM_EVENT_SUSPEND };
 	if (state == VGA_SWITCHEROO_ON) {
-		printk(KERN_INFO "i915: switched on\n");
+		pr_info("switched on\n");
 		dev->switch_power_state = DRM_SWITCH_POWER_CHANGING;
 		/* i915 resume handler doesn't set to D0 */
 		pci_set_power_state(dev->pdev, PCI_D0);
 		i915_resume(dev);
 		dev->switch_power_state = DRM_SWITCH_POWER_ON;
 	} else {
-		printk(KERN_ERR "i915: switched off\n");
+		pr_err("switched off\n");
 		dev->switch_power_state = DRM_SWITCH_POWER_CHANGING;
 		i915_suspend(dev, pmm);
 		dev->switch_power_state = DRM_SWITCH_POWER_OFF;
@@ -1183,88 +1271,11 @@ static bool i915_switcheroo_can_switch(struct pci_dev *pdev)
 	return can_switch;
 }
 
-static bool
-intel_enable_ppgtt(struct drm_device *dev)
-{
-	if (i915_enable_ppgtt >= 0)
-		return i915_enable_ppgtt;
-
-#ifdef CONFIG_INTEL_IOMMU
-	/* Disable ppgtt on SNB if VT-d is on. */
-	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped)
-		return false;
-#endif
-
-	return true;
-}
-
-static int i915_load_gem_init(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long prealloc_size, gtt_size, mappable_size;
-	int ret;
-
-	prealloc_size = dev_priv->mm.gtt->stolen_size;
-	gtt_size = dev_priv->mm.gtt->gtt_total_entries << PAGE_SHIFT;
-	mappable_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
-
-	/* Basic memrange allocator for stolen space */
-	drm_mm_init(&dev_priv->mm.stolen, 0, prealloc_size);
-
-	mutex_lock(&dev->struct_mutex);
-	if (intel_enable_ppgtt(dev) && HAS_ALIASING_PPGTT(dev)) {
-		/* PPGTT pdes are stolen from global gtt ptes, so shrink the
-		 * aperture accordingly when using aliasing ppgtt. */
-		gtt_size -= I915_PPGTT_PD_ENTRIES*PAGE_SIZE;
-		/* For paranoia keep the guard page in between. */
-		gtt_size -= PAGE_SIZE;
-
-		i915_gem_do_init(dev, 0, mappable_size, gtt_size);
-
-		ret = i915_gem_init_aliasing_ppgtt(dev);
-		if (ret) {
-			mutex_unlock(&dev->struct_mutex);
-			return ret;
-		}
-	} else {
-		/* Let GEM Manage all of the aperture.
-		 *
-		 * However, leave one page at the end still bound to the scratch
-		 * page.  There are a number of places where the hardware
-		 * apparently prefetches past the end of the object, and we've
-		 * seen multiple hangs with the GPU head pointer stuck in a
-		 * batchbuffer bound at the last page of the aperture.  One page
-		 * should be enough to keep any prefetching inside of the
-		 * aperture.
-		 */
-		i915_gem_do_init(dev, 0, mappable_size, gtt_size - PAGE_SIZE);
-	}
-
-	ret = i915_gem_init_hw(dev);
-	mutex_unlock(&dev->struct_mutex);
-	if (ret) {
-		i915_gem_cleanup_aliasing_ppgtt(dev);
-		return ret;
-	}
-
-	/* Try to set up FBC with a reasonable compressed buffer size */
-	if (I915_HAS_FBC(dev) && i915_powersave) {
-		int cfb_size;
-
-		/* Leave 1M for line length buffer & misc. */
-
-		/* Try to get a 32M buffer... */
-		if (prealloc_size > (36*1024*1024))
-			cfb_size = 32*1024*1024;
-		else /* fall back to 7/8 of the stolen space */
-			cfb_size = prealloc_size * 7 / 8;
-		i915_setup_compression(dev, cfb_size);
-	}
-
-	/* Allow hardware batchbuffers unless told otherwise. */
-	dev_priv->allow_batchbuffer = 1;
-	return 0;
-}
+static const struct vga_switcheroo_client_ops i915_switcheroo_ops = {
+	.set_gpu_state = i915_switcheroo_set_state,
+	.reprobe = NULL,
+	.can_switch = i915_switcheroo_can_switch,
+};
 
 static int i915_load_modeset_init(struct drm_device *dev)
 {
@@ -1288,22 +1299,22 @@ static int i915_load_modeset_init(struct drm_device *dev)
 
 	intel_register_dsm_handler();
 
-	ret = vga_switcheroo_register_client(dev->pdev,
-					     i915_switcheroo_set_state,
-					     NULL,
-					     i915_switcheroo_can_switch);
+	ret = vga_switcheroo_register_client(dev->pdev, &i915_switcheroo_ops);
 	if (ret)
 		goto cleanup_vga_client;
 
-	/* IIR "flip pending" bit means done if this bit is set */
-	if (IS_GEN3(dev) && (I915_READ(ECOSKPD) & ECO_FLIP_DONE))
-		dev_priv->flip_pending_is_done = true;
+	/* Initialise stolen first so that we may reserve preallocated
+	 * objects for the BIOS to KMS transition.
+	 */
+	ret = i915_gem_init_stolen(dev);
+	if (ret)
+		goto cleanup_vga_switcheroo;
 
 	intel_modeset_init(dev);
 
-	ret = i915_load_gem_init(dev);
+	ret = i915_gem_init(dev);
 	if (ret)
-		goto cleanup_vga_switcheroo;
+		goto cleanup_gem_stolen;
 
 	intel_modeset_gem_init(dev);
 
@@ -1333,6 +1344,8 @@ cleanup_gem:
 	i915_gem_cleanup_ringbuffer(dev);
 	mutex_unlock(&dev->struct_mutex);
 	i915_gem_cleanup_aliasing_ppgtt(dev);
+cleanup_gem_stolen:
+	i915_gem_cleanup_stolen(dev);
 cleanup_vga_switcheroo:
 	vga_switcheroo_unregister_client(dev->pdev);
 cleanup_vga_client:
@@ -1365,572 +1378,26 @@ void i915_master_destroy(struct drm_device *dev, struct drm_master *master)
 	master->driver_priv = NULL;
 }
 
-static void i915_pineview_get_mem_freq(struct drm_device *dev)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 tmp;
-
-	tmp = I915_READ(CLKCFG);
-
-	switch (tmp & CLKCFG_FSB_MASK) {
-	case CLKCFG_FSB_533:
-		dev_priv->fsb_freq = 533; /* 133*4 */
-		break;
-	case CLKCFG_FSB_800:
-		dev_priv->fsb_freq = 800; /* 200*4 */
-		break;
-	case CLKCFG_FSB_667:
-		dev_priv->fsb_freq =  667; /* 167*4 */
-		break;
-	case CLKCFG_FSB_400:
-		dev_priv->fsb_freq = 400; /* 100*4 */
-		break;
-	}
-
-	switch (tmp & CLKCFG_MEM_MASK) {
-	case CLKCFG_MEM_533:
-		dev_priv->mem_freq = 533;
-		break;
-	case CLKCFG_MEM_667:
-		dev_priv->mem_freq = 667;
-		break;
-	case CLKCFG_MEM_800:
-		dev_priv->mem_freq = 800;
-		break;
-	}
-
-	/* detect pineview DDR3 setting */
-	tmp = I915_READ(CSHRDDR3CTL);
-	dev_priv->is_ddr3 = (tmp & CSHRDDR3CTL_DDR3) ? 1 : 0;
-}
-
-static void i915_ironlake_get_mem_freq(struct drm_device *dev)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	u16 ddrpll, csipll;
-
-	ddrpll = I915_READ16(DDRMPLL1);
-	csipll = I915_READ16(CSIPLL0);
-
-	switch (ddrpll & 0xff) {
-	case 0xc:
-		dev_priv->mem_freq = 800;
-		break;
-	case 0x10:
-		dev_priv->mem_freq = 1066;
-		break;
-	case 0x14:
-		dev_priv->mem_freq = 1333;
-		break;
-	case 0x18:
-		dev_priv->mem_freq = 1600;
-		break;
-	default:
-		DRM_DEBUG_DRIVER("unknown memory frequency 0x%02x\n",
-				 ddrpll & 0xff);
-		dev_priv->mem_freq = 0;
-		break;
-	}
-
-	dev_priv->r_t = dev_priv->mem_freq;
-
-	switch (csipll & 0x3ff) {
-	case 0x00c:
-		dev_priv->fsb_freq = 3200;
-		break;
-	case 0x00e:
-		dev_priv->fsb_freq = 3733;
-		break;
-	case 0x010:
-		dev_priv->fsb_freq = 4266;
-		break;
-	case 0x012:
-		dev_priv->fsb_freq = 4800;
-		break;
-	case 0x014:
-		dev_priv->fsb_freq = 5333;
-		break;
-	case 0x016:
-		dev_priv->fsb_freq = 5866;
-		break;
-	case 0x018:
-		dev_priv->fsb_freq = 6400;
-		break;
-	default:
-		DRM_DEBUG_DRIVER("unknown fsb frequency 0x%04x\n",
-				 csipll & 0x3ff);
-		dev_priv->fsb_freq = 0;
-		break;
-	}
-
-	if (dev_priv->fsb_freq == 3200) {
-		dev_priv->c_m = 0;
-	} else if (dev_priv->fsb_freq > 3200 && dev_priv->fsb_freq <= 4800) {
-		dev_priv->c_m = 1;
-	} else {
-		dev_priv->c_m = 2;
-	}
-}
-
-static const struct cparams {
-	u16 i;
-	u16 t;
-	u16 m;
-	u16 c;
-} cparams[] = {
-	{ 1, 1333, 301, 28664 },
-	{ 1, 1066, 294, 24460 },
-	{ 1, 800, 294, 25192 },
-	{ 0, 1333, 276, 27605 },
-	{ 0, 1066, 276, 27605 },
-	{ 0, 800, 231, 23784 },
-};
-
-unsigned long i915_chipset_val(struct drm_i915_private *dev_priv)
-{
-	u64 total_count, diff, ret;
-	u32 count1, count2, count3, m = 0, c = 0;
-	unsigned long now = jiffies_to_msecs(jiffies), diff1;
-	int i;
-
-	diff1 = now - dev_priv->last_time1;
-
-	/* Prevent division-by-zero if we are asking too fast.
-	 * Also, we don't get interesting results if we are polling
-	 * faster than once in 10ms, so just return the saved value
-	 * in such cases.
-	 */
-	if (diff1 <= 10)
-		return dev_priv->chipset_power;
-
-	count1 = I915_READ(DMIEC);
-	count2 = I915_READ(DDREC);
-	count3 = I915_READ(CSIEC);
-
-	total_count = count1 + count2 + count3;
-
-	/* FIXME: handle per-counter overflow */
-	if (total_count < dev_priv->last_count1) {
-		diff = ~0UL - dev_priv->last_count1;
-		diff += total_count;
-	} else {
-		diff = total_count - dev_priv->last_count1;
-	}
-
-	for (i = 0; i < ARRAY_SIZE(cparams); i++) {
-		if (cparams[i].i == dev_priv->c_m &&
-		    cparams[i].t == dev_priv->r_t) {
-			m = cparams[i].m;
-			c = cparams[i].c;
-			break;
-		}
-	}
-
-	diff = div_u64(diff, diff1);
-	ret = ((m * diff) + c);
-	ret = div_u64(ret, 10);
-
-	dev_priv->last_count1 = total_count;
-	dev_priv->last_time1 = now;
-
-	dev_priv->chipset_power = ret;
-
-	return ret;
-}
-
-unsigned long i915_mch_val(struct drm_i915_private *dev_priv)
-{
-	unsigned long m, x, b;
-	u32 tsfs;
-
-	tsfs = I915_READ(TSFS);
-
-	m = ((tsfs & TSFS_SLOPE_MASK) >> TSFS_SLOPE_SHIFT);
-	x = I915_READ8(TR1);
-
-	b = tsfs & TSFS_INTR_MASK;
-
-	return ((m * x) / 127) - b;
-}
-
-static u16 pvid_to_extvid(struct drm_i915_private *dev_priv, u8 pxvid)
-{
-	static const struct v_table {
-		u16 vd; /* in .1 mil */
-		u16 vm; /* in .1 mil */
-	} v_table[] = {
-		{ 0, 0, },
-		{ 375, 0, },
-		{ 500, 0, },
-		{ 625, 0, },
-		{ 750, 0, },
-		{ 875, 0, },
-		{ 1000, 0, },
-		{ 1125, 0, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4250, 3125, },
-		{ 4375, 3250, },
-		{ 4500, 3375, },
-		{ 4625, 3500, },
-		{ 4750, 3625, },
-		{ 4875, 3750, },
-		{ 5000, 3875, },
-		{ 5125, 4000, },
-		{ 5250, 4125, },
-		{ 5375, 4250, },
-		{ 5500, 4375, },
-		{ 5625, 4500, },
-		{ 5750, 4625, },
-		{ 5875, 4750, },
-		{ 6000, 4875, },
-		{ 6125, 5000, },
-		{ 6250, 5125, },
-		{ 6375, 5250, },
-		{ 6500, 5375, },
-		{ 6625, 5500, },
-		{ 6750, 5625, },
-		{ 6875, 5750, },
-		{ 7000, 5875, },
-		{ 7125, 6000, },
-		{ 7250, 6125, },
-		{ 7375, 6250, },
-		{ 7500, 6375, },
-		{ 7625, 6500, },
-		{ 7750, 6625, },
-		{ 7875, 6750, },
-		{ 8000, 6875, },
-		{ 8125, 7000, },
-		{ 8250, 7125, },
-		{ 8375, 7250, },
-		{ 8500, 7375, },
-		{ 8625, 7500, },
-		{ 8750, 7625, },
-		{ 8875, 7750, },
-		{ 9000, 7875, },
-		{ 9125, 8000, },
-		{ 9250, 8125, },
-		{ 9375, 8250, },
-		{ 9500, 8375, },
-		{ 9625, 8500, },
-		{ 9750, 8625, },
-		{ 9875, 8750, },
-		{ 10000, 8875, },
-		{ 10125, 9000, },
-		{ 10250, 9125, },
-		{ 10375, 9250, },
-		{ 10500, 9375, },
-		{ 10625, 9500, },
-		{ 10750, 9625, },
-		{ 10875, 9750, },
-		{ 11000, 9875, },
-		{ 11125, 10000, },
-		{ 11250, 10125, },
-		{ 11375, 10250, },
-		{ 11500, 10375, },
-		{ 11625, 10500, },
-		{ 11750, 10625, },
-		{ 11875, 10750, },
-		{ 12000, 10875, },
-		{ 12125, 11000, },
-		{ 12250, 11125, },
-		{ 12375, 11250, },
-		{ 12500, 11375, },
-		{ 12625, 11500, },
-		{ 12750, 11625, },
-		{ 12875, 11750, },
-		{ 13000, 11875, },
-		{ 13125, 12000, },
-		{ 13250, 12125, },
-		{ 13375, 12250, },
-		{ 13500, 12375, },
-		{ 13625, 12500, },
-		{ 13750, 12625, },
-		{ 13875, 12750, },
-		{ 14000, 12875, },
-		{ 14125, 13000, },
-		{ 14250, 13125, },
-		{ 14375, 13250, },
-		{ 14500, 13375, },
-		{ 14625, 13500, },
-		{ 14750, 13625, },
-		{ 14875, 13750, },
-		{ 15000, 13875, },
-		{ 15125, 14000, },
-		{ 15250, 14125, },
-		{ 15375, 14250, },
-		{ 15500, 14375, },
-		{ 15625, 14500, },
-		{ 15750, 14625, },
-		{ 15875, 14750, },
-		{ 16000, 14875, },
-		{ 16125, 15000, },
-	};
-	if (dev_priv->info->is_mobile)
-		return v_table[pxvid].vm;
-	else
-		return v_table[pxvid].vd;
-}
-
-void i915_update_gfx_val(struct drm_i915_private *dev_priv)
+static void
+i915_mtrr_setup(struct drm_i915_private *dev_priv, unsigned long base,
+		unsigned long size)
 {
-	struct timespec now, diff1;
-	u64 diff;
-	unsigned long diffms;
-	u32 count;
-
-	if (dev_priv->info->gen != 5)
-		return;
-
-	getrawmonotonic(&now);
-	diff1 = timespec_sub(now, dev_priv->last_time2);
+	dev_priv->mm.gtt_mtrr = -1;
 
-	/* Don't divide by 0 */
-	diffms = diff1.tv_sec * 1000 + diff1.tv_nsec / 1000000;
-	if (!diffms)
+#if defined(CONFIG_X86_PAT)
+	if (cpu_has_pat)
 		return;
+#endif
 
-	count = I915_READ(GFXEC);
-
-	if (count < dev_priv->last_count2) {
-		diff = ~0UL - dev_priv->last_count2;
-		diff += count;
-	} else {
-		diff = count - dev_priv->last_count2;
-	}
-
-	dev_priv->last_count2 = count;
-	dev_priv->last_time2 = now;
-
-	/* More magic constants... */
-	diff = diff * 1181;
-	diff = div_u64(diff, diffms * 10);
-	dev_priv->gfx_power = diff;
-}
-
-unsigned long i915_gfx_val(struct drm_i915_private *dev_priv)
-{
-	unsigned long t, corr, state1, corr2, state2;
-	u32 pxvid, ext_v;
-
-	pxvid = I915_READ(PXVFREQ_BASE + (dev_priv->cur_delay * 4));
-	pxvid = (pxvid >> 24) & 0x7f;
-	ext_v = pvid_to_extvid(dev_priv, pxvid);
-
-	state1 = ext_v;
-
-	t = i915_mch_val(dev_priv);
-
-	/* Revel in the empirically derived constants */
-
-	/* Correction factor in 1/100000 units */
-	if (t > 80)
-		corr = ((t * 2349) + 135940);
-	else if (t >= 50)
-		corr = ((t * 964) + 29317);
-	else /* < 50 */
-		corr = ((t * 301) + 1004);
-
-	corr = corr * ((150142 * state1) / 10000 - 78642);
-	corr /= 100000;
-	corr2 = (corr * dev_priv->corr);
-
-	state2 = (corr2 * state1) / 10000;
-	state2 /= 100; /* convert to mW */
-
-	i915_update_gfx_val(dev_priv);
-
-	return dev_priv->gfx_power + state2;
-}
-
-/* Global for IPS driver to get at the current i915 device */
-static struct drm_i915_private *i915_mch_dev;
-/*
- * Lock protecting IPS related data structures
- *   - i915_mch_dev
- *   - dev_priv->max_delay
- *   - dev_priv->min_delay
- *   - dev_priv->fmax
- *   - dev_priv->gpu_busy
- */
-static DEFINE_SPINLOCK(mchdev_lock);
-
-/**
- * i915_read_mch_val - return value for IPS use
- *
- * Calculate and return a value for the IPS driver to use when deciding whether
- * we have thermal and power headroom to increase CPU or GPU power budget.
- */
-unsigned long i915_read_mch_val(void)
-{
-	struct drm_i915_private *dev_priv;
-	unsigned long chipset_val, graphics_val, ret = 0;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev)
-		goto out_unlock;
-	dev_priv = i915_mch_dev;
-
-	chipset_val = i915_chipset_val(dev_priv);
-	graphics_val = i915_gfx_val(dev_priv);
-
-	ret = chipset_val + graphics_val;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_read_mch_val);
-
-/**
- * i915_gpu_raise - raise GPU frequency limit
- *
- * Raise the limit; IPS indicates we have thermal headroom.
- */
-bool i915_gpu_raise(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = true;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev) {
-		ret = false;
-		goto out_unlock;
-	}
-	dev_priv = i915_mch_dev;
-
-	if (dev_priv->max_delay > dev_priv->fmax)
-		dev_priv->max_delay--;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_raise);
-
-/**
- * i915_gpu_lower - lower GPU frequency limit
- *
- * IPS indicates we're close to a thermal limit, so throttle back the GPU
- * frequency maximum.
- */
-bool i915_gpu_lower(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = true;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev) {
-		ret = false;
-		goto out_unlock;
-	}
-	dev_priv = i915_mch_dev;
-
-	if (dev_priv->max_delay < dev_priv->min_delay)
-		dev_priv->max_delay++;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_lower);
-
-/**
- * i915_gpu_busy - indicate GPU business to IPS
- *
- * Tell the IPS driver whether or not the GPU is busy.
- */
-bool i915_gpu_busy(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = false;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev)
-		goto out_unlock;
-	dev_priv = i915_mch_dev;
-
-	ret = dev_priv->busy;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_busy);
-
-/**
- * i915_gpu_turbo_disable - disable graphics turbo
- *
- * Disable graphics turbo by resetting the max frequency and setting the
- * current frequency to the default.
- */
-bool i915_gpu_turbo_disable(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = true;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev) {
-		ret = false;
-		goto out_unlock;
-	}
-	dev_priv = i915_mch_dev;
-
-	dev_priv->max_delay = dev_priv->fstart;
-
-	if (!ironlake_set_drps(dev_priv->dev, dev_priv->fstart))
-		ret = false;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_turbo_disable);
-
-/**
- * Tells the intel_ips driver that the i915 driver is now loaded, if
- * IPS got loaded first.
- *
- * This awkward dance is so that neither module has to depend on the
- * other in order for IPS to do the appropriate communication of
- * GPU turbo limits to i915.
- */
-static void
-ips_ping_for_i915_load(void)
-{
-	void (*link)(void);
-
-	link = symbol_get(ips_link_to_i915_driver);
-	if (link) {
-		link();
-		symbol_put(ips_link_to_i915_driver);
+	/* Set up a WC MTRR for non-PAT systems.  This is more common than
+	 * one would think, because the kernel disables PAT on first
+	 * generation Core chips because WC PAT gets overridden by a UC
+	 * MTRR if present.  Even if a UC MTRR isn't present.
+	 */
+	dev_priv->mm.gtt_mtrr = mtrr_add(base, size, MTRR_TYPE_WRCOMB, 1);
+	if (dev_priv->mm.gtt_mtrr < 0) {
+		DRM_INFO("MTRR allocation failed.  Graphics "
+			 "performance may suffer.\n");
 	}
 }
 
@@ -1948,8 +1415,16 @@ ips_ping_for_i915_load(void)
 int i915_driver_load(struct drm_device *dev, unsigned long flags)
 {
 	struct drm_i915_private *dev_priv;
+	struct intel_device_info *info;
 	int ret = 0, mmio_bar;
-	uint32_t agp_size;
+	uint32_t aperture_size;
+
+	info = (struct intel_device_info *) flags;
+
+	/* Refuse to load on gen6+ without kms enabled. */
+	if (info->gen >= 6 && !drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 
 	/* i915 has 4 more counters */
 	dev->counters += 4;
@@ -1964,7 +1439,7 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
 	dev->dev_private = (void *)dev_priv;
 	dev_priv->dev = dev;
-	dev_priv->info = (struct intel_device_info *) flags;
+	dev_priv->info = info;
 
 	if (i915_get_bridge_dev(dev)) {
 		ret = -EIO;
@@ -2003,27 +1478,16 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto out_rmmap;
 	}
 
-	agp_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
+	aperture_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
 
 	dev_priv->mm.gtt_mapping =
-		io_mapping_create_wc(dev->agp->base, agp_size);
+		io_mapping_create_wc(dev->agp->base, aperture_size);
 	if (dev_priv->mm.gtt_mapping == NULL) {
 		ret = -EIO;
 		goto out_rmmap;
 	}
 
-	/* Set up a WC MTRR for non-PAT systems.  This is more common than
-	 * one would think, because the kernel disables PAT on first
-	 * generation Core chips because WC PAT gets overridden by a UC
-	 * MTRR if present.  Even if a UC MTRR isn't present.
-	 */
-	dev_priv->mm.gtt_mtrr = mtrr_add(dev->agp->base,
-					 agp_size,
-					 MTRR_TYPE_WRCOMB, 1);
-	if (dev_priv->mm.gtt_mtrr < 0) {
-		DRM_INFO("MTRR allocation failed.  Graphics "
-			 "performance may suffer.\n");
-	}
+	i915_mtrr_setup(dev_priv, dev->agp->base, aperture_size);
 
 	/* The i915 workqueue is primarily used for batched retirement of
 	 * requests (and thus managing bo) once the task has been completed
@@ -2047,9 +1511,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto out_mtrrfree;
 	}
 
-	/* enable GEM by default */
-	dev_priv->has_gem = 1;
-
 	intel_irq_init(dev);
 
 	/* Try to make sure MCHBAR is enabled before poking at it */
@@ -2069,11 +1530,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 			goto out_gem_unload;
 	}
 
-	if (IS_PINEVIEW(dev))
-		i915_pineview_get_mem_freq(dev);
-	else if (IS_GEN5(dev))
-		i915_ironlake_get_mem_freq(dev);
-
 	/* On the 945G/GM, the chipset reports the MSI capability on the
 	 * integrated graphics even though the support isn't actually there
 	 * according to the published specs.  It doesn't appear to function
@@ -2093,7 +1549,7 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	spin_lock_init(&dev_priv->error_lock);
 	spin_lock_init(&dev_priv->rps_lock);
 
-	if (IS_IVYBRIDGE(dev))
+	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev))
 		dev_priv->num_pipe = 3;
 	else if (IS_MOBILE(dev) || !IS_GEN2(dev))
 		dev_priv->num_pipe = 2;
@@ -2117,6 +1573,8 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		}
 	}
 
+	i915_setup_sysfs(dev);
+
 	/* Must be done after probing outputs */
 	intel_opregion_init(dev);
 	acpi_video_register();
@@ -2124,14 +1582,8 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	setup_timer(&dev_priv->hangcheck_timer, i915_hangcheck_elapsed,
 		    (unsigned long) dev);
 
-	if (IS_GEN5(dev)) {
-		spin_lock(&mchdev_lock);
-		i915_mch_dev = dev_priv;
-		dev_priv->mchdev_lock = &mchdev_lock;
-		spin_unlock(&mchdev_lock);
-
-		ips_ping_for_i915_load();
-	}
+	if (IS_GEN5(dev))
+		intel_gpu_ips_init(dev_priv);
 
 	return 0;
 
@@ -2166,17 +1618,18 @@ int i915_driver_unload(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	spin_lock(&mchdev_lock);
-	i915_mch_dev = NULL;
-	spin_unlock(&mchdev_lock);
+	intel_gpu_ips_teardown();
+
+	i915_teardown_sysfs(dev);
 
 	if (dev_priv->mm.inactive_shrinker.shrink)
 		unregister_shrinker(&dev_priv->mm.inactive_shrinker);
 
 	mutex_lock(&dev->struct_mutex);
-	ret = i915_gpu_idle(dev, true);
+	ret = i915_gpu_idle(dev);
 	if (ret)
 		DRM_ERROR("failed to idle hardware: %d\n", ret);
+	i915_gem_retire_requests(dev);
 	mutex_unlock(&dev->struct_mutex);
 
 	/* Cancel the retire work handler, which should be idle now. */
@@ -2228,8 +1681,7 @@ int i915_driver_unload(struct drm_device *dev)
 		i915_gem_cleanup_ringbuffer(dev);
 		mutex_unlock(&dev->struct_mutex);
 		i915_gem_cleanup_aliasing_ppgtt(dev);
-		if (I915_HAS_FBC(dev) && i915_powersave)
-			i915_cleanup_compression(dev);
+		i915_gem_cleanup_stolen(dev);
 		drm_mm_takedown(&dev_priv->mm.stolen);
 
 		intel_cleanup_overlay(dev);
@@ -2277,7 +1729,7 @@ int i915_driver_open(struct drm_device *dev, struct drm_file *file)
  * mode setting case, we want to restore the kernel's initial mode (just
  * in case the last client left us in a bad state).
  *
- * Additionally, in the non-mode setting case, we'll tear down the AGP
+ * Additionally, in the non-mode setting case, we'll tear down the GTT
  * and DMA structures, since the kernel won't be using them, and clea
  * up any GEM state.
  */
@@ -2322,7 +1774,7 @@ struct drm_ioctl_desc i915_ioctls[] = {
 	DRM_IOCTL_DEF_DRV(I915_INIT_HEAP, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
 	DRM_IOCTL_DEF_DRV(I915_CMDBUFFER, i915_cmdbuffer, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_DESTROY_HEAP,  drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(I915_SET_VBLANK_PIPE,  i915_vblank_pipe_set, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF_DRV(I915_SET_VBLANK_PIPE,  drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
 	DRM_IOCTL_DEF_DRV(I915_GET_VBLANK_PIPE,  i915_vblank_pipe_get, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_VBLANK_SWAP, i915_vblank_swap, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_HWS_ADDR, i915_set_status_page, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
@@ -2355,16 +1807,10 @@ struct drm_ioctl_desc i915_ioctls[] = {
 
 int i915_max_ioctl = DRM_ARRAY_SIZE(i915_ioctls);
 
-/**
- * Determine if the device really is AGP or not.
- *
- * All Intel graphics chipsets are treated as AGP, even if they are really
- * PCI-e.
- *
- * \param dev   The device to be tested.
- *
- * \returns
- * A value of 1 is always retured to indictate every i9x5 is AGP.
+/*
+ * This is really ugly: Because old userspace abused the linux agp interface to
+ * manage the gtt, we need to claim that all intel devices are agp.  For
+ * otherwise the drm core refuses to initialize the agp support code.
  */
 int i915_driver_device_is_agp(struct drm_device * dev)
 {
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index ae8a64f9f845..238a52165833 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -84,6 +84,12 @@ MODULE_PARM_DESC(lvds_downclock,
 		"Use panel (LVDS/eDP) downclocking for power savings "
 		"(default: false)");
 
+int i915_lvds_channel_mode __read_mostly;
+module_param_named(lvds_channel_mode, i915_lvds_channel_mode, int, 0600);
+MODULE_PARM_DESC(lvds_channel_mode,
+		 "Specify LVDS channel mode "
+		 "(0=probe BIOS [default], 1=single-channel, 2=dual-channel)");
+
 int i915_panel_use_ssc __read_mostly = -1;
 module_param_named(lvds_use_ssc, i915_panel_use_ssc, int, 0600);
 MODULE_PARM_DESC(lvds_use_ssc,
@@ -93,8 +99,8 @@ MODULE_PARM_DESC(lvds_use_ssc,
 int i915_vbt_sdvo_panel_type __read_mostly = -1;
 module_param_named(vbt_sdvo_panel_type, i915_vbt_sdvo_panel_type, int, 0600);
 MODULE_PARM_DESC(vbt_sdvo_panel_type,
-		"Override selection of SDVO panel mode in the VBT "
-		"(default: auto)");
+		"Override/Ignore selection of SDVO panel mode in the VBT "
+		"(-2=ignore, -1=auto [default], index in VBT BIOS table)");
 
 static bool i915_try_reset __read_mostly = true;
 module_param_named(reset, i915_try_reset, bool, 0600);
@@ -209,6 +215,7 @@ static const struct intel_device_info intel_ironlake_d_info = {
 	.gen = 5,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_bsd_ring = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_ironlake_m_info = {
@@ -216,6 +223,7 @@ static const struct intel_device_info intel_ironlake_m_info = {
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_fbc = 1,
 	.has_bsd_ring = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_sandybridge_d_info = {
@@ -224,6 +232,7 @@ static const struct intel_device_info intel_sandybridge_d_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_sandybridge_m_info = {
@@ -233,6 +242,7 @@ static const struct intel_device_info intel_sandybridge_m_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_ivybridge_d_info = {
@@ -241,6 +251,7 @@ static const struct intel_device_info intel_ivybridge_d_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_ivybridge_m_info = {
@@ -250,6 +261,43 @@ static const struct intel_device_info intel_ivybridge_m_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
+};
+
+static const struct intel_device_info intel_valleyview_m_info = {
+	.gen = 7, .is_mobile = 1,
+	.need_gfx_hws = 1, .has_hotplug = 1,
+	.has_fbc = 0,
+	.has_bsd_ring = 1,
+	.has_blt_ring = 1,
+	.is_valleyview = 1,
+};
+
+static const struct intel_device_info intel_valleyview_d_info = {
+	.gen = 7,
+	.need_gfx_hws = 1, .has_hotplug = 1,
+	.has_fbc = 0,
+	.has_bsd_ring = 1,
+	.has_blt_ring = 1,
+	.is_valleyview = 1,
+};
+
+static const struct intel_device_info intel_haswell_d_info = {
+	.is_haswell = 1, .gen = 7,
+	.need_gfx_hws = 1, .has_hotplug = 1,
+	.has_bsd_ring = 1,
+	.has_blt_ring = 1,
+	.has_llc = 1,
+	.has_pch_split = 1,
+};
+
+static const struct intel_device_info intel_haswell_m_info = {
+	.is_haswell = 1, .gen = 7, .is_mobile = 1,
+	.need_gfx_hws = 1, .has_hotplug = 1,
+	.has_bsd_ring = 1,
+	.has_blt_ring = 1,
+	.has_llc = 1,
+	.has_pch_split = 1,
 };
 
 static const struct pci_device_id pciidlist[] = {		/* aka */
@@ -297,6 +345,13 @@ static const struct pci_device_id pciidlist[] = {		/* aka */
 	INTEL_VGA_DEVICE(0x0162, &intel_ivybridge_d_info), /* GT2 desktop */
 	INTEL_VGA_DEVICE(0x015a, &intel_ivybridge_d_info), /* GT1 server */
 	INTEL_VGA_DEVICE(0x016a, &intel_ivybridge_d_info), /* GT2 server */
+	INTEL_VGA_DEVICE(0x0402, &intel_haswell_d_info), /* GT1 desktop */
+	INTEL_VGA_DEVICE(0x0412, &intel_haswell_d_info), /* GT2 desktop */
+	INTEL_VGA_DEVICE(0x040a, &intel_haswell_d_info), /* GT1 server */
+	INTEL_VGA_DEVICE(0x041a, &intel_haswell_d_info), /* GT2 server */
+	INTEL_VGA_DEVICE(0x0406, &intel_haswell_m_info), /* GT1 mobile */
+	INTEL_VGA_DEVICE(0x0416, &intel_haswell_m_info), /* GT2 mobile */
+	INTEL_VGA_DEVICE(0x0c16, &intel_haswell_d_info), /* SDV */
 	{0, 0, 0}
 };
 
@@ -308,6 +363,7 @@ MODULE_DEVICE_TABLE(pci, pciidlist);
 #define INTEL_PCH_IBX_DEVICE_ID_TYPE	0x3b00
 #define INTEL_PCH_CPT_DEVICE_ID_TYPE	0x1c00
 #define INTEL_PCH_PPT_DEVICE_ID_TYPE	0x1e00
+#define INTEL_PCH_LPT_DEVICE_ID_TYPE	0x8c00
 
 void intel_detect_pch(struct drm_device *dev)
 {
@@ -328,20 +384,45 @@ void intel_detect_pch(struct drm_device *dev)
 
 			if (id == INTEL_PCH_IBX_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_IBX;
+				dev_priv->num_pch_pll = 2;
 				DRM_DEBUG_KMS("Found Ibex Peak PCH\n");
 			} else if (id == INTEL_PCH_CPT_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_CPT;
+				dev_priv->num_pch_pll = 2;
 				DRM_DEBUG_KMS("Found CougarPoint PCH\n");
 			} else if (id == INTEL_PCH_PPT_DEVICE_ID_TYPE) {
 				/* PantherPoint is CPT compatible */
 				dev_priv->pch_type = PCH_CPT;
+				dev_priv->num_pch_pll = 2;
 				DRM_DEBUG_KMS("Found PatherPoint PCH\n");
+			} else if (id == INTEL_PCH_LPT_DEVICE_ID_TYPE) {
+				dev_priv->pch_type = PCH_LPT;
+				dev_priv->num_pch_pll = 0;
+				DRM_DEBUG_KMS("Found LynxPoint PCH\n");
 			}
+			BUG_ON(dev_priv->num_pch_pll > I915_NUM_PLLS);
 		}
 		pci_dev_put(pch);
 	}
 }
 
+bool i915_semaphore_is_enabled(struct drm_device *dev)
+{
+	if (INTEL_INFO(dev)->gen < 6)
+		return 0;
+
+	if (i915_semaphores >= 0)
+		return i915_semaphores;
+
+#ifdef CONFIG_INTEL_IOMMU
+	/* Enable semaphores on SNB when IO remapping is off */
+	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped)
+		return false;
+#endif
+
+	return 1;
+}
+
 void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
 {
 	int count;
@@ -366,7 +447,7 @@ void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv)
 	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_MT_ACK) & 1))
 		udelay(10);
 
-	I915_WRITE_NOTRACE(FORCEWAKE_MT, (1<<16) | 1);
+	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_ENABLE(1));
 	POSTING_READ(FORCEWAKE_MT);
 
 	count = 0;
@@ -408,7 +489,7 @@ void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv)
 
 void __gen6_gt_force_wake_mt_put(struct drm_i915_private *dev_priv)
 {
-	I915_WRITE_NOTRACE(FORCEWAKE_MT, (1<<16) | 0);
+	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_DISABLE(1));
 	/* The below doubles as a POSTING_READ */
 	gen6_gt_check_fifodbg(dev_priv);
 }
@@ -446,6 +527,31 @@ int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv)
 	return ret;
 }
 
+void vlv_force_wake_get(struct drm_i915_private *dev_priv)
+{
+	int count;
+
+	count = 0;
+
+	/* Already awake? */
+	if ((I915_READ(0x130094) & 0xa1) == 0xa1)
+		return;
+
+	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffffffff);
+	POSTING_READ(FORCEWAKE_VLV);
+
+	count = 0;
+	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & 1) == 0)
+		udelay(10);
+}
+
+void vlv_force_wake_put(struct drm_i915_private *dev_priv)
+{
+	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffff0000);
+	/* FIXME: confirm VLV behavior with Punit folks */
+	POSTING_READ(FORCEWAKE_VLV);
+}
+
 static int i915_drm_freeze(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -525,15 +631,16 @@ static int i915_drm_thaw(struct drm_device *dev)
 
 	/* KMS EnterVT equivalent */
 	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
+		if (HAS_PCH_SPLIT(dev))
+			ironlake_init_pch_refclk(dev);
+
 		mutex_lock(&dev->struct_mutex);
 		dev_priv->mm.suspended = 0;
 
 		error = i915_gem_init_hw(dev);
 		mutex_unlock(&dev->struct_mutex);
 
-		if (HAS_PCH_SPLIT(dev))
-			ironlake_init_pch_refclk(dev);
-
+		intel_modeset_init_hw(dev);
 		drm_mode_config_reset(dev);
 		drm_irq_install(dev);
 
@@ -541,9 +648,6 @@ static int i915_drm_thaw(struct drm_device *dev)
 		mutex_lock(&dev->mode_config.mutex);
 		drm_helper_resume_force_mode(dev);
 		mutex_unlock(&dev->mode_config.mutex);
-
-		if (IS_IRONLAKE_M(dev))
-			ironlake_enable_rc6(dev);
 	}
 
 	intel_opregion_init(dev);
@@ -576,7 +680,7 @@ int i915_resume(struct drm_device *dev)
 	return 0;
 }
 
-static int i8xx_do_reset(struct drm_device *dev, u8 flags)
+static int i8xx_do_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
@@ -610,11 +714,12 @@ static int i965_reset_complete(struct drm_device *dev)
 {
 	u8 gdrst;
 	pci_read_config_byte(dev->pdev, I965_GDRST, &gdrst);
-	return gdrst & 0x1;
+	return (gdrst & GRDOM_RESET_ENABLE) == 0;
 }
 
-static int i965_do_reset(struct drm_device *dev, u8 flags)
+static int i965_do_reset(struct drm_device *dev)
 {
+	int ret;
 	u8 gdrst;
 
 	/*
@@ -623,20 +728,43 @@ static int i965_do_reset(struct drm_device *dev, u8 flags)
 	 * triggers the reset; when done, the hardware will clear it.
 	 */
 	pci_read_config_byte(dev->pdev, I965_GDRST, &gdrst);
-	pci_write_config_byte(dev->pdev, I965_GDRST, gdrst | flags | 0x1);
+	pci_write_config_byte(dev->pdev, I965_GDRST,
+			      gdrst | GRDOM_RENDER |
+			      GRDOM_RESET_ENABLE);
+	ret =  wait_for(i965_reset_complete(dev), 500);
+	if (ret)
+		return ret;
+
+	/* We can't reset render&media without also resetting display ... */
+	pci_read_config_byte(dev->pdev, I965_GDRST, &gdrst);
+	pci_write_config_byte(dev->pdev, I965_GDRST,
+			      gdrst | GRDOM_MEDIA |
+			      GRDOM_RESET_ENABLE);
 
 	return wait_for(i965_reset_complete(dev), 500);
 }
 
-static int ironlake_do_reset(struct drm_device *dev, u8 flags)
+static int ironlake_do_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
-	I915_WRITE(MCHBAR_MIRROR_BASE + ILK_GDSR, gdrst | flags | 0x1);
+	u32 gdrst;
+	int ret;
+
+	gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
+	I915_WRITE(MCHBAR_MIRROR_BASE + ILK_GDSR,
+		   gdrst | GRDOM_RENDER | GRDOM_RESET_ENABLE);
+	ret = wait_for(I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR) & 0x1, 500);
+	if (ret)
+		return ret;
+
+	/* We can't reset render&media without also resetting display ... */
+	gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
+	I915_WRITE(MCHBAR_MIRROR_BASE + ILK_GDSR,
+		   gdrst | GRDOM_MEDIA | GRDOM_RESET_ENABLE);
 	return wait_for(I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR) & 0x1, 500);
 }
 
-static int gen6_do_reset(struct drm_device *dev, u8 flags)
+static int gen6_do_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int	ret;
@@ -671,10 +799,44 @@ static int gen6_do_reset(struct drm_device *dev, u8 flags)
 	return ret;
 }
 
+static int intel_gpu_reset(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret = -ENODEV;
+
+	switch (INTEL_INFO(dev)->gen) {
+	case 7:
+	case 6:
+		ret = gen6_do_reset(dev);
+		break;
+	case 5:
+		ret = ironlake_do_reset(dev);
+		break;
+	case 4:
+		ret = i965_do_reset(dev);
+		break;
+	case 2:
+		ret = i8xx_do_reset(dev);
+		break;
+	}
+
+	/* Also reset the gpu hangman. */
+	if (dev_priv->stop_rings) {
+		DRM_DEBUG("Simulated gpu hang, resetting stop_rings\n");
+		dev_priv->stop_rings = 0;
+		if (ret == -ENODEV) {
+			DRM_ERROR("Reset not implemented, but ignoring "
+				  "error for simulated gpu hangs\n");
+			ret = 0;
+		}
+	}
+
+	return ret;
+}
+
 /**
  * i915_reset - reset chip after a hang
  * @dev: drm device to reset
- * @flags: reset domains
  *
  * Reset the chip.  Useful if a hang is detected. Returns zero on successful
  * reset or otherwise an error code.
@@ -687,14 +849,9 @@ static int gen6_do_reset(struct drm_device *dev, u8 flags)
  *   - re-init interrupt state
  *   - re-init display
  */
-int i915_reset(struct drm_device *dev, u8 flags)
+int i915_reset(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	/*
-	 * We really should only reset the display subsystem if we actually
-	 * need to
-	 */
-	bool need_display = true;
 	int ret;
 
 	if (!i915_try_reset)
@@ -703,26 +860,16 @@ int i915_reset(struct drm_device *dev, u8 flags)
 	if (!mutex_trylock(&dev->struct_mutex))
 		return -EBUSY;
 
+	dev_priv->stop_rings = 0;
+
 	i915_gem_reset(dev);
 
 	ret = -ENODEV;
-	if (get_seconds() - dev_priv->last_gpu_reset < 5) {
+	if (get_seconds() - dev_priv->last_gpu_reset < 5)
 		DRM_ERROR("GPU hanging too fast, declaring wedged!\n");
-	} else switch (INTEL_INFO(dev)->gen) {
-	case 7:
-	case 6:
-		ret = gen6_do_reset(dev, flags);
-		break;
-	case 5:
-		ret = ironlake_do_reset(dev, flags);
-		break;
-	case 4:
-		ret = i965_do_reset(dev, flags);
-		break;
-	case 2:
-		ret = i8xx_do_reset(dev, flags);
-		break;
-	}
+	else
+		ret = intel_gpu_reset(dev);
+
 	dev_priv->last_gpu_reset = get_seconds();
 	if (ret) {
 		DRM_ERROR("Failed to reset chip.\n");
@@ -746,36 +893,27 @@ int i915_reset(struct drm_device *dev, u8 flags)
 	 */
 	if (drm_core_check_feature(dev, DRIVER_MODESET) ||
 			!dev_priv->mm.suspended) {
+		struct intel_ring_buffer *ring;
+		int i;
+
 		dev_priv->mm.suspended = 0;
 
 		i915_gem_init_swizzling(dev);
 
-		dev_priv->ring[RCS].init(&dev_priv->ring[RCS]);
-		if (HAS_BSD(dev))
-		    dev_priv->ring[VCS].init(&dev_priv->ring[VCS]);
-		if (HAS_BLT(dev))
-		    dev_priv->ring[BCS].init(&dev_priv->ring[BCS]);
+		for_each_ring(ring, dev_priv, i)
+			ring->init(ring);
 
 		i915_gem_init_ppgtt(dev);
 
 		mutex_unlock(&dev->struct_mutex);
-		drm_irq_uninstall(dev);
-		drm_mode_config_reset(dev);
-		drm_irq_install(dev);
-		mutex_lock(&dev->struct_mutex);
-	}
 
-	mutex_unlock(&dev->struct_mutex);
+		if (drm_core_check_feature(dev, DRIVER_MODESET))
+			intel_modeset_init_hw(dev);
 
-	/*
-	 * Perform a full modeset as on later generations, e.g. Ironlake, we may
-	 * need to retrain the display link and cannot just restore the register
-	 * values.
-	 */
-	if (need_display) {
-		mutex_lock(&dev->mode_config.mutex);
-		drm_helper_resume_force_mode(dev);
-		mutex_unlock(&dev->mode_config.mutex);
+		drm_irq_uninstall(dev);
+		drm_irq_install(dev);
+	} else {
+		mutex_unlock(&dev->struct_mutex);
 	}
 
 	return 0;
@@ -874,7 +1012,7 @@ static const struct dev_pm_ops i915_pm_ops = {
 	.restore = i915_pm_resume,
 };
 
-static struct vm_operations_struct i915_gem_vm_ops = {
+static const struct vm_operations_struct i915_gem_vm_ops = {
 	.fault = i915_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
@@ -901,7 +1039,7 @@ static struct drm_driver driver = {
 	 */
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_REQUIRE_AGP | /* DRIVER_USE_MTRR |*/
-	    DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | DRIVER_GEM,
+	    DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | DRIVER_GEM | DRIVER_PRIME,
 	.load = i915_driver_load,
 	.unload = i915_driver_unload,
 	.open = i915_driver_open,
@@ -924,6 +1062,12 @@ static struct drm_driver driver = {
 	.gem_init_object = i915_gem_init_object,
 	.gem_free_object = i915_gem_free_object,
 	.gem_vm_ops = &i915_gem_vm_ops,
+
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export = i915_gem_prime_export,
+	.gem_prime_import = i915_gem_prime_import,
+
 	.dumb_create = i915_gem_dumb_create,
 	.dumb_map_offset = i915_gem_mmap_gtt,
 	.dumb_destroy = i915_gem_dumb_destroy,
@@ -993,6 +1137,13 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL and additional rights");
 
+/* We give fast paths for the really cool registers */
+#define NEEDS_FORCE_WAKE(dev_priv, reg) \
+       (((dev_priv)->info->gen >= 6) && \
+        ((reg) < 0x40000) &&            \
+        ((reg) != FORCEWAKE)) && \
+       (!IS_VALLEYVIEW((dev_priv)->dev))
+
 #define __i915_read(x, y) \
 u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg) { \
 	u##x val = 0; \
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 5fabc6c31fec..377c21f531e4 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -38,6 +38,8 @@
 #include <linux/i2c-algo-bit.h>
 #include <drm/intel-gtt.h>
 #include <linux/backlight.h>
+#include <linux/intel-iommu.h>
+#include <linux/kref.h>
 
 /* General customization:
  */
@@ -63,10 +65,30 @@ enum plane {
 };
 #define plane_name(p) ((p) + 'A')
 
+enum port {
+	PORT_A = 0,
+	PORT_B,
+	PORT_C,
+	PORT_D,
+	PORT_E,
+	I915_MAX_PORTS
+};
+#define port_name(p) ((p) + 'A')
+
 #define I915_GEM_GPU_DOMAINS	(~(I915_GEM_DOMAIN_CPU | I915_GEM_DOMAIN_GTT))
 
 #define for_each_pipe(p) for ((p) = 0; (p) < dev_priv->num_pipe; (p)++)
 
+struct intel_pch_pll {
+	int refcount; /* count of number of CRTCs sharing this PLL */
+	int active; /* count of number of active CRTCs (i.e. DPMS on) */
+	bool on; /* is the PLL actually active? Disabled during modeset */
+	int pll_reg;
+	int fp0_reg;
+	int fp1_reg;
+};
+#define I915_NUM_PLLS 2
+
 /* Interface history:
  *
  * 1.1: Original.
@@ -111,11 +133,11 @@ struct opregion_asle;
 struct drm_i915_private;
 
 struct intel_opregion {
-	struct opregion_header *header;
-	struct opregion_acpi *acpi;
-	struct opregion_swsci *swsci;
-	struct opregion_asle *asle;
-	void *vbt;
+	struct opregion_header __iomem *header;
+	struct opregion_acpi __iomem *acpi;
+	struct opregion_swsci __iomem *swsci;
+	struct opregion_asle __iomem *asle;
+	void __iomem *vbt;
 	u32 __iomem *lid_state;
 };
 #define OPREGION_SIZE            (8*1024)
@@ -135,7 +157,6 @@ struct drm_i915_master_private {
 struct drm_i915_fence_reg {
 	struct list_head lru_list;
 	struct drm_i915_gem_object *obj;
-	uint32_t setup_seqno;
 	int pin_count;
 };
 
@@ -151,8 +172,11 @@ struct sdvo_device_mapping {
 struct intel_display_error_state;
 
 struct drm_i915_error_state {
+	struct kref ref;
 	u32 eir;
 	u32 pgtbl_er;
+	u32 ier;
+	bool waiting[I915_NUM_RINGS];
 	u32 pipestat[I915_MAX_PIPES];
 	u32 tail[I915_NUM_RINGS];
 	u32 head[I915_NUM_RINGS];
@@ -218,11 +242,15 @@ struct drm_i915_display_funcs {
 	void (*update_wm)(struct drm_device *dev);
 	void (*update_sprite_wm)(struct drm_device *dev, int pipe,
 				 uint32_t sprite_width, int pixel_size);
+	void (*sanitize_pm)(struct drm_device *dev);
+	void (*update_linetime_wm)(struct drm_device *dev, int pipe,
+				 struct drm_display_mode *mode);
 	int (*crtc_mode_set)(struct drm_crtc *crtc,
 			     struct drm_display_mode *mode,
 			     struct drm_display_mode *adjusted_mode,
 			     int x, int y,
 			     struct drm_framebuffer *old_fb);
+	void (*off)(struct drm_crtc *crtc);
 	void (*write_eld)(struct drm_connector *connector,
 			  struct drm_crtc *crtc);
 	void (*fdi_link_train)(struct drm_crtc *crtc);
@@ -255,6 +283,9 @@ struct intel_device_info {
 	u8 is_broadwater:1;
 	u8 is_crestline:1;
 	u8 is_ivybridge:1;
+	u8 is_valleyview:1;
+	u8 has_pch_split:1;
+	u8 is_haswell:1;
 	u8 has_fbc:1;
 	u8 has_pipe_cxsr:1;
 	u8 has_hotplug:1;
@@ -291,10 +322,12 @@ enum no_fbc_reason {
 enum intel_pch {
 	PCH_IBX,	/* Ibexpeak PCH */
 	PCH_CPT,	/* Cougarpoint PCH */
+	PCH_LPT,	/* Lynxpoint PCH */
 };
 
 #define QUIRK_PIPEA_FORCE (1<<0)
 #define QUIRK_LVDS_SSC_DISABLE (1<<1)
+#define QUIRK_INVERT_BRIGHTNESS (1<<2)
 
 struct intel_fbdev;
 struct intel_fbc_work;
@@ -302,7 +335,6 @@ struct intel_fbc_work;
 struct intel_gmbus {
 	struct i2c_adapter adapter;
 	bool force_bit;
-	bool has_gpio;
 	u32 reg0;
 	u32 gpio_reg;
 	struct i2c_algo_bit_data bit_algo;
@@ -314,7 +346,6 @@ typedef struct drm_i915_private {
 
 	const struct intel_device_info *info;
 
-	int has_gem;
 	int relative_constants_mode;
 
 	void __iomem *regs;
@@ -326,19 +357,23 @@ typedef struct drm_i915_private {
 	/** gt_lock is also taken in irq contexts. */
 	struct spinlock gt_lock;
 
-	struct intel_gmbus *gmbus;
+	struct intel_gmbus gmbus[GMBUS_NUM_PORTS];
 
 	/** gmbus_mutex protects against concurrent usage of the single hw gmbus
 	 * controller on different i2c buses. */
 	struct mutex gmbus_mutex;
 
+	/**
+	 * Base address of the gmbus and gpio block.
+	 */
+	uint32_t gpio_mmio_base;
+
 	struct pci_dev *bridge_dev;
 	struct intel_ring_buffer ring[I915_NUM_RINGS];
 	uint32_t next_seqno;
 
 	drm_dma_handle_t *status_page_dmah;
 	uint32_t counter;
-	drm_local_map_t hws_map;
 	struct drm_i915_gem_object *pwrctx;
 	struct drm_i915_gem_object *renderctx;
 
@@ -354,6 +389,10 @@ typedef struct drm_i915_private {
 
 	/* protects the irq masks */
 	spinlock_t irq_lock;
+
+	/* DPIO indirect register protection */
+	spinlock_t dpio_lock;
+
 	/** Cached value of IMR to avoid reads in updating the bitfield */
 	u32 pipestat[2];
 	u32 irq_mask;
@@ -363,22 +402,20 @@ typedef struct drm_i915_private {
 	u32 hotplug_supported_mask;
 	struct work_struct hotplug_work;
 
-	int tex_lru_log_granularity;
-	int allow_batchbuffer;
 	unsigned int sr01, adpa, ppcr, dvob, dvoc, lvds;
-	int vblank_pipe;
 	int num_pipe;
+	int num_pch_pll;
 
 	/* For hangcheck timer */
 #define DRM_I915_HANGCHECK_PERIOD 1500 /* in ms */
 	struct timer_list hangcheck_timer;
 	int hangcheck_count;
-	uint32_t last_acthd;
-	uint32_t last_acthd_bsd;
-	uint32_t last_acthd_blt;
+	uint32_t last_acthd[I915_NUM_RINGS];
 	uint32_t last_instdone;
 	uint32_t last_instdone1;
 
+	unsigned int stop_rings;
+
 	unsigned long cfb_size;
 	unsigned int cfb_fb;
 	enum plane cfb_plane;
@@ -405,6 +442,8 @@ typedef struct drm_i915_private {
 	unsigned int lvds_use_ssc:1;
 	unsigned int display_clock_mode:1;
 	int lvds_ssc_freq;
+	unsigned int bios_lvds_val; /* initial [PCH_]LVDS reg val in VBIOS */
+	unsigned int lvds_val; /* used for checking LVDS channel mode */
 	struct {
 		int rate;
 		int lanes;
@@ -428,6 +467,7 @@ typedef struct drm_i915_private {
 	unsigned int fsb_freq, mem_freq, is_ddr3;
 
 	spinlock_t error_lock;
+	/* Protected by dev->error_lock. */
 	struct drm_i915_error_state *first_error;
 	struct work_struct error_work;
 	struct completion error_completion;
@@ -652,24 +692,10 @@ typedef struct drm_i915_private {
 		 */
 		struct list_head inactive_list;
 
-		/**
-		 * LRU list of objects which are not in the ringbuffer but
-		 * are still pinned in the GTT.
-		 */
-		struct list_head pinned_list;
-
 		/** LRU list of objects with fence regs on them. */
 		struct list_head fence_list;
 
 		/**
-		 * List of objects currently pending being freed.
-		 *
-		 * These objects are no longer in use, but due to a signal
-		 * we were prevented from freeing them at the appointed time.
-		 */
-		struct list_head deferred_free_list;
-
-		/**
 		 * We leave the user IRQ off as much as possible,
 		 * but this means that requests will finish and never
 		 * be retired once the system goes idle. Set a timer to
@@ -717,6 +743,16 @@ typedef struct drm_i915_private {
 		size_t object_memory;
 		u32 object_count;
 	} mm;
+
+	/* Old dri1 support infrastructure, beware the dragons ya fools entering
+	 * here! */
+	struct {
+		unsigned allow_batchbuffer : 1;
+		u32 __iomem *gfx_hws_cpu_addr;
+	} dri1;
+
+	/* Kernel Modesetting */
+
 	struct sdvo_device_mapping sdvo_mappings[2];
 	/* indicate whether the LVDS_BORDER should be enabled or not */
 	unsigned int lvds_border_bits;
@@ -726,7 +762,8 @@ typedef struct drm_i915_private {
 	struct drm_crtc *plane_to_crtc_mapping[3];
 	struct drm_crtc *pipe_to_crtc_mapping[3];
 	wait_queue_head_t pending_flip_queue;
-	bool flip_pending_is_done;
+
+	struct intel_pch_pll pch_plls[I915_NUM_PLLS];
 
 	/* Reclocking support */
 	bool render_reclock_avail;
@@ -781,6 +818,11 @@ typedef struct drm_i915_private {
 	struct drm_property *force_audio_property;
 } drm_i915_private_t;
 
+/* Iterate over initialised rings */
+#define for_each_ring(ring__, dev_priv__, i__) \
+	for ((i__) = 0; (i__) < I915_NUM_RINGS; (i__)++) \
+		if (((ring__) = &(dev_priv__)->ring[(i__)]), intel_ring_initialized((ring__)))
+
 enum hdmi_force_audio {
 	HDMI_AUDIO_OFF_DVI = -2,	/* no aux data for HDMI-DVI converter */
 	HDMI_AUDIO_OFF,			/* force turn off HDMI audio */
@@ -844,7 +886,14 @@ struct drm_i915_gem_object {
 	 * Current tiling mode for the object.
 	 */
 	unsigned int tiling_mode:2;
-	unsigned int tiling_changed:1;
+	/**
+	 * Whether the tiling parameters for the currently associated fence
+	 * register have changed. Note that for the purposes of tracking
+	 * tiling changes we also treat the unfenced register, the register
+	 * slot that the object occupies whilst it executes a fenced
+	 * command (such as BLT on gen2/3), as a "fence".
+	 */
+	unsigned int fence_dirty:1;
 
 	/** How many users have pinned this object in GTT space. The following
 	 * users can each hold at most one reference: pwrite/pread, pin_ioctl
@@ -881,6 +930,7 @@ struct drm_i915_gem_object {
 	unsigned int cache_level:2;
 
 	unsigned int has_aliasing_ppgtt_mapping:1;
+	unsigned int has_global_gtt_mapping:1;
 
 	struct page **pages;
 
@@ -890,6 +940,8 @@ struct drm_i915_gem_object {
 	struct scatterlist *sg_list;
 	int num_sg;
 
+	/* prime dma-buf support */
+	struct sg_table *sg_table;
 	/**
 	 * Used for performing relocations during execbuffer insertion.
 	 */
@@ -904,13 +956,12 @@ struct drm_i915_gem_object {
 	 */
 	uint32_t gtt_offset;
 
-	/** Breadcrumb of last rendering to the buffer. */
-	uint32_t last_rendering_seqno;
 	struct intel_ring_buffer *ring;
 
+	/** Breadcrumb of last rendering to the buffer. */
+	uint32_t last_rendering_seqno;
 	/** Breadcrumb of last fenced GPU access to the buffer. */
 	uint32_t last_fenced_seqno;
-	struct intel_ring_buffer *last_fenced_ring;
 
 	/** Current tiling stride for the object, if it's tiled. */
 	uint32_t stride;
@@ -918,13 +969,6 @@ struct drm_i915_gem_object {
 	/** Record of address bit 17 of each page at last unbind. */
 	unsigned long *bit_17;
 
-
-	/**
-	 * If present, while GEM_DOMAIN_CPU is in the read domain this array
-	 * flags which individual pages are valid.
-	 */
-	uint8_t *page_cpu_valid;
-
 	/** User space pin count and filp owning the pin */
 	uint32_t user_pin_count;
 	struct drm_file *pin_filp;
@@ -1001,6 +1045,8 @@ struct drm_i915_file_private {
 #define IS_IRONLAKE_D(dev)	((dev)->pci_device == 0x0042)
 #define IS_IRONLAKE_M(dev)	((dev)->pci_device == 0x0046)
 #define IS_IVYBRIDGE(dev)	(INTEL_INFO(dev)->is_ivybridge)
+#define IS_VALLEYVIEW(dev)	(INTEL_INFO(dev)->is_valleyview)
+#define IS_HASWELL(dev)	(INTEL_INFO(dev)->is_haswell)
 #define IS_MOBILE(dev)		(INTEL_INFO(dev)->is_mobile)
 
 /*
@@ -1044,10 +1090,11 @@ struct drm_i915_file_private {
 #define HAS_PIPE_CXSR(dev) (INTEL_INFO(dev)->has_pipe_cxsr)
 #define I915_HAS_FBC(dev) (INTEL_INFO(dev)->has_fbc)
 
-#define HAS_PCH_SPLIT(dev) (IS_GEN5(dev) || IS_GEN6(dev) || IS_IVYBRIDGE(dev))
+#define HAS_PCH_SPLIT(dev) (INTEL_INFO(dev)->has_pch_split)
 #define HAS_PIPE_CONTROL(dev) (INTEL_INFO(dev)->gen >= 5)
 
 #define INTEL_PCH_TYPE(dev) (((struct drm_i915_private *)(dev)->dev_private)->pch_type)
+#define HAS_PCH_LPT(dev) (INTEL_PCH_TYPE(dev) == PCH_LPT)
 #define HAS_PCH_CPT(dev) (INTEL_PCH_TYPE(dev) == PCH_CPT)
 #define HAS_PCH_IBX(dev) (INTEL_PCH_TYPE(dev) == PCH_IBX)
 
@@ -1081,6 +1128,7 @@ extern int i915_panel_ignore_lid __read_mostly;
 extern unsigned int i915_powersave __read_mostly;
 extern int i915_semaphores __read_mostly;
 extern unsigned int i915_lvds_downclock __read_mostly;
+extern int i915_lvds_channel_mode __read_mostly;
 extern int i915_panel_use_ssc __read_mostly;
 extern int i915_vbt_sdvo_panel_type __read_mostly;
 extern int i915_enable_rc6 __read_mostly;
@@ -1094,6 +1142,7 @@ extern int i915_master_create(struct drm_device *dev, struct drm_master *master)
 extern void i915_master_destroy(struct drm_device *dev, struct drm_master *master);
 
 				/* i915_dma.c */
+void i915_update_dri1_breadcrumb(struct drm_device *dev);
 extern void i915_kernel_lost_context(struct drm_device * dev);
 extern int i915_driver_load(struct drm_device *, unsigned long flags);
 extern int i915_driver_unload(struct drm_device *);
@@ -1104,12 +1153,14 @@ extern void i915_driver_preclose(struct drm_device *dev,
 extern void i915_driver_postclose(struct drm_device *dev,
 				  struct drm_file *file_priv);
 extern int i915_driver_device_is_agp(struct drm_device * dev);
+#ifdef CONFIG_COMPAT
 extern long i915_compat_ioctl(struct file *filp, unsigned int cmd,
 			      unsigned long arg);
+#endif
 extern int i915_emit_box(struct drm_device *dev,
 			 struct drm_clip_rect *box,
 			 int DR1, int DR4);
-extern int i915_reset(struct drm_device *dev, u8 flags);
+extern int i915_reset(struct drm_device *dev);
 extern unsigned long i915_chipset_val(struct drm_i915_private *dev_priv);
 extern unsigned long i915_mch_val(struct drm_i915_private *dev_priv);
 extern unsigned long i915_gfx_val(struct drm_i915_private *dev_priv);
@@ -1119,19 +1170,10 @@ extern void i915_update_gfx_val(struct drm_i915_private *dev_priv);
 /* i915_irq.c */
 void i915_hangcheck_elapsed(unsigned long data);
 void i915_handle_error(struct drm_device *dev, bool wedged);
-extern int i915_irq_emit(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv);
-extern int i915_irq_wait(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv);
 
 extern void intel_irq_init(struct drm_device *dev);
 
-extern int i915_vblank_pipe_set(struct drm_device *dev, void *data,
-				struct drm_file *file_priv);
-extern int i915_vblank_pipe_get(struct drm_device *dev, void *data,
-				struct drm_file *file_priv);
-extern int i915_vblank_swap(struct drm_device *dev, void *data,
-			    struct drm_file *file_priv);
+void i915_error_state_free(struct kref *error_ref);
 
 void
 i915_enable_pipestat(drm_i915_private_t *dev_priv, int pipe, u32 mask);
@@ -1205,8 +1247,12 @@ int __must_check i915_gem_object_unbind(struct drm_i915_gem_object *obj);
 void i915_gem_release_mmap(struct drm_i915_gem_object *obj);
 void i915_gem_lastclose(struct drm_device *dev);
 
+int i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj,
+				  gfp_t gfpmask);
 int __must_check i915_mutex_lock_interruptible(struct drm_device *dev);
 int __must_check i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj);
+int i915_gem_object_sync(struct drm_i915_gem_object *obj,
+			 struct intel_ring_buffer *to);
 void i915_gem_object_move_to_active(struct drm_i915_gem_object *obj,
 				    struct intel_ring_buffer *ring,
 				    u32 seqno);
@@ -1229,17 +1275,18 @@ i915_seqno_passed(uint32_t seq1, uint32_t seq2)
 
 u32 i915_gem_next_request_seqno(struct intel_ring_buffer *ring);
 
-int __must_check i915_gem_object_get_fence(struct drm_i915_gem_object *obj,
-					   struct intel_ring_buffer *pipelined);
+int __must_check i915_gem_object_get_fence(struct drm_i915_gem_object *obj);
 int __must_check i915_gem_object_put_fence(struct drm_i915_gem_object *obj);
 
-static inline void
+static inline bool
 i915_gem_object_pin_fence(struct drm_i915_gem_object *obj)
 {
 	if (obj->fence_reg != I915_FENCE_REG_NONE) {
 		struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
 		dev_priv->fence_regs[obj->fence_reg].pin_count++;
-	}
+		return true;
+	} else
+		return false;
 }
 
 static inline void
@@ -1260,27 +1307,25 @@ int __must_check i915_gem_object_set_domain(struct drm_i915_gem_object *obj,
 					    uint32_t read_domains,
 					    uint32_t write_domain);
 int __must_check i915_gem_object_finish_gpu(struct drm_i915_gem_object *obj);
+int __must_check i915_gem_init(struct drm_device *dev);
 int __must_check i915_gem_init_hw(struct drm_device *dev);
 void i915_gem_init_swizzling(struct drm_device *dev);
 void i915_gem_init_ppgtt(struct drm_device *dev);
 void i915_gem_cleanup_ringbuffer(struct drm_device *dev);
-void i915_gem_do_init(struct drm_device *dev,
-		      unsigned long start,
-		      unsigned long mappable_end,
-		      unsigned long end);
-int __must_check i915_gpu_idle(struct drm_device *dev, bool do_retire);
+int __must_check i915_gpu_idle(struct drm_device *dev);
 int __must_check i915_gem_idle(struct drm_device *dev);
 int __must_check i915_add_request(struct intel_ring_buffer *ring,
 				  struct drm_file *file,
 				  struct drm_i915_gem_request *request);
 int __must_check i915_wait_request(struct intel_ring_buffer *ring,
-				   uint32_t seqno,
-				   bool do_retire);
+				   uint32_t seqno);
 int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
 int __must_check
 i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj,
 				  bool write);
 int __must_check
+i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write);
+int __must_check
 i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 				     u32 alignment,
 				     struct intel_ring_buffer *pipelined);
@@ -1301,6 +1346,13 @@ i915_gem_get_unfenced_gtt_alignment(struct drm_device *dev,
 int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
 				    enum i915_cache_level cache_level);
 
+struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf);
+
+struct dma_buf *i915_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *gem_obj, int flags);
+
+
 /* i915_gem_gtt.c */
 int __must_check i915_gem_init_aliasing_ppgtt(struct drm_device *dev);
 void i915_gem_cleanup_aliasing_ppgtt(struct drm_device *dev);
@@ -1311,18 +1363,24 @@ void i915_ppgtt_unbind_object(struct i915_hw_ppgtt *ppgtt,
 			      struct drm_i915_gem_object *obj);
 
 void i915_gem_restore_gtt_mappings(struct drm_device *dev);
-int __must_check i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj);
-void i915_gem_gtt_rebind_object(struct drm_i915_gem_object *obj,
+int __must_check i915_gem_gtt_prepare_object(struct drm_i915_gem_object *obj);
+void i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj,
 				enum i915_cache_level cache_level);
 void i915_gem_gtt_unbind_object(struct drm_i915_gem_object *obj);
+void i915_gem_gtt_finish_object(struct drm_i915_gem_object *obj);
+void i915_gem_init_global_gtt(struct drm_device *dev,
+			      unsigned long start,
+			      unsigned long mappable_end,
+			      unsigned long end);
 
 /* i915_gem_evict.c */
 int __must_check i915_gem_evict_something(struct drm_device *dev, int min_size,
 					  unsigned alignment, bool mappable);
-int __must_check i915_gem_evict_everything(struct drm_device *dev,
-					   bool purgeable_only);
-int __must_check i915_gem_evict_inactive(struct drm_device *dev,
-					 bool purgeable_only);
+int i915_gem_evict_everything(struct drm_device *dev, bool purgeable_only);
+
+/* i915_gem_stolen.c */
+int i915_gem_init_stolen(struct drm_device *dev);
+void i915_gem_cleanup_stolen(struct drm_device *dev);
 
 /* i915_gem_tiling.c */
 void i915_gem_detect_bit_6_swizzle(struct drm_device *dev);
@@ -1354,9 +1412,20 @@ extern int i915_restore_state(struct drm_device *dev);
 extern int i915_save_state(struct drm_device *dev);
 extern int i915_restore_state(struct drm_device *dev);
 
+/* i915_sysfs.c */
+void i915_setup_sysfs(struct drm_device *dev_priv);
+void i915_teardown_sysfs(struct drm_device *dev_priv);
+
 /* intel_i2c.c */
 extern int intel_setup_gmbus(struct drm_device *dev);
 extern void intel_teardown_gmbus(struct drm_device *dev);
+extern inline bool intel_gmbus_is_port_valid(unsigned port)
+{
+	return (port >= GMBUS_PORT_SSC && port <= GMBUS_PORT_DPD);
+}
+
+extern struct i2c_adapter *intel_gmbus_get_adapter(
+		struct drm_i915_private *dev_priv, unsigned port);
 extern void intel_gmbus_set_speed(struct i2c_adapter *adapter, int speed);
 extern void intel_gmbus_force_bit(struct i2c_adapter *adapter, bool force_bit);
 extern inline bool intel_gmbus_is_forced_bit(struct i2c_adapter *adapter)
@@ -1391,6 +1460,7 @@ static inline void intel_unregister_dsm_handler(void) { return; }
 #endif /* CONFIG_ACPI */
 
 /* modesetting */
+extern void intel_modeset_init_hw(struct drm_device *dev);
 extern void intel_modeset_init(struct drm_device *dev);
 extern void intel_modeset_gem_init(struct drm_device *dev);
 extern void intel_modeset_cleanup(struct drm_device *dev);
@@ -1403,12 +1473,17 @@ extern void ironlake_enable_rc6(struct drm_device *dev);
 extern void gen6_set_rps(struct drm_device *dev, u8 val);
 extern void intel_detect_pch(struct drm_device *dev);
 extern int intel_trans_dp_port_sel(struct drm_crtc *crtc);
+extern int intel_enable_rc6(const struct drm_device *dev);
 
+extern bool i915_semaphore_is_enabled(struct drm_device *dev);
 extern void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv);
 extern void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv);
 extern void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv);
 extern void __gen6_gt_force_wake_mt_put(struct drm_i915_private *dev_priv);
 
+extern void vlv_force_wake_get(struct drm_i915_private *dev_priv);
+extern void vlv_force_wake_put(struct drm_i915_private *dev_priv);
+
 /* overlay */
 #ifdef CONFIG_DEBUG_FS
 extern struct intel_overlay_error_state *intel_overlay_capture_error_state(struct drm_device *dev);
@@ -1420,28 +1495,6 @@ extern void intel_display_print_error_state(struct seq_file *m,
 					    struct intel_display_error_state *error);
 #endif
 
-#define LP_RING(d) (&((struct drm_i915_private *)(d))->ring[RCS])
-
-#define BEGIN_LP_RING(n) \
-	intel_ring_begin(LP_RING(dev_priv), (n))
-
-#define OUT_RING(x) \
-	intel_ring_emit(LP_RING(dev_priv), x)
-
-#define ADVANCE_LP_RING() \
-	intel_ring_advance(LP_RING(dev_priv))
-
-/**
- * Lock test for when it's just for synchronization of ring access.
- *
- * In that case, we don't need to do it when GEM is initialized as nobody else
- * has access to the ring.
- */
-#define RING_LOCK_TEST_WITH_RETURN(dev, file) do {			\
-	if (LP_RING(dev->dev_private)->obj == NULL)			\
-		LOCK_TEST_WITH_RETURN(dev, file);			\
-} while (0)
-
 /* On SNB platform, before reading ring registers forcewake bit
  * must be set to prevent GT core from power down and stale values being
  * returned.
@@ -1450,12 +1503,6 @@ void gen6_gt_force_wake_get(struct drm_i915_private *dev_priv);
 void gen6_gt_force_wake_put(struct drm_i915_private *dev_priv);
 int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv);
 
-/* We give fast paths for the really cool registers */
-#define NEEDS_FORCE_WAKE(dev_priv, reg) \
-	(((dev_priv)->info->gen >= 6) && \
-	 ((reg) < 0x40000) &&		 \
-	 ((reg) != FORCEWAKE))
-
 #define __i915_read(x, y) \
 	u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg);
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 0d1e4b7b4b99..c1e5c66553df 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -35,31 +35,41 @@
 #include <linux/slab.h>
 #include <linux/swap.h>
 #include <linux/pci.h>
+#include <linux/dma-buf.h>
 
 static __must_check int i915_gem_object_flush_gpu_write_domain(struct drm_i915_gem_object *obj);
 static void i915_gem_object_flush_gtt_write_domain(struct drm_i915_gem_object *obj);
 static void i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *obj);
-static __must_check int i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj,
-							  bool write);
-static __must_check int i915_gem_object_set_cpu_read_domain_range(struct drm_i915_gem_object *obj,
-								  uint64_t offset,
-								  uint64_t size);
-static void i915_gem_object_set_to_full_cpu_read_domain(struct drm_i915_gem_object *obj);
 static __must_check int i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 						    unsigned alignment,
 						    bool map_and_fenceable);
-static void i915_gem_clear_fence_reg(struct drm_device *dev,
-				     struct drm_i915_fence_reg *reg);
 static int i915_gem_phys_pwrite(struct drm_device *dev,
 				struct drm_i915_gem_object *obj,
 				struct drm_i915_gem_pwrite *args,
 				struct drm_file *file);
-static void i915_gem_free_object_tail(struct drm_i915_gem_object *obj);
+
+static void i915_gem_write_fence(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj);
+static void i915_gem_object_update_fence(struct drm_i915_gem_object *obj,
+					 struct drm_i915_fence_reg *fence,
+					 bool enable);
 
 static int i915_gem_inactive_shrink(struct shrinker *shrinker,
 				    struct shrink_control *sc);
 static void i915_gem_object_truncate(struct drm_i915_gem_object *obj);
 
+static inline void i915_gem_object_fence_lost(struct drm_i915_gem_object *obj)
+{
+	if (obj->tiling_mode)
+		i915_gem_release_mmap(obj);
+
+	/* As we do not have an associated fence register, we will force
+	 * a tiling change if we ever need to acquire one.
+	 */
+	obj->fence_dirty = false;
+	obj->fence_reg = I915_FENCE_REG_NONE;
+}
+
 /* some bookkeeping */
 static void i915_gem_info_add_obj(struct drm_i915_private *dev_priv,
 				  size_t size)
@@ -122,26 +132,7 @@ int i915_mutex_lock_interruptible(struct drm_device *dev)
 static inline bool
 i915_gem_object_is_inactive(struct drm_i915_gem_object *obj)
 {
-	return obj->gtt_space && !obj->active && obj->pin_count == 0;
-}
-
-void i915_gem_do_init(struct drm_device *dev,
-		      unsigned long start,
-		      unsigned long mappable_end,
-		      unsigned long end)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	drm_mm_init(&dev_priv->mm.gtt_space, start, end - start);
-
-	dev_priv->mm.gtt_start = start;
-	dev_priv->mm.gtt_mappable_end = mappable_end;
-	dev_priv->mm.gtt_end = end;
-	dev_priv->mm.gtt_total = end - start;
-	dev_priv->mm.mappable_gtt_total = min(end, mappable_end) - start;
-
-	/* Take over this portion of the GTT */
-	intel_gtt_clear_range(start / PAGE_SIZE, (end-start) / PAGE_SIZE);
+	return !obj->active;
 }
 
 int
@@ -150,12 +141,20 @@ i915_gem_init_ioctl(struct drm_device *dev, void *data,
 {
 	struct drm_i915_gem_init *args = data;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	if (args->gtt_start >= args->gtt_end ||
 	    (args->gtt_end | args->gtt_start) & (PAGE_SIZE - 1))
 		return -EINVAL;
 
+	/* GEM with user mode setting was never supported on ilk and later. */
+	if (INTEL_INFO(dev)->gen >= 5)
+		return -ENODEV;
+
 	mutex_lock(&dev->struct_mutex);
-	i915_gem_do_init(dev, args->gtt_start, args->gtt_end, args->gtt_end);
+	i915_gem_init_global_gtt(dev, args->gtt_start,
+				 args->gtt_end, args->gtt_end);
 	mutex_unlock(&dev->struct_mutex);
 
 	return 0;
@@ -170,13 +169,11 @@ i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 	struct drm_i915_gem_object *obj;
 	size_t pinned;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	pinned = 0;
 	mutex_lock(&dev->struct_mutex);
-	list_for_each_entry(obj, &dev_priv->mm.pinned_list, mm_list)
-		pinned += obj->gtt_space->size;
+	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list)
+		if (obj->pin_count)
+			pinned += obj->gtt_space->size;
 	mutex_unlock(&dev->struct_mutex);
 
 	args->aper_size = dev_priv->mm.gtt_total;
@@ -247,6 +244,7 @@ i915_gem_create_ioctl(struct drm_device *dev, void *data,
 		      struct drm_file *file)
 {
 	struct drm_i915_gem_create *args = data;
+
 	return i915_gem_create(file, dev,
 			       args->size, &args->handle);
 }
@@ -259,66 +257,6 @@ static int i915_gem_object_needs_bit17_swizzle(struct drm_i915_gem_object *obj)
 		obj->tiling_mode != I915_TILING_NONE;
 }
 
-/**
- * This is the fast shmem pread path, which attempts to copy_from_user directly
- * from the backing pages of the object to the user's address space.  On a
- * fault, it fails so we can fall back to i915_gem_shmem_pwrite_slow().
- */
-static int
-i915_gem_shmem_pread_fast(struct drm_device *dev,
-			  struct drm_i915_gem_object *obj,
-			  struct drm_i915_gem_pread *args,
-			  struct drm_file *file)
-{
-	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
-	ssize_t remain;
-	loff_t offset;
-	char __user *user_data;
-	int page_offset, page_length;
-
-	user_data = (char __user *) (uintptr_t) args->data_ptr;
-	remain = args->size;
-
-	offset = args->offset;
-
-	while (remain > 0) {
-		struct page *page;
-		char *vaddr;
-		int ret;
-
-		/* Operation in this page
-		 *
-		 * page_offset = offset within page
-		 * page_length = bytes to copy for this page
-		 */
-		page_offset = offset_in_page(offset);
-		page_length = remain;
-		if ((page_offset + remain) > PAGE_SIZE)
-			page_length = PAGE_SIZE - page_offset;
-
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page))
-			return PTR_ERR(page);
-
-		vaddr = kmap_atomic(page);
-		ret = __copy_to_user_inatomic(user_data,
-					      vaddr + page_offset,
-					      page_length);
-		kunmap_atomic(vaddr);
-
-		mark_page_accessed(page);
-		page_cache_release(page);
-		if (ret)
-			return -EFAULT;
-
-		remain -= page_length;
-		user_data += page_length;
-		offset += page_length;
-	}
-
-	return 0;
-}
-
 static inline int
 __copy_to_user_swizzled(char __user *cpu_vaddr,
 			const char *gpu_vaddr, int gpu_offset,
@@ -346,8 +284,8 @@ __copy_to_user_swizzled(char __user *cpu_vaddr,
 }
 
 static inline int
-__copy_from_user_swizzled(char __user *gpu_vaddr, int gpu_offset,
-			  const char *cpu_vaddr,
+__copy_from_user_swizzled(char *gpu_vaddr, int gpu_offset,
+			  const char __user *cpu_vaddr,
 			  int length)
 {
 	int ret, cpu_offset = 0;
@@ -371,37 +309,121 @@ __copy_from_user_swizzled(char __user *gpu_vaddr, int gpu_offset,
 	return 0;
 }
 
-/**
- * This is the fallback shmem pread path, which allocates temporary storage
- * in kernel space to copy_to_user into outside of the struct_mutex, so we
- * can copy out of the object's backing pages while holding the struct mutex
- * and not take page faults.
- */
+/* Per-page copy function for the shmem pread fastpath.
+ * Flushes invalid cachelines before reading the target if
+ * needs_clflush is set. */
 static int
-i915_gem_shmem_pread_slow(struct drm_device *dev,
-			  struct drm_i915_gem_object *obj,
-			  struct drm_i915_gem_pread *args,
-			  struct drm_file *file)
+shmem_pread_fast(struct page *page, int shmem_page_offset, int page_length,
+		 char __user *user_data,
+		 bool page_do_bit17_swizzling, bool needs_clflush)
+{
+	char *vaddr;
+	int ret;
+
+	if (unlikely(page_do_bit17_swizzling))
+		return -EINVAL;
+
+	vaddr = kmap_atomic(page);
+	if (needs_clflush)
+		drm_clflush_virt_range(vaddr + shmem_page_offset,
+				       page_length);
+	ret = __copy_to_user_inatomic(user_data,
+				      vaddr + shmem_page_offset,
+				      page_length);
+	kunmap_atomic(vaddr);
+
+	return ret;
+}
+
+static void
+shmem_clflush_swizzled_range(char *addr, unsigned long length,
+			     bool swizzled)
+{
+	if (unlikely(swizzled)) {
+		unsigned long start = (unsigned long) addr;
+		unsigned long end = (unsigned long) addr + length;
+
+		/* For swizzling simply ensure that we always flush both
+		 * channels. Lame, but simple and it works. Swizzled
+		 * pwrite/pread is far from a hotpath - current userspace
+		 * doesn't use it at all. */
+		start = round_down(start, 128);
+		end = round_up(end, 128);
+
+		drm_clflush_virt_range((void *)start, end - start);
+	} else {
+		drm_clflush_virt_range(addr, length);
+	}
+
+}
+
+/* Only difference to the fast-path function is that this can handle bit17
+ * and uses non-atomic copy and kmap functions. */
+static int
+shmem_pread_slow(struct page *page, int shmem_page_offset, int page_length,
+		 char __user *user_data,
+		 bool page_do_bit17_swizzling, bool needs_clflush)
+{
+	char *vaddr;
+	int ret;
+
+	vaddr = kmap(page);
+	if (needs_clflush)
+		shmem_clflush_swizzled_range(vaddr + shmem_page_offset,
+					     page_length,
+					     page_do_bit17_swizzling);
+
+	if (page_do_bit17_swizzling)
+		ret = __copy_to_user_swizzled(user_data,
+					      vaddr, shmem_page_offset,
+					      page_length);
+	else
+		ret = __copy_to_user(user_data,
+				     vaddr + shmem_page_offset,
+				     page_length);
+	kunmap(page);
+
+	return ret;
+}
+
+static int
+i915_gem_shmem_pread(struct drm_device *dev,
+		     struct drm_i915_gem_object *obj,
+		     struct drm_i915_gem_pread *args,
+		     struct drm_file *file)
 {
 	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
 	char __user *user_data;
 	ssize_t remain;
 	loff_t offset;
-	int shmem_page_offset, page_length, ret;
+	int shmem_page_offset, page_length, ret = 0;
 	int obj_do_bit17_swizzling, page_do_bit17_swizzling;
+	int hit_slowpath = 0;
+	int prefaulted = 0;
+	int needs_clflush = 0;
+	int release_page;
 
 	user_data = (char __user *) (uintptr_t) args->data_ptr;
 	remain = args->size;
 
 	obj_do_bit17_swizzling = i915_gem_object_needs_bit17_swizzle(obj);
 
-	offset = args->offset;
+	if (!(obj->base.read_domains & I915_GEM_DOMAIN_CPU)) {
+		/* If we're not in the cpu read domain, set ourself into the gtt
+		 * read domain and manually flush cachelines (if required). This
+		 * optimizes for the case when the gpu will dirty the data
+		 * anyway again before the next pread happens. */
+		if (obj->cache_level == I915_CACHE_NONE)
+			needs_clflush = 1;
+		ret = i915_gem_object_set_to_gtt_domain(obj, false);
+		if (ret)
+			return ret;
+	}
 
-	mutex_unlock(&dev->struct_mutex);
+	offset = args->offset;
 
 	while (remain > 0) {
 		struct page *page;
-		char *vaddr;
 
 		/* Operation in this page
 		 *
@@ -413,28 +435,51 @@ i915_gem_shmem_pread_slow(struct drm_device *dev,
 		if ((shmem_page_offset + page_length) > PAGE_SIZE)
 			page_length = PAGE_SIZE - shmem_page_offset;
 
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page)) {
-			ret = PTR_ERR(page);
-			goto out;
+		if (obj->pages) {
+			page = obj->pages[offset >> PAGE_SHIFT];
+			release_page = 0;
+		} else {
+			page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
+			if (IS_ERR(page)) {
+				ret = PTR_ERR(page);
+				goto out;
+			}
+			release_page = 1;
 		}
 
 		page_do_bit17_swizzling = obj_do_bit17_swizzling &&
 			(page_to_phys(page) & (1 << 17)) != 0;
 
-		vaddr = kmap(page);
-		if (page_do_bit17_swizzling)
-			ret = __copy_to_user_swizzled(user_data,
-						      vaddr, shmem_page_offset,
-						      page_length);
-		else
-			ret = __copy_to_user(user_data,
-					     vaddr + shmem_page_offset,
-					     page_length);
-		kunmap(page);
+		ret = shmem_pread_fast(page, shmem_page_offset, page_length,
+				       user_data, page_do_bit17_swizzling,
+				       needs_clflush);
+		if (ret == 0)
+			goto next_page;
 
-		mark_page_accessed(page);
+		hit_slowpath = 1;
+		page_cache_get(page);
+		mutex_unlock(&dev->struct_mutex);
+
+		if (!prefaulted) {
+			ret = fault_in_multipages_writeable(user_data, remain);
+			/* Userspace is tricking us, but we've already clobbered
+			 * its pages with the prefault and promised to write the
+			 * data up to the first fault. Hence ignore any errors
+			 * and just continue. */
+			(void)ret;
+			prefaulted = 1;
+		}
+
+		ret = shmem_pread_slow(page, shmem_page_offset, page_length,
+				       user_data, page_do_bit17_swizzling,
+				       needs_clflush);
+
+		mutex_lock(&dev->struct_mutex);
 		page_cache_release(page);
+next_page:
+		mark_page_accessed(page);
+		if (release_page)
+			page_cache_release(page);
 
 		if (ret) {
 			ret = -EFAULT;
@@ -447,10 +492,11 @@ i915_gem_shmem_pread_slow(struct drm_device *dev,
 	}
 
 out:
-	mutex_lock(&dev->struct_mutex);
-	/* Fixup: Kill any reinstated backing storage pages */
-	if (obj->madv == __I915_MADV_PURGED)
-		i915_gem_object_truncate(obj);
+	if (hit_slowpath) {
+		/* Fixup: Kill any reinstated backing storage pages */
+		if (obj->madv == __I915_MADV_PURGED)
+			i915_gem_object_truncate(obj);
+	}
 
 	return ret;
 }
@@ -476,11 +522,6 @@ i915_gem_pread_ioctl(struct drm_device *dev, void *data,
 		       args->size))
 		return -EFAULT;
 
-	ret = fault_in_pages_writeable((char __user *)(uintptr_t)args->data_ptr,
-				       args->size);
-	if (ret)
-		return -EFAULT;
-
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -498,19 +539,17 @@ i915_gem_pread_ioctl(struct drm_device *dev, void *data,
 		goto out;
 	}
 
-	trace_i915_gem_object_pread(obj, args->offset, args->size);
-
-	ret = i915_gem_object_set_cpu_read_domain_range(obj,
-							args->offset,
-							args->size);
-	if (ret)
+	/* prime objects have no backing filp to GEM pread/pwrite
+	 * pages from.
+	 */
+	if (!obj->base.filp) {
+		ret = -EINVAL;
 		goto out;
+	}
 
-	ret = -EFAULT;
-	if (!i915_gem_object_needs_bit17_swizzle(obj))
-		ret = i915_gem_shmem_pread_fast(dev, obj, args, file);
-	if (ret == -EFAULT)
-		ret = i915_gem_shmem_pread_slow(dev, obj, args, file);
+	trace_i915_gem_object_pread(obj, args->offset, args->size);
+
+	ret = i915_gem_shmem_pread(dev, obj, args, file);
 
 out:
 	drm_gem_object_unreference(&obj->base);
@@ -529,40 +568,19 @@ fast_user_write(struct io_mapping *mapping,
 		char __user *user_data,
 		int length)
 {
-	char *vaddr_atomic;
+	void __iomem *vaddr_atomic;
+	void *vaddr;
 	unsigned long unwritten;
 
 	vaddr_atomic = io_mapping_map_atomic_wc(mapping, page_base);
-	unwritten = __copy_from_user_inatomic_nocache(vaddr_atomic + page_offset,
+	/* We can use the cpu mem copy function because this is X86. */
+	vaddr = (void __force*)vaddr_atomic + page_offset;
+	unwritten = __copy_from_user_inatomic_nocache(vaddr,
 						      user_data, length);
 	io_mapping_unmap_atomic(vaddr_atomic);
 	return unwritten;
 }
 
-/* Here's the write path which can sleep for
- * page faults
- */
-
-static inline void
-slow_kernel_write(struct io_mapping *mapping,
-		  loff_t gtt_base, int gtt_offset,
-		  struct page *user_page, int user_offset,
-		  int length)
-{
-	char __iomem *dst_vaddr;
-	char *src_vaddr;
-
-	dst_vaddr = io_mapping_map_wc(mapping, gtt_base);
-	src_vaddr = kmap(user_page);
-
-	memcpy_toio(dst_vaddr + gtt_offset,
-		    src_vaddr + user_offset,
-		    length);
-
-	kunmap(user_page);
-	io_mapping_unmap(dst_vaddr);
-}
-
 /**
  * This is the fast pwrite path, where we copy the data directly from the
  * user into the GTT, uncached.
@@ -577,7 +595,19 @@ i915_gem_gtt_pwrite_fast(struct drm_device *dev,
 	ssize_t remain;
 	loff_t offset, page_base;
 	char __user *user_data;
-	int page_offset, page_length;
+	int page_offset, page_length, ret;
+
+	ret = i915_gem_object_pin(obj, 0, true);
+	if (ret)
+		goto out;
+
+	ret = i915_gem_object_set_to_gtt_domain(obj, true);
+	if (ret)
+		goto out_unpin;
+
+	ret = i915_gem_object_put_fence(obj);
+	if (ret)
+		goto out_unpin;
 
 	user_data = (char __user *) (uintptr_t) args->data_ptr;
 	remain = args->size;
@@ -602,214 +632,133 @@ i915_gem_gtt_pwrite_fast(struct drm_device *dev,
 		 * retry in the slow path.
 		 */
 		if (fast_user_write(dev_priv->mm.gtt_mapping, page_base,
-				    page_offset, user_data, page_length))
-			return -EFAULT;
+				    page_offset, user_data, page_length)) {
+			ret = -EFAULT;
+			goto out_unpin;
+		}
 
 		remain -= page_length;
 		user_data += page_length;
 		offset += page_length;
 	}
 
-	return 0;
+out_unpin:
+	i915_gem_object_unpin(obj);
+out:
+	return ret;
 }
 
-/**
- * This is the fallback GTT pwrite path, which uses get_user_pages to pin
- * the memory and maps it using kmap_atomic for copying.
- *
- * This code resulted in x11perf -rgb10text consuming about 10% more CPU
- * than using i915_gem_gtt_pwrite_fast on a G45 (32-bit).
- */
+/* Per-page copy function for the shmem pwrite fastpath.
+ * Flushes invalid cachelines before writing to the target if
+ * needs_clflush_before is set and flushes out any written cachelines after
+ * writing if needs_clflush is set. */
 static int
-i915_gem_gtt_pwrite_slow(struct drm_device *dev,
-			 struct drm_i915_gem_object *obj,
-			 struct drm_i915_gem_pwrite *args,
-			 struct drm_file *file)
+shmem_pwrite_fast(struct page *page, int shmem_page_offset, int page_length,
+		  char __user *user_data,
+		  bool page_do_bit17_swizzling,
+		  bool needs_clflush_before,
+		  bool needs_clflush_after)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	ssize_t remain;
-	loff_t gtt_page_base, offset;
-	loff_t first_data_page, last_data_page, num_pages;
-	loff_t pinned_pages, i;
-	struct page **user_pages;
-	struct mm_struct *mm = current->mm;
-	int gtt_page_offset, data_page_offset, data_page_index, page_length;
+	char *vaddr;
 	int ret;
-	uint64_t data_ptr = args->data_ptr;
-
-	remain = args->size;
-
-	/* Pin the user pages containing the data.  We can't fault while
-	 * holding the struct mutex, and all of the pwrite implementations
-	 * want to hold it while dereferencing the user data.
-	 */
-	first_data_page = data_ptr / PAGE_SIZE;
-	last_data_page = (data_ptr + args->size - 1) / PAGE_SIZE;
-	num_pages = last_data_page - first_data_page + 1;
-
-	user_pages = drm_malloc_ab(num_pages, sizeof(struct page *));
-	if (user_pages == NULL)
-		return -ENOMEM;
-
-	mutex_unlock(&dev->struct_mutex);
-	down_read(&mm->mmap_sem);
-	pinned_pages = get_user_pages(current, mm, (uintptr_t)args->data_ptr,
-				      num_pages, 0, 0, user_pages, NULL);
-	up_read(&mm->mmap_sem);
-	mutex_lock(&dev->struct_mutex);
-	if (pinned_pages < num_pages) {
-		ret = -EFAULT;
-		goto out_unpin_pages;
-	}
-
-	ret = i915_gem_object_set_to_gtt_domain(obj, true);
-	if (ret)
-		goto out_unpin_pages;
-
-	ret = i915_gem_object_put_fence(obj);
-	if (ret)
-		goto out_unpin_pages;
-
-	offset = obj->gtt_offset + args->offset;
-
-	while (remain > 0) {
-		/* Operation in this page
-		 *
-		 * gtt_page_base = page offset within aperture
-		 * gtt_page_offset = offset within page in aperture
-		 * data_page_index = page number in get_user_pages return
-		 * data_page_offset = offset with data_page_index page.
-		 * page_length = bytes to copy for this page
-		 */
-		gtt_page_base = offset & PAGE_MASK;
-		gtt_page_offset = offset_in_page(offset);
-		data_page_index = data_ptr / PAGE_SIZE - first_data_page;
-		data_page_offset = offset_in_page(data_ptr);
-
-		page_length = remain;
-		if ((gtt_page_offset + page_length) > PAGE_SIZE)
-			page_length = PAGE_SIZE - gtt_page_offset;
-		if ((data_page_offset + page_length) > PAGE_SIZE)
-			page_length = PAGE_SIZE - data_page_offset;
 
-		slow_kernel_write(dev_priv->mm.gtt_mapping,
-				  gtt_page_base, gtt_page_offset,
-				  user_pages[data_page_index],
-				  data_page_offset,
-				  page_length);
-
-		remain -= page_length;
-		offset += page_length;
-		data_ptr += page_length;
-	}
+	if (unlikely(page_do_bit17_swizzling))
+		return -EINVAL;
 
-out_unpin_pages:
-	for (i = 0; i < pinned_pages; i++)
-		page_cache_release(user_pages[i]);
-	drm_free_large(user_pages);
+	vaddr = kmap_atomic(page);
+	if (needs_clflush_before)
+		drm_clflush_virt_range(vaddr + shmem_page_offset,
+				       page_length);
+	ret = __copy_from_user_inatomic_nocache(vaddr + shmem_page_offset,
+						user_data,
+						page_length);
+	if (needs_clflush_after)
+		drm_clflush_virt_range(vaddr + shmem_page_offset,
+				       page_length);
+	kunmap_atomic(vaddr);
 
 	return ret;
 }
 
-/**
- * This is the fast shmem pwrite path, which attempts to directly
- * copy_from_user into the kmapped pages backing the object.
- */
+/* Only difference to the fast-path function is that this can handle bit17
+ * and uses non-atomic copy and kmap functions. */
 static int
-i915_gem_shmem_pwrite_fast(struct drm_device *dev,
-			   struct drm_i915_gem_object *obj,
-			   struct drm_i915_gem_pwrite *args,
-			   struct drm_file *file)
+shmem_pwrite_slow(struct page *page, int shmem_page_offset, int page_length,
+		  char __user *user_data,
+		  bool page_do_bit17_swizzling,
+		  bool needs_clflush_before,
+		  bool needs_clflush_after)
 {
-	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
-	ssize_t remain;
-	loff_t offset;
-	char __user *user_data;
-	int page_offset, page_length;
-
-	user_data = (char __user *) (uintptr_t) args->data_ptr;
-	remain = args->size;
-
-	offset = args->offset;
-	obj->dirty = 1;
-
-	while (remain > 0) {
-		struct page *page;
-		char *vaddr;
-		int ret;
-
-		/* Operation in this page
-		 *
-		 * page_offset = offset within page
-		 * page_length = bytes to copy for this page
-		 */
-		page_offset = offset_in_page(offset);
-		page_length = remain;
-		if ((page_offset + remain) > PAGE_SIZE)
-			page_length = PAGE_SIZE - page_offset;
-
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page))
-			return PTR_ERR(page);
+	char *vaddr;
+	int ret;
 
-		vaddr = kmap_atomic(page);
-		ret = __copy_from_user_inatomic(vaddr + page_offset,
+	vaddr = kmap(page);
+	if (unlikely(needs_clflush_before || page_do_bit17_swizzling))
+		shmem_clflush_swizzled_range(vaddr + shmem_page_offset,
+					     page_length,
+					     page_do_bit17_swizzling);
+	if (page_do_bit17_swizzling)
+		ret = __copy_from_user_swizzled(vaddr, shmem_page_offset,
 						user_data,
 						page_length);
-		kunmap_atomic(vaddr);
-
-		set_page_dirty(page);
-		mark_page_accessed(page);
-		page_cache_release(page);
-
-		/* If we get a fault while copying data, then (presumably) our
-		 * source page isn't available.  Return the error and we'll
-		 * retry in the slow path.
-		 */
-		if (ret)
-			return -EFAULT;
-
-		remain -= page_length;
-		user_data += page_length;
-		offset += page_length;
-	}
+	else
+		ret = __copy_from_user(vaddr + shmem_page_offset,
+				       user_data,
+				       page_length);
+	if (needs_clflush_after)
+		shmem_clflush_swizzled_range(vaddr + shmem_page_offset,
+					     page_length,
+					     page_do_bit17_swizzling);
+	kunmap(page);
 
-	return 0;
+	return ret;
 }
 
-/**
- * This is the fallback shmem pwrite path, which uses get_user_pages to pin
- * the memory and maps it using kmap_atomic for copying.
- *
- * This avoids taking mmap_sem for faulting on the user's address while the
- * struct_mutex is held.
- */
 static int
-i915_gem_shmem_pwrite_slow(struct drm_device *dev,
-			   struct drm_i915_gem_object *obj,
-			   struct drm_i915_gem_pwrite *args,
-			   struct drm_file *file)
+i915_gem_shmem_pwrite(struct drm_device *dev,
+		      struct drm_i915_gem_object *obj,
+		      struct drm_i915_gem_pwrite *args,
+		      struct drm_file *file)
 {
 	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
 	ssize_t remain;
 	loff_t offset;
 	char __user *user_data;
-	int shmem_page_offset, page_length, ret;
+	int shmem_page_offset, page_length, ret = 0;
 	int obj_do_bit17_swizzling, page_do_bit17_swizzling;
+	int hit_slowpath = 0;
+	int needs_clflush_after = 0;
+	int needs_clflush_before = 0;
+	int release_page;
 
 	user_data = (char __user *) (uintptr_t) args->data_ptr;
 	remain = args->size;
 
 	obj_do_bit17_swizzling = i915_gem_object_needs_bit17_swizzle(obj);
 
+	if (obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
+		/* If we're not in the cpu write domain, set ourself into the gtt
+		 * write domain and manually flush cachelines (if required). This
+		 * optimizes for the case when the gpu will use the data
+		 * right away and we therefore have to clflush anyway. */
+		if (obj->cache_level == I915_CACHE_NONE)
+			needs_clflush_after = 1;
+		ret = i915_gem_object_set_to_gtt_domain(obj, true);
+		if (ret)
+			return ret;
+	}
+	/* Same trick applies for invalidate partially written cachelines before
+	 * writing.  */
+	if (!(obj->base.read_domains & I915_GEM_DOMAIN_CPU)
+	    && obj->cache_level == I915_CACHE_NONE)
+		needs_clflush_before = 1;
+
 	offset = args->offset;
 	obj->dirty = 1;
 
-	mutex_unlock(&dev->struct_mutex);
-
 	while (remain > 0) {
 		struct page *page;
-		char *vaddr;
+		int partial_cacheline_write;
 
 		/* Operation in this page
 		 *
@@ -822,29 +771,51 @@ i915_gem_shmem_pwrite_slow(struct drm_device *dev,
 		if ((shmem_page_offset + page_length) > PAGE_SIZE)
 			page_length = PAGE_SIZE - shmem_page_offset;
 
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page)) {
-			ret = PTR_ERR(page);
-			goto out;
+		/* If we don't overwrite a cacheline completely we need to be
+		 * careful to have up-to-date data by first clflushing. Don't
+		 * overcomplicate things and flush the entire patch. */
+		partial_cacheline_write = needs_clflush_before &&
+			((shmem_page_offset | page_length)
+				& (boot_cpu_data.x86_clflush_size - 1));
+
+		if (obj->pages) {
+			page = obj->pages[offset >> PAGE_SHIFT];
+			release_page = 0;
+		} else {
+			page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
+			if (IS_ERR(page)) {
+				ret = PTR_ERR(page);
+				goto out;
+			}
+			release_page = 1;
 		}
 
 		page_do_bit17_swizzling = obj_do_bit17_swizzling &&
 			(page_to_phys(page) & (1 << 17)) != 0;
 
-		vaddr = kmap(page);
-		if (page_do_bit17_swizzling)
-			ret = __copy_from_user_swizzled(vaddr, shmem_page_offset,
-							user_data,
-							page_length);
-		else
-			ret = __copy_from_user(vaddr + shmem_page_offset,
-					       user_data,
-					       page_length);
-		kunmap(page);
+		ret = shmem_pwrite_fast(page, shmem_page_offset, page_length,
+					user_data, page_do_bit17_swizzling,
+					partial_cacheline_write,
+					needs_clflush_after);
+		if (ret == 0)
+			goto next_page;
+
+		hit_slowpath = 1;
+		page_cache_get(page);
+		mutex_unlock(&dev->struct_mutex);
+
+		ret = shmem_pwrite_slow(page, shmem_page_offset, page_length,
+					user_data, page_do_bit17_swizzling,
+					partial_cacheline_write,
+					needs_clflush_after);
 
+		mutex_lock(&dev->struct_mutex);
+		page_cache_release(page);
+next_page:
 		set_page_dirty(page);
 		mark_page_accessed(page);
-		page_cache_release(page);
+		if (release_page)
+			page_cache_release(page);
 
 		if (ret) {
 			ret = -EFAULT;
@@ -857,17 +828,21 @@ i915_gem_shmem_pwrite_slow(struct drm_device *dev,
 	}
 
 out:
-	mutex_lock(&dev->struct_mutex);
-	/* Fixup: Kill any reinstated backing storage pages */
-	if (obj->madv == __I915_MADV_PURGED)
-		i915_gem_object_truncate(obj);
-	/* and flush dirty cachelines in case the object isn't in the cpu write
-	 * domain anymore. */
-	if (obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
-		i915_gem_clflush_object(obj);
-		intel_gtt_chipset_flush();
+	if (hit_slowpath) {
+		/* Fixup: Kill any reinstated backing storage pages */
+		if (obj->madv == __I915_MADV_PURGED)
+			i915_gem_object_truncate(obj);
+		/* and flush dirty cachelines in case the object isn't in the cpu write
+		 * domain anymore. */
+		if (obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
+			i915_gem_clflush_object(obj);
+			intel_gtt_chipset_flush();
+		}
 	}
 
+	if (needs_clflush_after)
+		intel_gtt_chipset_flush();
+
 	return ret;
 }
 
@@ -892,8 +867,8 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 		       args->size))
 		return -EFAULT;
 
-	ret = fault_in_pages_readable((char __user *)(uintptr_t)args->data_ptr,
-				      args->size);
+	ret = fault_in_multipages_readable((char __user *)(uintptr_t)args->data_ptr,
+					   args->size);
 	if (ret)
 		return -EFAULT;
 
@@ -914,8 +889,17 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 		goto out;
 	}
 
+	/* prime objects have no backing filp to GEM pread/pwrite
+	 * pages from.
+	 */
+	if (!obj->base.filp) {
+		ret = -EINVAL;
+		goto out;
+	}
+
 	trace_i915_gem_object_pwrite(obj, args->offset, args->size);
 
+	ret = -EFAULT;
 	/* We can only do the GTT pwrite on untiled buffers, as otherwise
 	 * it would end up going through the fenced access, and we'll get
 	 * different detiling behavior between reading and writing.
@@ -928,42 +912,18 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 	}
 
 	if (obj->gtt_space &&
+	    obj->cache_level == I915_CACHE_NONE &&
+	    obj->tiling_mode == I915_TILING_NONE &&
+	    obj->map_and_fenceable &&
 	    obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
-		ret = i915_gem_object_pin(obj, 0, true);
-		if (ret)
-			goto out;
-
-		ret = i915_gem_object_set_to_gtt_domain(obj, true);
-		if (ret)
-			goto out_unpin;
-
-		ret = i915_gem_object_put_fence(obj);
-		if (ret)
-			goto out_unpin;
-
 		ret = i915_gem_gtt_pwrite_fast(dev, obj, args, file);
-		if (ret == -EFAULT)
-			ret = i915_gem_gtt_pwrite_slow(dev, obj, args, file);
-
-out_unpin:
-		i915_gem_object_unpin(obj);
-
-		if (ret != -EFAULT)
-			goto out;
-		/* Fall through to the shmfs paths because the gtt paths might
-		 * fail with non-page-backed user pointers (e.g. gtt mappings
-		 * when moving data between textures). */
+		/* Note that the gtt paths might fail with non-page-backed user
+		 * pointers (e.g. gtt mappings when moving data between
+		 * textures). Fallback to the shmem path in that case. */
 	}
 
-	ret = i915_gem_object_set_to_cpu_domain(obj, 1);
-	if (ret)
-		goto out;
-
-	ret = -EFAULT;
-	if (!i915_gem_object_needs_bit17_swizzle(obj))
-		ret = i915_gem_shmem_pwrite_fast(dev, obj, args, file);
 	if (ret == -EFAULT)
-		ret = i915_gem_shmem_pwrite_slow(dev, obj, args, file);
+		ret = i915_gem_shmem_pwrite(dev, obj, args, file);
 
 out:
 	drm_gem_object_unreference(&obj->base);
@@ -986,9 +946,6 @@ i915_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	uint32_t write_domain = args->write_domain;
 	int ret;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	/* Only handle setting domains to types used by the CPU. */
 	if (write_domain & I915_GEM_GPU_DOMAINS)
 		return -EINVAL;
@@ -1042,9 +999,6 @@ i915_gem_sw_finish_ioctl(struct drm_device *dev, void *data,
 	struct drm_i915_gem_object *obj;
 	int ret = 0;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -1080,13 +1034,18 @@ i915_gem_mmap_ioctl(struct drm_device *dev, void *data,
 	struct drm_gem_object *obj;
 	unsigned long addr;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	obj = drm_gem_object_lookup(dev, file, args->handle);
 	if (obj == NULL)
 		return -ENOENT;
 
+	/* prime objects have no backing filp to GEM mmap
+	 * pages from.
+	 */
+	if (!obj->filp) {
+		drm_gem_object_unreference_unlocked(obj);
+		return -EINVAL;
+	}
+
 	addr = vm_mmap(obj->filp, 0, args->size,
 		       PROT_READ | PROT_WRITE, MAP_SHARED,
 		       args->offset);
@@ -1151,10 +1110,10 @@ int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 			goto unlock;
 	}
 
-	if (obj->tiling_mode == I915_TILING_NONE)
-		ret = i915_gem_object_put_fence(obj);
-	else
-		ret = i915_gem_object_get_fence(obj, NULL);
+	if (!obj->has_global_gtt_mapping)
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
+
+	ret = i915_gem_object_get_fence(obj);
 	if (ret)
 		goto unlock;
 
@@ -1308,9 +1267,6 @@ i915_gem_mmap_gtt(struct drm_file *file,
 	struct drm_i915_gem_object *obj;
 	int ret;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -1368,14 +1324,10 @@ i915_gem_mmap_gtt_ioctl(struct drm_device *dev, void *data,
 {
 	struct drm_i915_gem_mmap_gtt *args = data;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	return i915_gem_mmap_gtt(file, dev, args->handle, &args->offset);
 }
 
-
-static int
+int
 i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj,
 			      gfp_t gfpmask)
 {
@@ -1384,6 +1336,9 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj,
 	struct inode *inode;
 	struct page *page;
 
+	if (obj->pages || obj->sg_table)
+		return 0;
+
 	/* Get the list of pages out of our struct file.  They'll be pinned
 	 * at this point until we release them.
 	 */
@@ -1425,6 +1380,9 @@ i915_gem_object_put_pages_gtt(struct drm_i915_gem_object *obj)
 	int page_count = obj->base.size / PAGE_SIZE;
 	int i;
 
+	if (!obj->pages)
+		return;
+
 	BUG_ON(obj->madv == __I915_MADV_PURGED);
 
 	if (i915_gem_object_needs_bit17_swizzle(obj))
@@ -1473,7 +1431,6 @@ i915_gem_object_move_to_active(struct drm_i915_gem_object *obj,
 
 	if (obj->fenced_gpu_access) {
 		obj->last_fenced_seqno = seqno;
-		obj->last_fenced_ring = ring;
 
 		/* Bump MRU to take account of the delayed flush */
 		if (obj->fence_reg != I915_FENCE_REG_NONE) {
@@ -1512,15 +1469,11 @@ i915_gem_object_move_to_inactive(struct drm_i915_gem_object *obj)
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (obj->pin_count != 0)
-		list_move_tail(&obj->mm_list, &dev_priv->mm.pinned_list);
-	else
-		list_move_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
+	list_move_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
 
 	BUG_ON(!list_empty(&obj->gpu_write_list));
 	BUG_ON(!obj->active);
 	obj->ring = NULL;
-	obj->last_fenced_ring = NULL;
 
 	i915_gem_object_move_off_active(obj);
 	obj->fenced_gpu_access = false;
@@ -1546,6 +1499,9 @@ i915_gem_object_truncate(struct drm_i915_gem_object *obj)
 	inode = obj->base.filp->f_path.dentry->d_inode;
 	shmem_truncate_range(inode, 0, (loff_t)-1);
 
+	if (obj->base.map_list.map)
+		drm_gem_free_mmap_offset(&obj->base);
+
 	obj->madv = __I915_MADV_PURGED;
 }
 
@@ -1711,30 +1667,29 @@ static void i915_gem_reset_fences(struct drm_device *dev)
 
 	for (i = 0; i < dev_priv->num_fence_regs; i++) {
 		struct drm_i915_fence_reg *reg = &dev_priv->fence_regs[i];
-		struct drm_i915_gem_object *obj = reg->obj;
 
-		if (!obj)
-			continue;
+		i915_gem_write_fence(dev, i, NULL);
 
-		if (obj->tiling_mode)
-			i915_gem_release_mmap(obj);
+		if (reg->obj)
+			i915_gem_object_fence_lost(reg->obj);
 
-		reg->obj->fence_reg = I915_FENCE_REG_NONE;
-		reg->obj->fenced_gpu_access = false;
-		reg->obj->last_fenced_seqno = 0;
-		reg->obj->last_fenced_ring = NULL;
-		i915_gem_clear_fence_reg(dev, reg);
+		reg->pin_count = 0;
+		reg->obj = NULL;
+		INIT_LIST_HEAD(&reg->lru_list);
 	}
+
+	INIT_LIST_HEAD(&dev_priv->mm.fence_list);
 }
 
 void i915_gem_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj;
+	struct intel_ring_buffer *ring;
 	int i;
 
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		i915_gem_reset_ring_lists(dev_priv, &dev_priv->ring[i]);
+	for_each_ring(ring, dev_priv, i)
+		i915_gem_reset_ring_lists(dev_priv, ring);
 
 	/* Remove anything from the flushing lists. The GPU cache is likely
 	 * to be lost on reset along with the data, so simply move the
@@ -1839,24 +1794,11 @@ void
 i915_gem_retire_requests(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	int i;
 
-	if (!list_empty(&dev_priv->mm.deferred_free_list)) {
-	    struct drm_i915_gem_object *obj, *next;
-
-	    /* We must be careful that during unbind() we do not
-	     * accidentally infinitely recurse into retire requests.
-	     * Currently:
-	     *   retire -> free -> unbind -> wait -> retire_ring
-	     */
-	    list_for_each_entry_safe(obj, next,
-				     &dev_priv->mm.deferred_free_list,
-				     mm_list)
-		    i915_gem_free_object_tail(obj);
-	}
-
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		i915_gem_retire_requests_ring(&dev_priv->ring[i]);
+	for_each_ring(ring, dev_priv, i)
+		i915_gem_retire_requests_ring(ring);
 }
 
 static void
@@ -1864,6 +1806,7 @@ i915_gem_retire_work_handler(struct work_struct *work)
 {
 	drm_i915_private_t *dev_priv;
 	struct drm_device *dev;
+	struct intel_ring_buffer *ring;
 	bool idle;
 	int i;
 
@@ -1883,9 +1826,7 @@ i915_gem_retire_work_handler(struct work_struct *work)
 	 * objects indefinitely.
 	 */
 	idle = true;
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		struct intel_ring_buffer *ring = &dev_priv->ring[i];
-
+	for_each_ring(ring, dev_priv, i) {
 		if (!list_empty(&ring->gpu_write_list)) {
 			struct drm_i915_gem_request *request;
 			int ret;
@@ -1907,20 +1848,10 @@ i915_gem_retire_work_handler(struct work_struct *work)
 	mutex_unlock(&dev->struct_mutex);
 }
 
-/**
- * Waits for a sequence number to be signaled, and cleans up the
- * request and object lists appropriately for that event.
- */
-int
-i915_wait_request(struct intel_ring_buffer *ring,
-		  uint32_t seqno,
-		  bool do_retire)
+static int
+i915_gem_check_wedge(struct drm_i915_private *dev_priv)
 {
-	drm_i915_private_t *dev_priv = ring->dev->dev_private;
-	u32 ier;
-	int ret = 0;
-
-	BUG_ON(seqno == 0);
+	BUG_ON(!mutex_is_locked(&dev_priv->dev->struct_mutex));
 
 	if (atomic_read(&dev_priv->mm.wedged)) {
 		struct completion *x = &dev_priv->error_completion;
@@ -1935,6 +1866,20 @@ i915_wait_request(struct intel_ring_buffer *ring,
 		return recovery_complete ? -EIO : -EAGAIN;
 	}
 
+	return 0;
+}
+
+/*
+ * Compare seqno against outstanding lazy request. Emit a request if they are
+ * equal.
+ */
+static int
+i915_gem_check_olr(struct intel_ring_buffer *ring, u32 seqno)
+{
+	int ret = 0;
+
+	BUG_ON(!mutex_is_locked(&ring->dev->struct_mutex));
+
 	if (seqno == ring->outstanding_lazy_request) {
 		struct drm_i915_gem_request *request;
 
@@ -1948,54 +1893,67 @@ i915_wait_request(struct intel_ring_buffer *ring,
 			return ret;
 		}
 
-		seqno = request->seqno;
+		BUG_ON(seqno != request->seqno);
 	}
 
-	if (!i915_seqno_passed(ring->get_seqno(ring), seqno)) {
-		if (HAS_PCH_SPLIT(ring->dev))
-			ier = I915_READ(DEIER) | I915_READ(GTIER);
-		else
-			ier = I915_READ(IER);
-		if (!ier) {
-			DRM_ERROR("something (likely vbetool) disabled "
-				  "interrupts, re-enabling\n");
-			ring->dev->driver->irq_preinstall(ring->dev);
-			ring->dev->driver->irq_postinstall(ring->dev);
-		}
+	return ret;
+}
 
-		trace_i915_gem_request_wait_begin(ring, seqno);
-
-		ring->waiting_seqno = seqno;
-		if (ring->irq_get(ring)) {
-			if (dev_priv->mm.interruptible)
-				ret = wait_event_interruptible(ring->irq_queue,
-							       i915_seqno_passed(ring->get_seqno(ring), seqno)
-							       || atomic_read(&dev_priv->mm.wedged));
-			else
-				wait_event(ring->irq_queue,
-					   i915_seqno_passed(ring->get_seqno(ring), seqno)
-					   || atomic_read(&dev_priv->mm.wedged));
-
-			ring->irq_put(ring);
-		} else if (wait_for_atomic(i915_seqno_passed(ring->get_seqno(ring),
-							     seqno) ||
-					   atomic_read(&dev_priv->mm.wedged), 3000))
-			ret = -EBUSY;
-		ring->waiting_seqno = 0;
-
-		trace_i915_gem_request_wait_end(ring, seqno);
-	}
+static int __wait_seqno(struct intel_ring_buffer *ring, u32 seqno,
+			bool interruptible)
+{
+	drm_i915_private_t *dev_priv = ring->dev->dev_private;
+	int ret = 0;
+
+	if (i915_seqno_passed(ring->get_seqno(ring), seqno))
+		return 0;
+
+	trace_i915_gem_request_wait_begin(ring, seqno);
+	if (WARN_ON(!ring->irq_get(ring)))
+		return -ENODEV;
+
+#define EXIT_COND \
+	(i915_seqno_passed(ring->get_seqno(ring), seqno) || \
+	atomic_read(&dev_priv->mm.wedged))
+
+	if (interruptible)
+		ret = wait_event_interruptible(ring->irq_queue,
+					       EXIT_COND);
+	else
+		wait_event(ring->irq_queue, EXIT_COND);
+
+	ring->irq_put(ring);
+	trace_i915_gem_request_wait_end(ring, seqno);
+#undef EXIT_COND
+
+	return ret;
+}
+
+/**
+ * Waits for a sequence number to be signaled, and cleans up the
+ * request and object lists appropriately for that event.
+ */
+int
+i915_wait_request(struct intel_ring_buffer *ring,
+		  uint32_t seqno)
+{
+	drm_i915_private_t *dev_priv = ring->dev->dev_private;
+	int ret = 0;
+
+	BUG_ON(seqno == 0);
+
+	ret = i915_gem_check_wedge(dev_priv);
+	if (ret)
+		return ret;
+
+	ret = i915_gem_check_olr(ring, seqno);
+	if (ret)
+		return ret;
+
+	ret = __wait_seqno(ring, seqno, dev_priv->mm.interruptible);
 	if (atomic_read(&dev_priv->mm.wedged))
 		ret = -EAGAIN;
 
-	/* Directly dispatch request retiring.  While we have the work queue
-	 * to handle this, the waiter on a request often wants an associated
-	 * buffer to have made it to the inactive list, and we would need
-	 * a separate wait queue to handle that.
-	 */
-	if (ret == 0 && do_retire)
-		i915_gem_retire_requests_ring(ring);
-
 	return ret;
 }
 
@@ -2017,15 +1975,58 @@ i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj)
 	 * it.
 	 */
 	if (obj->active) {
-		ret = i915_wait_request(obj->ring, obj->last_rendering_seqno,
-					true);
+		ret = i915_wait_request(obj->ring, obj->last_rendering_seqno);
 		if (ret)
 			return ret;
+		i915_gem_retire_requests_ring(obj->ring);
 	}
 
 	return 0;
 }
 
+/**
+ * i915_gem_object_sync - sync an object to a ring.
+ *
+ * @obj: object which may be in use on another ring.
+ * @to: ring we wish to use the object on. May be NULL.
+ *
+ * This code is meant to abstract object synchronization with the GPU.
+ * Calling with NULL implies synchronizing the object with the CPU
+ * rather than a particular GPU ring.
+ *
+ * Returns 0 if successful, else propagates up the lower layer error.
+ */
+int
+i915_gem_object_sync(struct drm_i915_gem_object *obj,
+		     struct intel_ring_buffer *to)
+{
+	struct intel_ring_buffer *from = obj->ring;
+	u32 seqno;
+	int ret, idx;
+
+	if (from == NULL || to == from)
+		return 0;
+
+	if (to == NULL || !i915_semaphore_is_enabled(obj->base.dev))
+		return i915_gem_object_wait_rendering(obj);
+
+	idx = intel_ring_sync_index(from, to);
+
+	seqno = obj->last_rendering_seqno;
+	if (seqno <= from->sync_seqno[idx])
+		return 0;
+
+	ret = i915_gem_check_olr(obj->ring, seqno);
+	if (ret)
+		return ret;
+
+	ret = to->sync_to(to, from, seqno);
+	if (!ret)
+		from->sync_seqno[idx] = seqno;
+
+	return ret;
+}
+
 static void i915_gem_object_finish_gtt(struct drm_i915_gem_object *obj)
 {
 	u32 old_write_domain, old_read_domains;
@@ -2068,7 +2069,7 @@ i915_gem_object_unbind(struct drm_i915_gem_object *obj)
 	}
 
 	ret = i915_gem_object_finish_gpu(obj);
-	if (ret == -ERESTARTSYS)
+	if (ret)
 		return ret;
 	/* Continue on if we fail due to EIO, the GPU is hung so we
 	 * should be safe and we need to cleanup or else we might
@@ -2095,16 +2096,18 @@ i915_gem_object_unbind(struct drm_i915_gem_object *obj)
 
 	/* release the fence reg _after_ flushing */
 	ret = i915_gem_object_put_fence(obj);
-	if (ret == -ERESTARTSYS)
+	if (ret)
 		return ret;
 
 	trace_i915_gem_object_unbind(obj);
 
-	i915_gem_gtt_unbind_object(obj);
+	if (obj->has_global_gtt_mapping)
+		i915_gem_gtt_unbind_object(obj);
 	if (obj->has_aliasing_ppgtt_mapping) {
 		i915_ppgtt_unbind_object(dev_priv->mm.aliasing_ppgtt, obj);
 		obj->has_aliasing_ppgtt_mapping = 0;
 	}
+	i915_gem_gtt_finish_object(obj);
 
 	i915_gem_object_put_pages_gtt(obj);
 
@@ -2145,7 +2148,7 @@ i915_gem_flush_ring(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static int i915_ring_idle(struct intel_ring_buffer *ring, bool do_retire)
+static int i915_ring_idle(struct intel_ring_buffer *ring)
 {
 	int ret;
 
@@ -2159,208 +2162,201 @@ static int i915_ring_idle(struct intel_ring_buffer *ring, bool do_retire)
 			return ret;
 	}
 
-	return i915_wait_request(ring, i915_gem_next_request_seqno(ring),
-				 do_retire);
+	return i915_wait_request(ring, i915_gem_next_request_seqno(ring));
 }
 
-int i915_gpu_idle(struct drm_device *dev, bool do_retire)
+int i915_gpu_idle(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	int ret, i;
 
 	/* Flush everything onto the inactive list. */
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		ret = i915_ring_idle(&dev_priv->ring[i], do_retire);
+	for_each_ring(ring, dev_priv, i) {
+		ret = i915_ring_idle(ring);
 		if (ret)
 			return ret;
+
+		/* Is the device fubar? */
+		if (WARN_ON(!list_empty(&ring->gpu_write_list)))
+			return -EBUSY;
 	}
 
 	return 0;
 }
 
-static int sandybridge_write_fence_reg(struct drm_i915_gem_object *obj,
-				       struct intel_ring_buffer *pipelined)
+static void sandybridge_write_fence_reg(struct drm_device *dev, int reg,
+					struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	int regnum = obj->fence_reg;
 	uint64_t val;
 
-	val = (uint64_t)((obj->gtt_offset + size - 4096) &
-			 0xfffff000) << 32;
-	val |= obj->gtt_offset & 0xfffff000;
-	val |= (uint64_t)((obj->stride / 128) - 1) <<
-		SANDYBRIDGE_FENCE_PITCH_SHIFT;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
 
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I965_FENCE_TILING_Y_SHIFT;
-	val |= I965_FENCE_REG_VALID;
+		val = (uint64_t)((obj->gtt_offset + size - 4096) &
+				 0xfffff000) << 32;
+		val |= obj->gtt_offset & 0xfffff000;
+		val |= (uint64_t)((obj->stride / 128) - 1) <<
+			SANDYBRIDGE_FENCE_PITCH_SHIFT;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 6);
-		if (ret)
-			return ret;
-
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(2));
-		intel_ring_emit(pipelined, FENCE_REG_SANDYBRIDGE_0 + regnum*8);
-		intel_ring_emit(pipelined, (u32)val);
-		intel_ring_emit(pipelined, FENCE_REG_SANDYBRIDGE_0 + regnum*8 + 4);
-		intel_ring_emit(pipelined, (u32)(val >> 32));
-		intel_ring_advance(pipelined);
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I965_FENCE_TILING_Y_SHIFT;
+		val |= I965_FENCE_REG_VALID;
 	} else
-		I915_WRITE64(FENCE_REG_SANDYBRIDGE_0 + regnum * 8, val);
+		val = 0;
 
-	return 0;
+	I915_WRITE64(FENCE_REG_SANDYBRIDGE_0 + reg * 8, val);
+	POSTING_READ(FENCE_REG_SANDYBRIDGE_0 + reg * 8);
 }
 
-static int i965_write_fence_reg(struct drm_i915_gem_object *obj,
-				struct intel_ring_buffer *pipelined)
+static void i965_write_fence_reg(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	int regnum = obj->fence_reg;
 	uint64_t val;
 
-	val = (uint64_t)((obj->gtt_offset + size - 4096) &
-		    0xfffff000) << 32;
-	val |= obj->gtt_offset & 0xfffff000;
-	val |= ((obj->stride / 128) - 1) << I965_FENCE_PITCH_SHIFT;
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I965_FENCE_TILING_Y_SHIFT;
-	val |= I965_FENCE_REG_VALID;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 6);
-		if (ret)
-			return ret;
-
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(2));
-		intel_ring_emit(pipelined, FENCE_REG_965_0 + regnum*8);
-		intel_ring_emit(pipelined, (u32)val);
-		intel_ring_emit(pipelined, FENCE_REG_965_0 + regnum*8 + 4);
-		intel_ring_emit(pipelined, (u32)(val >> 32));
-		intel_ring_advance(pipelined);
+		val = (uint64_t)((obj->gtt_offset + size - 4096) &
+				 0xfffff000) << 32;
+		val |= obj->gtt_offset & 0xfffff000;
+		val |= ((obj->stride / 128) - 1) << I965_FENCE_PITCH_SHIFT;
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I965_FENCE_TILING_Y_SHIFT;
+		val |= I965_FENCE_REG_VALID;
 	} else
-		I915_WRITE64(FENCE_REG_965_0 + regnum * 8, val);
+		val = 0;
 
-	return 0;
+	I915_WRITE64(FENCE_REG_965_0 + reg * 8, val);
+	POSTING_READ(FENCE_REG_965_0 + reg * 8);
 }
 
-static int i915_write_fence_reg(struct drm_i915_gem_object *obj,
-				struct intel_ring_buffer *pipelined)
+static void i915_write_fence_reg(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	u32 fence_reg, val, pitch_val;
-	int tile_width;
-
-	if (WARN((obj->gtt_offset & ~I915_FENCE_START_MASK) ||
-		 (size & -size) != size ||
-		 (obj->gtt_offset & (size - 1)),
-		 "object 0x%08x [fenceable? %d] not 1M or pot-size (0x%08x) aligned\n",
-		 obj->gtt_offset, obj->map_and_fenceable, size))
-		return -EINVAL;
+	u32 val;
 
-	if (obj->tiling_mode == I915_TILING_Y && HAS_128_BYTE_Y_TILING(dev))
-		tile_width = 128;
-	else
-		tile_width = 512;
-
-	/* Note: pitch better be a power of two tile widths */
-	pitch_val = obj->stride / tile_width;
-	pitch_val = ffs(pitch_val) - 1;
-
-	val = obj->gtt_offset;
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I830_FENCE_TILING_Y_SHIFT;
-	val |= I915_FENCE_SIZE_BITS(size);
-	val |= pitch_val << I830_FENCE_PITCH_SHIFT;
-	val |= I830_FENCE_REG_VALID;
-
-	fence_reg = obj->fence_reg;
-	if (fence_reg < 8)
-		fence_reg = FENCE_REG_830_0 + fence_reg * 4;
-	else
-		fence_reg = FENCE_REG_945_8 + (fence_reg - 8) * 4;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
+		int pitch_val;
+		int tile_width;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 4);
-		if (ret)
-			return ret;
+		WARN((obj->gtt_offset & ~I915_FENCE_START_MASK) ||
+		     (size & -size) != size ||
+		     (obj->gtt_offset & (size - 1)),
+		     "object 0x%08x [fenceable? %d] not 1M or pot-size (0x%08x) aligned\n",
+		     obj->gtt_offset, obj->map_and_fenceable, size);
 
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(pipelined, fence_reg);
-		intel_ring_emit(pipelined, val);
-		intel_ring_advance(pipelined);
+		if (obj->tiling_mode == I915_TILING_Y && HAS_128_BYTE_Y_TILING(dev))
+			tile_width = 128;
+		else
+			tile_width = 512;
+
+		/* Note: pitch better be a power of two tile widths */
+		pitch_val = obj->stride / tile_width;
+		pitch_val = ffs(pitch_val) - 1;
+
+		val = obj->gtt_offset;
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I830_FENCE_TILING_Y_SHIFT;
+		val |= I915_FENCE_SIZE_BITS(size);
+		val |= pitch_val << I830_FENCE_PITCH_SHIFT;
+		val |= I830_FENCE_REG_VALID;
 	} else
-		I915_WRITE(fence_reg, val);
+		val = 0;
 
-	return 0;
+	if (reg < 8)
+		reg = FENCE_REG_830_0 + reg * 4;
+	else
+		reg = FENCE_REG_945_8 + (reg - 8) * 4;
+
+	I915_WRITE(reg, val);
+	POSTING_READ(reg);
 }
 
-static int i830_write_fence_reg(struct drm_i915_gem_object *obj,
-				struct intel_ring_buffer *pipelined)
+static void i830_write_fence_reg(struct drm_device *dev, int reg,
+				struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	int regnum = obj->fence_reg;
 	uint32_t val;
-	uint32_t pitch_val;
 
-	if (WARN((obj->gtt_offset & ~I830_FENCE_START_MASK) ||
-		 (size & -size) != size ||
-		 (obj->gtt_offset & (size - 1)),
-		 "object 0x%08x not 512K or pot-size 0x%08x aligned\n",
-		 obj->gtt_offset, size))
-		return -EINVAL;
-
-	pitch_val = obj->stride / 128;
-	pitch_val = ffs(pitch_val) - 1;
-
-	val = obj->gtt_offset;
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I830_FENCE_TILING_Y_SHIFT;
-	val |= I830_FENCE_SIZE_BITS(size);
-	val |= pitch_val << I830_FENCE_PITCH_SHIFT;
-	val |= I830_FENCE_REG_VALID;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
+		uint32_t pitch_val;
+
+		WARN((obj->gtt_offset & ~I830_FENCE_START_MASK) ||
+		     (size & -size) != size ||
+		     (obj->gtt_offset & (size - 1)),
+		     "object 0x%08x not 512K or pot-size 0x%08x aligned\n",
+		     obj->gtt_offset, size);
+
+		pitch_val = obj->stride / 128;
+		pitch_val = ffs(pitch_val) - 1;
+
+		val = obj->gtt_offset;
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I830_FENCE_TILING_Y_SHIFT;
+		val |= I830_FENCE_SIZE_BITS(size);
+		val |= pitch_val << I830_FENCE_PITCH_SHIFT;
+		val |= I830_FENCE_REG_VALID;
+	} else
+		val = 0;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 4);
-		if (ret)
-			return ret;
+	I915_WRITE(FENCE_REG_830_0 + reg * 4, val);
+	POSTING_READ(FENCE_REG_830_0 + reg * 4);
+}
 
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(pipelined, FENCE_REG_830_0 + regnum*4);
-		intel_ring_emit(pipelined, val);
-		intel_ring_advance(pipelined);
-	} else
-		I915_WRITE(FENCE_REG_830_0 + regnum * 4, val);
+static void i915_gem_write_fence(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj)
+{
+	switch (INTEL_INFO(dev)->gen) {
+	case 7:
+	case 6: sandybridge_write_fence_reg(dev, reg, obj); break;
+	case 5:
+	case 4: i965_write_fence_reg(dev, reg, obj); break;
+	case 3: i915_write_fence_reg(dev, reg, obj); break;
+	case 2: i830_write_fence_reg(dev, reg, obj); break;
+	default: break;
+	}
+}
 
-	return 0;
+static inline int fence_number(struct drm_i915_private *dev_priv,
+			       struct drm_i915_fence_reg *fence)
+{
+	return fence - dev_priv->fence_regs;
 }
 
-static bool ring_passed_seqno(struct intel_ring_buffer *ring, u32 seqno)
+static void i915_gem_object_update_fence(struct drm_i915_gem_object *obj,
+					 struct drm_i915_fence_reg *fence,
+					 bool enable)
 {
-	return i915_seqno_passed(ring->get_seqno(ring), seqno);
+	struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
+	int reg = fence_number(dev_priv, fence);
+
+	i915_gem_write_fence(obj->base.dev, reg, enable ? obj : NULL);
+
+	if (enable) {
+		obj->fence_reg = reg;
+		fence->obj = obj;
+		list_move_tail(&fence->lru_list, &dev_priv->mm.fence_list);
+	} else {
+		obj->fence_reg = I915_FENCE_REG_NONE;
+		fence->obj = NULL;
+		list_del_init(&fence->lru_list);
+	}
 }
 
 static int
-i915_gem_object_flush_fence(struct drm_i915_gem_object *obj,
-			    struct intel_ring_buffer *pipelined)
+i915_gem_object_flush_fence(struct drm_i915_gem_object *obj)
 {
 	int ret;
 
 	if (obj->fenced_gpu_access) {
 		if (obj->base.write_domain & I915_GEM_GPU_DOMAINS) {
-			ret = i915_gem_flush_ring(obj->last_fenced_ring,
+			ret = i915_gem_flush_ring(obj->ring,
 						  0, obj->base.write_domain);
 			if (ret)
 				return ret;
@@ -2369,18 +2365,12 @@ i915_gem_object_flush_fence(struct drm_i915_gem_object *obj,
 		obj->fenced_gpu_access = false;
 	}
 
-	if (obj->last_fenced_seqno && pipelined != obj->last_fenced_ring) {
-		if (!ring_passed_seqno(obj->last_fenced_ring,
-				       obj->last_fenced_seqno)) {
-			ret = i915_wait_request(obj->last_fenced_ring,
-						obj->last_fenced_seqno,
-						true);
-			if (ret)
-				return ret;
-		}
+	if (obj->last_fenced_seqno) {
+		ret = i915_wait_request(obj->ring, obj->last_fenced_seqno);
+		if (ret)
+			return ret;
 
 		obj->last_fenced_seqno = 0;
-		obj->last_fenced_ring = NULL;
 	}
 
 	/* Ensure that all CPU reads are completed before installing a fence
@@ -2395,34 +2385,29 @@ i915_gem_object_flush_fence(struct drm_i915_gem_object *obj,
 int
 i915_gem_object_put_fence(struct drm_i915_gem_object *obj)
 {
+	struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
 	int ret;
 
-	if (obj->tiling_mode)
-		i915_gem_release_mmap(obj);
-
-	ret = i915_gem_object_flush_fence(obj, NULL);
+	ret = i915_gem_object_flush_fence(obj);
 	if (ret)
 		return ret;
 
-	if (obj->fence_reg != I915_FENCE_REG_NONE) {
-		struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
-
-		WARN_ON(dev_priv->fence_regs[obj->fence_reg].pin_count);
-		i915_gem_clear_fence_reg(obj->base.dev,
-					 &dev_priv->fence_regs[obj->fence_reg]);
+	if (obj->fence_reg == I915_FENCE_REG_NONE)
+		return 0;
 
-		obj->fence_reg = I915_FENCE_REG_NONE;
-	}
+	i915_gem_object_update_fence(obj,
+				     &dev_priv->fence_regs[obj->fence_reg],
+				     false);
+	i915_gem_object_fence_lost(obj);
 
 	return 0;
 }
 
 static struct drm_i915_fence_reg *
-i915_find_fence_reg(struct drm_device *dev,
-		    struct intel_ring_buffer *pipelined)
+i915_find_fence_reg(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_fence_reg *reg, *first, *avail;
+	struct drm_i915_fence_reg *reg, *avail;
 	int i;
 
 	/* First try to find a free reg */
@@ -2440,204 +2425,77 @@ i915_find_fence_reg(struct drm_device *dev,
 		return NULL;
 
 	/* None available, try to steal one or wait for a user to finish */
-	avail = first = NULL;
 	list_for_each_entry(reg, &dev_priv->mm.fence_list, lru_list) {
 		if (reg->pin_count)
 			continue;
 
-		if (first == NULL)
-			first = reg;
-
-		if (!pipelined ||
-		    !reg->obj->last_fenced_ring ||
-		    reg->obj->last_fenced_ring == pipelined) {
-			avail = reg;
-			break;
-		}
+		return reg;
 	}
 
-	if (avail == NULL)
-		avail = first;
-
-	return avail;
+	return NULL;
 }
 
 /**
- * i915_gem_object_get_fence - set up a fence reg for an object
+ * i915_gem_object_get_fence - set up fencing for an object
  * @obj: object to map through a fence reg
- * @pipelined: ring on which to queue the change, or NULL for CPU access
- * @interruptible: must we wait uninterruptibly for the register to retire?
  *
  * When mapping objects through the GTT, userspace wants to be able to write
  * to them without having to worry about swizzling if the object is tiled.
- *
  * This function walks the fence regs looking for a free one for @obj,
  * stealing one if it can't find any.
  *
  * It then sets up the reg based on the object's properties: address, pitch
  * and tiling format.
+ *
+ * For an untiled surface, this removes any existing fence.
  */
 int
-i915_gem_object_get_fence(struct drm_i915_gem_object *obj,
-			  struct intel_ring_buffer *pipelined)
+i915_gem_object_get_fence(struct drm_i915_gem_object *obj)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	bool enable = obj->tiling_mode != I915_TILING_NONE;
 	struct drm_i915_fence_reg *reg;
 	int ret;
 
-	/* XXX disable pipelining. There are bugs. Shocking. */
-	pipelined = NULL;
+	/* Have we updated the tiling parameters upon the object and so
+	 * will need to serialise the write to the associated fence register?
+	 */
+	if (obj->fence_dirty) {
+		ret = i915_gem_object_flush_fence(obj);
+		if (ret)
+			return ret;
+	}
 
 	/* Just update our place in the LRU if our fence is getting reused. */
 	if (obj->fence_reg != I915_FENCE_REG_NONE) {
 		reg = &dev_priv->fence_regs[obj->fence_reg];
-		list_move_tail(&reg->lru_list, &dev_priv->mm.fence_list);
-
-		if (obj->tiling_changed) {
-			ret = i915_gem_object_flush_fence(obj, pipelined);
-			if (ret)
-				return ret;
-
-			if (!obj->fenced_gpu_access && !obj->last_fenced_seqno)
-				pipelined = NULL;
-
-			if (pipelined) {
-				reg->setup_seqno =
-					i915_gem_next_request_seqno(pipelined);
-				obj->last_fenced_seqno = reg->setup_seqno;
-				obj->last_fenced_ring = pipelined;
-			}
-
-			goto update;
+		if (!obj->fence_dirty) {
+			list_move_tail(&reg->lru_list,
+				       &dev_priv->mm.fence_list);
+			return 0;
 		}
+	} else if (enable) {
+		reg = i915_find_fence_reg(dev);
+		if (reg == NULL)
+			return -EDEADLK;
 
-		if (!pipelined) {
-			if (reg->setup_seqno) {
-				if (!ring_passed_seqno(obj->last_fenced_ring,
-						       reg->setup_seqno)) {
-					ret = i915_wait_request(obj->last_fenced_ring,
-								reg->setup_seqno,
-								true);
-					if (ret)
-						return ret;
-				}
+		if (reg->obj) {
+			struct drm_i915_gem_object *old = reg->obj;
 
-				reg->setup_seqno = 0;
-			}
-		} else if (obj->last_fenced_ring &&
-			   obj->last_fenced_ring != pipelined) {
-			ret = i915_gem_object_flush_fence(obj, pipelined);
+			ret = i915_gem_object_flush_fence(old);
 			if (ret)
 				return ret;
-		}
-
-		return 0;
-	}
-
-	reg = i915_find_fence_reg(dev, pipelined);
-	if (reg == NULL)
-		return -EDEADLK;
-
-	ret = i915_gem_object_flush_fence(obj, pipelined);
-	if (ret)
-		return ret;
-
-	if (reg->obj) {
-		struct drm_i915_gem_object *old = reg->obj;
-
-		drm_gem_object_reference(&old->base);
-
-		if (old->tiling_mode)
-			i915_gem_release_mmap(old);
 
-		ret = i915_gem_object_flush_fence(old, pipelined);
-		if (ret) {
-			drm_gem_object_unreference(&old->base);
-			return ret;
+			i915_gem_object_fence_lost(old);
 		}
+	} else
+		return 0;
 
-		if (old->last_fenced_seqno == 0 && obj->last_fenced_seqno == 0)
-			pipelined = NULL;
-
-		old->fence_reg = I915_FENCE_REG_NONE;
-		old->last_fenced_ring = pipelined;
-		old->last_fenced_seqno =
-			pipelined ? i915_gem_next_request_seqno(pipelined) : 0;
-
-		drm_gem_object_unreference(&old->base);
-	} else if (obj->last_fenced_seqno == 0)
-		pipelined = NULL;
-
-	reg->obj = obj;
-	list_move_tail(&reg->lru_list, &dev_priv->mm.fence_list);
-	obj->fence_reg = reg - dev_priv->fence_regs;
-	obj->last_fenced_ring = pipelined;
-
-	reg->setup_seqno =
-		pipelined ? i915_gem_next_request_seqno(pipelined) : 0;
-	obj->last_fenced_seqno = reg->setup_seqno;
-
-update:
-	obj->tiling_changed = false;
-	switch (INTEL_INFO(dev)->gen) {
-	case 7:
-	case 6:
-		ret = sandybridge_write_fence_reg(obj, pipelined);
-		break;
-	case 5:
-	case 4:
-		ret = i965_write_fence_reg(obj, pipelined);
-		break;
-	case 3:
-		ret = i915_write_fence_reg(obj, pipelined);
-		break;
-	case 2:
-		ret = i830_write_fence_reg(obj, pipelined);
-		break;
-	}
-
-	return ret;
-}
-
-/**
- * i915_gem_clear_fence_reg - clear out fence register info
- * @obj: object to clear
- *
- * Zeroes out the fence register itself and clears out the associated
- * data structures in dev_priv and obj.
- */
-static void
-i915_gem_clear_fence_reg(struct drm_device *dev,
-			 struct drm_i915_fence_reg *reg)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	uint32_t fence_reg = reg - dev_priv->fence_regs;
-
-	switch (INTEL_INFO(dev)->gen) {
-	case 7:
-	case 6:
-		I915_WRITE64(FENCE_REG_SANDYBRIDGE_0 + fence_reg*8, 0);
-		break;
-	case 5:
-	case 4:
-		I915_WRITE64(FENCE_REG_965_0 + fence_reg*8, 0);
-		break;
-	case 3:
-		if (fence_reg >= 8)
-			fence_reg = FENCE_REG_945_8 + (fence_reg - 8) * 4;
-		else
-	case 2:
-			fence_reg = FENCE_REG_830_0 + fence_reg * 4;
-
-		I915_WRITE(fence_reg, 0);
-		break;
-	}
+	i915_gem_object_update_fence(obj, reg, enable);
+	obj->fence_dirty = false;
 
-	list_del_init(&reg->lru_list);
-	reg->obj = NULL;
-	reg->setup_seqno = 0;
-	reg->pin_count = 0;
+	return 0;
 }
 
 /**
@@ -2749,7 +2607,7 @@ i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 		return ret;
 	}
 
-	ret = i915_gem_gtt_bind_object(obj);
+	ret = i915_gem_gtt_prepare_object(obj);
 	if (ret) {
 		i915_gem_object_put_pages_gtt(obj);
 		drm_mm_put_block(obj->gtt_space);
@@ -2761,6 +2619,9 @@ i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 		goto search_free;
 	}
 
+	if (!dev_priv->mm.aliasing_ppgtt)
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
+
 	list_add_tail(&obj->gtt_list, &dev_priv->mm.gtt_list);
 	list_add_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
 
@@ -2878,6 +2739,7 @@ i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *obj)
 int
 i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write)
 {
+	drm_i915_private_t *dev_priv = obj->base.dev->dev_private;
 	uint32_t old_write_domain, old_read_domains;
 	int ret;
 
@@ -2918,6 +2780,10 @@ i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write)
 					    old_read_domains,
 					    old_write_domain);
 
+	/* And bump the LRU for this access */
+	if (i915_gem_object_is_inactive(obj))
+		list_move_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
+
 	return 0;
 }
 
@@ -2953,7 +2819,8 @@ int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
 				return ret;
 		}
 
-		i915_gem_gtt_rebind_object(obj, cache_level);
+		if (obj->has_global_gtt_mapping)
+			i915_gem_gtt_bind_object(obj, cache_level);
 		if (obj->has_aliasing_ppgtt_mapping)
 			i915_ppgtt_bind_object(dev_priv->mm.aliasing_ppgtt,
 					       obj, cache_level);
@@ -2990,11 +2857,6 @@ int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
  * Prepare buffer for display plane (scanout, cursors, etc).
  * Can be called from an uninterruptible phase (modesetting) and allows
  * any flushes to be pipelined (for pageflips).
- *
- * For the display plane, we want to be in the GTT but out of any write
- * domains. So in many ways this looks like set_to_gtt_domain() apart from the
- * ability to pipeline the waits, pinning and any additional subtleties
- * that may differentiate the display plane from ordinary buffers.
  */
 int
 i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
@@ -3009,8 +2871,8 @@ i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 		return ret;
 
 	if (pipelined != obj->ring) {
-		ret = i915_gem_object_wait_rendering(obj);
-		if (ret == -ERESTARTSYS)
+		ret = i915_gem_object_sync(obj, pipelined);
+		if (ret)
 			return ret;
 	}
 
@@ -3082,7 +2944,7 @@ i915_gem_object_finish_gpu(struct drm_i915_gem_object *obj)
  * This function returns when the move is complete, including waiting on
  * flushes to occur.
  */
-static int
+int
 i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write)
 {
 	uint32_t old_write_domain, old_read_domains;
@@ -3095,17 +2957,14 @@ i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write)
 	if (ret)
 		return ret;
 
-	ret = i915_gem_object_wait_rendering(obj);
-	if (ret)
-		return ret;
+	if (write || obj->pending_gpu_write) {
+		ret = i915_gem_object_wait_rendering(obj);
+		if (ret)
+			return ret;
+	}
 
 	i915_gem_object_flush_gtt_write_domain(obj);
 
-	/* If we have a partially-valid cache of the object in the CPU,
-	 * finish invalidating it and free the per-page flags.
-	 */
-	i915_gem_object_set_to_full_cpu_read_domain(obj);
-
 	old_write_domain = obj->base.write_domain;
 	old_read_domains = obj->base.read_domains;
 
@@ -3136,113 +2995,6 @@ i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write)
 	return 0;
 }
 
-/**
- * Moves the object from a partially CPU read to a full one.
- *
- * Note that this only resolves i915_gem_object_set_cpu_read_domain_range(),
- * and doesn't handle transitioning from !(read_domains & I915_GEM_DOMAIN_CPU).
- */
-static void
-i915_gem_object_set_to_full_cpu_read_domain(struct drm_i915_gem_object *obj)
-{
-	if (!obj->page_cpu_valid)
-		return;
-
-	/* If we're partially in the CPU read domain, finish moving it in.
-	 */
-	if (obj->base.read_domains & I915_GEM_DOMAIN_CPU) {
-		int i;
-
-		for (i = 0; i <= (obj->base.size - 1) / PAGE_SIZE; i++) {
-			if (obj->page_cpu_valid[i])
-				continue;
-			drm_clflush_pages(obj->pages + i, 1);
-		}
-	}
-
-	/* Free the page_cpu_valid mappings which are now stale, whether
-	 * or not we've got I915_GEM_DOMAIN_CPU.
-	 */
-	kfree(obj->page_cpu_valid);
-	obj->page_cpu_valid = NULL;
-}
-
-/**
- * Set the CPU read domain on a range of the object.
- *
- * The object ends up with I915_GEM_DOMAIN_CPU in its read flags although it's
- * not entirely valid.  The page_cpu_valid member of the object flags which
- * pages have been flushed, and will be respected by
- * i915_gem_object_set_to_cpu_domain() if it's called on to get a valid mapping
- * of the whole object.
- *
- * This function returns when the move is complete, including waiting on
- * flushes to occur.
- */
-static int
-i915_gem_object_set_cpu_read_domain_range(struct drm_i915_gem_object *obj,
-					  uint64_t offset, uint64_t size)
-{
-	uint32_t old_read_domains;
-	int i, ret;
-
-	if (offset == 0 && size == obj->base.size)
-		return i915_gem_object_set_to_cpu_domain(obj, 0);
-
-	ret = i915_gem_object_flush_gpu_write_domain(obj);
-	if (ret)
-		return ret;
-
-	ret = i915_gem_object_wait_rendering(obj);
-	if (ret)
-		return ret;
-
-	i915_gem_object_flush_gtt_write_domain(obj);
-
-	/* If we're already fully in the CPU read domain, we're done. */
-	if (obj->page_cpu_valid == NULL &&
-	    (obj->base.read_domains & I915_GEM_DOMAIN_CPU) != 0)
-		return 0;
-
-	/* Otherwise, create/clear the per-page CPU read domain flag if we're
-	 * newly adding I915_GEM_DOMAIN_CPU
-	 */
-	if (obj->page_cpu_valid == NULL) {
-		obj->page_cpu_valid = kzalloc(obj->base.size / PAGE_SIZE,
-					      GFP_KERNEL);
-		if (obj->page_cpu_valid == NULL)
-			return -ENOMEM;
-	} else if ((obj->base.read_domains & I915_GEM_DOMAIN_CPU) == 0)
-		memset(obj->page_cpu_valid, 0, obj->base.size / PAGE_SIZE);
-
-	/* Flush the cache on any pages that are still invalid from the CPU's
-	 * perspective.
-	 */
-	for (i = offset / PAGE_SIZE; i <= (offset + size - 1) / PAGE_SIZE;
-	     i++) {
-		if (obj->page_cpu_valid[i])
-			continue;
-
-		drm_clflush_pages(obj->pages + i, 1);
-
-		obj->page_cpu_valid[i] = 1;
-	}
-
-	/* It should now be out of any other write domains, and we can update
-	 * the domain values for our changes.
-	 */
-	BUG_ON((obj->base.write_domain & ~I915_GEM_DOMAIN_CPU) != 0);
-
-	old_read_domains = obj->base.read_domains;
-	obj->base.read_domains |= I915_GEM_DOMAIN_CPU;
-
-	trace_i915_gem_object_change_domain(obj,
-					    old_read_domains,
-					    obj->base.write_domain);
-
-	return 0;
-}
-
 /* Throttle our rendering by waiting until the ring has completed our requests
  * emitted over 20 msec ago.
  *
@@ -3280,28 +3032,7 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file)
 	if (seqno == 0)
 		return 0;
 
-	ret = 0;
-	if (!i915_seqno_passed(ring->get_seqno(ring), seqno)) {
-		/* And wait for the seqno passing without holding any locks and
-		 * causing extra latency for others. This is safe as the irq
-		 * generation is designed to be run atomically and so is
-		 * lockless.
-		 */
-		if (ring->irq_get(ring)) {
-			ret = wait_event_interruptible(ring->irq_queue,
-						       i915_seqno_passed(ring->get_seqno(ring), seqno)
-						       || atomic_read(&dev_priv->mm.wedged));
-			ring->irq_put(ring);
-
-			if (ret == 0 && atomic_read(&dev_priv->mm.wedged))
-				ret = -EIO;
-		} else if (wait_for_atomic(i915_seqno_passed(ring->get_seqno(ring),
-							     seqno) ||
-				    atomic_read(&dev_priv->mm.wedged), 3000)) {
-			ret = -EBUSY;
-		}
-	}
-
+	ret = __wait_seqno(ring, seqno, true);
 	if (ret == 0)
 		queue_delayed_work(dev_priv->wq, &dev_priv->mm.retire_work, 0);
 
@@ -3313,12 +3044,9 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 		    uint32_t alignment,
 		    bool map_and_fenceable)
 {
-	struct drm_device *dev = obj->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
 	BUG_ON(obj->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT);
-	WARN_ON(i915_verify_lists(dev));
 
 	if (obj->gtt_space != NULL) {
 		if ((alignment && obj->gtt_offset & (alignment - 1)) ||
@@ -3343,34 +3071,23 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 			return ret;
 	}
 
-	if (obj->pin_count++ == 0) {
-		if (!obj->active)
-			list_move_tail(&obj->mm_list,
-				       &dev_priv->mm.pinned_list);
-	}
+	if (!obj->has_global_gtt_mapping && map_and_fenceable)
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
+
+	obj->pin_count++;
 	obj->pin_mappable |= map_and_fenceable;
 
-	WARN_ON(i915_verify_lists(dev));
 	return 0;
 }
 
 void
 i915_gem_object_unpin(struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	WARN_ON(i915_verify_lists(dev));
 	BUG_ON(obj->pin_count == 0);
 	BUG_ON(obj->gtt_space == NULL);
 
-	if (--obj->pin_count == 0) {
-		if (!obj->active)
-			list_move_tail(&obj->mm_list,
-				       &dev_priv->mm.inactive_list);
+	if (--obj->pin_count == 0)
 		obj->pin_mappable = false;
-	}
-	WARN_ON(i915_verify_lists(dev));
 }
 
 int
@@ -3494,20 +3211,9 @@ i915_gem_busy_ioctl(struct drm_device *dev, void *data,
 		if (obj->base.write_domain & I915_GEM_GPU_DOMAINS) {
 			ret = i915_gem_flush_ring(obj->ring,
 						  0, obj->base.write_domain);
-		} else if (obj->ring->outstanding_lazy_request ==
-			   obj->last_rendering_seqno) {
-			struct drm_i915_gem_request *request;
-
-			/* This ring is not being cleared by active usage,
-			 * so emit a request to do so.
-			 */
-			request = kzalloc(sizeof(*request), GFP_KERNEL);
-			if (request) {
-				ret = i915_add_request(obj->ring, NULL, request);
-				if (ret)
-					kfree(request);
-			} else
-				ret = -ENOMEM;
+		} else {
+			ret = i915_gem_check_olr(obj->ring,
+						 obj->last_rendering_seqno);
 		}
 
 		/* Update the active list for the hardware's current position.
@@ -3643,46 +3349,42 @@ int i915_gem_init_object(struct drm_gem_object *obj)
 	return 0;
 }
 
-static void i915_gem_free_object_tail(struct drm_i915_gem_object *obj)
+void i915_gem_free_object(struct drm_gem_object *gem_obj)
 {
+	struct drm_i915_gem_object *obj = to_intel_bo(gem_obj);
 	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	int ret;
-
-	ret = i915_gem_object_unbind(obj);
-	if (ret == -ERESTARTSYS) {
-		list_move(&obj->mm_list,
-			  &dev_priv->mm.deferred_free_list);
-		return;
-	}
 
 	trace_i915_gem_object_destroy(obj);
 
+	if (gem_obj->import_attach)
+		drm_prime_gem_destroy(gem_obj, obj->sg_table);
+
+	if (obj->phys_obj)
+		i915_gem_detach_phys_object(dev, obj);
+
+	obj->pin_count = 0;
+	if (WARN_ON(i915_gem_object_unbind(obj) == -ERESTARTSYS)) {
+		bool was_interruptible;
+
+		was_interruptible = dev_priv->mm.interruptible;
+		dev_priv->mm.interruptible = false;
+
+		WARN_ON(i915_gem_object_unbind(obj));
+
+		dev_priv->mm.interruptible = was_interruptible;
+	}
+
 	if (obj->base.map_list.map)
 		drm_gem_free_mmap_offset(&obj->base);
 
 	drm_gem_object_release(&obj->base);
 	i915_gem_info_remove_obj(dev_priv, obj->base.size);
 
-	kfree(obj->page_cpu_valid);
 	kfree(obj->bit_17);
 	kfree(obj);
 }
 
-void i915_gem_free_object(struct drm_gem_object *gem_obj)
-{
-	struct drm_i915_gem_object *obj = to_intel_bo(gem_obj);
-	struct drm_device *dev = obj->base.dev;
-
-	while (obj->pin_count > 0)
-		i915_gem_object_unpin(obj);
-
-	if (obj->phys_obj)
-		i915_gem_detach_phys_object(dev, obj);
-
-	i915_gem_free_object_tail(obj);
-}
-
 int
 i915_gem_idle(struct drm_device *dev)
 {
@@ -3696,20 +3398,16 @@ i915_gem_idle(struct drm_device *dev)
 		return 0;
 	}
 
-	ret = i915_gpu_idle(dev, true);
+	ret = i915_gpu_idle(dev);
 	if (ret) {
 		mutex_unlock(&dev->struct_mutex);
 		return ret;
 	}
+	i915_gem_retire_requests(dev);
 
 	/* Under UMS, be paranoid and evict. */
-	if (!drm_core_check_feature(dev, DRIVER_MODESET)) {
-		ret = i915_gem_evict_inactive(dev, false);
-		if (ret) {
-			mutex_unlock(&dev->struct_mutex);
-			return ret;
-		}
-	}
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		i915_gem_evict_everything(dev, false);
 
 	i915_gem_reset_fences(dev);
 
@@ -3747,9 +3445,9 @@ void i915_gem_init_swizzling(struct drm_device *dev)
 
 	I915_WRITE(TILECTL, I915_READ(TILECTL) | TILECTL_SWZCTL);
 	if (IS_GEN6(dev))
-		I915_WRITE(ARB_MODE, ARB_MODE_ENABLE(ARB_MODE_SWIZZLE_SNB));
+		I915_WRITE(ARB_MODE, _MASKED_BIT_ENABLE(ARB_MODE_SWIZZLE_SNB));
 	else
-		I915_WRITE(ARB_MODE, ARB_MODE_ENABLE(ARB_MODE_SWIZZLE_IVB));
+		I915_WRITE(ARB_MODE, _MASKED_BIT_ENABLE(ARB_MODE_SWIZZLE_IVB));
 }
 
 void i915_gem_init_ppgtt(struct drm_device *dev)
@@ -3787,21 +3485,27 @@ void i915_gem_init_ppgtt(struct drm_device *dev)
 	pd_offset <<= 16;
 
 	if (INTEL_INFO(dev)->gen == 6) {
-		uint32_t ecochk = I915_READ(GAM_ECOCHK);
+		uint32_t ecochk, gab_ctl, ecobits;
+
+		ecobits = I915_READ(GAC_ECO_BITS); 
+		I915_WRITE(GAC_ECO_BITS, ecobits | ECOBITS_PPGTT_CACHE64B);
+
+		gab_ctl = I915_READ(GAB_CTL);
+		I915_WRITE(GAB_CTL, gab_ctl | GAB_CTL_CONT_AFTER_PAGEFAULT);
+
+		ecochk = I915_READ(GAM_ECOCHK);
 		I915_WRITE(GAM_ECOCHK, ecochk | ECOCHK_SNB_BIT |
 				       ECOCHK_PPGTT_CACHE64B);
-		I915_WRITE(GFX_MODE, GFX_MODE_ENABLE(GFX_PPGTT_ENABLE));
+		I915_WRITE(GFX_MODE, _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
 	} else if (INTEL_INFO(dev)->gen >= 7) {
 		I915_WRITE(GAM_ECOCHK, ECOCHK_PPGTT_CACHE64B);
 		/* GFX_MODE is per-ring on gen7+ */
 	}
 
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		ring = &dev_priv->ring[i];
-
+	for_each_ring(ring, dev_priv, i) {
 		if (INTEL_INFO(dev)->gen >= 7)
 			I915_WRITE(RING_MODE_GEN7(ring),
-				   GFX_MODE_ENABLE(GFX_PPGTT_ENABLE));
+				   _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
 
 		I915_WRITE(RING_PP_DIR_DCLV(ring), PP_DIR_DCLV_2G);
 		I915_WRITE(RING_PP_DIR_BASE(ring), pd_offset);
@@ -3845,14 +3549,80 @@ cleanup_render_ring:
 	return ret;
 }
 
+static bool
+intel_enable_ppgtt(struct drm_device *dev)
+{
+	if (i915_enable_ppgtt >= 0)
+		return i915_enable_ppgtt;
+
+#ifdef CONFIG_INTEL_IOMMU
+	/* Disable ppgtt on SNB if VT-d is on. */
+	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped)
+		return false;
+#endif
+
+	return true;
+}
+
+int i915_gem_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long gtt_size, mappable_size;
+	int ret;
+
+	gtt_size = dev_priv->mm.gtt->gtt_total_entries << PAGE_SHIFT;
+	mappable_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
+
+	mutex_lock(&dev->struct_mutex);
+	if (intel_enable_ppgtt(dev) && HAS_ALIASING_PPGTT(dev)) {
+		/* PPGTT pdes are stolen from global gtt ptes, so shrink the
+		 * aperture accordingly when using aliasing ppgtt. */
+		gtt_size -= I915_PPGTT_PD_ENTRIES*PAGE_SIZE;
+
+		i915_gem_init_global_gtt(dev, 0, mappable_size, gtt_size);
+
+		ret = i915_gem_init_aliasing_ppgtt(dev);
+		if (ret) {
+			mutex_unlock(&dev->struct_mutex);
+			return ret;
+		}
+	} else {
+		/* Let GEM Manage all of the aperture.
+		 *
+		 * However, leave one page at the end still bound to the scratch
+		 * page.  There are a number of places where the hardware
+		 * apparently prefetches past the end of the object, and we've
+		 * seen multiple hangs with the GPU head pointer stuck in a
+		 * batchbuffer bound at the last page of the aperture.  One page
+		 * should be enough to keep any prefetching inside of the
+		 * aperture.
+		 */
+		i915_gem_init_global_gtt(dev, 0, mappable_size,
+					 gtt_size);
+	}
+
+	ret = i915_gem_init_hw(dev);
+	mutex_unlock(&dev->struct_mutex);
+	if (ret) {
+		i915_gem_cleanup_aliasing_ppgtt(dev);
+		return ret;
+	}
+
+	/* Allow hardware batchbuffers unless told otherwise, but not for KMS. */
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		dev_priv->dri1.allow_batchbuffer = 1;
+	return 0;
+}
+
 void
 i915_gem_cleanup_ringbuffer(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	int i;
 
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		intel_cleanup_ring_buffer(&dev_priv->ring[i]);
+	for_each_ring(ring, dev_priv, i)
+		intel_cleanup_ring_buffer(ring);
 }
 
 int
@@ -3860,7 +3630,7 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
 		       struct drm_file *file_priv)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	int ret, i;
+	int ret;
 
 	if (drm_core_check_feature(dev, DRIVER_MODESET))
 		return 0;
@@ -3882,10 +3652,6 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
 	BUG_ON(!list_empty(&dev_priv->mm.active_list));
 	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
 	BUG_ON(!list_empty(&dev_priv->mm.inactive_list));
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		BUG_ON(!list_empty(&dev_priv->ring[i].active_list));
-		BUG_ON(!list_empty(&dev_priv->ring[i].request_list));
-	}
 	mutex_unlock(&dev->struct_mutex);
 
 	ret = drm_irq_install(dev);
@@ -3944,9 +3710,7 @@ i915_gem_load(struct drm_device *dev)
 	INIT_LIST_HEAD(&dev_priv->mm.active_list);
 	INIT_LIST_HEAD(&dev_priv->mm.flushing_list);
 	INIT_LIST_HEAD(&dev_priv->mm.inactive_list);
-	INIT_LIST_HEAD(&dev_priv->mm.pinned_list);
 	INIT_LIST_HEAD(&dev_priv->mm.fence_list);
-	INIT_LIST_HEAD(&dev_priv->mm.deferred_free_list);
 	INIT_LIST_HEAD(&dev_priv->mm.gtt_list);
 	for (i = 0; i < I915_NUM_RINGS; i++)
 		init_ring_lists(&dev_priv->ring[i]);
@@ -3958,12 +3722,8 @@ i915_gem_load(struct drm_device *dev)
 
 	/* On GEN3 we really need to make sure the ARB C3 LP bit is set */
 	if (IS_GEN3(dev)) {
-		u32 tmp = I915_READ(MI_ARB_STATE);
-		if (!(tmp & MI_ARB_C3_LP_WRITE_ENABLE)) {
-			/* arb state is a masked write, so set bit + bit in mask */
-			tmp = MI_ARB_C3_LP_WRITE_ENABLE | (MI_ARB_C3_LP_WRITE_ENABLE << MI_ARB_MASK_SHIFT);
-			I915_WRITE(MI_ARB_STATE, tmp);
-		}
+		I915_WRITE(MI_ARB_STATE,
+			   _MASKED_BIT_ENABLE(MI_ARB_C3_LP_WRITE_ENABLE));
 	}
 
 	dev_priv->relative_constants_mode = I915_EXEC_CONSTANTS_REL_GENERAL;
@@ -3978,9 +3738,7 @@ i915_gem_load(struct drm_device *dev)
 		dev_priv->num_fence_regs = 8;
 
 	/* Initialize fence registers to zero */
-	for (i = 0; i < dev_priv->num_fence_regs; i++) {
-		i915_gem_clear_fence_reg(dev, &dev_priv->fence_regs[i]);
-	}
+	i915_gem_reset_fences(dev);
 
 	i915_gem_detect_bit_6_swizzle(dev);
 	init_waitqueue_head(&dev_priv->pending_flip_queue);
@@ -4268,7 +4026,7 @@ rescan:
 		 * This has a dramatic impact to reduce the number of
 		 * OOM-killer events whilst running the GPU aggressively.
 		 */
-		if (i915_gpu_idle(dev, true) == 0)
+		if (i915_gpu_idle(dev) == 0)
 			goto rescan;
 	}
 	mutex_unlock(&dev->struct_mutex);
diff --git a/drivers/gpu/drm/i915/i915_gem_debug.c b/drivers/gpu/drm/i915/i915_gem_debug.c
index cc93cac242d6..a4f6aaabca99 100644
--- a/drivers/gpu/drm/i915/i915_gem_debug.c
+++ b/drivers/gpu/drm/i915/i915_gem_debug.c
@@ -114,22 +114,6 @@ i915_verify_lists(struct drm_device *dev)
 		}
 	}
 
-	list_for_each_entry(obj, &dev_priv->mm.pinned_list, list) {
-		if (obj->base.dev != dev ||
-		    !atomic_read(&obj->base.refcount.refcount)) {
-			DRM_ERROR("freed pinned %p\n", obj);
-			err++;
-			break;
-		} else if (!obj->pin_count || obj->active ||
-			   (obj->base.write_domain & I915_GEM_GPU_DOMAINS)) {
-			DRM_ERROR("invalid pinned %p (p %d a %d w %x)\n",
-				  obj,
-				  obj->pin_count, obj->active,
-				  obj->base.write_domain);
-			err++;
-		}
-	}
-
 	return warned = err;
 }
 #endif /* WATCH_INACTIVE */
diff --git a/drivers/gpu/drm/i915/i915_gem_dmabuf.c b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
new file mode 100644
index 000000000000..8e269178d6a5
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright 2012 Red Hat Inc
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "i915_drv.h"
+#include <linux/dma-buf.h>
+
+static struct sg_table *i915_gem_map_dma_buf(struct dma_buf_attachment *attachment,
+				      enum dma_data_direction dir)
+{
+	struct drm_i915_gem_object *obj = attachment->dmabuf->priv;
+	struct drm_device *dev = obj->base.dev;
+	int npages = obj->base.size / PAGE_SIZE;
+	struct sg_table *sg = NULL;
+	int ret;
+	int nents;
+
+	ret = i915_mutex_lock_interruptible(dev);
+	if (ret)
+		return ERR_PTR(ret);
+
+	if (!obj->pages) {
+		ret = i915_gem_object_get_pages_gtt(obj, __GFP_NORETRY | __GFP_NOWARN);
+		if (ret)
+			goto out;
+	}
+
+	/* link the pages into an SG then map the sg */
+	sg = drm_prime_pages_to_sg(obj->pages, npages);
+	nents = dma_map_sg(attachment->dev, sg->sgl, sg->nents, dir);
+out:
+	mutex_unlock(&dev->struct_mutex);
+	return sg;
+}
+
+static void i915_gem_unmap_dma_buf(struct dma_buf_attachment *attachment,
+			    struct sg_table *sg, enum dma_data_direction dir)
+{
+	dma_unmap_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	sg_free_table(sg);
+	kfree(sg);
+}
+
+static void i915_gem_dmabuf_release(struct dma_buf *dma_buf)
+{
+	struct drm_i915_gem_object *obj = dma_buf->priv;
+
+	if (obj->base.export_dma_buf == dma_buf) {
+		/* drop the reference on the export fd holds */
+		obj->base.export_dma_buf = NULL;
+		drm_gem_object_unreference_unlocked(&obj->base);
+	}
+}
+
+static void *i915_gem_dmabuf_kmap_atomic(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void i915_gem_dmabuf_kunmap_atomic(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+static void *i915_gem_dmabuf_kmap(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void i915_gem_dmabuf_kunmap(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+
+static const struct dma_buf_ops i915_dmabuf_ops =  {
+	.map_dma_buf = i915_gem_map_dma_buf,
+	.unmap_dma_buf = i915_gem_unmap_dma_buf,
+	.release = i915_gem_dmabuf_release,
+	.kmap = i915_gem_dmabuf_kmap,
+	.kmap_atomic = i915_gem_dmabuf_kmap_atomic,
+	.kunmap = i915_gem_dmabuf_kunmap,
+	.kunmap_atomic = i915_gem_dmabuf_kunmap_atomic,
+};
+
+struct dma_buf *i915_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *gem_obj, int flags)
+{
+	struct drm_i915_gem_object *obj = to_intel_bo(gem_obj);
+
+	return dma_buf_export(obj, &i915_dmabuf_ops,
+						  obj->base.size, 0600);
+}
+
+struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct drm_i915_gem_object *obj;
+	int npages;
+	int size;
+	int ret;
+
+	/* is this one of own objects? */
+	if (dma_buf->ops == &i915_dmabuf_ops) {
+		obj = dma_buf->priv;
+		/* is it from our device? */
+		if (obj->base.dev == dev) {
+			drm_gem_object_reference(&obj->base);
+			return &obj->base;
+		}
+	}
+
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_CAST(attach);
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	size = dma_buf->size;
+	npages = size / PAGE_SIZE;
+
+	obj = kzalloc(sizeof(*obj), GFP_KERNEL);
+	if (obj == NULL) {
+		ret = -ENOMEM;
+		goto fail_unmap;
+	}
+
+	ret = drm_gem_private_object_init(dev, &obj->base, size);
+	if (ret) {
+		kfree(obj);
+		goto fail_unmap;
+	}
+
+	obj->sg_table = sg;
+	obj->base.import_attach = attach;
+
+	return &obj->base;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index 21a82710f4b2..ae7c24e12e52 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -35,6 +35,9 @@
 static bool
 mark_free(struct drm_i915_gem_object *obj, struct list_head *unwind)
 {
+	if (obj->pin_count)
+		return false;
+
 	list_add(&obj->exec_list, unwind);
 	return drm_mm_scan_add_block(obj->gtt_space);
 }
@@ -90,7 +93,7 @@ i915_gem_evict_something(struct drm_device *dev, int min_size,
 	/* Now merge in the soon-to-be-expired objects... */
 	list_for_each_entry(obj, &dev_priv->mm.active_list, mm_list) {
 		/* Does the object require an outstanding flush? */
-		if (obj->base.write_domain || obj->pin_count)
+		if (obj->base.write_domain)
 			continue;
 
 		if (mark_free(obj, &unwind_list))
@@ -99,14 +102,11 @@ i915_gem_evict_something(struct drm_device *dev, int min_size,
 
 	/* Finally add anything with a pending flush (in order of retirement) */
 	list_for_each_entry(obj, &dev_priv->mm.flushing_list, mm_list) {
-		if (obj->pin_count)
-			continue;
-
 		if (mark_free(obj, &unwind_list))
 			goto found;
 	}
 	list_for_each_entry(obj, &dev_priv->mm.active_list, mm_list) {
-		if (!obj->base.write_domain || obj->pin_count)
+		if (!obj->base.write_domain)
 			continue;
 
 		if (mark_free(obj, &unwind_list))
@@ -166,8 +166,9 @@ int
 i915_gem_evict_everything(struct drm_device *dev, bool purgeable_only)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	int ret;
+	struct drm_i915_gem_object *obj, *next;
 	bool lists_empty;
+	int ret;
 
 	lists_empty = (list_empty(&dev_priv->mm.inactive_list) &&
 		       list_empty(&dev_priv->mm.flushing_list) &&
@@ -177,29 +178,24 @@ i915_gem_evict_everything(struct drm_device *dev, bool purgeable_only)
 
 	trace_i915_gem_evict_everything(dev, purgeable_only);
 
-	/* Flush everything (on to the inactive lists) and evict */
-	ret = i915_gpu_idle(dev, true);
+	/* The gpu_idle will flush everything in the write domain to the
+	 * active list. Then we must move everything off the active list
+	 * with retire requests.
+	 */
+	ret = i915_gpu_idle(dev);
 	if (ret)
 		return ret;
 
-	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
+	i915_gem_retire_requests(dev);
 
-	return i915_gem_evict_inactive(dev, purgeable_only);
-}
-
-/** Unbinds all inactive objects. */
-int
-i915_gem_evict_inactive(struct drm_device *dev, bool purgeable_only)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct drm_i915_gem_object *obj, *next;
+	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
 
+	/* Having flushed everything, unbind() should never raise an error */
 	list_for_each_entry_safe(obj, next,
 				 &dev_priv->mm.inactive_list, mm_list) {
 		if (!purgeable_only || obj->madv != I915_MADV_WILLNEED) {
-			int ret = i915_gem_object_unbind(obj);
-			if (ret)
-				return ret;
+			if (obj->pin_count == 0)
+				WARN_ON(i915_gem_object_unbind(obj));
 		}
 	}
 
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index de431942ded4..974a9f1068a3 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -266,6 +266,12 @@ eb_destroy(struct eb_objects *eb)
 	kfree(eb);
 }
 
+static inline int use_cpu_reloc(struct drm_i915_gem_object *obj)
+{
+	return (obj->base.write_domain == I915_GEM_DOMAIN_CPU ||
+		obj->cache_level != I915_CACHE_NONE);
+}
+
 static int
 i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 				   struct eb_objects *eb,
@@ -273,6 +279,7 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_gem_object *target_obj;
+	struct drm_i915_gem_object *target_i915_obj;
 	uint32_t target_offset;
 	int ret = -EINVAL;
 
@@ -281,7 +288,8 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 	if (unlikely(target_obj == NULL))
 		return -ENOENT;
 
-	target_offset = to_intel_bo(target_obj)->gtt_offset;
+	target_i915_obj = to_intel_bo(target_obj);
+	target_offset = target_i915_obj->gtt_offset;
 
 	/* The target buffer should have appeared before us in the
 	 * exec_object list, so it should have a GTT space bound by now.
@@ -352,11 +360,19 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		return ret;
 	}
 
+	/* We can't wait for rendering with pagefaults disabled */
+	if (obj->active && in_atomic())
+		return -EFAULT;
+
 	reloc->delta += target_offset;
-	if (obj->base.write_domain == I915_GEM_DOMAIN_CPU) {
+	if (use_cpu_reloc(obj)) {
 		uint32_t page_offset = reloc->offset & ~PAGE_MASK;
 		char *vaddr;
 
+		ret = i915_gem_object_set_to_cpu_domain(obj, 1);
+		if (ret)
+			return ret;
+
 		vaddr = kmap_atomic(obj->pages[reloc->offset >> PAGE_SHIFT]);
 		*(uint32_t *)(vaddr + page_offset) = reloc->delta;
 		kunmap_atomic(vaddr);
@@ -365,11 +381,11 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		uint32_t __iomem *reloc_entry;
 		void __iomem *reloc_page;
 
-		/* We can't wait for rendering with pagefaults disabled */
-		if (obj->active && in_atomic())
-			return -EFAULT;
+		ret = i915_gem_object_set_to_gtt_domain(obj, true);
+		if (ret)
+			return ret;
 
-		ret = i915_gem_object_set_to_gtt_domain(obj, 1);
+		ret = i915_gem_object_put_fence(obj);
 		if (ret)
 			return ret;
 
@@ -383,6 +399,16 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		io_mapping_unmap_atomic(reloc_page);
 	}
 
+	/* Sandybridge PPGTT errata: We need a global gtt mapping for MI and
+	 * pipe_control writes because the gpu doesn't properly redirect them
+	 * through the ppgtt for non_secure batchbuffers. */
+	if (unlikely(IS_GEN6(dev) &&
+	    reloc->write_domain == I915_GEM_DOMAIN_INSTRUCTION &&
+	    !target_i915_obj->has_global_gtt_mapping)) {
+		i915_gem_gtt_bind_object(target_i915_obj,
+					 target_i915_obj->cache_level);
+	}
+
 	/* and update the user's relocation entry */
 	reloc->presumed_offset = target_offset;
 
@@ -393,30 +419,46 @@ static int
 i915_gem_execbuffer_relocate_object(struct drm_i915_gem_object *obj,
 				    struct eb_objects *eb)
 {
+#define N_RELOC(x) ((x) / sizeof(struct drm_i915_gem_relocation_entry))
+	struct drm_i915_gem_relocation_entry stack_reloc[N_RELOC(512)];
 	struct drm_i915_gem_relocation_entry __user *user_relocs;
 	struct drm_i915_gem_exec_object2 *entry = obj->exec_entry;
-	int i, ret;
+	int remain, ret;
 
 	user_relocs = (void __user *)(uintptr_t)entry->relocs_ptr;
-	for (i = 0; i < entry->relocation_count; i++) {
-		struct drm_i915_gem_relocation_entry reloc;
 
-		if (__copy_from_user_inatomic(&reloc,
-					      user_relocs+i,
-					      sizeof(reloc)))
+	remain = entry->relocation_count;
+	while (remain) {
+		struct drm_i915_gem_relocation_entry *r = stack_reloc;
+		int count = remain;
+		if (count > ARRAY_SIZE(stack_reloc))
+			count = ARRAY_SIZE(stack_reloc);
+		remain -= count;
+
+		if (__copy_from_user_inatomic(r, user_relocs, count*sizeof(r[0])))
 			return -EFAULT;
 
-		ret = i915_gem_execbuffer_relocate_entry(obj, eb, &reloc);
-		if (ret)
-			return ret;
+		do {
+			u64 offset = r->presumed_offset;
 
-		if (__copy_to_user_inatomic(&user_relocs[i].presumed_offset,
-					    &reloc.presumed_offset,
-					    sizeof(reloc.presumed_offset)))
-			return -EFAULT;
+			ret = i915_gem_execbuffer_relocate_entry(obj, eb, r);
+			if (ret)
+				return ret;
+
+			if (r->presumed_offset != offset &&
+			    __copy_to_user_inatomic(&user_relocs->presumed_offset,
+						    &r->presumed_offset,
+						    sizeof(r->presumed_offset))) {
+				return -EFAULT;
+			}
+
+			user_relocs++;
+			r++;
+		} while (--count);
 	}
 
 	return 0;
+#undef N_RELOC
 }
 
 static int
@@ -465,6 +507,13 @@ i915_gem_execbuffer_relocate(struct drm_device *dev,
 #define  __EXEC_OBJECT_HAS_FENCE (1<<31)
 
 static int
+need_reloc_mappable(struct drm_i915_gem_object *obj)
+{
+	struct drm_i915_gem_exec_object2 *entry = obj->exec_entry;
+	return entry->relocation_count && !use_cpu_reloc(obj);
+}
+
+static int
 pin_and_fence_object(struct drm_i915_gem_object *obj,
 		     struct intel_ring_buffer *ring)
 {
@@ -477,8 +526,7 @@ pin_and_fence_object(struct drm_i915_gem_object *obj,
 		has_fenced_gpu_access &&
 		entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
 		obj->tiling_mode != I915_TILING_NONE;
-	need_mappable =
-		entry->relocation_count ? true : need_fence;
+	need_mappable = need_fence || need_reloc_mappable(obj);
 
 	ret = i915_gem_object_pin(obj, entry->alignment, need_mappable);
 	if (ret)
@@ -486,18 +534,13 @@ pin_and_fence_object(struct drm_i915_gem_object *obj,
 
 	if (has_fenced_gpu_access) {
 		if (entry->flags & EXEC_OBJECT_NEEDS_FENCE) {
-			if (obj->tiling_mode) {
-				ret = i915_gem_object_get_fence(obj, ring);
-				if (ret)
-					goto err_unpin;
+			ret = i915_gem_object_get_fence(obj);
+			if (ret)
+				goto err_unpin;
 
+			if (i915_gem_object_pin_fence(obj))
 				entry->flags |= __EXEC_OBJECT_HAS_FENCE;
-				i915_gem_object_pin_fence(obj);
-			} else {
-				ret = i915_gem_object_put_fence(obj);
-				if (ret)
-					goto err_unpin;
-			}
+
 			obj->pending_fenced_gpu_access = true;
 		}
 	}
@@ -535,8 +578,7 @@ i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 			has_fenced_gpu_access &&
 			entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
 			obj->tiling_mode != I915_TILING_NONE;
-		need_mappable =
-			entry->relocation_count ? true : need_fence;
+		need_mappable = need_fence || need_reloc_mappable(obj);
 
 		if (need_mappable)
 			list_move(&obj->exec_list, &ordered_objects);
@@ -576,8 +618,7 @@ i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 				has_fenced_gpu_access &&
 				entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
 				obj->tiling_mode != I915_TILING_NONE;
-			need_mappable =
-				entry->relocation_count ? true : need_fence;
+			need_mappable = need_fence || need_reloc_mappable(obj);
 
 			if ((entry->alignment && obj->gtt_offset & (entry->alignment - 1)) ||
 			    (need_mappable && !obj->map_and_fenceable))
@@ -798,64 +839,6 @@ i915_gem_execbuffer_flush(struct drm_device *dev,
 	return 0;
 }
 
-static bool
-intel_enable_semaphores(struct drm_device *dev)
-{
-	if (INTEL_INFO(dev)->gen < 6)
-		return 0;
-
-	if (i915_semaphores >= 0)
-		return i915_semaphores;
-
-	/* Disable semaphores on SNB */
-	if (INTEL_INFO(dev)->gen == 6)
-		return 0;
-
-	return 1;
-}
-
-static int
-i915_gem_execbuffer_sync_rings(struct drm_i915_gem_object *obj,
-			       struct intel_ring_buffer *to)
-{
-	struct intel_ring_buffer *from = obj->ring;
-	u32 seqno;
-	int ret, idx;
-
-	if (from == NULL || to == from)
-		return 0;
-
-	/* XXX gpu semaphores are implicated in various hard hangs on SNB */
-	if (!intel_enable_semaphores(obj->base.dev))
-		return i915_gem_object_wait_rendering(obj);
-
-	idx = intel_ring_sync_index(from, to);
-
-	seqno = obj->last_rendering_seqno;
-	if (seqno <= from->sync_seqno[idx])
-		return 0;
-
-	if (seqno == from->outstanding_lazy_request) {
-		struct drm_i915_gem_request *request;
-
-		request = kzalloc(sizeof(*request), GFP_KERNEL);
-		if (request == NULL)
-			return -ENOMEM;
-
-		ret = i915_add_request(from, NULL, request);
-		if (ret) {
-			kfree(request);
-			return ret;
-		}
-
-		seqno = request->seqno;
-	}
-
-	from->sync_seqno[idx] = seqno;
-
-	return to->sync_to(to, from, seqno - 1);
-}
-
 static int
 i915_gem_execbuffer_wait_for_flips(struct intel_ring_buffer *ring, u32 flips)
 {
@@ -917,7 +900,7 @@ i915_gem_execbuffer_move_to_gpu(struct intel_ring_buffer *ring,
 	}
 
 	list_for_each_entry(obj, objects, exec_list) {
-		ret = i915_gem_execbuffer_sync_rings(obj, ring);
+		ret = i915_gem_object_sync(obj, ring);
 		if (ret)
 			return ret;
 	}
@@ -955,7 +938,7 @@ validate_exec_list(struct drm_i915_gem_exec_object2 *exec,
 		if (!access_ok(VERIFY_WRITE, ptr, length))
 			return -EFAULT;
 
-		if (fault_in_pages_readable(ptr, length))
+		if (fault_in_multipages_readable(ptr, length))
 			return -EFAULT;
 	}
 
@@ -984,11 +967,14 @@ i915_gem_execbuffer_move_to_active(struct list_head *objects,
 			obj->pending_gpu_write = true;
 			list_move_tail(&obj->gpu_write_list,
 				       &ring->gpu_write_list);
-			intel_mark_busy(ring->dev, obj);
+			if (obj->pin_count) /* check for potential scanout */
+				intel_mark_busy(ring->dev, obj);
 		}
 
 		trace_i915_gem_object_change_domain(obj, old_read, old_write);
 	}
+
+	intel_mark_busy(ring->dev, NULL);
 }
 
 static void
@@ -1078,17 +1064,9 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		ring = &dev_priv->ring[RCS];
 		break;
 	case I915_EXEC_BSD:
-		if (!HAS_BSD(dev)) {
-			DRM_DEBUG("execbuf with invalid ring (BSD)\n");
-			return -EINVAL;
-		}
 		ring = &dev_priv->ring[VCS];
 		break;
 	case I915_EXEC_BLT:
-		if (!HAS_BLT(dev)) {
-			DRM_DEBUG("execbuf with invalid ring (BLT)\n");
-			return -EINVAL;
-		}
 		ring = &dev_priv->ring[BCS];
 		break;
 	default:
@@ -1096,6 +1074,11 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 			  (int)(args->flags & I915_EXEC_RING_MASK));
 		return -EINVAL;
 	}
+	if (!intel_ring_initialized(ring)) {
+		DRM_DEBUG("execbuf with invalid ring: %d\n",
+			  (int)(args->flags & I915_EXEC_RING_MASK));
+		return -EINVAL;
+	}
 
 	mode = args->flags & I915_EXEC_CONSTANTS_MASK;
 	mask = I915_EXEC_CONSTANTS_MASK;
@@ -1133,11 +1116,17 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 			return -EINVAL;
 		}
 
+		if (INTEL_INFO(dev)->gen >= 5) {
+			DRM_DEBUG("clip rectangles are only valid on pre-gen5\n");
+			return -EINVAL;
+		}
+
 		if (args->num_cliprects > UINT_MAX / sizeof(*cliprects)) {
 			DRM_DEBUG("execbuf with %u cliprects\n",
 				  args->num_cliprects);
 			return -EINVAL;
 		}
+
 		cliprects = kmalloc(args->num_cliprects * sizeof(*cliprects),
 				    GFP_KERNEL);
 		if (cliprects == NULL) {
@@ -1242,9 +1231,10 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 			 * so every billion or so execbuffers, we need to stall
 			 * the GPU in order to reset the counters.
 			 */
-			ret = i915_gpu_idle(dev, true);
+			ret = i915_gpu_idle(dev);
 			if (ret)
 				goto err;
+			i915_gem_retire_requests(dev);
 
 			BUG_ON(ring->sync_seqno[i]);
 		}
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index a135c61f4119..9fd25a435536 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -96,11 +96,10 @@ int i915_gem_init_aliasing_ppgtt(struct drm_device *dev)
 					     GFP_KERNEL);
 		if (!ppgtt->pt_dma_addr)
 			goto err_pt_alloc;
-	}
 
-	for (i = 0; i < ppgtt->num_pd_entries; i++) {
-		dma_addr_t pt_addr;
-		if (dev_priv->mm.gtt->needs_dmar) {
+		for (i = 0; i < ppgtt->num_pd_entries; i++) {
+			dma_addr_t pt_addr;
+
 			pt_addr = pci_map_page(dev->pdev, ppgtt->pt_pages[i],
 					       0, 4096,
 					       PCI_DMA_BIDIRECTIONAL);
@@ -112,8 +111,7 @@ int i915_gem_init_aliasing_ppgtt(struct drm_device *dev)
 
 			}
 			ppgtt->pt_dma_addr[i] = pt_addr;
-		} else
-			pt_addr = page_to_phys(ppgtt->pt_pages[i]);
+		}
 	}
 
 	ppgtt->scratch_page_dma_addr = dev_priv->mm.gtt->scratch_page_dma;
@@ -269,7 +267,13 @@ void i915_ppgtt_bind_object(struct i915_hw_ppgtt *ppgtt,
 		BUG();
 	}
 
-	if (dev_priv->mm.gtt->needs_dmar) {
+	if (obj->sg_table) {
+		i915_ppgtt_insert_sg_entries(ppgtt,
+					     obj->sg_table->sgl,
+					     obj->sg_table->nents,
+					     obj->gtt_space->start >> PAGE_SHIFT,
+					     pte_flags);
+	} else if (dev_priv->mm.gtt->needs_dmar) {
 		BUG_ON(!obj->sg_list);
 
 		i915_ppgtt_insert_sg_entries(ppgtt,
@@ -319,7 +323,7 @@ static bool do_idling(struct drm_i915_private *dev_priv)
 
 	if (unlikely(dev_priv->mm.gtt->do_idle_maps)) {
 		dev_priv->mm.interruptible = false;
-		if (i915_gpu_idle(dev_priv->dev, false)) {
+		if (i915_gpu_idle(dev_priv->dev)) {
 			DRM_ERROR("Couldn't idle GPU\n");
 			/* Wait a bit, in hopes it avoids the hang */
 			udelay(10);
@@ -346,48 +350,39 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 
 	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list) {
 		i915_gem_clflush_object(obj);
-		i915_gem_gtt_rebind_object(obj, obj->cache_level);
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
 	}
 
 	intel_gtt_chipset_flush();
 }
 
-int i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj)
+int i915_gem_gtt_prepare_object(struct drm_i915_gem_object *obj)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned int agp_type = cache_level_to_agp_type(dev, obj->cache_level);
-	int ret;
-
-	if (dev_priv->mm.gtt->needs_dmar) {
-		ret = intel_gtt_map_memory(obj->pages,
-					   obj->base.size >> PAGE_SHIFT,
-					   &obj->sg_list,
-					   &obj->num_sg);
-		if (ret != 0)
-			return ret;
-
-		intel_gtt_insert_sg_entries(obj->sg_list,
-					    obj->num_sg,
-					    obj->gtt_space->start >> PAGE_SHIFT,
-					    agp_type);
-	} else
-		intel_gtt_insert_pages(obj->gtt_space->start >> PAGE_SHIFT,
-				       obj->base.size >> PAGE_SHIFT,
-				       obj->pages,
-				       agp_type);
 
-	return 0;
+	if (dev_priv->mm.gtt->needs_dmar)
+		return intel_gtt_map_memory(obj->pages,
+					    obj->base.size >> PAGE_SHIFT,
+					    &obj->sg_list,
+					    &obj->num_sg);
+	else
+		return 0;
 }
 
-void i915_gem_gtt_rebind_object(struct drm_i915_gem_object *obj,
-				enum i915_cache_level cache_level)
+void i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj,
+			      enum i915_cache_level cache_level)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned int agp_type = cache_level_to_agp_type(dev, cache_level);
 
-	if (dev_priv->mm.gtt->needs_dmar) {
+	if (obj->sg_table) {
+		intel_gtt_insert_sg_entries(obj->sg_table->sgl,
+					    obj->sg_table->nents,
+					    obj->gtt_space->start >> PAGE_SHIFT,
+					    agp_type);
+	} else if (dev_priv->mm.gtt->needs_dmar) {
 		BUG_ON(!obj->sg_list);
 
 		intel_gtt_insert_sg_entries(obj->sg_list,
@@ -399,19 +394,26 @@ void i915_gem_gtt_rebind_object(struct drm_i915_gem_object *obj,
 				       obj->base.size >> PAGE_SHIFT,
 				       obj->pages,
 				       agp_type);
+
+	obj->has_global_gtt_mapping = 1;
 }
 
 void i915_gem_gtt_unbind_object(struct drm_i915_gem_object *obj)
 {
+	intel_gtt_clear_range(obj->gtt_space->start >> PAGE_SHIFT,
+			      obj->base.size >> PAGE_SHIFT);
+
+	obj->has_global_gtt_mapping = 0;
+}
+
+void i915_gem_gtt_finish_object(struct drm_i915_gem_object *obj)
+{
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	bool interruptible;
 
 	interruptible = do_idling(dev_priv);
 
-	intel_gtt_clear_range(obj->gtt_space->start >> PAGE_SHIFT,
-			      obj->base.size >> PAGE_SHIFT);
-
 	if (obj->sg_list) {
 		intel_gtt_unmap_memory(obj->sg_list, obj->num_sg);
 		obj->sg_list = NULL;
@@ -419,3 +421,23 @@ void i915_gem_gtt_unbind_object(struct drm_i915_gem_object *obj)
 
 	undo_idling(dev_priv, interruptible);
 }
+
+void i915_gem_init_global_gtt(struct drm_device *dev,
+			      unsigned long start,
+			      unsigned long mappable_end,
+			      unsigned long end)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+
+	/* Substract the guard page ... */
+	drm_mm_init(&dev_priv->mm.gtt_space, start, end - start - PAGE_SIZE);
+
+	dev_priv->mm.gtt_start = start;
+	dev_priv->mm.gtt_mappable_end = mappable_end;
+	dev_priv->mm.gtt_end = end;
+	dev_priv->mm.gtt_total = end - start;
+	dev_priv->mm.mappable_gtt_total = min(end, mappable_end) - start;
+
+	/* ... but ensure that we clear the entire range. */
+	intel_gtt_clear_range(start / PAGE_SIZE, (end-start) / PAGE_SIZE);
+}
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
new file mode 100644
index 000000000000..ada2e90a2a60
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -0,0 +1,202 @@
+/*
+ * Copyright © 2008-2012 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Eric Anholt <eric@anholt.net>
+ *    Chris Wilson <chris@chris-wilson.co.uk>
+ *
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "i915_drm.h"
+#include "i915_drv.h"
+
+/*
+ * The BIOS typically reserves some of the system's memory for the exclusive
+ * use of the integrated graphics. This memory is no longer available for
+ * use by the OS and so the user finds that his system has less memory
+ * available than he put in. We refer to this memory as stolen.
+ *
+ * The BIOS will allocate its framebuffer from the stolen memory. Our
+ * goal is try to reuse that object for our own fbcon which must always
+ * be available for panics. Anything else we can reuse the stolen memory
+ * for is a boon.
+ */
+
+#define PTE_ADDRESS_MASK		0xfffff000
+#define PTE_ADDRESS_MASK_HIGH		0x000000f0 /* i915+ */
+#define PTE_MAPPING_TYPE_UNCACHED	(0 << 1)
+#define PTE_MAPPING_TYPE_DCACHE		(1 << 1) /* i830 only */
+#define PTE_MAPPING_TYPE_CACHED		(3 << 1)
+#define PTE_MAPPING_TYPE_MASK		(3 << 1)
+#define PTE_VALID			(1 << 0)
+
+/**
+ * i915_stolen_to_phys - take an offset into stolen memory and turn it into
+ *                       a physical one
+ * @dev: drm device
+ * @offset: address to translate
+ *
+ * Some chip functions require allocations from stolen space and need the
+ * physical address of the memory in question.
+ */
+static unsigned long i915_stolen_to_phys(struct drm_device *dev, u32 offset)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct pci_dev *pdev = dev_priv->bridge_dev;
+	u32 base;
+
+#if 0
+	/* On the machines I have tested the Graphics Base of Stolen Memory
+	 * is unreliable, so compute the base by subtracting the stolen memory
+	 * from the Top of Low Usable DRAM which is where the BIOS places
+	 * the graphics stolen memory.
+	 */
+	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
+		/* top 32bits are reserved = 0 */
+		pci_read_config_dword(pdev, 0xA4, &base);
+	} else {
+		/* XXX presume 8xx is the same as i915 */
+		pci_bus_read_config_dword(pdev->bus, 2, 0x5C, &base);
+	}
+#else
+	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
+		u16 val;
+		pci_read_config_word(pdev, 0xb0, &val);
+		base = val >> 4 << 20;
+	} else {
+		u8 val;
+		pci_read_config_byte(pdev, 0x9c, &val);
+		base = val >> 3 << 27;
+	}
+	base -= dev_priv->mm.gtt->stolen_size;
+#endif
+
+	return base + offset;
+}
+
+static void i915_warn_stolen(struct drm_device *dev)
+{
+	DRM_INFO("not enough stolen space for compressed buffer, disabling\n");
+	DRM_INFO("hint: you may be able to increase stolen memory size in the BIOS to avoid this\n");
+}
+
+static void i915_setup_compression(struct drm_device *dev, int size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_mm_node *compressed_fb, *uninitialized_var(compressed_llb);
+	unsigned long cfb_base;
+	unsigned long ll_base = 0;
+
+	/* Just in case the BIOS is doing something questionable. */
+	intel_disable_fbc(dev);
+
+	compressed_fb = drm_mm_search_free(&dev_priv->mm.stolen, size, 4096, 0);
+	if (compressed_fb)
+		compressed_fb = drm_mm_get_block(compressed_fb, size, 4096);
+	if (!compressed_fb)
+		goto err;
+
+	cfb_base = i915_stolen_to_phys(dev, compressed_fb->start);
+	if (!cfb_base)
+		goto err_fb;
+
+	if (!(IS_GM45(dev) || HAS_PCH_SPLIT(dev))) {
+		compressed_llb = drm_mm_search_free(&dev_priv->mm.stolen,
+						    4096, 4096, 0);
+		if (compressed_llb)
+			compressed_llb = drm_mm_get_block(compressed_llb,
+							  4096, 4096);
+		if (!compressed_llb)
+			goto err_fb;
+
+		ll_base = i915_stolen_to_phys(dev, compressed_llb->start);
+		if (!ll_base)
+			goto err_llb;
+	}
+
+	dev_priv->cfb_size = size;
+
+	dev_priv->compressed_fb = compressed_fb;
+	if (HAS_PCH_SPLIT(dev))
+		I915_WRITE(ILK_DPFC_CB_BASE, compressed_fb->start);
+	else if (IS_GM45(dev)) {
+		I915_WRITE(DPFC_CB_BASE, compressed_fb->start);
+	} else {
+		I915_WRITE(FBC_CFB_BASE, cfb_base);
+		I915_WRITE(FBC_LL_BASE, ll_base);
+		dev_priv->compressed_llb = compressed_llb;
+	}
+
+	DRM_DEBUG_KMS("FBC base 0x%08lx, ll base 0x%08lx, size %dM\n",
+		      cfb_base, ll_base, size >> 20);
+	return;
+
+err_llb:
+	drm_mm_put_block(compressed_llb);
+err_fb:
+	drm_mm_put_block(compressed_fb);
+err:
+	dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
+	i915_warn_stolen(dev);
+}
+
+static void i915_cleanup_compression(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	drm_mm_put_block(dev_priv->compressed_fb);
+	if (dev_priv->compressed_llb)
+		drm_mm_put_block(dev_priv->compressed_llb);
+}
+
+void i915_gem_cleanup_stolen(struct drm_device *dev)
+{
+	if (I915_HAS_FBC(dev) && i915_powersave)
+		i915_cleanup_compression(dev);
+}
+
+int i915_gem_init_stolen(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long prealloc_size = dev_priv->mm.gtt->stolen_size;
+
+	/* Basic memrange allocator for stolen space */
+	drm_mm_init(&dev_priv->mm.stolen, 0, prealloc_size);
+
+	/* Try to set up FBC with a reasonable compressed buffer size */
+	if (I915_HAS_FBC(dev) && i915_powersave) {
+		int cfb_size;
+
+		/* Leave 1M for line length buffer & misc. */
+
+		/* Try to get a 32M buffer... */
+		if (prealloc_size > (36*1024*1024))
+			cfb_size = 32*1024*1024;
+		else /* fall back to 7/8 of the stolen space */
+			cfb_size = prealloc_size * 7 / 8;
+		i915_setup_compression(dev, cfb_size);
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index 1a9306665987..b964df51cec7 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -354,9 +354,15 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		/* We need to rebind the object if its current allocation
 		 * no longer meets the alignment restrictions for its new
 		 * tiling mode. Otherwise we can just leave it alone, but
-		 * need to ensure that any fence register is cleared.
+		 * need to ensure that any fence register is updated before
+		 * the next fenced (either through the GTT or by the BLT unit
+		 * on older GPUs) access.
+		 *
+		 * After updating the tiling parameters, we then flag whether
+		 * we need to update an associated fence register. Note this
+		 * has to also include the unfenced register the GPU uses
+		 * whilst executing a fenced command for an untiled object.
 		 */
-		i915_gem_release_mmap(obj);
 
 		obj->map_and_fenceable =
 			obj->gtt_space == NULL ||
@@ -374,9 +380,15 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		}
 
 		if (ret == 0) {
-			obj->tiling_changed = true;
+			obj->fence_dirty =
+				obj->fenced_gpu_access ||
+				obj->fence_reg != I915_FENCE_REG_NONE;
+
 			obj->tiling_mode = args->tiling_mode;
 			obj->stride = args->stride;
+
+			/* Force the fence to be reacquired for GTT access */
+			i915_gem_release_mmap(obj);
 		}
 	}
 	/* we have to maintain this existing ABI... */
diff --git a/drivers/gpu/drm/i915/i915_ioc32.c b/drivers/gpu/drm/i915/i915_ioc32.c
index 13b028994b2b..0e72abb9f701 100644
--- a/drivers/gpu/drm/i915/i915_ioc32.c
+++ b/drivers/gpu/drm/i915/i915_ioc32.c
@@ -34,6 +34,7 @@
 #include "drmP.h"
 #include "drm.h"
 #include "i915_drm.h"
+#include "i915_drv.h"
 
 typedef struct _drm_i915_batchbuffer32 {
 	int start;		/* agp offset */
@@ -181,7 +182,7 @@ static int compat_i915_alloc(struct file *file, unsigned int cmd,
 			 (unsigned long)request);
 }
 
-drm_ioctl_compat_t *i915_compat_ioctls[] = {
+static drm_ioctl_compat_t *i915_compat_ioctls[] = {
 	[DRM_I915_BATCHBUFFER] = compat_i915_batchbuffer,
 	[DRM_I915_CMDBUFFER] = compat_i915_cmdbuffer,
 	[DRM_I915_GETPARAM] = compat_i915_getparam,
@@ -189,6 +190,7 @@ drm_ioctl_compat_t *i915_compat_ioctls[] = {
 	[DRM_I915_ALLOC] = compat_i915_alloc
 };
 
+#ifdef CONFIG_COMPAT
 /**
  * Called whenever a 32-bit process running under a 64-bit kernel
  * performs an ioctl on /dev/dri/card<n>.
@@ -217,3 +219,4 @@ long i915_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 
 	return ret;
 }
+#endif
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index afd4e03e337e..cc4a63307611 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -26,6 +26,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/sysrq.h>
 #include <linux/slab.h>
 #include "drmP.h"
@@ -35,35 +37,6 @@
 #include "i915_trace.h"
 #include "intel_drv.h"
 
-#define MAX_NOPID ((u32)~0)
-
-/**
- * Interrupts that are always left unmasked.
- *
- * Since pipe events are edge-triggered from the PIPESTAT register to IIR,
- * we leave them always unmasked in IMR and then control enabling them through
- * PIPESTAT alone.
- */
-#define I915_INTERRUPT_ENABLE_FIX			\
-	(I915_ASLE_INTERRUPT |				\
-	 I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |		\
-	 I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |		\
-	 I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |	\
-	 I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |	\
-	 I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
-
-/** Interrupts that we mask and unmask at runtime. */
-#define I915_INTERRUPT_ENABLE_VAR (I915_USER_INTERRUPT | I915_BSD_USER_INTERRUPT)
-
-#define I915_PIPE_VBLANK_STATUS	(PIPE_START_VBLANK_INTERRUPT_STATUS |\
-				 PIPE_VBLANK_INTERRUPT_STATUS)
-
-#define I915_PIPE_VBLANK_ENABLE	(PIPE_START_VBLANK_INTERRUPT_ENABLE |\
-				 PIPE_VBLANK_INTERRUPT_ENABLE)
-
-#define DRM_I915_VBLANK_PIPE_ALL	(DRM_I915_VBLANK_PIPE_A | \
-					 DRM_I915_VBLANK_PIPE_B)
-
 /* For display hotplug interrupt */
 static void
 ironlake_enable_display_irq(drm_i915_private_t *dev_priv, u32 mask)
@@ -118,6 +91,10 @@ void intel_enable_asle(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	unsigned long irqflags;
 
+	/* FIXME: opregion/asle for VLV */
+	if (IS_VALLEYVIEW(dev))
+		return;
+
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
 
 	if (HAS_PCH_SPLIT(dev))
@@ -354,15 +331,12 @@ static void notify_ring(struct drm_device *dev,
 			struct intel_ring_buffer *ring)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 seqno;
 
 	if (ring->obj == NULL)
 		return;
 
-	seqno = ring->get_seqno(ring);
-	trace_i915_gem_request_complete(ring, seqno);
+	trace_i915_gem_request_complete(ring, ring->get_seqno(ring));
 
-	ring->irq_seqno = seqno;
 	wake_up_all(&ring->irq_queue);
 	if (i915_enable_hangcheck) {
 		dev_priv->hangcheck_count = 0;
@@ -424,13 +398,145 @@ static void gen6_pm_rps_work(struct work_struct *work)
 	mutex_unlock(&dev_priv->dev->struct_mutex);
 }
 
-static void pch_irq_handler(struct drm_device *dev)
+static void snb_gt_irq_handler(struct drm_device *dev,
+			       struct drm_i915_private *dev_priv,
+			       u32 gt_iir)
+{
+
+	if (gt_iir & (GEN6_RENDER_USER_INTERRUPT |
+		      GEN6_RENDER_PIPE_CONTROL_NOTIFY_INTERRUPT))
+		notify_ring(dev, &dev_priv->ring[RCS]);
+	if (gt_iir & GEN6_BSD_USER_INTERRUPT)
+		notify_ring(dev, &dev_priv->ring[VCS]);
+	if (gt_iir & GEN6_BLITTER_USER_INTERRUPT)
+		notify_ring(dev, &dev_priv->ring[BCS]);
+
+	if (gt_iir & (GT_GEN6_BLT_CS_ERROR_INTERRUPT |
+		      GT_GEN6_BSD_CS_ERROR_INTERRUPT |
+		      GT_RENDER_CS_ERROR_INTERRUPT)) {
+		DRM_ERROR("GT error interrupt 0x%08x\n", gt_iir);
+		i915_handle_error(dev, false);
+	}
+}
+
+static void gen6_queue_rps_work(struct drm_i915_private *dev_priv,
+				u32 pm_iir)
+{
+	unsigned long flags;
+
+	/*
+	 * IIR bits should never already be set because IMR should
+	 * prevent an interrupt from being shown in IIR. The warning
+	 * displays a case where we've unsafely cleared
+	 * dev_priv->pm_iir. Although missing an interrupt of the same
+	 * type is not a problem, it displays a problem in the logic.
+	 *
+	 * The mask bit in IMR is cleared by rps_work.
+	 */
+
+	spin_lock_irqsave(&dev_priv->rps_lock, flags);
+	WARN(dev_priv->pm_iir & pm_iir, "Missed a PM interrupt\n");
+	dev_priv->pm_iir |= pm_iir;
+	I915_WRITE(GEN6_PMIMR, dev_priv->pm_iir);
+	POSTING_READ(GEN6_PMIMR);
+	spin_unlock_irqrestore(&dev_priv->rps_lock, flags);
+
+	queue_work(dev_priv->wq, &dev_priv->rps_work);
+}
+
+static irqreturn_t valleyview_irq_handler(DRM_IRQ_ARGS)
 {
+	struct drm_device *dev = (struct drm_device *) arg;
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 pch_iir;
+	u32 iir, gt_iir, pm_iir;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned long irqflags;
 	int pipe;
+	u32 pipe_stats[I915_MAX_PIPES];
+	u32 vblank_status;
+	int vblank = 0;
+	bool blc_event;
 
-	pch_iir = I915_READ(SDEIIR);
+	atomic_inc(&dev_priv->irq_received);
+
+	vblank_status = PIPE_START_VBLANK_INTERRUPT_STATUS |
+		PIPE_VBLANK_INTERRUPT_STATUS;
+
+	while (true) {
+		iir = I915_READ(VLV_IIR);
+		gt_iir = I915_READ(GTIIR);
+		pm_iir = I915_READ(GEN6_PMIIR);
+
+		if (gt_iir == 0 && pm_iir == 0 && iir == 0)
+			goto out;
+
+		ret = IRQ_HANDLED;
+
+		snb_gt_irq_handler(dev, dev_priv, gt_iir);
+
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/*
+			 * Clear the PIPE*STAT regs before the IIR
+			 */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		/* Consume port.  Then clear IIR or we'll miss events */
+		if (iir & I915_DISPLAY_PORT_INTERRUPT) {
+			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+
+			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
+					 hotplug_status);
+			if (hotplug_status & dev_priv->hotplug_supported_mask)
+				queue_work(dev_priv->wq,
+					   &dev_priv->hotplug_work);
+
+			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
+			I915_READ(PORT_HOTPLUG_STAT);
+		}
+
+
+		if (iir & I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT) {
+			drm_handle_vblank(dev, 0);
+			vblank++;
+			intel_finish_page_flip(dev, 0);
+		}
+
+		if (iir & I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT) {
+			drm_handle_vblank(dev, 1);
+			vblank++;
+			intel_finish_page_flip(dev, 0);
+		}
+
+		if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
+			blc_event = true;
+
+		if (pm_iir & GEN6_PM_DEFERRED_EVENTS)
+			gen6_queue_rps_work(dev_priv, pm_iir);
+
+		I915_WRITE(GTIIR, gt_iir);
+		I915_WRITE(GEN6_PMIIR, pm_iir);
+		I915_WRITE(VLV_IIR, iir);
+	}
+
+out:
+	return ret;
+}
+
+static void pch_irq_handler(struct drm_device *dev, u32 pch_iir)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
 
 	if (pch_iir & SDE_AUDIO_POWER_MASK)
 		DRM_DEBUG_DRIVER("PCH audio power change on port %d\n",
@@ -471,91 +577,77 @@ static irqreturn_t ivybridge_irq_handler(DRM_IRQ_ARGS)
 {
 	struct drm_device *dev = (struct drm_device *) arg;
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	int ret = IRQ_NONE;
-	u32 de_iir, gt_iir, de_ier, pch_iir, pm_iir;
-	struct drm_i915_master_private *master_priv;
+	u32 de_iir, gt_iir, de_ier, pm_iir;
+	irqreturn_t ret = IRQ_NONE;
+	int i;
 
 	atomic_inc(&dev_priv->irq_received);
 
 	/* disable master interrupt before clearing iir  */
 	de_ier = I915_READ(DEIER);
 	I915_WRITE(DEIER, de_ier & ~DE_MASTER_IRQ_CONTROL);
-	POSTING_READ(DEIER);
 
-	de_iir = I915_READ(DEIIR);
 	gt_iir = I915_READ(GTIIR);
-	pch_iir = I915_READ(SDEIIR);
-	pm_iir = I915_READ(GEN6_PMIIR);
-
-	if (de_iir == 0 && gt_iir == 0 && pch_iir == 0 && pm_iir == 0)
-		goto done;
-
-	ret = IRQ_HANDLED;
-
-	if (dev->primary->master) {
-		master_priv = dev->primary->master->driver_priv;
-		if (master_priv->sarea_priv)
-			master_priv->sarea_priv->last_dispatch =
-				READ_BREADCRUMB(dev_priv);
+	if (gt_iir) {
+		snb_gt_irq_handler(dev, dev_priv, gt_iir);
+		I915_WRITE(GTIIR, gt_iir);
+		ret = IRQ_HANDLED;
 	}
 
-	if (gt_iir & (GT_USER_INTERRUPT | GT_PIPE_NOTIFY))
-		notify_ring(dev, &dev_priv->ring[RCS]);
-	if (gt_iir & GT_GEN6_BSD_USER_INTERRUPT)
-		notify_ring(dev, &dev_priv->ring[VCS]);
-	if (gt_iir & GT_BLT_USER_INTERRUPT)
-		notify_ring(dev, &dev_priv->ring[BCS]);
-
-	if (de_iir & DE_GSE_IVB)
-		intel_opregion_gse_intr(dev);
-
-	if (de_iir & DE_PLANEA_FLIP_DONE_IVB) {
-		intel_prepare_page_flip(dev, 0);
-		intel_finish_page_flip_plane(dev, 0);
-	}
+	de_iir = I915_READ(DEIIR);
+	if (de_iir) {
+		if (de_iir & DE_GSE_IVB)
+			intel_opregion_gse_intr(dev);
+
+		for (i = 0; i < 3; i++) {
+			if (de_iir & (DE_PLANEA_FLIP_DONE_IVB << (5 * i))) {
+				intel_prepare_page_flip(dev, i);
+				intel_finish_page_flip_plane(dev, i);
+			}
+			if (de_iir & (DE_PIPEA_VBLANK_IVB << (5 * i)))
+				drm_handle_vblank(dev, i);
+		}
 
-	if (de_iir & DE_PLANEB_FLIP_DONE_IVB) {
-		intel_prepare_page_flip(dev, 1);
-		intel_finish_page_flip_plane(dev, 1);
-	}
+		/* check event from PCH */
+		if (de_iir & DE_PCH_EVENT_IVB) {
+			u32 pch_iir = I915_READ(SDEIIR);
 
-	if (de_iir & DE_PIPEA_VBLANK_IVB)
-		drm_handle_vblank(dev, 0);
+			if (pch_iir & SDE_HOTPLUG_MASK_CPT)
+				queue_work(dev_priv->wq, &dev_priv->hotplug_work);
+			pch_irq_handler(dev, pch_iir);
 
-	if (de_iir & DE_PIPEB_VBLANK_IVB)
-		drm_handle_vblank(dev, 1);
+			/* clear PCH hotplug event before clear CPU irq */
+			I915_WRITE(SDEIIR, pch_iir);
+		}
 
-	/* check event from PCH */
-	if (de_iir & DE_PCH_EVENT_IVB) {
-		if (pch_iir & SDE_HOTPLUG_MASK_CPT)
-			queue_work(dev_priv->wq, &dev_priv->hotplug_work);
-		pch_irq_handler(dev);
+		I915_WRITE(DEIIR, de_iir);
+		ret = IRQ_HANDLED;
 	}
 
-	if (pm_iir & GEN6_PM_DEFERRED_EVENTS) {
-		unsigned long flags;
-		spin_lock_irqsave(&dev_priv->rps_lock, flags);
-		WARN(dev_priv->pm_iir & pm_iir, "Missed a PM interrupt\n");
-		dev_priv->pm_iir |= pm_iir;
-		I915_WRITE(GEN6_PMIMR, dev_priv->pm_iir);
-		POSTING_READ(GEN6_PMIMR);
-		spin_unlock_irqrestore(&dev_priv->rps_lock, flags);
-		queue_work(dev_priv->wq, &dev_priv->rps_work);
+	pm_iir = I915_READ(GEN6_PMIIR);
+	if (pm_iir) {
+		if (pm_iir & GEN6_PM_DEFERRED_EVENTS)
+			gen6_queue_rps_work(dev_priv, pm_iir);
+		I915_WRITE(GEN6_PMIIR, pm_iir);
+		ret = IRQ_HANDLED;
 	}
 
-	/* should clear PCH hotplug event before clear CPU irq */
-	I915_WRITE(SDEIIR, pch_iir);
-	I915_WRITE(GTIIR, gt_iir);
-	I915_WRITE(DEIIR, de_iir);
-	I915_WRITE(GEN6_PMIIR, pm_iir);
-
-done:
 	I915_WRITE(DEIER, de_ier);
 	POSTING_READ(DEIER);
 
 	return ret;
 }
 
+static void ilk_gt_irq_handler(struct drm_device *dev,
+			       struct drm_i915_private *dev_priv,
+			       u32 gt_iir)
+{
+	if (gt_iir & (GT_USER_INTERRUPT | GT_PIPE_NOTIFY))
+		notify_ring(dev, &dev_priv->ring[RCS]);
+	if (gt_iir & GT_BSD_USER_INTERRUPT)
+		notify_ring(dev, &dev_priv->ring[VCS]);
+}
+
 static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 {
 	struct drm_device *dev = (struct drm_device *) arg;
@@ -563,14 +655,9 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 	int ret = IRQ_NONE;
 	u32 de_iir, gt_iir, de_ier, pch_iir, pm_iir;
 	u32 hotplug_mask;
-	struct drm_i915_master_private *master_priv;
-	u32 bsd_usr_interrupt = GT_BSD_USER_INTERRUPT;
 
 	atomic_inc(&dev_priv->irq_received);
 
-	if (IS_GEN6(dev))
-		bsd_usr_interrupt = GT_GEN6_BSD_USER_INTERRUPT;
-
 	/* disable master interrupt before clearing iir  */
 	de_ier = I915_READ(DEIER);
 	I915_WRITE(DEIER, de_ier & ~DE_MASTER_IRQ_CONTROL);
@@ -592,19 +679,10 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 
 	ret = IRQ_HANDLED;
 
-	if (dev->primary->master) {
-		master_priv = dev->primary->master->driver_priv;
-		if (master_priv->sarea_priv)
-			master_priv->sarea_priv->last_dispatch =
-				READ_BREADCRUMB(dev_priv);
-	}
-
-	if (gt_iir & (GT_USER_INTERRUPT | GT_PIPE_NOTIFY))
-		notify_ring(dev, &dev_priv->ring[RCS]);
-	if (gt_iir & bsd_usr_interrupt)
-		notify_ring(dev, &dev_priv->ring[VCS]);
-	if (gt_iir & GT_BLT_USER_INTERRUPT)
-		notify_ring(dev, &dev_priv->ring[BCS]);
+	if (IS_GEN5(dev))
+		ilk_gt_irq_handler(dev, dev_priv, gt_iir);
+	else
+		snb_gt_irq_handler(dev, dev_priv, gt_iir);
 
 	if (de_iir & DE_GSE)
 		intel_opregion_gse_intr(dev);
@@ -629,7 +707,7 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 	if (de_iir & DE_PCH_EVENT) {
 		if (pch_iir & hotplug_mask)
 			queue_work(dev_priv->wq, &dev_priv->hotplug_work);
-		pch_irq_handler(dev);
+		pch_irq_handler(dev, pch_iir);
 	}
 
 	if (de_iir & DE_PCU_EVENT) {
@@ -637,25 +715,8 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 		i915_handle_rps_change(dev);
 	}
 
-	if (IS_GEN6(dev) && pm_iir & GEN6_PM_DEFERRED_EVENTS) {
-		/*
-		 * IIR bits should never already be set because IMR should
-		 * prevent an interrupt from being shown in IIR. The warning
-		 * displays a case where we've unsafely cleared
-		 * dev_priv->pm_iir. Although missing an interrupt of the same
-		 * type is not a problem, it displays a problem in the logic.
-		 *
-		 * The mask bit in IMR is cleared by rps_work.
-		 */
-		unsigned long flags;
-		spin_lock_irqsave(&dev_priv->rps_lock, flags);
-		WARN(dev_priv->pm_iir & pm_iir, "Missed a PM interrupt\n");
-		dev_priv->pm_iir |= pm_iir;
-		I915_WRITE(GEN6_PMIMR, dev_priv->pm_iir);
-		POSTING_READ(GEN6_PMIMR);
-		spin_unlock_irqrestore(&dev_priv->rps_lock, flags);
-		queue_work(dev_priv->wq, &dev_priv->rps_work);
-	}
+	if (IS_GEN6(dev) && pm_iir & GEN6_PM_DEFERRED_EVENTS)
+		gen6_queue_rps_work(dev_priv, pm_iir);
 
 	/* should clear PCH hotplug event before clear CPU irq */
 	I915_WRITE(SDEIIR, pch_iir);
@@ -691,7 +752,7 @@ static void i915_error_work_func(struct work_struct *work)
 	if (atomic_read(&dev_priv->mm.wedged)) {
 		DRM_DEBUG_DRIVER("resetting chip\n");
 		kobject_uevent_env(&dev->primary->kdev.kobj, KOBJ_CHANGE, reset_event);
-		if (!i915_reset(dev, GRDOM_RENDER)) {
+		if (!i915_reset(dev)) {
 			atomic_set(&dev_priv->mm.wedged, 0);
 			kobject_uevent_env(&dev->primary->kdev.kobj, KOBJ_CHANGE, reset_done_event);
 		}
@@ -727,7 +788,8 @@ i915_error_object_create(struct drm_i915_private *dev_priv,
 			goto unwind;
 
 		local_irq_save(flags);
-		if (reloc_offset < dev_priv->mm.gtt_mappable_end) {
+		if (reloc_offset < dev_priv->mm.gtt_mappable_end &&
+		    src->has_global_gtt_mapping) {
 			void __iomem *s;
 
 			/* Simply ignore tiling or any overlapping fence.
@@ -782,10 +844,11 @@ i915_error_object_free(struct drm_i915_error_object *obj)
 	kfree(obj);
 }
 
-static void
-i915_error_state_free(struct drm_device *dev,
-		      struct drm_i915_error_state *error)
+void
+i915_error_state_free(struct kref *error_ref)
 {
+	struct drm_i915_error_state *error = container_of(error_ref,
+							  typeof(*error), ref);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(error->ring); i++) {
@@ -798,37 +861,56 @@ i915_error_state_free(struct drm_device *dev,
 	kfree(error->overlay);
 	kfree(error);
 }
+static void capture_bo(struct drm_i915_error_buffer *err,
+		       struct drm_i915_gem_object *obj)
+{
+	err->size = obj->base.size;
+	err->name = obj->base.name;
+	err->seqno = obj->last_rendering_seqno;
+	err->gtt_offset = obj->gtt_offset;
+	err->read_domains = obj->base.read_domains;
+	err->write_domain = obj->base.write_domain;
+	err->fence_reg = obj->fence_reg;
+	err->pinned = 0;
+	if (obj->pin_count > 0)
+		err->pinned = 1;
+	if (obj->user_pin_count > 0)
+		err->pinned = -1;
+	err->tiling = obj->tiling_mode;
+	err->dirty = obj->dirty;
+	err->purgeable = obj->madv != I915_MADV_WILLNEED;
+	err->ring = obj->ring ? obj->ring->id : -1;
+	err->cache_level = obj->cache_level;
+}
 
-static u32 capture_bo_list(struct drm_i915_error_buffer *err,
-			   int count,
-			   struct list_head *head)
+static u32 capture_active_bo(struct drm_i915_error_buffer *err,
+			     int count, struct list_head *head)
 {
 	struct drm_i915_gem_object *obj;
 	int i = 0;
 
 	list_for_each_entry(obj, head, mm_list) {
-		err->size = obj->base.size;
-		err->name = obj->base.name;
-		err->seqno = obj->last_rendering_seqno;
-		err->gtt_offset = obj->gtt_offset;
-		err->read_domains = obj->base.read_domains;
-		err->write_domain = obj->base.write_domain;
-		err->fence_reg = obj->fence_reg;
-		err->pinned = 0;
-		if (obj->pin_count > 0)
-			err->pinned = 1;
-		if (obj->user_pin_count > 0)
-			err->pinned = -1;
-		err->tiling = obj->tiling_mode;
-		err->dirty = obj->dirty;
-		err->purgeable = obj->madv != I915_MADV_WILLNEED;
-		err->ring = obj->ring ? obj->ring->id : -1;
-		err->cache_level = obj->cache_level;
-
+		capture_bo(err++, obj);
 		if (++i == count)
 			break;
+	}
 
-		err++;
+	return i;
+}
+
+static u32 capture_pinned_bo(struct drm_i915_error_buffer *err,
+			     int count, struct list_head *head)
+{
+	struct drm_i915_gem_object *obj;
+	int i = 0;
+
+	list_for_each_entry(obj, head, gtt_list) {
+		if (obj->pin_count == 0)
+			continue;
+
+		capture_bo(err++, obj);
+		if (++i == count)
+			break;
 	}
 
 	return i;
@@ -901,7 +983,6 @@ static void i915_record_ring_state(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	if (INTEL_INFO(dev)->gen >= 6) {
-		error->faddr[ring->id] = I915_READ(RING_DMA_FADD(ring->mmio_base));
 		error->fault_reg[ring->id] = I915_READ(RING_FAULT_REG(ring));
 		error->semaphore_mboxes[ring->id][0]
 			= I915_READ(RING_SYNC_0(ring->mmio_base));
@@ -910,6 +991,7 @@ static void i915_record_ring_state(struct drm_device *dev,
 	}
 
 	if (INTEL_INFO(dev)->gen >= 4) {
+		error->faddr[ring->id] = I915_READ(RING_DMA_FADD(ring->mmio_base));
 		error->ipeir[ring->id] = I915_READ(RING_IPEIR(ring->mmio_base));
 		error->ipehr[ring->id] = I915_READ(RING_IPEHR(ring->mmio_base));
 		error->instdone[ring->id] = I915_READ(RING_INSTDONE(ring->mmio_base));
@@ -919,11 +1001,13 @@ static void i915_record_ring_state(struct drm_device *dev,
 			error->bbaddr = I915_READ64(BB_ADDR);
 		}
 	} else {
+		error->faddr[ring->id] = I915_READ(DMA_FADD_I8XX);
 		error->ipeir[ring->id] = I915_READ(IPEIR);
 		error->ipehr[ring->id] = I915_READ(IPEHR);
 		error->instdone[ring->id] = I915_READ(INSTDONE);
 	}
 
+	error->waiting[ring->id] = waitqueue_active(&ring->irq_queue);
 	error->instpm[ring->id] = I915_READ(RING_INSTPM(ring->mmio_base));
 	error->seqno[ring->id] = ring->get_seqno(ring);
 	error->acthd[ring->id] = intel_ring_get_active_head(ring);
@@ -938,15 +1022,11 @@ static void i915_gem_record_rings(struct drm_device *dev,
 				  struct drm_i915_error_state *error)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	struct drm_i915_gem_request *request;
 	int i, count;
 
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		struct intel_ring_buffer *ring = &dev_priv->ring[i];
-
-		if (ring->obj == NULL)
-			continue;
-
+	for_each_ring(ring, dev_priv, i) {
 		i915_record_ring_state(dev, error, ring);
 
 		error->ring[i].batchbuffer =
@@ -1013,8 +1093,19 @@ static void i915_capture_error_state(struct drm_device *dev)
 	DRM_INFO("capturing error event; look for more information in /debug/dri/%d/i915_error_state\n",
 		 dev->primary->index);
 
+	kref_init(&error->ref);
 	error->eir = I915_READ(EIR);
 	error->pgtbl_er = I915_READ(PGTBL_ER);
+
+	if (HAS_PCH_SPLIT(dev))
+		error->ier = I915_READ(DEIER) | I915_READ(GTIER);
+	else if (IS_VALLEYVIEW(dev))
+		error->ier = I915_READ(GTIER) | I915_READ(VLV_IER);
+	else if (IS_GEN2(dev))
+		error->ier = I915_READ16(IER);
+	else
+		error->ier = I915_READ(IER);
+
 	for_each_pipe(pipe)
 		error->pipestat[pipe] = I915_READ(PIPESTAT(pipe));
 
@@ -1034,8 +1125,9 @@ static void i915_capture_error_state(struct drm_device *dev)
 	list_for_each_entry(obj, &dev_priv->mm.active_list, mm_list)
 		i++;
 	error->active_bo_count = i;
-	list_for_each_entry(obj, &dev_priv->mm.pinned_list, mm_list)
-		i++;
+	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list)
+		if (obj->pin_count)
+			i++;
 	error->pinned_bo_count = i - error->active_bo_count;
 
 	error->active_bo = NULL;
@@ -1050,15 +1142,15 @@ static void i915_capture_error_state(struct drm_device *dev)
 
 	if (error->active_bo)
 		error->active_bo_count =
-			capture_bo_list(error->active_bo,
-					error->active_bo_count,
-					&dev_priv->mm.active_list);
+			capture_active_bo(error->active_bo,
+					  error->active_bo_count,
+					  &dev_priv->mm.active_list);
 
 	if (error->pinned_bo)
 		error->pinned_bo_count =
-			capture_bo_list(error->pinned_bo,
-					error->pinned_bo_count,
-					&dev_priv->mm.pinned_list);
+			capture_pinned_bo(error->pinned_bo,
+					  error->pinned_bo_count,
+					  &dev_priv->mm.gtt_list);
 
 	do_gettimeofday(&error->time);
 
@@ -1073,7 +1165,7 @@ static void i915_capture_error_state(struct drm_device *dev)
 	spin_unlock_irqrestore(&dev_priv->error_lock, flags);
 
 	if (error)
-		i915_error_state_free(dev, error);
+		i915_error_state_free(&error->ref);
 }
 
 void i915_destroy_error_state(struct drm_device *dev)
@@ -1088,7 +1180,7 @@ void i915_destroy_error_state(struct drm_device *dev)
 	spin_unlock_irqrestore(&dev_priv->error_lock, flags);
 
 	if (error)
-		i915_error_state_free(dev, error);
+		kref_put(&error->ref, i915_error_state_free);
 }
 #else
 #define i915_capture_error_state(x)
@@ -1103,33 +1195,26 @@ static void i915_report_and_clear_eir(struct drm_device *dev)
 	if (!eir)
 		return;
 
-	printk(KERN_ERR "render error detected, EIR: 0x%08x\n",
-	       eir);
+	pr_err("render error detected, EIR: 0x%08x\n", eir);
 
 	if (IS_G4X(dev)) {
 		if (eir & (GM45_ERROR_MEM_PRIV | GM45_ERROR_CP_PRIV)) {
 			u32 ipeir = I915_READ(IPEIR_I965);
 
-			printk(KERN_ERR "  IPEIR: 0x%08x\n",
-			       I915_READ(IPEIR_I965));
-			printk(KERN_ERR "  IPEHR: 0x%08x\n",
-			       I915_READ(IPEHR_I965));
-			printk(KERN_ERR "  INSTDONE: 0x%08x\n",
+			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR_I965));
+			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR_I965));
+			pr_err("  INSTDONE: 0x%08x\n",
 			       I915_READ(INSTDONE_I965));
-			printk(KERN_ERR "  INSTPS: 0x%08x\n",
-			       I915_READ(INSTPS));
-			printk(KERN_ERR "  INSTDONE1: 0x%08x\n",
-			       I915_READ(INSTDONE1));
-			printk(KERN_ERR "  ACTHD: 0x%08x\n",
-			       I915_READ(ACTHD_I965));
+			pr_err("  INSTPS: 0x%08x\n", I915_READ(INSTPS));
+			pr_err("  INSTDONE1: 0x%08x\n", I915_READ(INSTDONE1));
+			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD_I965));
 			I915_WRITE(IPEIR_I965, ipeir);
 			POSTING_READ(IPEIR_I965);
 		}
 		if (eir & GM45_ERROR_PAGE_TABLE) {
 			u32 pgtbl_err = I915_READ(PGTBL_ER);
-			printk(KERN_ERR "page table error\n");
-			printk(KERN_ERR "  PGTBL_ER: 0x%08x\n",
-			       pgtbl_err);
+			pr_err("page table error\n");
+			pr_err("  PGTBL_ER: 0x%08x\n", pgtbl_err);
 			I915_WRITE(PGTBL_ER, pgtbl_err);
 			POSTING_READ(PGTBL_ER);
 		}
@@ -1138,53 +1223,42 @@ static void i915_report_and_clear_eir(struct drm_device *dev)
 	if (!IS_GEN2(dev)) {
 		if (eir & I915_ERROR_PAGE_TABLE) {
 			u32 pgtbl_err = I915_READ(PGTBL_ER);
-			printk(KERN_ERR "page table error\n");
-			printk(KERN_ERR "  PGTBL_ER: 0x%08x\n",
-			       pgtbl_err);
+			pr_err("page table error\n");
+			pr_err("  PGTBL_ER: 0x%08x\n", pgtbl_err);
 			I915_WRITE(PGTBL_ER, pgtbl_err);
 			POSTING_READ(PGTBL_ER);
 		}
 	}
 
 	if (eir & I915_ERROR_MEMORY_REFRESH) {
-		printk(KERN_ERR "memory refresh error:\n");
+		pr_err("memory refresh error:\n");
 		for_each_pipe(pipe)
-			printk(KERN_ERR "pipe %c stat: 0x%08x\n",
+			pr_err("pipe %c stat: 0x%08x\n",
 			       pipe_name(pipe), I915_READ(PIPESTAT(pipe)));
 		/* pipestat has already been acked */
 	}
 	if (eir & I915_ERROR_INSTRUCTION) {
-		printk(KERN_ERR "instruction error\n");
-		printk(KERN_ERR "  INSTPM: 0x%08x\n",
-		       I915_READ(INSTPM));
+		pr_err("instruction error\n");
+		pr_err("  INSTPM: 0x%08x\n", I915_READ(INSTPM));
 		if (INTEL_INFO(dev)->gen < 4) {
 			u32 ipeir = I915_READ(IPEIR);
 
-			printk(KERN_ERR "  IPEIR: 0x%08x\n",
-			       I915_READ(IPEIR));
-			printk(KERN_ERR "  IPEHR: 0x%08x\n",
-			       I915_READ(IPEHR));
-			printk(KERN_ERR "  INSTDONE: 0x%08x\n",
-			       I915_READ(INSTDONE));
-			printk(KERN_ERR "  ACTHD: 0x%08x\n",
-			       I915_READ(ACTHD));
+			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR));
+			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR));
+			pr_err("  INSTDONE: 0x%08x\n", I915_READ(INSTDONE));
+			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD));
 			I915_WRITE(IPEIR, ipeir);
 			POSTING_READ(IPEIR);
 		} else {
 			u32 ipeir = I915_READ(IPEIR_I965);
 
-			printk(KERN_ERR "  IPEIR: 0x%08x\n",
-			       I915_READ(IPEIR_I965));
-			printk(KERN_ERR "  IPEHR: 0x%08x\n",
-			       I915_READ(IPEHR_I965));
-			printk(KERN_ERR "  INSTDONE: 0x%08x\n",
+			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR_I965));
+			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR_I965));
+			pr_err("  INSTDONE: 0x%08x\n",
 			       I915_READ(INSTDONE_I965));
-			printk(KERN_ERR "  INSTPS: 0x%08x\n",
-			       I915_READ(INSTPS));
-			printk(KERN_ERR "  INSTDONE1: 0x%08x\n",
-			       I915_READ(INSTDONE1));
-			printk(KERN_ERR "  ACTHD: 0x%08x\n",
-			       I915_READ(ACTHD_I965));
+			pr_err("  INSTPS: 0x%08x\n", I915_READ(INSTPS));
+			pr_err("  INSTDONE1: 0x%08x\n", I915_READ(INSTDONE1));
+			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD_I965));
 			I915_WRITE(IPEIR_I965, ipeir);
 			POSTING_READ(IPEIR_I965);
 		}
@@ -1217,6 +1291,8 @@ static void i915_report_and_clear_eir(struct drm_device *dev)
 void i915_handle_error(struct drm_device *dev, bool wedged)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
+	int i;
 
 	i915_capture_error_state(dev);
 	i915_report_and_clear_eir(dev);
@@ -1228,11 +1304,8 @@ void i915_handle_error(struct drm_device *dev, bool wedged)
 		/*
 		 * Wakeup waiting processes so they don't hang
 		 */
-		wake_up_all(&dev_priv->ring[RCS].irq_queue);
-		if (HAS_BSD(dev))
-			wake_up_all(&dev_priv->ring[VCS].irq_queue);
-		if (HAS_BLT(dev))
-			wake_up_all(&dev_priv->ring[BCS].irq_queue);
+		for_each_ring(ring, dev_priv, i)
+			wake_up_all(&ring->irq_queue);
 	}
 
 	queue_work(dev_priv->wq, &dev_priv->error_work);
@@ -1265,7 +1338,8 @@ static void i915_pageflip_stall_check(struct drm_device *dev, int pipe)
 	obj = work->pending_flip_obj;
 	if (INTEL_INFO(dev)->gen >= 4) {
 		int dspsurf = DSPSURF(intel_crtc->plane);
-		stall_detected = I915_READ(dspsurf) == obj->gtt_offset;
+		stall_detected = I915_HI_DISPBASE(I915_READ(dspsurf)) ==
+					obj->gtt_offset;
 	} else {
 		int dspaddr = DSPADDR(intel_crtc->plane);
 		stall_detected = I915_READ(dspaddr) == (obj->gtt_offset +
@@ -1281,248 +1355,6 @@ static void i915_pageflip_stall_check(struct drm_device *dev, int pipe)
 	}
 }
 
-static irqreturn_t i915_driver_irq_handler(DRM_IRQ_ARGS)
-{
-	struct drm_device *dev = (struct drm_device *) arg;
-	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	struct drm_i915_master_private *master_priv;
-	u32 iir, new_iir;
-	u32 pipe_stats[I915_MAX_PIPES];
-	u32 vblank_status;
-	int vblank = 0;
-	unsigned long irqflags;
-	int irq_received;
-	int ret = IRQ_NONE, pipe;
-	bool blc_event = false;
-
-	atomic_inc(&dev_priv->irq_received);
-
-	iir = I915_READ(IIR);
-
-	if (INTEL_INFO(dev)->gen >= 4)
-		vblank_status = PIPE_START_VBLANK_INTERRUPT_STATUS;
-	else
-		vblank_status = PIPE_VBLANK_INTERRUPT_STATUS;
-
-	for (;;) {
-		irq_received = iir != 0;
-
-		/* Can't rely on pipestat interrupt bit in iir as it might
-		 * have been cleared after the pipestat interrupt was received.
-		 * It doesn't set the bit in iir again, but it still produces
-		 * interrupts (for non-MSI).
-		 */
-		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
-			i915_handle_error(dev, false);
-
-		for_each_pipe(pipe) {
-			int reg = PIPESTAT(pipe);
-			pipe_stats[pipe] = I915_READ(reg);
-
-			/*
-			 * Clear the PIPE*STAT regs before the IIR
-			 */
-			if (pipe_stats[pipe] & 0x8000ffff) {
-				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
-					DRM_DEBUG_DRIVER("pipe %c underrun\n",
-							 pipe_name(pipe));
-				I915_WRITE(reg, pipe_stats[pipe]);
-				irq_received = 1;
-			}
-		}
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
-
-		if (!irq_received)
-			break;
-
-		ret = IRQ_HANDLED;
-
-		/* Consume port.  Then clear IIR or we'll miss events */
-		if ((I915_HAS_HOTPLUG(dev)) &&
-		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
-			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
-
-			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
-				  hotplug_status);
-			if (hotplug_status & dev_priv->hotplug_supported_mask)
-				queue_work(dev_priv->wq,
-					   &dev_priv->hotplug_work);
-
-			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
-			I915_READ(PORT_HOTPLUG_STAT);
-		}
-
-		I915_WRITE(IIR, iir);
-		new_iir = I915_READ(IIR); /* Flush posted writes */
-
-		if (dev->primary->master) {
-			master_priv = dev->primary->master->driver_priv;
-			if (master_priv->sarea_priv)
-				master_priv->sarea_priv->last_dispatch =
-					READ_BREADCRUMB(dev_priv);
-		}
-
-		if (iir & I915_USER_INTERRUPT)
-			notify_ring(dev, &dev_priv->ring[RCS]);
-		if (iir & I915_BSD_USER_INTERRUPT)
-			notify_ring(dev, &dev_priv->ring[VCS]);
-
-		if (iir & I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT) {
-			intel_prepare_page_flip(dev, 0);
-			if (dev_priv->flip_pending_is_done)
-				intel_finish_page_flip_plane(dev, 0);
-		}
-
-		if (iir & I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT) {
-			intel_prepare_page_flip(dev, 1);
-			if (dev_priv->flip_pending_is_done)
-				intel_finish_page_flip_plane(dev, 1);
-		}
-
-		for_each_pipe(pipe) {
-			if (pipe_stats[pipe] & vblank_status &&
-			    drm_handle_vblank(dev, pipe)) {
-				vblank++;
-				if (!dev_priv->flip_pending_is_done) {
-					i915_pageflip_stall_check(dev, pipe);
-					intel_finish_page_flip(dev, pipe);
-				}
-			}
-
-			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
-				blc_event = true;
-		}
-
-
-		if (blc_event || (iir & I915_ASLE_INTERRUPT))
-			intel_opregion_asle_intr(dev);
-
-		/* With MSI, interrupts are only generated when iir
-		 * transitions from zero to nonzero.  If another bit got
-		 * set while we were handling the existing iir bits, then
-		 * we would never get another interrupt.
-		 *
-		 * This is fine on non-MSI as well, as if we hit this path
-		 * we avoid exiting the interrupt handler only to generate
-		 * another one.
-		 *
-		 * Note that for MSI this could cause a stray interrupt report
-		 * if an interrupt landed in the time between writing IIR and
-		 * the posting read.  This should be rare enough to never
-		 * trigger the 99% of 100,000 interrupts test for disabling
-		 * stray interrupts.
-		 */
-		iir = new_iir;
-	}
-
-	return ret;
-}
-
-static int i915_emit_irq(struct drm_device * dev)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-
-	i915_kernel_lost_context(dev);
-
-	DRM_DEBUG_DRIVER("\n");
-
-	dev_priv->counter++;
-	if (dev_priv->counter > 0x7FFFFFFFUL)
-		dev_priv->counter = 1;
-	if (master_priv->sarea_priv)
-		master_priv->sarea_priv->last_enqueue = dev_priv->counter;
-
-	if (BEGIN_LP_RING(4) == 0) {
-		OUT_RING(MI_STORE_DWORD_INDEX);
-		OUT_RING(I915_BREADCRUMB_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
-		OUT_RING(dev_priv->counter);
-		OUT_RING(MI_USER_INTERRUPT);
-		ADVANCE_LP_RING();
-	}
-
-	return dev_priv->counter;
-}
-
-static int i915_wait_irq(struct drm_device * dev, int irq_nr)
-{
-	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-	int ret = 0;
-	struct intel_ring_buffer *ring = LP_RING(dev_priv);
-
-	DRM_DEBUG_DRIVER("irq_nr=%d breadcrumb=%d\n", irq_nr,
-		  READ_BREADCRUMB(dev_priv));
-
-	if (READ_BREADCRUMB(dev_priv) >= irq_nr) {
-		if (master_priv->sarea_priv)
-			master_priv->sarea_priv->last_dispatch = READ_BREADCRUMB(dev_priv);
-		return 0;
-	}
-
-	if (master_priv->sarea_priv)
-		master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
-
-	if (ring->irq_get(ring)) {
-		DRM_WAIT_ON(ret, ring->irq_queue, 3 * DRM_HZ,
-			    READ_BREADCRUMB(dev_priv) >= irq_nr);
-		ring->irq_put(ring);
-	} else if (wait_for(READ_BREADCRUMB(dev_priv) >= irq_nr, 3000))
-		ret = -EBUSY;
-
-	if (ret == -EBUSY) {
-		DRM_ERROR("EBUSY -- rec: %d emitted: %d\n",
-			  READ_BREADCRUMB(dev_priv), (int)dev_priv->counter);
-	}
-
-	return ret;
-}
-
-/* Needs the lock as it touches the ring.
- */
-int i915_irq_emit(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	drm_i915_irq_emit_t *emit = data;
-	int result;
-
-	if (!dev_priv || !LP_RING(dev_priv)->virtual_start) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	mutex_lock(&dev->struct_mutex);
-	result = i915_emit_irq(dev);
-	mutex_unlock(&dev->struct_mutex);
-
-	if (DRM_COPY_TO_USER(emit->irq_seq, &result, sizeof(int))) {
-		DRM_ERROR("copy_to_user\n");
-		return -EFAULT;
-	}
-
-	return 0;
-}
-
-/* Doesn't need the hardware lock.
- */
-int i915_irq_wait(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	drm_i915_irq_wait_t *irqwait = data;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	return i915_wait_irq(dev, irqwait->irq_seq);
-}
-
 /* Called from drm generic code, passed 'crtc' which
  * we use as a pipe index
  */
@@ -1544,7 +1376,7 @@ static int i915_enable_vblank(struct drm_device *dev, int pipe)
 
 	/* maintain vblank delivery even in deep C-states */
 	if (dev_priv->info->gen == 3)
-		I915_WRITE(INSTPM, INSTPM_AGPBUSY_DIS << 16);
+		I915_WRITE(INSTPM, _MASKED_BIT_DISABLE(INSTPM_AGPBUSY_DIS));
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
@@ -1575,8 +1407,34 @@ static int ivybridge_enable_vblank(struct drm_device *dev, int pipe)
 		return -EINVAL;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ironlake_enable_display_irq(dev_priv, (pipe == 0) ?
-				    DE_PIPEA_VBLANK_IVB : DE_PIPEB_VBLANK_IVB);
+	ironlake_enable_display_irq(dev_priv,
+				    DE_PIPEA_VBLANK_IVB << (5 * pipe));
+	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+	return 0;
+}
+
+static int valleyview_enable_vblank(struct drm_device *dev, int pipe)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	unsigned long irqflags;
+	u32 dpfl, imr;
+
+	if (!i915_pipe_enabled(dev, pipe))
+		return -EINVAL;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	dpfl = I915_READ(VLV_DPFLIPSTAT);
+	imr = I915_READ(VLV_IMR);
+	if (pipe == 0) {
+		dpfl |= PIPEA_VBLANK_INT_EN;
+		imr &= ~I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT;
+	} else {
+		dpfl |= PIPEA_VBLANK_INT_EN;
+		imr &= ~I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
+	}
+	I915_WRITE(VLV_DPFLIPSTAT, dpfl);
+	I915_WRITE(VLV_IMR, imr);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
@@ -1592,8 +1450,7 @@ static void i915_disable_vblank(struct drm_device *dev, int pipe)
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
 	if (dev_priv->info->gen == 3)
-		I915_WRITE(INSTPM,
-			   INSTPM_AGPBUSY_DIS << 16 | INSTPM_AGPBUSY_DIS);
+		I915_WRITE(INSTPM, _MASKED_BIT_ENABLE(INSTPM_AGPBUSY_DIS));
 
 	i915_disable_pipestat(dev_priv, pipe,
 			      PIPE_VBLANK_INTERRUPT_ENABLE |
@@ -1618,63 +1475,30 @@ static void ivybridge_disable_vblank(struct drm_device *dev, int pipe)
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ironlake_disable_display_irq(dev_priv, (pipe == 0) ?
-				     DE_PIPEA_VBLANK_IVB : DE_PIPEB_VBLANK_IVB);
+	ironlake_disable_display_irq(dev_priv,
+				     DE_PIPEA_VBLANK_IVB << (pipe * 5));
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
-/* Set the vblank monitor pipe
- */
-int i915_vblank_pipe_set(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
+static void valleyview_disable_vblank(struct drm_device *dev, int pipe)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-int i915_vblank_pipe_get(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	drm_i915_vblank_pipe_t *pipe = data;
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	unsigned long irqflags;
+	u32 dpfl, imr;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
+	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	dpfl = I915_READ(VLV_DPFLIPSTAT);
+	imr = I915_READ(VLV_IMR);
+	if (pipe == 0) {
+		dpfl &= ~PIPEA_VBLANK_INT_EN;
+		imr |= I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT;
+	} else {
+		dpfl &= ~PIPEB_VBLANK_INT_EN;
+		imr |= I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
 	}
-
-	pipe->pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
-
-	return 0;
-}
-
-/**
- * Schedule buffer swap at given vertical blank.
- */
-int i915_vblank_swap(struct drm_device *dev, void *data,
-		     struct drm_file *file_priv)
-{
-	/* The delayed swap mechanism was fundamentally racy, and has been
-	 * removed.  The model was that the client requested a delayed flip/swap
-	 * from the kernel, then waited for vblank before continuing to perform
-	 * rendering.  The problem was that the kernel might wake the client
-	 * up before it dispatched the vblank swap (since the lock has to be
-	 * held while touching the ringbuffer), in which case the client would
-	 * clear and start the next frame before the swap occurred, and
-	 * flicker would occur in addition to likely missing the vblank.
-	 *
-	 * In the absence of this ioctl, userland falls back to a correct path
-	 * of waiting for a vblank, then dispatching the swap on its own.
-	 * Context switching to userland and back is plenty fast enough for
-	 * meeting the requirements of vblank swapping.
-	 */
-	return -EINVAL;
+	I915_WRITE(VLV_IMR, imr);
+	I915_WRITE(VLV_DPFLIPSTAT, dpfl);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
 static u32
@@ -1689,11 +1513,9 @@ static bool i915_hangcheck_ring_idle(struct intel_ring_buffer *ring, bool *err)
 	if (list_empty(&ring->request_list) ||
 	    i915_seqno_passed(ring->get_seqno(ring), ring_last_seqno(ring))) {
 		/* Issue a wake-up to catch stuck h/w. */
-		if (ring->waiting_seqno && waitqueue_active(&ring->irq_queue)) {
-			DRM_ERROR("Hangcheck timer elapsed... %s idle [waiting on %d, at %d], missed IRQ?\n",
-				  ring->name,
-				  ring->waiting_seqno,
-				  ring->get_seqno(ring));
+		if (waitqueue_active(&ring->irq_queue)) {
+			DRM_ERROR("Hangcheck timer elapsed... %s idle\n",
+				  ring->name);
 			wake_up_all(&ring->irq_queue);
 			*err = true;
 		}
@@ -1716,6 +1538,35 @@ static bool kick_ring(struct intel_ring_buffer *ring)
 	return false;
 }
 
+static bool i915_hangcheck_hung(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+
+	if (dev_priv->hangcheck_count++ > 1) {
+		bool hung = true;
+
+		DRM_ERROR("Hangcheck timer elapsed... GPU hung\n");
+		i915_handle_error(dev, true);
+
+		if (!IS_GEN2(dev)) {
+			struct intel_ring_buffer *ring;
+			int i;
+
+			/* Is the chip hanging on a WAIT_FOR_EVENT?
+			 * If so we can simply poke the RB_WAIT bit
+			 * and break the hang. This should work on
+			 * all but the second generation chipsets.
+			 */
+			for_each_ring(ring, dev_priv, i)
+				hung &= !kick_ring(ring);
+		}
+
+		return hung;
+	}
+
+	return false;
+}
+
 /**
  * This is called when the chip hasn't reported back with completed
  * batchbuffers in a long time. The first time this is called we simply record
@@ -1726,19 +1577,31 @@ void i915_hangcheck_elapsed(unsigned long data)
 {
 	struct drm_device *dev = (struct drm_device *)data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	uint32_t acthd, instdone, instdone1, acthd_bsd, acthd_blt;
-	bool err = false;
+	uint32_t acthd[I915_NUM_RINGS], instdone, instdone1;
+	struct intel_ring_buffer *ring;
+	bool err = false, idle;
+	int i;
 
 	if (!i915_enable_hangcheck)
 		return;
 
+	memset(acthd, 0, sizeof(acthd));
+	idle = true;
+	for_each_ring(ring, dev_priv, i) {
+	    idle &= i915_hangcheck_ring_idle(ring, &err);
+	    acthd[i] = intel_ring_get_active_head(ring);
+	}
+
 	/* If all work is done then ACTHD clearly hasn't advanced. */
-	if (i915_hangcheck_ring_idle(&dev_priv->ring[RCS], &err) &&
-	    i915_hangcheck_ring_idle(&dev_priv->ring[VCS], &err) &&
-	    i915_hangcheck_ring_idle(&dev_priv->ring[BCS], &err)) {
-		dev_priv->hangcheck_count = 0;
-		if (err)
+	if (idle) {
+		if (err) {
+			if (i915_hangcheck_hung(dev))
+				return;
+
 			goto repeat;
+		}
+
+		dev_priv->hangcheck_count = 0;
 		return;
 	}
 
@@ -1749,47 +1612,16 @@ void i915_hangcheck_elapsed(unsigned long data)
 		instdone = I915_READ(INSTDONE_I965);
 		instdone1 = I915_READ(INSTDONE1);
 	}
-	acthd = intel_ring_get_active_head(&dev_priv->ring[RCS]);
-	acthd_bsd = HAS_BSD(dev) ?
-		intel_ring_get_active_head(&dev_priv->ring[VCS]) : 0;
-	acthd_blt = HAS_BLT(dev) ?
-		intel_ring_get_active_head(&dev_priv->ring[BCS]) : 0;
 
-	if (dev_priv->last_acthd == acthd &&
-	    dev_priv->last_acthd_bsd == acthd_bsd &&
-	    dev_priv->last_acthd_blt == acthd_blt &&
+	if (memcmp(dev_priv->last_acthd, acthd, sizeof(acthd)) == 0 &&
 	    dev_priv->last_instdone == instdone &&
 	    dev_priv->last_instdone1 == instdone1) {
-		if (dev_priv->hangcheck_count++ > 1) {
-			DRM_ERROR("Hangcheck timer elapsed... GPU hung\n");
-			i915_handle_error(dev, true);
-
-			if (!IS_GEN2(dev)) {
-				/* Is the chip hanging on a WAIT_FOR_EVENT?
-				 * If so we can simply poke the RB_WAIT bit
-				 * and break the hang. This should work on
-				 * all but the second generation chipsets.
-				 */
-				if (kick_ring(&dev_priv->ring[RCS]))
-					goto repeat;
-
-				if (HAS_BSD(dev) &&
-				    kick_ring(&dev_priv->ring[VCS]))
-					goto repeat;
-
-				if (HAS_BLT(dev) &&
-				    kick_ring(&dev_priv->ring[BCS]))
-					goto repeat;
-			}
-
+		if (i915_hangcheck_hung(dev))
 			return;
-		}
 	} else {
 		dev_priv->hangcheck_count = 0;
 
-		dev_priv->last_acthd = acthd;
-		dev_priv->last_acthd_bsd = acthd_bsd;
-		dev_priv->last_acthd_blt = acthd_blt;
+		memcpy(dev_priv->last_acthd, acthd, sizeof(acthd));
 		dev_priv->last_instdone = instdone;
 		dev_priv->last_instdone1 = instdone1;
 	}
@@ -1808,10 +1640,6 @@ static void ironlake_irq_preinstall(struct drm_device *dev)
 
 	atomic_set(&dev_priv->irq_received, 0);
 
-	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
-	INIT_WORK(&dev_priv->error_work, i915_error_work_func);
-	if (IS_GEN6(dev) || IS_IVYBRIDGE(dev))
-		INIT_WORK(&dev_priv->rps_work, gen6_pm_rps_work);
 
 	I915_WRITE(HWSTAM, 0xeffe);
 
@@ -1832,6 +1660,38 @@ static void ironlake_irq_preinstall(struct drm_device *dev)
 	POSTING_READ(SDEIER);
 }
 
+static void valleyview_irq_preinstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	atomic_set(&dev_priv->irq_received, 0);
+
+	/* VLV magic */
+	I915_WRITE(VLV_IMR, 0);
+	I915_WRITE(RING_IMR(RENDER_RING_BASE), 0);
+	I915_WRITE(RING_IMR(GEN6_BSD_RING_BASE), 0);
+	I915_WRITE(RING_IMR(BLT_RING_BASE), 0);
+
+	/* and GT */
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIMR, 0xffffffff);
+	I915_WRITE(GTIER, 0x0);
+	POSTING_READ(GTIER);
+
+	I915_WRITE(DPINVGTT, 0xff);
+
+	I915_WRITE(PORT_HOTPLUG_EN, 0);
+	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0xffff);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(VLV_IMR, 0xffffffff);
+	I915_WRITE(VLV_IER, 0x0);
+	POSTING_READ(VLV_IER);
+}
+
 /*
  * Enable digital hotplug on the PCH, and configure the DP short pulse
  * duration to 2ms (which is the minimum in the Display Port spec)
@@ -1861,13 +1721,6 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 	u32 render_irqs;
 	u32 hotplug_mask;
 
-	DRM_INIT_WAITQUEUE(&dev_priv->ring[RCS].irq_queue);
-	if (HAS_BSD(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[VCS].irq_queue);
-	if (HAS_BLT(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[BCS].irq_queue);
-
-	dev_priv->vblank_pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
 	dev_priv->irq_mask = ~display_mask;
 
 	/* should always can generate irq */
@@ -1884,8 +1737,8 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 	if (IS_GEN6(dev))
 		render_irqs =
 			GT_USER_INTERRUPT |
-			GT_GEN6_BSD_USER_INTERRUPT |
-			GT_BLT_USER_INTERRUPT;
+			GEN6_BSD_USER_INTERRUPT |
+			GEN6_BLITTER_USER_INTERRUPT;
 	else
 		render_irqs =
 			GT_USER_INTERRUPT |
@@ -1930,26 +1783,24 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	/* enable kind of interrupts always enabled */
-	u32 display_mask = DE_MASTER_IRQ_CONTROL | DE_GSE_IVB |
-		DE_PCH_EVENT_IVB | DE_PLANEA_FLIP_DONE_IVB |
-		DE_PLANEB_FLIP_DONE_IVB;
+	u32 display_mask =
+		DE_MASTER_IRQ_CONTROL | DE_GSE_IVB | DE_PCH_EVENT_IVB |
+		DE_PLANEC_FLIP_DONE_IVB |
+		DE_PLANEB_FLIP_DONE_IVB |
+		DE_PLANEA_FLIP_DONE_IVB;
 	u32 render_irqs;
 	u32 hotplug_mask;
 
-	DRM_INIT_WAITQUEUE(&dev_priv->ring[RCS].irq_queue);
-	if (HAS_BSD(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[VCS].irq_queue);
-	if (HAS_BLT(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[BCS].irq_queue);
-
-	dev_priv->vblank_pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
 	dev_priv->irq_mask = ~display_mask;
 
 	/* should always can generate irq */
 	I915_WRITE(DEIIR, I915_READ(DEIIR));
 	I915_WRITE(DEIMR, dev_priv->irq_mask);
-	I915_WRITE(DEIER, display_mask | DE_PIPEA_VBLANK_IVB |
-		   DE_PIPEB_VBLANK_IVB);
+	I915_WRITE(DEIER,
+		   display_mask |
+		   DE_PIPEC_VBLANK_IVB |
+		   DE_PIPEB_VBLANK_IVB |
+		   DE_PIPEA_VBLANK_IVB);
 	POSTING_READ(DEIER);
 
 	dev_priv->gt_irq_mask = ~0;
@@ -1957,8 +1808,8 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 	I915_WRITE(GTIIR, I915_READ(GTIIR));
 	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
 
-	render_irqs = GT_USER_INTERRUPT | GT_GEN6_BSD_USER_INTERRUPT |
-		GT_BLT_USER_INTERRUPT;
+	render_irqs = GT_USER_INTERRUPT | GEN6_BSD_USER_INTERRUPT |
+		GEN6_BLITTER_USER_INTERRUPT;
 	I915_WRITE(GTIER, render_irqs);
 	POSTING_READ(GTIER);
 
@@ -1978,15 +1829,496 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
-static void i915_driver_irq_preinstall(struct drm_device * dev)
+static int valleyview_irq_postinstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 render_irqs;
+	u32 enable_mask;
+	u32 hotplug_en = I915_READ(PORT_HOTPLUG_EN);
+	u16 msid;
+
+	enable_mask = I915_DISPLAY_PORT_INTERRUPT;
+	enable_mask |= I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT |
+		I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
+
+	dev_priv->irq_mask = ~enable_mask;
+
+	dev_priv->pipestat[0] = 0;
+	dev_priv->pipestat[1] = 0;
+
+	/* Hack for broken MSIs on VLV */
+	pci_write_config_dword(dev_priv->dev->pdev, 0x94, 0xfee00000);
+	pci_read_config_word(dev->pdev, 0x98, &msid);
+	msid &= 0xff; /* mask out delivery bits */
+	msid |= (1<<14);
+	pci_write_config_word(dev_priv->dev->pdev, 0x98, msid);
+
+	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
+	I915_WRITE(VLV_IER, enable_mask);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(PIPESTAT(0), 0xffff);
+	I915_WRITE(PIPESTAT(1), 0xffff);
+	POSTING_READ(VLV_IER);
+
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+
+	render_irqs = GT_GEN6_BLT_FLUSHDW_NOTIFY_INTERRUPT |
+		GT_GEN6_BLT_CS_ERROR_INTERRUPT |
+		GT_GEN6_BLT_USER_INTERRUPT |
+		GT_GEN6_BSD_USER_INTERRUPT |
+		GT_GEN6_BSD_CS_ERROR_INTERRUPT |
+		GT_GEN7_L3_PARITY_ERROR_INTERRUPT |
+		GT_PIPE_NOTIFY |
+		GT_RENDER_CS_ERROR_INTERRUPT |
+		GT_SYNC_STATUS |
+		GT_USER_INTERRUPT;
+
+	dev_priv->gt_irq_mask = ~render_irqs;
+
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIMR, 0);
+	I915_WRITE(GTIER, render_irqs);
+	POSTING_READ(GTIER);
+
+	/* ack & enable invalid PTE error interrupts */
+#if 0 /* FIXME: add support to irq handler for checking these bits */
+	I915_WRITE(DPINVGTT, DPINVGTT_STATUS_MASK);
+	I915_WRITE(DPINVGTT, DPINVGTT_EN_MASK);
+#endif
+
+	I915_WRITE(VLV_MASTER_IER, MASTER_INTERRUPT_ENABLE);
+#if 0 /* FIXME: check register definitions; some have moved */
+	/* Note HDMI and DP share bits */
+	if (dev_priv->hotplug_supported_mask & HDMIB_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMIB_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & HDMIC_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMIC_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & HDMID_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMID_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS)
+		hotplug_en |= SDVOC_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS)
+		hotplug_en |= SDVOB_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
+		hotplug_en |= CRT_HOTPLUG_INT_EN;
+		hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
+	}
+#endif
+
+	I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+
+	return 0;
+}
+
+static void valleyview_irq_uninstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	if (!dev_priv)
+		return;
+
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0xffff);
+
+	I915_WRITE(HWSTAM, 0xffffffff);
+	I915_WRITE(PORT_HOTPLUG_EN, 0);
+	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0xffff);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(VLV_IMR, 0xffffffff);
+	I915_WRITE(VLV_IER, 0x0);
+	POSTING_READ(VLV_IER);
+}
+
+static void ironlake_irq_uninstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+
+	if (!dev_priv)
+		return;
+
+	I915_WRITE(HWSTAM, 0xffffffff);
+
+	I915_WRITE(DEIMR, 0xffffffff);
+	I915_WRITE(DEIER, 0x0);
+	I915_WRITE(DEIIR, I915_READ(DEIIR));
+
+	I915_WRITE(GTIMR, 0xffffffff);
+	I915_WRITE(GTIER, 0x0);
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+
+	I915_WRITE(SDEIMR, 0xffffffff);
+	I915_WRITE(SDEIER, 0x0);
+	I915_WRITE(SDEIIR, I915_READ(SDEIIR));
+}
+
+static void i8xx_irq_preinstall(struct drm_device * dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	int pipe;
 
 	atomic_set(&dev_priv->irq_received, 0);
 
-	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
-	INIT_WORK(&dev_priv->error_work, i915_error_work_func);
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0);
+	I915_WRITE16(IMR, 0xffff);
+	I915_WRITE16(IER, 0x0);
+	POSTING_READ16(IER);
+}
+
+static int i8xx_irq_postinstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+
+	dev_priv->pipestat[0] = 0;
+	dev_priv->pipestat[1] = 0;
+
+	I915_WRITE16(EMR,
+		     ~(I915_ERROR_PAGE_TABLE | I915_ERROR_MEMORY_REFRESH));
+
+	/* Unmask the interrupts that we always want on. */
+	dev_priv->irq_mask =
+		~(I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		  I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		  I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		  I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |
+		  I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
+	I915_WRITE16(IMR, dev_priv->irq_mask);
+
+	I915_WRITE16(IER,
+		     I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		     I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		     I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT |
+		     I915_USER_INTERRUPT);
+	POSTING_READ16(IER);
+
+	return 0;
+}
+
+static irqreturn_t i8xx_irq_handler(DRM_IRQ_ARGS)
+{
+	struct drm_device *dev = (struct drm_device *) arg;
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u16 iir, new_iir;
+	u32 pipe_stats[2];
+	unsigned long irqflags;
+	int irq_received;
+	int pipe;
+	u16 flip_mask =
+		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
+
+	atomic_inc(&dev_priv->irq_received);
+
+	iir = I915_READ16(IIR);
+	if (iir == 0)
+		return IRQ_NONE;
+
+	while (iir & ~flip_mask) {
+		/* Can't rely on pipestat interrupt bit in iir as it might
+		 * have been cleared after the pipestat interrupt was received.
+		 * It doesn't set the bit in iir again, but it still produces
+		 * interrupts (for non-MSI).
+		 */
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
+			i915_handle_error(dev, false);
+
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/*
+			 * Clear the PIPE*STAT regs before the IIR
+			 */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+				irq_received = 1;
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		I915_WRITE16(IIR, iir & ~flip_mask);
+		new_iir = I915_READ16(IIR); /* Flush posted writes */
+
+		i915_update_dri1_breadcrumb(dev);
+
+		if (iir & I915_USER_INTERRUPT)
+			notify_ring(dev, &dev_priv->ring[RCS]);
+
+		if (pipe_stats[0] & PIPE_VBLANK_INTERRUPT_STATUS &&
+		    drm_handle_vblank(dev, 0)) {
+			if (iir & I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT) {
+				intel_prepare_page_flip(dev, 0);
+				intel_finish_page_flip(dev, 0);
+				flip_mask &= ~I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT;
+			}
+		}
+
+		if (pipe_stats[1] & PIPE_VBLANK_INTERRUPT_STATUS &&
+		    drm_handle_vblank(dev, 1)) {
+			if (iir & I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT) {
+				intel_prepare_page_flip(dev, 1);
+				intel_finish_page_flip(dev, 1);
+				flip_mask &= ~I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
+			}
+		}
+
+		iir = new_iir;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void i8xx_irq_uninstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	for_each_pipe(pipe) {
+		/* Clear enable bits; then clear status bits */
+		I915_WRITE(PIPESTAT(pipe), 0);
+		I915_WRITE(PIPESTAT(pipe), I915_READ(PIPESTAT(pipe)));
+	}
+	I915_WRITE16(IMR, 0xffff);
+	I915_WRITE16(IER, 0x0);
+	I915_WRITE16(IIR, I915_READ16(IIR));
+}
+
+static void i915_irq_preinstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	atomic_set(&dev_priv->irq_received, 0);
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		I915_WRITE(PORT_HOTPLUG_EN, 0);
+		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	}
+
+	I915_WRITE16(HWSTAM, 0xeffe);
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0);
+	I915_WRITE(IMR, 0xffffffff);
+	I915_WRITE(IER, 0x0);
+	POSTING_READ(IER);
+}
+
+static int i915_irq_postinstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 enable_mask;
+
+	dev_priv->pipestat[0] = 0;
+	dev_priv->pipestat[1] = 0;
+
+	I915_WRITE(EMR, ~(I915_ERROR_PAGE_TABLE | I915_ERROR_MEMORY_REFRESH));
+
+	/* Unmask the interrupts that we always want on. */
+	dev_priv->irq_mask =
+		~(I915_ASLE_INTERRUPT |
+		  I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		  I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		  I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		  I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |
+		  I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
+
+	enable_mask =
+		I915_ASLE_INTERRUPT |
+		I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT |
+		I915_USER_INTERRUPT;
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		/* Enable in IER... */
+		enable_mask |= I915_DISPLAY_PORT_INTERRUPT;
+		/* and unmask in IMR */
+		dev_priv->irq_mask &= ~I915_DISPLAY_PORT_INTERRUPT;
+	}
+
+	I915_WRITE(IMR, dev_priv->irq_mask);
+	I915_WRITE(IER, enable_mask);
+	POSTING_READ(IER);
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		u32 hotplug_en = I915_READ(PORT_HOTPLUG_EN);
+
+		if (dev_priv->hotplug_supported_mask & HDMIB_HOTPLUG_INT_STATUS)
+			hotplug_en |= HDMIB_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & HDMIC_HOTPLUG_INT_STATUS)
+			hotplug_en |= HDMIC_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & HDMID_HOTPLUG_INT_STATUS)
+			hotplug_en |= HDMID_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS)
+			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS)
+			hotplug_en |= SDVOB_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
+			hotplug_en |= CRT_HOTPLUG_INT_EN;
+			hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
+		}
+
+		/* Ignore TV since it's buggy */
+
+		I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+	}
+
+	intel_opregion_enable_asle(dev);
+
+	return 0;
+}
+
+static irqreturn_t i915_irq_handler(DRM_IRQ_ARGS)
+{
+	struct drm_device *dev = (struct drm_device *) arg;
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 iir, new_iir, pipe_stats[I915_MAX_PIPES];
+	unsigned long irqflags;
+	u32 flip_mask =
+		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
+	u32 flip[2] = {
+		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT,
+		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT
+	};
+	int pipe, ret = IRQ_NONE;
+
+	atomic_inc(&dev_priv->irq_received);
+
+	iir = I915_READ(IIR);
+	do {
+		bool irq_received = (iir & ~flip_mask) != 0;
+		bool blc_event = false;
+
+		/* Can't rely on pipestat interrupt bit in iir as it might
+		 * have been cleared after the pipestat interrupt was received.
+		 * It doesn't set the bit in iir again, but it still produces
+		 * interrupts (for non-MSI).
+		 */
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
+			i915_handle_error(dev, false);
+
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/* Clear the PIPE*STAT regs before the IIR */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+				irq_received = true;
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		if (!irq_received)
+			break;
+
+		/* Consume port.  Then clear IIR or we'll miss events */
+		if ((I915_HAS_HOTPLUG(dev)) &&
+		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
+			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+
+			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
+				  hotplug_status);
+			if (hotplug_status & dev_priv->hotplug_supported_mask)
+				queue_work(dev_priv->wq,
+					   &dev_priv->hotplug_work);
+
+			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
+			POSTING_READ(PORT_HOTPLUG_STAT);
+		}
+
+		I915_WRITE(IIR, iir & ~flip_mask);
+		new_iir = I915_READ(IIR); /* Flush posted writes */
+
+		if (iir & I915_USER_INTERRUPT)
+			notify_ring(dev, &dev_priv->ring[RCS]);
+
+		for_each_pipe(pipe) {
+			int plane = pipe;
+			if (IS_MOBILE(dev))
+				plane = !plane;
+			if (pipe_stats[pipe] & PIPE_VBLANK_INTERRUPT_STATUS &&
+			    drm_handle_vblank(dev, pipe)) {
+				if (iir & flip[plane]) {
+					intel_prepare_page_flip(dev, plane);
+					intel_finish_page_flip(dev, pipe);
+					flip_mask &= ~flip[plane];
+				}
+			}
+
+			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
+				blc_event = true;
+		}
+
+		if (blc_event || (iir & I915_ASLE_INTERRUPT))
+			intel_opregion_asle_intr(dev);
+
+		/* With MSI, interrupts are only generated when iir
+		 * transitions from zero to nonzero.  If another bit got
+		 * set while we were handling the existing iir bits, then
+		 * we would never get another interrupt.
+		 *
+		 * This is fine on non-MSI as well, as if we hit this path
+		 * we avoid exiting the interrupt handler only to generate
+		 * another one.
+		 *
+		 * Note that for MSI this could cause a stray interrupt report
+		 * if an interrupt landed in the time between writing IIR and
+		 * the posting read.  This should be rare enough to never
+		 * trigger the 99% of 100,000 interrupts test for disabling
+		 * stray interrupts.
+		 */
+		ret = IRQ_HANDLED;
+		iir = new_iir;
+	} while (iir & ~flip_mask);
+
+	i915_update_dri1_breadcrumb(dev);
+
+	return ret;
+}
+
+static void i915_irq_uninstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		I915_WRITE(PORT_HOTPLUG_EN, 0);
+		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	}
+
+	I915_WRITE16(HWSTAM, 0xffff);
+	for_each_pipe(pipe) {
+		/* Clear enable bits; then clear status bits */
+		I915_WRITE(PIPESTAT(pipe), 0);
+		I915_WRITE(PIPESTAT(pipe), I915_READ(PIPESTAT(pipe)));
+	}
+	I915_WRITE(IMR, 0xffffffff);
+	I915_WRITE(IER, 0x0);
+
+	I915_WRITE(IIR, I915_READ(IIR));
+}
+
+static void i965_irq_preinstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	atomic_set(&dev_priv->irq_received, 0);
 
 	if (I915_HAS_HOTPLUG(dev)) {
 		I915_WRITE(PORT_HOTPLUG_EN, 0);
@@ -2001,20 +2333,25 @@ static void i915_driver_irq_preinstall(struct drm_device * dev)
 	POSTING_READ(IER);
 }
 
-/*
- * Must be called after intel_modeset_init or hotplug interrupts won't be
- * enabled correctly.
- */
-static int i915_driver_irq_postinstall(struct drm_device *dev)
+static int i965_irq_postinstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 enable_mask = I915_INTERRUPT_ENABLE_FIX | I915_INTERRUPT_ENABLE_VAR;
+	u32 enable_mask;
 	u32 error_mask;
 
-	dev_priv->vblank_pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
-
 	/* Unmask the interrupts that we always want on. */
-	dev_priv->irq_mask = ~I915_INTERRUPT_ENABLE_FIX;
+	dev_priv->irq_mask = ~(I915_ASLE_INTERRUPT |
+			       I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+			       I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+			       I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+			       I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |
+			       I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
+
+	enable_mask = ~dev_priv->irq_mask;
+	enable_mask |= I915_USER_INTERRUPT;
+
+	if (IS_G4X(dev))
+		enable_mask |= I915_BSD_USER_INTERRUPT;
 
 	dev_priv->pipestat[0] = 0;
 	dev_priv->pipestat[1] = 0;
@@ -2081,31 +2418,124 @@ static int i915_driver_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
-static void ironlake_irq_uninstall(struct drm_device *dev)
+static irqreturn_t i965_irq_handler(DRM_IRQ_ARGS)
 {
+	struct drm_device *dev = (struct drm_device *) arg;
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 iir, new_iir;
+	u32 pipe_stats[I915_MAX_PIPES];
+	unsigned long irqflags;
+	int irq_received;
+	int ret = IRQ_NONE, pipe;
 
-	if (!dev_priv)
-		return;
+	atomic_inc(&dev_priv->irq_received);
 
-	dev_priv->vblank_pipe = 0;
+	iir = I915_READ(IIR);
 
-	I915_WRITE(HWSTAM, 0xffffffff);
+	for (;;) {
+		bool blc_event = false;
 
-	I915_WRITE(DEIMR, 0xffffffff);
-	I915_WRITE(DEIER, 0x0);
-	I915_WRITE(DEIIR, I915_READ(DEIIR));
+		irq_received = iir != 0;
 
-	I915_WRITE(GTIMR, 0xffffffff);
-	I915_WRITE(GTIER, 0x0);
-	I915_WRITE(GTIIR, I915_READ(GTIIR));
+		/* Can't rely on pipestat interrupt bit in iir as it might
+		 * have been cleared after the pipestat interrupt was received.
+		 * It doesn't set the bit in iir again, but it still produces
+		 * interrupts (for non-MSI).
+		 */
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
+			i915_handle_error(dev, false);
 
-	I915_WRITE(SDEIMR, 0xffffffff);
-	I915_WRITE(SDEIER, 0x0);
-	I915_WRITE(SDEIIR, I915_READ(SDEIIR));
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/*
+			 * Clear the PIPE*STAT regs before the IIR
+			 */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+				irq_received = 1;
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		if (!irq_received)
+			break;
+
+		ret = IRQ_HANDLED;
+
+		/* Consume port.  Then clear IIR or we'll miss events */
+		if ((I915_HAS_HOTPLUG(dev)) &&
+		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
+			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+
+			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
+				  hotplug_status);
+			if (hotplug_status & dev_priv->hotplug_supported_mask)
+				queue_work(dev_priv->wq,
+					   &dev_priv->hotplug_work);
+
+			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
+			I915_READ(PORT_HOTPLUG_STAT);
+		}
+
+		I915_WRITE(IIR, iir);
+		new_iir = I915_READ(IIR); /* Flush posted writes */
+
+		if (iir & I915_USER_INTERRUPT)
+			notify_ring(dev, &dev_priv->ring[RCS]);
+		if (iir & I915_BSD_USER_INTERRUPT)
+			notify_ring(dev, &dev_priv->ring[VCS]);
+
+		if (iir & I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT)
+			intel_prepare_page_flip(dev, 0);
+
+		if (iir & I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT)
+			intel_prepare_page_flip(dev, 1);
+
+		for_each_pipe(pipe) {
+			if (pipe_stats[pipe] & PIPE_START_VBLANK_INTERRUPT_STATUS &&
+			    drm_handle_vblank(dev, pipe)) {
+				i915_pageflip_stall_check(dev, pipe);
+				intel_finish_page_flip(dev, pipe);
+			}
+
+			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
+				blc_event = true;
+		}
+
+
+		if (blc_event || (iir & I915_ASLE_INTERRUPT))
+			intel_opregion_asle_intr(dev);
+
+		/* With MSI, interrupts are only generated when iir
+		 * transitions from zero to nonzero.  If another bit got
+		 * set while we were handling the existing iir bits, then
+		 * we would never get another interrupt.
+		 *
+		 * This is fine on non-MSI as well, as if we hit this path
+		 * we avoid exiting the interrupt handler only to generate
+		 * another one.
+		 *
+		 * Note that for MSI this could cause a stray interrupt report
+		 * if an interrupt landed in the time between writing IIR and
+		 * the posting read.  This should be rare enough to never
+		 * trigger the 99% of 100,000 interrupts test for disabling
+		 * stray interrupts.
+		 */
+		iir = new_iir;
+	}
+
+	i915_update_dri1_breadcrumb(dev);
+
+	return ret;
 }
 
-static void i915_driver_irq_uninstall(struct drm_device * dev)
+static void i965_irq_uninstall(struct drm_device * dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	int pipe;
@@ -2113,8 +2543,6 @@ static void i915_driver_irq_uninstall(struct drm_device * dev)
 	if (!dev_priv)
 		return;
 
-	dev_priv->vblank_pipe = 0;
-
 	if (I915_HAS_HOTPLUG(dev)) {
 		I915_WRITE(PORT_HOTPLUG_EN, 0);
 		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
@@ -2134,9 +2562,15 @@ static void i915_driver_irq_uninstall(struct drm_device * dev)
 
 void intel_irq_init(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
+	INIT_WORK(&dev_priv->error_work, i915_error_work_func);
+	INIT_WORK(&dev_priv->rps_work, gen6_pm_rps_work);
+
 	dev->driver->get_vblank_counter = i915_get_vblank_counter;
 	dev->max_vblank_count = 0xffffff; /* only 24 bits of frame count */
-	if (IS_G4X(dev) || IS_GEN5(dev) || IS_GEN6(dev) || IS_IVYBRIDGE(dev)) {
+	if (IS_G4X(dev) || INTEL_INFO(dev)->gen >= 5) {
 		dev->max_vblank_count = 0xffffffff; /* full 32 bit counter */
 		dev->driver->get_vblank_counter = gm45_get_vblank_counter;
 	}
@@ -2147,7 +2581,14 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->get_vblank_timestamp = NULL;
 	dev->driver->get_scanout_position = i915_get_crtc_scanoutpos;
 
-	if (IS_IVYBRIDGE(dev)) {
+	if (IS_VALLEYVIEW(dev)) {
+		dev->driver->irq_handler = valleyview_irq_handler;
+		dev->driver->irq_preinstall = valleyview_irq_preinstall;
+		dev->driver->irq_postinstall = valleyview_irq_postinstall;
+		dev->driver->irq_uninstall = valleyview_irq_uninstall;
+		dev->driver->enable_vblank = valleyview_enable_vblank;
+		dev->driver->disable_vblank = valleyview_disable_vblank;
+	} else if (IS_IVYBRIDGE(dev)) {
 		/* Share pre & uninstall handlers with ILK/SNB */
 		dev->driver->irq_handler = ivybridge_irq_handler;
 		dev->driver->irq_preinstall = ironlake_irq_preinstall;
@@ -2155,6 +2596,14 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->irq_uninstall = ironlake_irq_uninstall;
 		dev->driver->enable_vblank = ivybridge_enable_vblank;
 		dev->driver->disable_vblank = ivybridge_disable_vblank;
+	} else if (IS_HASWELL(dev)) {
+		/* Share interrupts handling with IVB */
+		dev->driver->irq_handler = ivybridge_irq_handler;
+		dev->driver->irq_preinstall = ironlake_irq_preinstall;
+		dev->driver->irq_postinstall = ivybridge_irq_postinstall;
+		dev->driver->irq_uninstall = ironlake_irq_uninstall;
+		dev->driver->enable_vblank = ivybridge_enable_vblank;
+		dev->driver->disable_vblank = ivybridge_disable_vblank;
 	} else if (HAS_PCH_SPLIT(dev)) {
 		dev->driver->irq_handler = ironlake_irq_handler;
 		dev->driver->irq_preinstall = ironlake_irq_preinstall;
@@ -2163,10 +2612,25 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->enable_vblank = ironlake_enable_vblank;
 		dev->driver->disable_vblank = ironlake_disable_vblank;
 	} else {
-		dev->driver->irq_preinstall = i915_driver_irq_preinstall;
-		dev->driver->irq_postinstall = i915_driver_irq_postinstall;
-		dev->driver->irq_uninstall = i915_driver_irq_uninstall;
-		dev->driver->irq_handler = i915_driver_irq_handler;
+		if (INTEL_INFO(dev)->gen == 2) {
+			dev->driver->irq_preinstall = i8xx_irq_preinstall;
+			dev->driver->irq_postinstall = i8xx_irq_postinstall;
+			dev->driver->irq_handler = i8xx_irq_handler;
+			dev->driver->irq_uninstall = i8xx_irq_uninstall;
+		} else if (INTEL_INFO(dev)->gen == 3) {
+			/* IIR "flip pending" means done if this bit is set */
+			I915_WRITE(ECOSKPD, _MASKED_BIT_DISABLE(ECO_FLIP_DONE));
+
+			dev->driver->irq_preinstall = i915_irq_preinstall;
+			dev->driver->irq_postinstall = i915_irq_postinstall;
+			dev->driver->irq_uninstall = i915_irq_uninstall;
+			dev->driver->irq_handler = i915_irq_handler;
+		} else {
+			dev->driver->irq_preinstall = i965_irq_preinstall;
+			dev->driver->irq_postinstall = i965_irq_postinstall;
+			dev->driver->irq_uninstall = i965_irq_uninstall;
+			dev->driver->irq_handler = i965_irq_handler;
+		}
 		dev->driver->enable_vblank = i915_enable_vblank;
 		dev->driver->disable_vblank = i915_disable_vblank;
 	}
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 9d24d65f0c3e..2d49b9507ed0 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -27,6 +27,11 @@
 
 #define _PIPE(pipe, a, b) ((a) + (pipe)*((b)-(a)))
 
+#define _PORT(port, a, b) ((a) + (port)*((b)-(a)))
+
+#define _MASKED_BIT_ENABLE(a) (((a) << 16) | (a))
+#define _MASKED_BIT_DISABLE(a) ((a) << 16)
+
 /*
  * The Bridge device's PCI config space has information about the
  * fb aperture size and the amount of pre-reserved memory.
@@ -77,6 +82,7 @@
 #define  GRDOM_FULL	(0<<2)
 #define  GRDOM_RENDER	(1<<2)
 #define  GRDOM_MEDIA	(3<<2)
+#define  GRDOM_RESET_ENABLE (1<<0)
 
 #define GEN6_MBCUNIT_SNPCR	0x900c /* for LLC config */
 #define   GEN6_MBC_SNPCR_SHIFT	21
@@ -125,6 +131,13 @@
 #define   ECOCHK_PPGTT_CACHE64B		(0x3<<3)
 #define   ECOCHK_PPGTT_CACHE4B		(0x0<<3)
 
+#define GAC_ECO_BITS			0x14090
+#define   ECOBITS_PPGTT_CACHE64B	(3<<8)
+#define   ECOBITS_PPGTT_CACHE4B		(0<<8)
+
+#define GAB_CTL				0x24000
+#define   GAB_CTL_CONT_AFTER_PAGEFAULT	(1<<8)
+
 /* VGA stuff */
 
 #define VGA_ST01_MDA 0x3ba
@@ -222,6 +235,7 @@
 #define   MI_BATCH_NON_SECURE	(1)
 #define   MI_BATCH_NON_SECURE_I965 (1<<8)
 #define MI_BATCH_BUFFER_START	MI_INSTR(0x31, 0)
+#define   MI_BATCH_GTT		    (2<<6) /* aliased with (1<<7) on gen4 */
 #define MI_SEMAPHORE_MBOX	MI_INSTR(0x16, 1) /* gen6+ */
 #define  MI_SEMAPHORE_GLOBAL_GTT    (1<<22)
 #define  MI_SEMAPHORE_UPDATE	    (1<<21)
@@ -301,6 +315,61 @@
 #define  DEBUG_RESET_RENDER		(1<<8)
 #define  DEBUG_RESET_DISPLAY		(1<<9)
 
+/*
+ * DPIO - a special bus for various display related registers to hide behind:
+ *  0x800c: m1, m2, n, p1, p2, k dividers
+ *  0x8014: REF and SFR select
+ *  0x8014: N divider, VCO select
+ *  0x801c/3c: core clock bits
+ *  0x8048/68: low pass filter coefficients
+ *  0x8100: fast clock controls
+ */
+#define DPIO_PKT			0x2100
+#define  DPIO_RID			(0<<24)
+#define  DPIO_OP_WRITE			(1<<16)
+#define  DPIO_OP_READ			(0<<16)
+#define  DPIO_PORTID			(0x12<<8)
+#define  DPIO_BYTE			(0xf<<4)
+#define  DPIO_BUSY			(1<<0) /* status only */
+#define DPIO_DATA			0x2104
+#define DPIO_REG			0x2108
+#define DPIO_CTL			0x2110
+#define  DPIO_MODSEL1			(1<<3) /* if ref clk b == 27 */
+#define  DPIO_MODSEL0			(1<<2) /* if ref clk a == 27 */
+#define  DPIO_SFR_BYPASS		(1<<1)
+#define  DPIO_RESET			(1<<0)
+
+#define _DPIO_DIV_A			0x800c
+#define   DPIO_POST_DIV_SHIFT		(28) /* 3 bits */
+#define   DPIO_K_SHIFT			(24) /* 4 bits */
+#define   DPIO_P1_SHIFT			(21) /* 3 bits */
+#define   DPIO_P2_SHIFT			(16) /* 5 bits */
+#define   DPIO_N_SHIFT			(12) /* 4 bits */
+#define   DPIO_ENABLE_CALIBRATION	(1<<11)
+#define   DPIO_M1DIV_SHIFT		(8) /* 3 bits */
+#define   DPIO_M2DIV_MASK		0xff
+#define _DPIO_DIV_B			0x802c
+#define DPIO_DIV(pipe) _PIPE(pipe, _DPIO_DIV_A, _DPIO_DIV_B)
+
+#define _DPIO_REFSFR_A			0x8014
+#define   DPIO_REFSEL_OVERRIDE		27
+#define   DPIO_PLL_MODESEL_SHIFT	24 /* 3 bits */
+#define   DPIO_BIAS_CURRENT_CTL_SHIFT	21 /* 3 bits, always 0x7 */
+#define   DPIO_PLL_REFCLK_SEL_SHIFT	16 /* 2 bits */
+#define   DPIO_DRIVER_CTL_SHIFT		12 /* always set to 0x8 */
+#define   DPIO_CLK_BIAS_CTL_SHIFT	8 /* always set to 0x5 */
+#define _DPIO_REFSFR_B			0x8034
+#define DPIO_REFSFR(pipe) _PIPE(pipe, _DPIO_REFSFR_A, _DPIO_REFSFR_B)
+
+#define _DPIO_CORE_CLK_A		0x801c
+#define _DPIO_CORE_CLK_B		0x803c
+#define DPIO_CORE_CLK(pipe) _PIPE(pipe, _DPIO_CORE_CLK_A, _DPIO_CORE_CLK_B)
+
+#define _DPIO_LFP_COEFF_A		0x8048
+#define _DPIO_LFP_COEFF_B		0x8068
+#define DPIO_LFP_COEFF(pipe) _PIPE(pipe, _DPIO_LFP_COEFF_A, _DPIO_LFP_COEFF_B)
+
+#define DPIO_FASTCLK_DISABLE		0x8100
 
 /*
  * Fence registers
@@ -360,8 +429,6 @@
 #define ARB_MODE		0x04030
 #define   ARB_MODE_SWIZZLE_SNB	(1<<4)
 #define   ARB_MODE_SWIZZLE_IVB	(1<<5)
-#define   ARB_MODE_ENABLE(x)	GFX_MODE_ENABLE(x)
-#define   ARB_MODE_DISABLE(x)	GFX_MODE_DISABLE(x)
 #define RENDER_HWS_PGA_GEN7	(0x04080)
 #define RING_FAULT_REG(ring)	(0x4094 + 0x100*(ring)->id)
 #define DONE_REG		0x40b0
@@ -417,6 +484,7 @@
 #define INSTDONE	0x02090
 #define NOPID		0x02094
 #define HWSTAM		0x02098
+#define DMA_FADD_I8XX	0x020d0
 
 #define ERROR_GEN6	0x040a0
 
@@ -432,6 +500,7 @@
  */
 # define _3D_CHICKEN2_WM_READ_PIPELINED			(1 << 14)
 #define _3D_CHICKEN3	0x02090
+#define  _3D_CHICKEN_SF_DISABLE_FASTCLIP_CULL		(1 << 5)
 
 #define MI_MODE		0x0209c
 # define VS_TIMER_DISPATCH				(1 << 6)
@@ -447,14 +516,16 @@
 #define   GFX_PSMI_GRANULARITY		(1<<10)
 #define   GFX_PPGTT_ENABLE		(1<<9)
 
-#define GFX_MODE_ENABLE(bit) (((bit) << 16) | (bit))
-#define GFX_MODE_DISABLE(bit) (((bit) << 16) | (0))
-
 #define SCPD0		0x0209c /* 915+ only */
 #define IER		0x020a0
 #define IIR		0x020a4
 #define IMR		0x020a8
 #define ISR		0x020ac
+#define VLV_IIR_RW	0x182084
+#define VLV_IER		0x1820a0
+#define VLV_IIR		0x1820a4
+#define VLV_IMR		0x1820a8
+#define VLV_ISR		0x1820ac
 #define   I915_PIPE_CONTROL_NOTIFY_INTERRUPT		(1<<18)
 #define   I915_DISPLAY_PORT_INTERRUPT			(1<<17)
 #define   I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT	(1<<15)
@@ -500,7 +571,6 @@
 #define LM_BURST_LENGTH     0x00000700
 #define LM_FIFO_WATERMARK   0x0000001F
 #define MI_ARB_STATE	0x020e4 /* 915+ only */
-#define   MI_ARB_MASK_SHIFT	  16	/* shift for enable bits */
 
 /* Make render/texture TLB fetches lower priorty than associated data
  *   fetches. This is not turned on by default
@@ -565,7 +635,6 @@
 #define   MI_ARB_DISPLAY_PRIORITY_B_A		(1 << 0)	/* display B > display A */
 
 #define CACHE_MODE_0	0x02120 /* 915+ only */
-#define   CM0_MASK_SHIFT          16
 #define   CM0_IZ_OPT_DISABLE      (1<<6)
 #define   CM0_ZR_OPT_DISABLE      (1<<5)
 #define	  CM0_STC_EVICT_DISABLE_LRA_SNB	(1<<5)
@@ -579,7 +648,12 @@
 #define   ECO_GATING_CX_ONLY	(1<<3)
 #define   ECO_FLIP_DONE		(1<<0)
 
-/* GEN6 interrupt control */
+#define CACHE_MODE_1		0x7004 /* IVB+ */
+#define   PIXEL_SUBSPAN_COLLECT_OPT_DISABLE (1<<6)
+
+/* GEN6 interrupt control
+ * Note that the per-ring interrupt bits do alias with the global interrupt bits
+ * in GTIMR. */
 #define GEN6_RENDER_HWSTAM	0x2098
 #define GEN6_RENDER_IMR		0x20a8
 #define   GEN6_RENDER_CONTEXT_SWITCH_INTERRUPT		(1 << 8)
@@ -615,6 +689,21 @@
 
 #define GEN6_BSD_RNCID			0x12198
 
+#define GEN7_FF_THREAD_MODE		0x20a0
+#define   GEN7_FF_SCHED_MASK		0x0077070
+#define   GEN7_FF_TS_SCHED_HS1		(0x5<<16)
+#define   GEN7_FF_TS_SCHED_HS0		(0x3<<16)
+#define   GEN7_FF_TS_SCHED_LOAD_BALANCE	(0x1<<16)
+#define   GEN7_FF_TS_SCHED_HW		(0x0<<16) /* Default */
+#define   GEN7_FF_VS_SCHED_HS1		(0x5<<12)
+#define   GEN7_FF_VS_SCHED_HS0		(0x3<<12)
+#define   GEN7_FF_VS_SCHED_LOAD_BALANCE	(0x1<<12) /* Default */
+#define   GEN7_FF_VS_SCHED_HW		(0x0<<12)
+#define   GEN7_FF_DS_SCHED_HS1		(0x5<<4)
+#define   GEN7_FF_DS_SCHED_HS0		(0x3<<4)
+#define   GEN7_FF_DS_SCHED_LOAD_BALANCE	(0x1<<4)  /* Default */
+#define   GEN7_FF_DS_SCHED_HW		(0x0<<4)
+
 /*
  * Framebuffer compression (915+ only)
  */
@@ -743,9 +832,9 @@
 #define   GMBUS_PORT_PANEL	3
 #define   GMBUS_PORT_DPC	4 /* HDMIC */
 #define   GMBUS_PORT_DPB	5 /* SDVO, HDMIB */
-				  /* 6 reserved */
-#define   GMBUS_PORT_DPD	7 /* HDMID */
-#define   GMBUS_NUM_PORTS       8
+#define   GMBUS_PORT_DPD	6 /* HDMID */
+#define   GMBUS_PORT_RESERVED	7 /* 7 reserved */
+#define   GMBUS_NUM_PORTS	(GMBUS_PORT_DPD - GMBUS_PORT_SSC + 1)
 #define GMBUS1			0x5104 /* command/status */
 #define   GMBUS_SW_CLR_INT	(1<<31)
 #define   GMBUS_SW_RDY		(1<<30)
@@ -797,7 +886,9 @@
 #define DPLL(pipe) _PIPE(pipe, _DPLL_A, _DPLL_B)
 #define   DPLL_VCO_ENABLE		(1 << 31)
 #define   DPLL_DVO_HIGH_SPEED		(1 << 30)
+#define   DPLL_EXT_BUFFER_ENABLE_VLV	(1 << 30)
 #define   DPLL_SYNCLOCK_ENABLE		(1 << 29)
+#define   DPLL_REFA_CLK_ENABLE_VLV	(1 << 29)
 #define   DPLL_VGA_MODE_DIS		(1 << 28)
 #define   DPLLB_MODE_DAC_SERIAL		(1 << 26) /* i915 */
 #define   DPLLB_MODE_LVDS		(2 << 26) /* i915 */
@@ -809,6 +900,7 @@
 #define   DPLL_P2_CLOCK_DIV_MASK	0x03000000 /* i915 */
 #define   DPLL_FPA01_P1_POST_DIV_MASK	0x00ff0000 /* i915 */
 #define   DPLL_FPA01_P1_POST_DIV_MASK_PINEVIEW	0x00ff8000 /* Pineview */
+#define   DPLL_INTEGRATED_CLOCK_VLV	(1<<13)
 
 #define SRX_INDEX		0x3c4
 #define SRX_DATA		0x3c5
@@ -904,6 +996,7 @@
 #define   DPLL_MD_VGA_UDI_MULTIPLIER_SHIFT	0
 #define _DPLL_B_MD 0x06020 /* 965+ only */
 #define DPLL_MD(pipe) _PIPE(pipe, _DPLL_A_MD, _DPLL_B_MD)
+
 #define _FPA0	0x06040
 #define _FPA1	0x06044
 #define _FPB0	0x06048
@@ -1044,6 +1137,9 @@
 #define RAMCLK_GATE_D		0x6210		/* CRL only */
 #define DEUC			0x6214          /* CRL only */
 
+#define FW_BLC_SELF_VLV		0x6500
+#define  FW_CSPWRDWNEN		(1<<15)
+
 /*
  * Palette regs
  */
@@ -1601,9 +1697,12 @@
 /* Video Data Island Packet control */
 #define VIDEO_DIP_DATA		0x61178
 #define VIDEO_DIP_CTL		0x61170
+/* Pre HSW: */
 #define   VIDEO_DIP_ENABLE		(1 << 31)
 #define   VIDEO_DIP_PORT_B		(1 << 29)
 #define   VIDEO_DIP_PORT_C		(2 << 29)
+#define   VIDEO_DIP_PORT_D		(3 << 29)
+#define   VIDEO_DIP_PORT_MASK		(3 << 29)
 #define   VIDEO_DIP_ENABLE_AVI		(1 << 21)
 #define   VIDEO_DIP_ENABLE_VENDOR	(2 << 21)
 #define   VIDEO_DIP_ENABLE_SPD		(8 << 21)
@@ -1614,6 +1713,10 @@
 #define   VIDEO_DIP_FREQ_ONCE		(0 << 16)
 #define   VIDEO_DIP_FREQ_VSYNC		(1 << 16)
 #define   VIDEO_DIP_FREQ_2VSYNC		(2 << 16)
+#define   VIDEO_DIP_FREQ_MASK		(3 << 16)
+/* HSW and later: */
+#define   VIDEO_DIP_ENABLE_AVI_HSW	(1 << 12)
+#define   VIDEO_DIP_ENABLE_SPD_HSW	(1 << 0)
 
 /* Panel power sequencing */
 #define PP_STATUS	0x61200
@@ -2380,7 +2483,8 @@
 
 /* Pipe A */
 #define _PIPEADSL		0x70000
-#define   DSL_LINEMASK		0x00000fff
+#define   DSL_LINEMASK_GEN2	0x00000fff
+#define   DSL_LINEMASK_GEN3	0x00001fff
 #define _PIPEACONF		0x70008
 #define   PIPECONF_ENABLE	(1<<31)
 #define   PIPECONF_DISABLE	0
@@ -2422,23 +2526,30 @@
 #define   PIPECONF_DITHER_TYPE_TEMP (3<<2)
 #define _PIPEASTAT		0x70024
 #define   PIPE_FIFO_UNDERRUN_STATUS		(1UL<<31)
+#define   SPRITE1_FLIPDONE_INT_EN_VLV		(1UL<<30)
 #define   PIPE_CRC_ERROR_ENABLE			(1UL<<29)
 #define   PIPE_CRC_DONE_ENABLE			(1UL<<28)
 #define   PIPE_GMBUS_EVENT_ENABLE		(1UL<<27)
+#define   PLANE_FLIP_DONE_INT_EN_VLV		(1UL<<26)
 #define   PIPE_HOTPLUG_INTERRUPT_ENABLE		(1UL<<26)
 #define   PIPE_VSYNC_INTERRUPT_ENABLE		(1UL<<25)
 #define   PIPE_DISPLAY_LINE_COMPARE_ENABLE	(1UL<<24)
 #define   PIPE_DPST_EVENT_ENABLE		(1UL<<23)
+#define   SPRITE0_FLIP_DONE_INT_EN_VLV		(1UL<<26)
 #define   PIPE_LEGACY_BLC_EVENT_ENABLE		(1UL<<22)
 #define   PIPE_ODD_FIELD_INTERRUPT_ENABLE	(1UL<<21)
 #define   PIPE_EVEN_FIELD_INTERRUPT_ENABLE	(1UL<<20)
 #define   PIPE_HOTPLUG_TV_INTERRUPT_ENABLE	(1UL<<18) /* pre-965 */
 #define   PIPE_START_VBLANK_INTERRUPT_ENABLE	(1UL<<18) /* 965 or later */
 #define   PIPE_VBLANK_INTERRUPT_ENABLE		(1UL<<17)
+#define   PIPEA_HBLANK_INT_EN_VLV		(1UL<<16)
 #define   PIPE_OVERLAY_UPDATED_ENABLE		(1UL<<16)
+#define   SPRITE1_FLIPDONE_INT_STATUS_VLV	(1UL<<15)
+#define   SPRITE0_FLIPDONE_INT_STATUS_VLV	(1UL<<15)
 #define   PIPE_CRC_ERROR_INTERRUPT_STATUS	(1UL<<13)
 #define   PIPE_CRC_DONE_INTERRUPT_STATUS	(1UL<<12)
 #define   PIPE_GMBUS_INTERRUPT_STATUS		(1UL<<11)
+#define   PLANE_FLIPDONE_INT_STATUS_VLV		(1UL<<10)
 #define   PIPE_HOTPLUG_INTERRUPT_STATUS		(1UL<<10)
 #define   PIPE_VSYNC_INTERRUPT_STATUS		(1UL<<9)
 #define   PIPE_DISPLAY_LINE_COMPARE_STATUS	(1UL<<8)
@@ -2463,6 +2574,40 @@
 #define PIPEFRAMEPIXEL(pipe)  _PIPE(pipe, _PIPEAFRAMEPIXEL, _PIPEBFRAMEPIXEL)
 #define PIPESTAT(pipe) _PIPE(pipe, _PIPEASTAT, _PIPEBSTAT)
 
+#define VLV_DPFLIPSTAT				0x70028
+#define   PIPEB_LINE_COMPARE_STATUS		(1<<29)
+#define   PIPEB_HLINE_INT_EN			(1<<28)
+#define   PIPEB_VBLANK_INT_EN			(1<<27)
+#define   SPRITED_FLIPDONE_INT_EN		(1<<26)
+#define   SPRITEC_FLIPDONE_INT_EN		(1<<25)
+#define   PLANEB_FLIPDONE_INT_EN		(1<<24)
+#define   PIPEA_LINE_COMPARE_STATUS		(1<<21)
+#define   PIPEA_HLINE_INT_EN			(1<<20)
+#define   PIPEA_VBLANK_INT_EN			(1<<19)
+#define   SPRITEB_FLIPDONE_INT_EN		(1<<18)
+#define   SPRITEA_FLIPDONE_INT_EN		(1<<17)
+#define   PLANEA_FLIPDONE_INT_EN		(1<<16)
+
+#define DPINVGTT				0x7002c /* VLV only */
+#define   CURSORB_INVALID_GTT_INT_EN		(1<<23)
+#define   CURSORA_INVALID_GTT_INT_EN		(1<<22)
+#define   SPRITED_INVALID_GTT_INT_EN		(1<<21)
+#define   SPRITEC_INVALID_GTT_INT_EN		(1<<20)
+#define   PLANEB_INVALID_GTT_INT_EN		(1<<19)
+#define   SPRITEB_INVALID_GTT_INT_EN		(1<<18)
+#define   SPRITEA_INVALID_GTT_INT_EN		(1<<17)
+#define   PLANEA_INVALID_GTT_INT_EN		(1<<16)
+#define   DPINVGTT_EN_MASK			0xff0000
+#define   CURSORB_INVALID_GTT_STATUS		(1<<7)
+#define   CURSORA_INVALID_GTT_STATUS		(1<<6)
+#define   SPRITED_INVALID_GTT_STATUS		(1<<5)
+#define   SPRITEC_INVALID_GTT_STATUS		(1<<4)
+#define   PLANEB_INVALID_GTT_STATUS		(1<<3)
+#define   SPRITEB_INVALID_GTT_STATUS		(1<<2)
+#define   SPRITEA_INVALID_GTT_STATUS		(1<<1)
+#define   PLANEA_INVALID_GTT_STATUS		(1<<0)
+#define   DPINVGTT_STATUS_MASK			0xff
+
 #define DSPARB			0x70030
 #define   DSPARB_CSTART_MASK	(0x7f << 7)
 #define   DSPARB_CSTART_SHIFT	7
@@ -2492,11 +2637,28 @@
 #define   DSPFW_HPLL_CURSOR_MASK	(0x3f<<16)
 #define   DSPFW_HPLL_SR_MASK		(0x1ff)
 
+/* drain latency register values*/
+#define DRAIN_LATENCY_PRECISION_32	32
+#define DRAIN_LATENCY_PRECISION_16	16
+#define VLV_DDL1			0x70050
+#define DDL_CURSORA_PRECISION_32	(1<<31)
+#define DDL_CURSORA_PRECISION_16	(0<<31)
+#define DDL_CURSORA_SHIFT		24
+#define DDL_PLANEA_PRECISION_32		(1<<7)
+#define DDL_PLANEA_PRECISION_16		(0<<7)
+#define VLV_DDL2			0x70054
+#define DDL_CURSORB_PRECISION_32	(1<<31)
+#define DDL_CURSORB_PRECISION_16	(0<<31)
+#define DDL_CURSORB_SHIFT		24
+#define DDL_PLANEB_PRECISION_32		(1<<7)
+#define DDL_PLANEB_PRECISION_16		(0<<7)
+
 /* FIFO watermark sizes etc */
 #define G4X_FIFO_LINE_SIZE	64
 #define I915_FIFO_LINE_SIZE	64
 #define I830_FIFO_LINE_SIZE	32
 
+#define VALLEYVIEW_FIFO_SIZE	255
 #define G4X_FIFO_SIZE		127
 #define I965_FIFO_SIZE		512
 #define I945_FIFO_SIZE		127
@@ -2504,6 +2666,7 @@
 #define I855GM_FIFO_SIZE	127 /* In cachelines */
 #define I830_FIFO_SIZE		95
 
+#define VALLEYVIEW_MAX_WM	0xff
 #define G4X_MAX_WM		0x3f
 #define I915_MAX_WM		0x3f
 
@@ -2518,6 +2681,7 @@
 #define PINEVIEW_CURSOR_DFT_WM	0
 #define PINEVIEW_CURSOR_GUARD_WM	5
 
+#define VALLEYVIEW_CURSOR_MAX_WM 64
 #define I965_CURSOR_FIFO	64
 #define I965_CURSOR_MAX_WM	32
 #define I965_CURSOR_DFT_WM	8
@@ -2726,6 +2890,13 @@
 #define DSPSURF(plane) _PIPE(plane, _DSPASURF, _DSPBSURF)
 #define DSPTILEOFF(plane) _PIPE(plane, _DSPATILEOFF, _DSPBTILEOFF)
 
+/* Display/Sprite base address macros */
+#define DISP_BASEADDR_MASK	(0xfffff000)
+#define I915_LO_DISPBASE(val)	(val & ~DISP_BASEADDR_MASK)
+#define I915_HI_DISPBASE(val)	(val & DISP_BASEADDR_MASK)
+#define I915_MODIFY_DISPBASE(reg, gfx_addr) \
+		(I915_WRITE(reg, gfx_addr | I915_LO_DISPBASE(I915_READ(reg))))
+
 /* VBIOS flags */
 #define SWF00			0x71410
 #define SWF01			0x71414
@@ -3058,25 +3229,38 @@
 #define DE_PCH_EVENT_IVB		(1<<28)
 #define DE_DP_A_HOTPLUG_IVB		(1<<27)
 #define DE_AUX_CHANNEL_A_IVB		(1<<26)
+#define DE_SPRITEC_FLIP_DONE_IVB	(1<<14)
+#define DE_PLANEC_FLIP_DONE_IVB		(1<<13)
+#define DE_PIPEC_VBLANK_IVB		(1<<10)
 #define DE_SPRITEB_FLIP_DONE_IVB	(1<<9)
-#define DE_SPRITEA_FLIP_DONE_IVB	(1<<4)
 #define DE_PLANEB_FLIP_DONE_IVB		(1<<8)
-#define DE_PLANEA_FLIP_DONE_IVB		(1<<3)
 #define DE_PIPEB_VBLANK_IVB		(1<<5)
+#define DE_SPRITEA_FLIP_DONE_IVB	(1<<4)
+#define DE_PLANEA_FLIP_DONE_IVB		(1<<3)
 #define DE_PIPEA_VBLANK_IVB		(1<<0)
 
+#define VLV_MASTER_IER			0x4400c /* Gunit master IER */
+#define   MASTER_INTERRUPT_ENABLE	(1<<31)
+
 #define DEISR   0x44000
 #define DEIMR   0x44004
 #define DEIIR   0x44008
 #define DEIER   0x4400c
 
-/* GT interrupt */
-#define GT_PIPE_NOTIFY		(1 << 4)
-#define GT_SYNC_STATUS          (1 << 2)
-#define GT_USER_INTERRUPT       (1 << 0)
-#define GT_BSD_USER_INTERRUPT   (1 << 5)
-#define GT_GEN6_BSD_USER_INTERRUPT	(1 << 12)
-#define GT_BLT_USER_INTERRUPT	(1 << 22)
+/* GT interrupt.
+ * Note that for gen6+ the ring-specific interrupt bits do alias with the
+ * corresponding bits in the per-ring interrupt control registers. */
+#define GT_GEN6_BLT_FLUSHDW_NOTIFY_INTERRUPT	(1 << 26)
+#define GT_GEN6_BLT_CS_ERROR_INTERRUPT		(1 << 25)
+#define GT_GEN6_BLT_USER_INTERRUPT		(1 << 22)
+#define GT_GEN6_BSD_CS_ERROR_INTERRUPT		(1 << 15)
+#define GT_GEN6_BSD_USER_INTERRUPT		(1 << 12)
+#define GT_BSD_USER_INTERRUPT			(1 << 5) /* ilk only */
+#define GT_GEN7_L3_PARITY_ERROR_INTERRUPT	(1 << 5)
+#define GT_PIPE_NOTIFY				(1 << 4)
+#define GT_RENDER_CS_ERROR_INTERRUPT		(1 << 3)
+#define GT_SYNC_STATUS				(1 << 2)
+#define GT_USER_INTERRUPT			(1 << 0)
 
 #define GTISR   0x44010
 #define GTIMR   0x44014
@@ -3226,15 +3410,15 @@
 
 #define _PCH_DPLL_A              0xc6014
 #define _PCH_DPLL_B              0xc6018
-#define PCH_DPLL(pipe) (pipe == 0 ?  _PCH_DPLL_A : _PCH_DPLL_B)
+#define _PCH_DPLL(pll) (pll == 0 ? _PCH_DPLL_A : _PCH_DPLL_B)
 
 #define _PCH_FPA0                0xc6040
 #define  FP_CB_TUNE		(0x3<<22)
 #define _PCH_FPA1                0xc6044
 #define _PCH_FPB0                0xc6048
 #define _PCH_FPB1                0xc604c
-#define PCH_FP0(pipe) (pipe == 0 ? _PCH_FPA0 : _PCH_FPB0)
-#define PCH_FP1(pipe) (pipe == 0 ? _PCH_FPA1 : _PCH_FPB1)
+#define _PCH_FP0(pll) (pll == 0 ? _PCH_FPA0 : _PCH_FPB0)
+#define _PCH_FP1(pll) (pll == 0 ? _PCH_FPA1 : _PCH_FPB1)
 
 #define PCH_DPLL_TEST           0xc606c
 
@@ -3329,6 +3513,57 @@
 #define TVIDEO_DIP_DATA(pipe) _PIPE(pipe, _VIDEO_DIP_DATA_A, _VIDEO_DIP_DATA_B)
 #define TVIDEO_DIP_GCP(pipe) _PIPE(pipe, _VIDEO_DIP_GCP_A, _VIDEO_DIP_GCP_B)
 
+#define VLV_VIDEO_DIP_CTL_A		0x60220
+#define VLV_VIDEO_DIP_DATA_A		0x60208
+#define VLV_VIDEO_DIP_GDCP_PAYLOAD_A	0x60210
+
+#define VLV_VIDEO_DIP_CTL_B		0x61170
+#define VLV_VIDEO_DIP_DATA_B		0x61174
+#define VLV_VIDEO_DIP_GDCP_PAYLOAD_B	0x61178
+
+#define VLV_TVIDEO_DIP_CTL(pipe) \
+	 _PIPE(pipe, VLV_VIDEO_DIP_CTL_A, VLV_VIDEO_DIP_CTL_B)
+#define VLV_TVIDEO_DIP_DATA(pipe) \
+	 _PIPE(pipe, VLV_VIDEO_DIP_DATA_A, VLV_VIDEO_DIP_DATA_B)
+#define VLV_TVIDEO_DIP_GCP(pipe) \
+	_PIPE(pipe, VLV_VIDEO_DIP_GDCP_PAYLOAD_A, VLV_VIDEO_DIP_GDCP_PAYLOAD_B)
+
+/* Haswell DIP controls */
+#define HSW_VIDEO_DIP_CTL_A		0x60200
+#define HSW_VIDEO_DIP_AVI_DATA_A	0x60220
+#define HSW_VIDEO_DIP_VS_DATA_A		0x60260
+#define HSW_VIDEO_DIP_SPD_DATA_A	0x602A0
+#define HSW_VIDEO_DIP_GMP_DATA_A	0x602E0
+#define HSW_VIDEO_DIP_VSC_DATA_A	0x60320
+#define HSW_VIDEO_DIP_AVI_ECC_A		0x60240
+#define HSW_VIDEO_DIP_VS_ECC_A		0x60280
+#define HSW_VIDEO_DIP_SPD_ECC_A		0x602C0
+#define HSW_VIDEO_DIP_GMP_ECC_A		0x60300
+#define HSW_VIDEO_DIP_VSC_ECC_A		0x60344
+#define HSW_VIDEO_DIP_GCP_A		0x60210
+
+#define HSW_VIDEO_DIP_CTL_B		0x61200
+#define HSW_VIDEO_DIP_AVI_DATA_B	0x61220
+#define HSW_VIDEO_DIP_VS_DATA_B		0x61260
+#define HSW_VIDEO_DIP_SPD_DATA_B	0x612A0
+#define HSW_VIDEO_DIP_GMP_DATA_B	0x612E0
+#define HSW_VIDEO_DIP_VSC_DATA_B	0x61320
+#define HSW_VIDEO_DIP_BVI_ECC_B		0x61240
+#define HSW_VIDEO_DIP_VS_ECC_B		0x61280
+#define HSW_VIDEO_DIP_SPD_ECC_B		0x612C0
+#define HSW_VIDEO_DIP_GMP_ECC_B		0x61300
+#define HSW_VIDEO_DIP_VSC_ECC_B		0x61344
+#define HSW_VIDEO_DIP_GCP_B		0x61210
+
+#define HSW_TVIDEO_DIP_CTL(pipe) \
+	 _PIPE(pipe, HSW_VIDEO_DIP_CTL_A, HSW_VIDEO_DIP_CTL_B)
+#define HSW_TVIDEO_DIP_AVI_DATA(pipe) \
+	 _PIPE(pipe, HSW_VIDEO_DIP_AVI_DATA_A, HSW_VIDEO_DIP_AVI_DATA_B)
+#define HSW_TVIDEO_DIP_SPD_DATA(pipe) \
+	 _PIPE(pipe, HSW_VIDEO_DIP_SPD_DATA_A, HSW_VIDEO_DIP_SPD_DATA_B)
+#define HSW_TVIDEO_DIP_GCP(pipe) \
+	_PIPE(pipe, HSW_VIDEO_DIP_GCP_A, HSW_VIDEO_DIP_GCP_B)
+
 #define _TRANS_HTOTAL_B          0xe1000
 #define _TRANS_HBLANK_B          0xe1004
 #define _TRANS_HSYNC_B           0xe1008
@@ -3489,6 +3724,9 @@
 #define  FDI_LINK_TRAIN_PATTERN_IDLE_CPT	(2<<8)
 #define  FDI_LINK_TRAIN_NORMAL_CPT		(3<<8)
 #define  FDI_LINK_TRAIN_PATTERN_MASK_CPT	(3<<8)
+/* LPT */
+#define  FDI_PORT_WIDTH_2X_LPT			(1<<19)
+#define  FDI_PORT_WIDTH_1X_LPT			(0<<19)
 
 #define _FDI_RXA_MISC            0xf0010
 #define _FDI_RXB_MISC            0xf1010
@@ -3549,6 +3787,7 @@
 #define  ADPA_CRT_HOTPLUG_FORCE_TRIGGER (1<<16)
 
 /* or SDVOB */
+#define VLV_HDMIB 0x61140
 #define HDMIB   0xe1140
 #define  PORT_ENABLE    (1 << 31)
 #define  TRANSCODER(pipe)       ((pipe) << 30)
@@ -3714,6 +3953,8 @@
 #define  EDP_LINK_TRAIN_VOL_EMP_MASK_IVB	(0x3f<<22)
 
 #define  FORCEWAKE				0xA18C
+#define  FORCEWAKE_VLV				0x1300b0
+#define  FORCEWAKE_ACK_VLV			0x1300b4
 #define  FORCEWAKE_ACK				0x130090
 #define  FORCEWAKE_MT				0xa188 /* multi-threaded */
 #define  FORCEWAKE_MT_ACK			0x130040
@@ -3731,6 +3972,7 @@
 
 #define GEN6_UCGCTL1				0x9400
 # define GEN6_BLBUNIT_CLOCK_GATE_DISABLE		(1 << 5)
+# define GEN6_CSUNIT_CLOCK_GATE_DISABLE			(1 << 7)
 
 #define GEN6_UCGCTL2				0x9404
 # define GEN6_RCZUNIT_CLOCK_GATE_DISABLE		(1 << 13)
@@ -3811,6 +4053,11 @@
 						 GEN6_PM_RP_DOWN_THRESHOLD | \
 						 GEN6_PM_RP_DOWN_TIMEOUT)
 
+#define GEN6_GT_GFX_RC6_LOCKED			0x138104
+#define GEN6_GT_GFX_RC6				0x138108
+#define GEN6_GT_GFX_RC6p			0x13810C
+#define GEN6_GT_GFX_RC6pp			0x138110
+
 #define GEN6_PCODE_MAILBOX			0x138124
 #define   GEN6_PCODE_READY			(1<<31)
 #define   GEN6_READ_OC_PARAMS			0xc
@@ -3870,4 +4117,197 @@
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI		(0xf << 16)
 #define   AUD_CONFIG_DISABLE_NCTS		(1 << 3)
 
+/* HSW Power Wells */
+#define HSW_PWR_WELL_CTL1		0x45400		/* BIOS */
+#define HSW_PWR_WELL_CTL2		0x45404		/* Driver */
+#define HSW_PWR_WELL_CTL3		0x45408		/* KVMR */
+#define HSW_PWR_WELL_CTL4		0x4540C		/* Debug */
+#define   HSW_PWR_WELL_ENABLE				(1<<31)
+#define   HSW_PWR_WELL_STATE				(1<<30)
+#define HSW_PWR_WELL_CTL5		0x45410
+#define   HSW_PWR_WELL_ENABLE_SINGLE_STEP	(1<<31)
+#define   HSW_PWR_WELL_PWR_GATE_OVERRIDE	(1<<20)
+#define   HSW_PWR_WELL_FORCE_ON				(1<<19)
+#define HSW_PWR_WELL_CTL6		0x45414
+
+/* Per-pipe DDI Function Control */
+#define PIPE_DDI_FUNC_CTL_A			0x60400
+#define PIPE_DDI_FUNC_CTL_B			0x61400
+#define PIPE_DDI_FUNC_CTL_C			0x62400
+#define PIPE_DDI_FUNC_CTL_EDP		0x6F400
+#define DDI_FUNC_CTL(pipe) _PIPE(pipe, \
+					PIPE_DDI_FUNC_CTL_A, \
+					PIPE_DDI_FUNC_CTL_B)
+#define  PIPE_DDI_FUNC_ENABLE		(1<<31)
+/* Those bits are ignored by pipe EDP since it can only connect to DDI A */
+#define  PIPE_DDI_PORT_MASK				(0xf<<28)
+#define  PIPE_DDI_SELECT_PORT(x)		((x)<<28)
+#define  PIPE_DDI_MODE_SELECT_HDMI		(0<<24)
+#define  PIPE_DDI_MODE_SELECT_DVI		(1<<24)
+#define  PIPE_DDI_MODE_SELECT_DP_SST	(2<<24)
+#define  PIPE_DDI_MODE_SELECT_DP_MST	(3<<24)
+#define  PIPE_DDI_MODE_SELECT_FDI		(4<<24)
+#define  PIPE_DDI_BPC_8					(0<<20)
+#define  PIPE_DDI_BPC_10				(1<<20)
+#define  PIPE_DDI_BPC_6					(2<<20)
+#define  PIPE_DDI_BPC_12				(3<<20)
+#define  PIPE_DDI_BFI_ENABLE			(1<<4)
+#define  PIPE_DDI_PORT_WIDTH_X1			(0<<1)
+#define  PIPE_DDI_PORT_WIDTH_X2			(1<<1)
+#define  PIPE_DDI_PORT_WIDTH_X4			(3<<1)
+
+/* DisplayPort Transport Control */
+#define DP_TP_CTL_A			0x64040
+#define DP_TP_CTL_B			0x64140
+#define DP_TP_CTL(port) _PORT(port, \
+					DP_TP_CTL_A, \
+					DP_TP_CTL_B)
+#define  DP_TP_CTL_ENABLE		(1<<31)
+#define  DP_TP_CTL_MODE_SST	(0<<27)
+#define  DP_TP_CTL_MODE_MST	(1<<27)
+#define  DP_TP_CTL_ENHANCED_FRAME_ENABLE	(1<<18)
+#define  DP_TP_CTL_FDI_AUTOTRAIN	(1<<15)
+#define  DP_TP_CTL_LINK_TRAIN_MASK		(7<<8)
+#define  DP_TP_CTL_LINK_TRAIN_PAT1		(0<<8)
+#define  DP_TP_CTL_LINK_TRAIN_PAT2		(1<<8)
+#define  DP_TP_CTL_LINK_TRAIN_NORMAL	(3<<8)
+
+/* DisplayPort Transport Status */
+#define DP_TP_STATUS_A			0x64044
+#define DP_TP_STATUS_B			0x64144
+#define DP_TP_STATUS(port) _PORT(port, \
+					DP_TP_STATUS_A, \
+					DP_TP_STATUS_B)
+#define  DP_TP_STATUS_AUTOTRAIN_DONE	(1<<12)
+
+/* DDI Buffer Control */
+#define DDI_BUF_CTL_A				0x64000
+#define DDI_BUF_CTL_B				0x64100
+#define DDI_BUF_CTL(port) _PORT(port, \
+					DDI_BUF_CTL_A, \
+					DDI_BUF_CTL_B)
+#define  DDI_BUF_CTL_ENABLE				(1<<31)
+#define  DDI_BUF_EMP_400MV_0DB_HSW		(0<<24)   /* Sel0 */
+#define  DDI_BUF_EMP_400MV_3_5DB_HSW	(1<<24)   /* Sel1 */
+#define  DDI_BUF_EMP_400MV_6DB_HSW		(2<<24)   /* Sel2 */
+#define  DDI_BUF_EMP_400MV_9_5DB_HSW	(3<<24)   /* Sel3 */
+#define  DDI_BUF_EMP_600MV_0DB_HSW		(4<<24)   /* Sel4 */
+#define  DDI_BUF_EMP_600MV_3_5DB_HSW	(5<<24)   /* Sel5 */
+#define  DDI_BUF_EMP_600MV_6DB_HSW		(6<<24)   /* Sel6 */
+#define  DDI_BUF_EMP_800MV_0DB_HSW		(7<<24)   /* Sel7 */
+#define  DDI_BUF_EMP_800MV_3_5DB_HSW	(8<<24)   /* Sel8 */
+#define  DDI_BUF_EMP_MASK				(0xf<<24)
+#define  DDI_BUF_IS_IDLE				(1<<7)
+#define  DDI_PORT_WIDTH_X1				(0<<1)
+#define  DDI_PORT_WIDTH_X2				(1<<1)
+#define  DDI_PORT_WIDTH_X4				(3<<1)
+#define  DDI_INIT_DISPLAY_DETECTED		(1<<0)
+
+/* DDI Buffer Translations */
+#define DDI_BUF_TRANS_A				0x64E00
+#define DDI_BUF_TRANS_B				0x64E60
+#define DDI_BUF_TRANS(port) _PORT(port, \
+					DDI_BUF_TRANS_A, \
+					DDI_BUF_TRANS_B)
+
+/* Sideband Interface (SBI) is programmed indirectly, via
+ * SBI_ADDR, which contains the register offset; and SBI_DATA,
+ * which contains the payload */
+#define SBI_ADDR				0xC6000
+#define SBI_DATA				0xC6004
+#define SBI_CTL_STAT			0xC6008
+#define  SBI_CTL_OP_CRRD		(0x6<<8)
+#define  SBI_CTL_OP_CRWR		(0x7<<8)
+#define  SBI_RESPONSE_FAIL		(0x1<<1)
+#define  SBI_RESPONSE_SUCCESS	(0x0<<1)
+#define  SBI_BUSY				(0x1<<0)
+#define  SBI_READY				(0x0<<0)
+
+/* SBI offsets */
+#define  SBI_SSCDIVINTPHASE6		0x0600
+#define   SBI_SSCDIVINTPHASE_DIVSEL_MASK	((0x7f)<<1)
+#define   SBI_SSCDIVINTPHASE_DIVSEL(x)		((x)<<1)
+#define   SBI_SSCDIVINTPHASE_INCVAL_MASK	((0x7f)<<8)
+#define   SBI_SSCDIVINTPHASE_INCVAL(x)		((x)<<8)
+#define   SBI_SSCDIVINTPHASE_DIR(x)			((x)<<15)
+#define   SBI_SSCDIVINTPHASE_PROPAGATE		(1<<0)
+#define  SBI_SSCCTL					0x020c
+#define  SBI_SSCCTL6				0x060C
+#define   SBI_SSCCTL_DISABLE		(1<<0)
+#define  SBI_SSCAUXDIV6				0x0610
+#define   SBI_SSCAUXDIV_FINALDIV2SEL(x)		((x)<<4)
+#define  SBI_DBUFF0					0x2a00
+
+/* LPT PIXCLK_GATE */
+#define PIXCLK_GATE				0xC6020
+#define  PIXCLK_GATE_UNGATE		1<<0
+#define  PIXCLK_GATE_GATE		0<<0
+
+/* SPLL */
+#define SPLL_CTL				0x46020
+#define  SPLL_PLL_ENABLE		(1<<31)
+#define  SPLL_PLL_SCC			(1<<28)
+#define  SPLL_PLL_NON_SCC		(2<<28)
+#define  SPLL_PLL_FREQ_810MHz	(0<<26)
+#define  SPLL_PLL_FREQ_1350MHz	(1<<26)
+
+/* WRPLL */
+#define WRPLL_CTL1				0x46040
+#define WRPLL_CTL2				0x46060
+#define  WRPLL_PLL_ENABLE				(1<<31)
+#define  WRPLL_PLL_SELECT_SSC			(0x01<<28)
+#define  WRPLL_PLL_SELECT_NON_SCC		(0x02<<28)
+#define  WRPLL_PLL_SELECT_LCPLL_2700	(0x03<<28)
+/* WRPLL divider programming */
+#define  WRPLL_DIVIDER_REFERENCE(x)		((x)<<0)
+#define  WRPLL_DIVIDER_POST(x)			((x)<<8)
+#define  WRPLL_DIVIDER_FEEDBACK(x)		((x)<<16)
+
+/* Port clock selection */
+#define PORT_CLK_SEL_A			0x46100
+#define PORT_CLK_SEL_B			0x46104
+#define PORT_CLK_SEL(port) _PORT(port, \
+					PORT_CLK_SEL_A, \
+					PORT_CLK_SEL_B)
+#define  PORT_CLK_SEL_LCPLL_2700	(0<<29)
+#define  PORT_CLK_SEL_LCPLL_1350	(1<<29)
+#define  PORT_CLK_SEL_LCPLL_810		(2<<29)
+#define  PORT_CLK_SEL_SPLL			(3<<29)
+#define  PORT_CLK_SEL_WRPLL1		(4<<29)
+#define  PORT_CLK_SEL_WRPLL2		(5<<29)
+
+/* Pipe clock selection */
+#define PIPE_CLK_SEL_A			0x46140
+#define PIPE_CLK_SEL_B			0x46144
+#define PIPE_CLK_SEL(pipe) _PIPE(pipe, \
+					PIPE_CLK_SEL_A, \
+					PIPE_CLK_SEL_B)
+/* For each pipe, we need to select the corresponding port clock */
+#define  PIPE_CLK_SEL_DISABLED	(0x0<<29)
+#define  PIPE_CLK_SEL_PORT(x)	((x+1)<<29)
+
+/* LCPLL Control */
+#define LCPLL_CTL				0x130040
+#define  LCPLL_PLL_DISABLE		(1<<31)
+#define  LCPLL_PLL_LOCK			(1<<30)
+#define  LCPLL_CD_CLOCK_DISABLE	(1<<25)
+#define  LCPLL_CD2X_CLOCK_DISABLE	(1<<23)
+
+/* Pipe WM_LINETIME - watermark line time */
+#define PIPE_WM_LINETIME_A		0x45270
+#define PIPE_WM_LINETIME_B		0x45274
+#define PIPE_WM_LINETIME(pipe) _PIPE(pipe, \
+					PIPE_WM_LINETIME_A, \
+					PIPE_WM_LINETIME_A)
+#define   PIPE_WM_LINETIME_MASK		(0x1ff)
+#define   PIPE_WM_LINETIME_TIME(x)			((x))
+#define   PIPE_WM_LINETIME_IPS_LINETIME_MASK	(0x1ff<<16)
+#define   PIPE_WM_LINETIME_IPS_LINETIME(x)		((x)<<16)
+
+/* SFUSE_STRAP */
+#define SFUSE_STRAP				0xc2014
+#define  SFUSE_STRAP_DDIB_DETECTED	(1<<2)
+#define  SFUSE_STRAP_DDIC_DETECTED	(1<<1)
+#define  SFUSE_STRAP_DDID_DETECTED	(1<<0)
+
 #endif /* _I915_REG_H_ */
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index 2b5eb229ff2c..0ede02a99d91 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -40,7 +40,7 @@ static bool i915_pipe_enabled(struct drm_device *dev, enum pipe pipe)
 		return false;
 
 	if (HAS_PCH_SPLIT(dev))
-		dpll_reg = PCH_DPLL(pipe);
+		dpll_reg = _PCH_DPLL(pipe);
 	else
 		dpll_reg = (pipe == PIPE_A) ? _DPLL_A : _DPLL_B;
 
@@ -876,22 +876,6 @@ int i915_restore_state(struct drm_device *dev)
 		I915_WRITE(IER, dev_priv->saveIER);
 		I915_WRITE(IMR, dev_priv->saveIMR);
 	}
-	mutex_unlock(&dev->struct_mutex);
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		intel_init_clock_gating(dev);
-
-	if (IS_IRONLAKE_M(dev)) {
-		ironlake_enable_drps(dev);
-		intel_init_emon(dev);
-	}
-
-	if (INTEL_INFO(dev)->gen >= 6) {
-		gen6_enable_rps(dev_priv);
-		gen6_update_ring_freq(dev_priv);
-	}
-
-	mutex_lock(&dev->struct_mutex);
 
 	/* Cache mode state */
 	I915_WRITE(CACHE_MODE_0, dev_priv->saveCACHE_MODE_0 | 0xffff0000);
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
new file mode 100644
index 000000000000..79f83445afa0
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright © 2012 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Ben Widawsky <ben@bwidawsk.net>
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/stat.h>
+#include <linux/sysfs.h>
+#include "i915_drv.h"
+
+static u32 calc_residency(struct drm_device *dev, const u32 reg)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u64 raw_time; /* 32b value may overflow during fixed point math */
+
+	if (!intel_enable_rc6(dev))
+		return 0;
+
+	raw_time = I915_READ(reg) * 128ULL;
+	return DIV_ROUND_UP_ULL(raw_time, 100000);
+}
+
+static ssize_t
+show_rc6_mask(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	return snprintf(buf, PAGE_SIZE, "%x", intel_enable_rc6(dminor->dev));
+}
+
+static ssize_t
+show_rc6_ms(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	u32 rc6_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6);
+	return snprintf(buf, PAGE_SIZE, "%u", rc6_residency);
+}
+
+static ssize_t
+show_rc6p_ms(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	u32 rc6p_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6p);
+	return snprintf(buf, PAGE_SIZE, "%u", rc6p_residency);
+}
+
+static ssize_t
+show_rc6pp_ms(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	u32 rc6pp_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6pp);
+	return snprintf(buf, PAGE_SIZE, "%u", rc6pp_residency);
+}
+
+static DEVICE_ATTR(rc6_enable, S_IRUGO, show_rc6_mask, NULL);
+static DEVICE_ATTR(rc6_residency_ms, S_IRUGO, show_rc6_ms, NULL);
+static DEVICE_ATTR(rc6p_residency_ms, S_IRUGO, show_rc6p_ms, NULL);
+static DEVICE_ATTR(rc6pp_residency_ms, S_IRUGO, show_rc6pp_ms, NULL);
+
+static struct attribute *rc6_attrs[] = {
+	&dev_attr_rc6_enable.attr,
+	&dev_attr_rc6_residency_ms.attr,
+	&dev_attr_rc6p_residency_ms.attr,
+	&dev_attr_rc6pp_residency_ms.attr,
+	NULL
+};
+
+static struct attribute_group rc6_attr_group = {
+	.name = power_group_name,
+	.attrs =  rc6_attrs
+};
+
+void i915_setup_sysfs(struct drm_device *dev)
+{
+	int ret;
+
+	/* ILK doesn't have any residency information */
+	if (INTEL_INFO(dev)->gen < 6)
+		return;
+
+	ret = sysfs_merge_group(&dev->primary->kdev.kobj, &rc6_attr_group);
+	if (ret)
+		DRM_ERROR("sysfs setup failed\n");
+}
+
+void i915_teardown_sysfs(struct drm_device *dev)
+{
+	sysfs_unmerge_group(&dev->primary->kdev.kobj, &rc6_attr_group);
+}
diff --git a/drivers/gpu/drm/i915/i915_trace_points.c b/drivers/gpu/drm/i915/i915_trace_points.c
index ead876eb6ea0..f1df2bd4ecf4 100644
--- a/drivers/gpu/drm/i915/i915_trace_points.c
+++ b/drivers/gpu/drm/i915/i915_trace_points.c
@@ -7,5 +7,7 @@
 
 #include "i915_drv.h"
 
+#ifndef __CHECKER__
 #define CREATE_TRACE_POINTS
 #include "i915_trace.h"
+#endif
diff --git a/drivers/gpu/drm/i915/intel_acpi.c b/drivers/gpu/drm/i915/intel_acpi.c
index bae3edf956a4..f413899475e9 100644
--- a/drivers/gpu/drm/i915/intel_acpi.c
+++ b/drivers/gpu/drm/i915/intel_acpi.c
@@ -9,6 +9,7 @@
 #include <acpi/acpi_drivers.h>
 
 #include "drmP.h"
+#include "i915_drv.h"
 
 #define INTEL_DSM_REVISION_ID 1 /* For Calpella anyway... */
 
@@ -182,8 +183,6 @@ static void intel_dsm_platform_mux_info(void)
 			DRM_DEBUG_DRIVER("  hpd mux info: %s\n",
 			       intel_dsm_mux_type(info->buffer.pointer[3]));
 		}
-	} else {
-		DRM_ERROR("MUX INFO call failed\n");
 	}
 
 out:
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index b48fc2a8410c..353459362f6f 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -174,6 +174,28 @@ get_lvds_dvo_timing(const struct bdb_lvds_lfp_data *lvds_lfp_data,
 	return (struct lvds_dvo_timing *)(entry + dvo_timing_offset);
 }
 
+/* get lvds_fp_timing entry
+ * this function may return NULL if the corresponding entry is invalid
+ */
+static const struct lvds_fp_timing *
+get_lvds_fp_timing(const struct bdb_header *bdb,
+		   const struct bdb_lvds_lfp_data *data,
+		   const struct bdb_lvds_lfp_data_ptrs *ptrs,
+		   int index)
+{
+	size_t data_ofs = (const u8 *)data - (const u8 *)bdb;
+	u16 data_size = ((const u16 *)data)[-1]; /* stored in header */
+	size_t ofs;
+
+	if (index >= ARRAY_SIZE(ptrs->ptr))
+		return NULL;
+	ofs = ptrs->ptr[index].fp_timing_offset;
+	if (ofs < data_ofs ||
+	    ofs + sizeof(struct lvds_fp_timing) > data_ofs + data_size)
+		return NULL;
+	return (const struct lvds_fp_timing *)((const u8 *)bdb + ofs);
+}
+
 /* Try to find integrated panel data */
 static void
 parse_lfp_panel_data(struct drm_i915_private *dev_priv,
@@ -183,6 +205,7 @@ parse_lfp_panel_data(struct drm_i915_private *dev_priv,
 	const struct bdb_lvds_lfp_data *lvds_lfp_data;
 	const struct bdb_lvds_lfp_data_ptrs *lvds_lfp_data_ptrs;
 	const struct lvds_dvo_timing *panel_dvo_timing;
+	const struct lvds_fp_timing *fp_timing;
 	struct drm_display_mode *panel_fixed_mode;
 	int i, downclock;
 
@@ -244,6 +267,19 @@ parse_lfp_panel_data(struct drm_i915_private *dev_priv,
 			      "Normal Clock %dKHz, downclock %dKHz\n",
 			      panel_fixed_mode->clock, 10*downclock);
 	}
+
+	fp_timing = get_lvds_fp_timing(bdb, lvds_lfp_data,
+				       lvds_lfp_data_ptrs,
+				       lvds_options->panel_type);
+	if (fp_timing) {
+		/* check the resolution, just to be sure */
+		if (fp_timing->x_res == panel_fixed_mode->hdisplay &&
+		    fp_timing->y_res == panel_fixed_mode->vdisplay) {
+			dev_priv->bios_lvds_val = fp_timing->lvds_reg_val;
+			DRM_DEBUG_KMS("VBT initial LVDS value %x\n",
+				      dev_priv->bios_lvds_val);
+		}
+	}
 }
 
 /* Try to find sdvo panel data */
@@ -256,6 +292,11 @@ parse_sdvo_panel_data(struct drm_i915_private *dev_priv,
 	int index;
 
 	index = i915_vbt_sdvo_panel_type;
+	if (index == -2) {
+		DRM_DEBUG_KMS("Ignore SDVO panel mode from BIOS VBT tables.\n");
+		return;
+	}
+
 	if (index == -1) {
 		struct bdb_sdvo_lvds_options *sdvo_lvds_options;
 
@@ -332,11 +373,11 @@ parse_general_definitions(struct drm_i915_private *dev_priv,
 		if (block_size >= sizeof(*general)) {
 			int bus_pin = general->crt_ddc_gmbus_pin;
 			DRM_DEBUG_KMS("crt_ddc_bus_pin: %d\n", bus_pin);
-			if (bus_pin >= 1 && bus_pin <= 6)
+			if (intel_gmbus_is_port_valid(bus_pin))
 				dev_priv->crt_ddc_pin = bus_pin;
 		} else {
 			DRM_DEBUG_KMS("BDB_GD too small (%d). Invalid.\n",
-				  block_size);
+				      block_size);
 		}
 	}
 }
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 90b9793fd5da..75a70c46ef1b 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -55,18 +55,36 @@ static struct intel_crt *intel_attached_crt(struct drm_connector *connector)
 			    struct intel_crt, base);
 }
 
-static void intel_crt_dpms(struct drm_encoder *encoder, int mode)
+static void pch_crt_dpms(struct drm_encoder *encoder, int mode)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 temp, reg;
+	u32 temp;
 
-	if (HAS_PCH_SPLIT(dev))
-		reg = PCH_ADPA;
-	else
-		reg = ADPA;
+	temp = I915_READ(PCH_ADPA);
+	temp &= ~ADPA_DAC_ENABLE;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		temp |= ADPA_DAC_ENABLE;
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		/* Just leave port enable cleared */
+		break;
+	}
+
+	I915_WRITE(PCH_ADPA, temp);
+}
 
-	temp = I915_READ(reg);
+static void gmch_crt_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 temp;
+
+	temp = I915_READ(ADPA);
 	temp &= ~(ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE);
 	temp &= ~ADPA_DAC_ENABLE;
 
@@ -85,7 +103,7 @@ static void intel_crt_dpms(struct drm_encoder *encoder, int mode)
 		break;
 	}
 
-	I915_WRITE(reg, temp);
+	I915_WRITE(ADPA, temp);
 }
 
 static int intel_crt_mode_valid(struct drm_connector *connector,
@@ -278,9 +296,10 @@ static bool intel_crt_detect_ddc(struct drm_connector *connector)
 	if (intel_ddc_probe(&crt->base, dev_priv->crt_ddc_pin)) {
 		struct edid *edid;
 		bool is_digital = false;
+		struct i2c_adapter *i2c;
 
-		edid = drm_get_edid(connector,
-			&dev_priv->gmbus[dev_priv->crt_ddc_pin].adapter);
+		i2c = intel_gmbus_get_adapter(dev_priv, dev_priv->crt_ddc_pin);
+		edid = drm_get_edid(connector, i2c);
 		/*
 		 * This may be a DVI-I connector with a shared DDC
 		 * link between analog and digital outputs, so we
@@ -476,15 +495,16 @@ static int intel_crt_get_modes(struct drm_connector *connector)
 	struct drm_device *dev = connector->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
+	struct i2c_adapter *i2c;
 
-	ret = intel_ddc_get_modes(connector,
-				 &dev_priv->gmbus[dev_priv->crt_ddc_pin].adapter);
+	i2c = intel_gmbus_get_adapter(dev_priv, dev_priv->crt_ddc_pin);
+	ret = intel_ddc_get_modes(connector, i2c);
 	if (ret || !IS_G4X(dev))
 		return ret;
 
 	/* Try to probe digital port for output in DVI-I -> VGA mode. */
-	return intel_ddc_get_modes(connector,
-				   &dev_priv->gmbus[GMBUS_PORT_DPB].adapter);
+	i2c = intel_gmbus_get_adapter(dev_priv, GMBUS_PORT_DPB);
+	return intel_ddc_get_modes(connector, i2c);
 }
 
 static int intel_crt_set_property(struct drm_connector *connector,
@@ -507,12 +527,20 @@ static void intel_crt_reset(struct drm_connector *connector)
  * Routines for controlling stuff on the analog port
  */
 
-static const struct drm_encoder_helper_funcs intel_crt_helper_funcs = {
-	.dpms = intel_crt_dpms,
+static const struct drm_encoder_helper_funcs pch_encoder_funcs = {
 	.mode_fixup = intel_crt_mode_fixup,
 	.prepare = intel_encoder_prepare,
 	.commit = intel_encoder_commit,
 	.mode_set = intel_crt_mode_set,
+	.dpms = pch_crt_dpms,
+};
+
+static const struct drm_encoder_helper_funcs gmch_encoder_funcs = {
+	.mode_fixup = intel_crt_mode_fixup,
+	.prepare = intel_encoder_prepare,
+	.commit = intel_encoder_commit,
+	.mode_set = intel_crt_mode_set,
+	.dpms = gmch_crt_dpms,
 };
 
 static const struct drm_connector_funcs intel_crt_connector_funcs = {
@@ -536,7 +564,7 @@ static const struct drm_encoder_funcs intel_crt_enc_funcs = {
 
 static int __init intel_no_crt_dmi_callback(const struct dmi_system_id *id)
 {
-	DRM_DEBUG_KMS("Skipping CRT initialization for %s\n", id->ident);
+	DRM_INFO("Skipping CRT initialization for %s\n", id->ident);
 	return 1;
 }
 
@@ -558,6 +586,7 @@ void intel_crt_init(struct drm_device *dev)
 	struct intel_crt *crt;
 	struct intel_connector *intel_connector;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	const struct drm_encoder_helper_funcs *encoder_helper_funcs;
 
 	/* Skip machines without VGA that falsely report hotplug events */
 	if (dmi_check_system(intel_no_crt))
@@ -586,14 +615,23 @@ void intel_crt_init(struct drm_device *dev)
 	crt->base.clone_mask = (1 << INTEL_SDVO_NON_TV_CLONE_BIT |
 				1 << INTEL_ANALOG_CLONE_BIT |
 				1 << INTEL_SDVO_LVDS_CLONE_BIT);
-	crt->base.crtc_mask = (1 << 0) | (1 << 1);
+	if (IS_HASWELL(dev))
+		crt->base.crtc_mask = (1 << 0);
+	else
+		crt->base.crtc_mask = (1 << 0) | (1 << 1);
+
 	if (IS_GEN2(dev))
 		connector->interlace_allowed = 0;
 	else
 		connector->interlace_allowed = 1;
 	connector->doublescan_allowed = 0;
 
-	drm_encoder_helper_add(&crt->base.base, &intel_crt_helper_funcs);
+	if (HAS_PCH_SPLIT(dev))
+		encoder_helper_funcs = &pch_encoder_funcs;
+	else
+		encoder_helper_funcs = &gmch_encoder_funcs;
+
+	drm_encoder_helper_add(&crt->base.base, encoder_helper_funcs);
 	drm_connector_helper_add(connector, &intel_crt_connector_helper_funcs);
 
 	drm_sysfs_connector_add(connector);
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
new file mode 100644
index 000000000000..46d1e886c692
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -0,0 +1,755 @@
+/*
+ * Copyright © 2012 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Eugeni Dodonov <eugeni.dodonov@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "intel_drv.h"
+
+/* HDMI/DVI modes ignore everything but the last 2 items. So we share
+ * them for both DP and FDI transports, allowing those ports to
+ * automatically adapt to HDMI connections as well
+ */
+static const u32 hsw_ddi_translations_dp[] = {
+	0x00FFFFFF, 0x0006000E,		/* DP parameters */
+	0x00D75FFF, 0x0005000A,
+	0x00C30FFF, 0x00040006,
+	0x80AAAFFF, 0x000B0000,
+	0x00FFFFFF, 0x0005000A,
+	0x00D75FFF, 0x000C0004,
+	0x80C30FFF, 0x000B0000,
+	0x00FFFFFF, 0x00040006,
+	0x80D75FFF, 0x000B0000,
+	0x00FFFFFF, 0x00040006		/* HDMI parameters */
+};
+
+static const u32 hsw_ddi_translations_fdi[] = {
+	0x00FFFFFF, 0x0007000E,		/* FDI parameters */
+	0x00D75FFF, 0x000F000A,
+	0x00C30FFF, 0x00060006,
+	0x00AAAFFF, 0x001E0000,
+	0x00FFFFFF, 0x000F000A,
+	0x00D75FFF, 0x00160004,
+	0x00C30FFF, 0x001E0000,
+	0x00FFFFFF, 0x00060006,
+	0x00D75FFF, 0x001E0000,
+	0x00FFFFFF, 0x00040006		/* HDMI parameters */
+};
+
+/* On Haswell, DDI port buffers must be programmed with correct values
+ * in advance. The buffer values are different for FDI and DP modes,
+ * but the HDMI/DVI fields are shared among those. So we program the DDI
+ * in either FDI or DP modes only, as HDMI connections will work with both
+ * of those
+ */
+void intel_prepare_ddi_buffers(struct drm_device *dev, enum port port, bool use_fdi_mode)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 reg;
+	int i;
+	const u32 *ddi_translations = ((use_fdi_mode) ?
+		hsw_ddi_translations_fdi :
+		hsw_ddi_translations_dp);
+
+	DRM_DEBUG_DRIVER("Initializing DDI buffers for port %c in %s mode\n",
+			port_name(port),
+			use_fdi_mode ? "FDI" : "DP");
+
+	WARN((use_fdi_mode && (port != PORT_E)),
+		"Programming port %c in FDI mode, this probably will not work.\n",
+		port_name(port));
+
+	for (i=0, reg=DDI_BUF_TRANS(port); i < ARRAY_SIZE(hsw_ddi_translations_fdi); i++) {
+		I915_WRITE(reg, ddi_translations[i]);
+		reg += 4;
+	}
+}
+
+/* Program DDI buffers translations for DP. By default, program ports A-D in DP
+ * mode and port E for FDI.
+ */
+void intel_prepare_ddi(struct drm_device *dev)
+{
+	int port;
+
+	if (IS_HASWELL(dev)) {
+		for (port = PORT_A; port < PORT_E; port++)
+			intel_prepare_ddi_buffers(dev, port, false);
+
+		/* DDI E is the suggested one to work in FDI mode, so program is as such by
+		 * default. It will have to be re-programmed in case a digital DP output
+		 * will be detected on it
+		 */
+		intel_prepare_ddi_buffers(dev, PORT_E, true);
+	}
+}
+
+static const long hsw_ddi_buf_ctl_values[] = {
+	DDI_BUF_EMP_400MV_0DB_HSW,
+	DDI_BUF_EMP_400MV_3_5DB_HSW,
+	DDI_BUF_EMP_400MV_6DB_HSW,
+	DDI_BUF_EMP_400MV_9_5DB_HSW,
+	DDI_BUF_EMP_600MV_0DB_HSW,
+	DDI_BUF_EMP_600MV_3_5DB_HSW,
+	DDI_BUF_EMP_600MV_6DB_HSW,
+	DDI_BUF_EMP_800MV_0DB_HSW,
+	DDI_BUF_EMP_800MV_3_5DB_HSW
+};
+
+
+/* Starting with Haswell, different DDI ports can work in FDI mode for
+ * connection to the PCH-located connectors. For this, it is necessary to train
+ * both the DDI port and PCH receiver for the desired DDI buffer settings.
+ *
+ * The recommended port to work in FDI mode is DDI E, which we use here. Also,
+ * please note that when FDI mode is active on DDI E, it shares 2 lines with
+ * DDI A (which is used for eDP)
+ */
+
+void hsw_fdi_link_train(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 reg, temp, i;
+
+	/* Configure CPU PLL, wait for warmup */
+	I915_WRITE(SPLL_CTL,
+			SPLL_PLL_ENABLE |
+			SPLL_PLL_FREQ_1350MHz |
+			SPLL_PLL_SCC);
+
+	/* Use SPLL to drive the output when in FDI mode */
+	I915_WRITE(PORT_CLK_SEL(PORT_E),
+			PORT_CLK_SEL_SPLL);
+	I915_WRITE(PIPE_CLK_SEL(pipe),
+			PIPE_CLK_SEL_PORT(PORT_E));
+
+	udelay(20);
+
+	/* Start the training iterating through available voltages and emphasis */
+	for (i=0; i < ARRAY_SIZE(hsw_ddi_buf_ctl_values); i++) {
+		/* Configure DP_TP_CTL with auto-training */
+		I915_WRITE(DP_TP_CTL(PORT_E),
+					DP_TP_CTL_FDI_AUTOTRAIN |
+					DP_TP_CTL_ENHANCED_FRAME_ENABLE |
+					DP_TP_CTL_LINK_TRAIN_PAT1 |
+					DP_TP_CTL_ENABLE);
+
+		/* Configure and enable DDI_BUF_CTL for DDI E with next voltage */
+		temp = I915_READ(DDI_BUF_CTL(PORT_E));
+		temp = (temp & ~DDI_BUF_EMP_MASK);
+		I915_WRITE(DDI_BUF_CTL(PORT_E),
+				temp |
+				DDI_BUF_CTL_ENABLE |
+				DDI_PORT_WIDTH_X2 |
+				hsw_ddi_buf_ctl_values[i]);
+
+		udelay(600);
+
+		/* Enable CPU FDI Receiver with auto-training */
+		reg = FDI_RX_CTL(pipe);
+		I915_WRITE(reg,
+				I915_READ(reg) |
+					FDI_LINK_TRAIN_AUTO |
+					FDI_RX_ENABLE |
+					FDI_LINK_TRAIN_PATTERN_1_CPT |
+					FDI_RX_ENHANCE_FRAME_ENABLE |
+					FDI_PORT_WIDTH_2X_LPT |
+					FDI_RX_PLL_ENABLE);
+		POSTING_READ(reg);
+		udelay(100);
+
+		temp = I915_READ(DP_TP_STATUS(PORT_E));
+		if (temp & DP_TP_STATUS_AUTOTRAIN_DONE) {
+			DRM_DEBUG_DRIVER("BUF_CTL training done on %d step\n", i);
+
+			/* Enable normal pixel sending for FDI */
+			I915_WRITE(DP_TP_CTL(PORT_E),
+						DP_TP_CTL_FDI_AUTOTRAIN |
+						DP_TP_CTL_LINK_TRAIN_NORMAL |
+						DP_TP_CTL_ENHANCED_FRAME_ENABLE |
+						DP_TP_CTL_ENABLE);
+
+			/* Enable PIPE_DDI_FUNC_CTL for the pipe to work in FDI mode */
+			temp = I915_READ(DDI_FUNC_CTL(pipe));
+			temp &= ~PIPE_DDI_PORT_MASK;
+			temp |= PIPE_DDI_SELECT_PORT(PORT_E) |
+					PIPE_DDI_MODE_SELECT_FDI |
+					PIPE_DDI_FUNC_ENABLE |
+					PIPE_DDI_PORT_WIDTH_X2;
+			I915_WRITE(DDI_FUNC_CTL(pipe),
+					temp);
+			break;
+		} else {
+			DRM_ERROR("Error training BUF_CTL %d\n", i);
+
+			/* Disable DP_TP_CTL and FDI_RX_CTL) and retry */
+			I915_WRITE(DP_TP_CTL(PORT_E),
+					I915_READ(DP_TP_CTL(PORT_E)) &
+						~DP_TP_CTL_ENABLE);
+			I915_WRITE(FDI_RX_CTL(pipe),
+					I915_READ(FDI_RX_CTL(pipe)) &
+						~FDI_RX_PLL_ENABLE);
+			continue;
+		}
+	}
+
+	DRM_DEBUG_KMS("FDI train done.\n");
+}
+
+/* For DDI connections, it is possible to support different outputs over the
+ * same DDI port, such as HDMI or DP or even VGA via FDI. So we don't know by
+ * the time the output is detected what exactly is on the other end of it. This
+ * function aims at providing support for this detection and proper output
+ * configuration.
+ */
+void intel_ddi_init(struct drm_device *dev, enum port port)
+{
+	/* For now, we don't do any proper output detection and assume that we
+	 * handle HDMI only */
+
+	switch(port){
+	case PORT_A:
+		/* We don't handle eDP and DP yet */
+		DRM_DEBUG_DRIVER("Found digital output on DDI port A\n");
+		break;
+	/* Assume that the  ports B, C and D are working in HDMI mode for now */
+	case PORT_B:
+	case PORT_C:
+	case PORT_D:
+		intel_hdmi_init(dev, DDI_BUF_CTL(port));
+		break;
+	default:
+		DRM_DEBUG_DRIVER("No handlers defined for port %d, skipping DDI initialization\n",
+				port);
+		break;
+	}
+}
+
+/* WRPLL clock dividers */
+struct wrpll_tmds_clock {
+	u32 clock;
+	u16 p;		/* Post divider */
+	u16 n2;		/* Feedback divider */
+	u16 r2;		/* Reference divider */
+};
+
+/* Table of matching values for WRPLL clocks programming for each frequency */
+static const struct wrpll_tmds_clock wrpll_tmds_clock_table[] = {
+	{19750,	38,	25,	18},
+	{20000,	48,	32,	18},
+	{21000,	36,	21,	15},
+	{21912,	42,	29,	17},
+	{22000,	36,	22,	15},
+	{23000,	36,	23,	15},
+	{23500,	40,	40,	23},
+	{23750,	26,	16,	14},
+	{23750,	26,	16,	14},
+	{24000,	36,	24,	15},
+	{25000,	36,	25,	15},
+	{25175,	26,	40,	33},
+	{25200,	30,	21,	15},
+	{26000,	36,	26,	15},
+	{27000,	30,	21,	14},
+	{27027,	18,	100,	111},
+	{27500,	30,	29,	19},
+	{28000,	34,	30,	17},
+	{28320,	26,	30,	22},
+	{28322,	32,	42,	25},
+	{28750,	24,	23,	18},
+	{29000,	30,	29,	18},
+	{29750,	32,	30,	17},
+	{30000,	30,	25,	15},
+	{30750,	30,	41,	24},
+	{31000,	30,	31,	18},
+	{31500,	30,	28,	16},
+	{32000,	30,	32,	18},
+	{32500,	28,	32,	19},
+	{33000,	24,	22,	15},
+	{34000,	28,	30,	17},
+	{35000,	26,	32,	19},
+	{35500,	24,	30,	19},
+	{36000,	26,	26,	15},
+	{36750,	26,	46,	26},
+	{37000,	24,	23,	14},
+	{37762,	22,	40,	26},
+	{37800,	20,	21,	15},
+	{38000,	24,	27,	16},
+	{38250,	24,	34,	20},
+	{39000,	24,	26,	15},
+	{40000,	24,	32,	18},
+	{40500,	20,	21,	14},
+	{40541,	22,	147,	89},
+	{40750,	18,	19,	14},
+	{41000,	16,	17,	14},
+	{41500,	22,	44,	26},
+	{41540,	22,	44,	26},
+	{42000,	18,	21,	15},
+	{42500,	22,	45,	26},
+	{43000,	20,	43,	27},
+	{43163,	20,	24,	15},
+	{44000,	18,	22,	15},
+	{44900,	20,	108,	65},
+	{45000,	20,	25,	15},
+	{45250,	20,	52,	31},
+	{46000,	18,	23,	15},
+	{46750,	20,	45,	26},
+	{47000,	20,	40,	23},
+	{48000,	18,	24,	15},
+	{49000,	18,	49,	30},
+	{49500,	16,	22,	15},
+	{50000,	18,	25,	15},
+	{50500,	18,	32,	19},
+	{51000,	18,	34,	20},
+	{52000,	18,	26,	15},
+	{52406,	14,	34,	25},
+	{53000,	16,	22,	14},
+	{54000,	16,	24,	15},
+	{54054,	16,	173,	108},
+	{54500,	14,	24,	17},
+	{55000,	12,	22,	18},
+	{56000,	14,	45,	31},
+	{56250,	16,	25,	15},
+	{56750,	14,	25,	17},
+	{57000,	16,	27,	16},
+	{58000,	16,	43,	25},
+	{58250,	16,	38,	22},
+	{58750,	16,	40,	23},
+	{59000,	14,	26,	17},
+	{59341,	14,	40,	26},
+	{59400,	16,	44,	25},
+	{60000,	16,	32,	18},
+	{60500,	12,	39,	29},
+	{61000,	14,	49,	31},
+	{62000,	14,	37,	23},
+	{62250,	14,	42,	26},
+	{63000,	12,	21,	15},
+	{63500,	14,	28,	17},
+	{64000,	12,	27,	19},
+	{65000,	14,	32,	19},
+	{65250,	12,	29,	20},
+	{65500,	12,	32,	22},
+	{66000,	12,	22,	15},
+	{66667,	14,	38,	22},
+	{66750,	10,	21,	17},
+	{67000,	14,	33,	19},
+	{67750,	14,	58,	33},
+	{68000,	14,	30,	17},
+	{68179,	14,	46,	26},
+	{68250,	14,	46,	26},
+	{69000,	12,	23,	15},
+	{70000,	12,	28,	18},
+	{71000,	12,	30,	19},
+	{72000,	12,	24,	15},
+	{73000,	10,	23,	17},
+	{74000,	12,	23,	14},
+	{74176,	8,	100,	91},
+	{74250,	10,	22,	16},
+	{74481,	12,	43,	26},
+	{74500,	10,	29,	21},
+	{75000,	12,	25,	15},
+	{75250,	10,	39,	28},
+	{76000,	12,	27,	16},
+	{77000,	12,	53,	31},
+	{78000,	12,	26,	15},
+	{78750,	12,	28,	16},
+	{79000,	10,	38,	26},
+	{79500,	10,	28,	19},
+	{80000,	12,	32,	18},
+	{81000,	10,	21,	14},
+	{81081,	6,	100,	111},
+	{81624,	8,	29,	24},
+	{82000,	8,	17,	14},
+	{83000,	10,	40,	26},
+	{83950,	10,	28,	18},
+	{84000,	10,	28,	18},
+	{84750,	6,	16,	17},
+	{85000,	6,	17,	18},
+	{85250,	10,	30,	19},
+	{85750,	10,	27,	17},
+	{86000,	10,	43,	27},
+	{87000,	10,	29,	18},
+	{88000,	10,	44,	27},
+	{88500,	10,	41,	25},
+	{89000,	10,	28,	17},
+	{89012,	6,	90,	91},
+	{89100,	10,	33,	20},
+	{90000,	10,	25,	15},
+	{91000,	10,	32,	19},
+	{92000,	10,	46,	27},
+	{93000,	10,	31,	18},
+	{94000,	10,	40,	23},
+	{94500,	10,	28,	16},
+	{95000,	10,	44,	25},
+	{95654,	10,	39,	22},
+	{95750,	10,	39,	22},
+	{96000,	10,	32,	18},
+	{97000,	8,	23,	16},
+	{97750,	8,	42,	29},
+	{98000,	8,	45,	31},
+	{99000,	8,	22,	15},
+	{99750,	8,	34,	23},
+	{100000,	6,	20,	18},
+	{100500,	6,	19,	17},
+	{101000,	6,	37,	33},
+	{101250,	8,	21,	14},
+	{102000,	6,	17,	15},
+	{102250,	6,	25,	22},
+	{103000,	8,	29,	19},
+	{104000,	8,	37,	24},
+	{105000,	8,	28,	18},
+	{106000,	8,	22,	14},
+	{107000,	8,	46,	29},
+	{107214,	8,	27,	17},
+	{108000,	8,	24,	15},
+	{108108,	8,	173,	108},
+	{109000,	6,	23,	19},
+	{109000,	6,	23,	19},
+	{110000,	6,	22,	18},
+	{110013,	6,	22,	18},
+	{110250,	8,	49,	30},
+	{110500,	8,	36,	22},
+	{111000,	8,	23,	14},
+	{111264,	8,	150,	91},
+	{111375,	8,	33,	20},
+	{112000,	8,	63,	38},
+	{112500,	8,	25,	15},
+	{113100,	8,	57,	34},
+	{113309,	8,	42,	25},
+	{114000,	8,	27,	16},
+	{115000,	6,	23,	18},
+	{116000,	8,	43,	25},
+	{117000,	8,	26,	15},
+	{117500,	8,	40,	23},
+	{118000,	6,	38,	29},
+	{119000,	8,	30,	17},
+	{119500,	8,	46,	26},
+	{119651,	8,	39,	22},
+	{120000,	8,	32,	18},
+	{121000,	6,	39,	29},
+	{121250,	6,	31,	23},
+	{121750,	6,	23,	17},
+	{122000,	6,	42,	31},
+	{122614,	6,	30,	22},
+	{123000,	6,	41,	30},
+	{123379,	6,	37,	27},
+	{124000,	6,	51,	37},
+	{125000,	6,	25,	18},
+	{125250,	4,	13,	14},
+	{125750,	4,	27,	29},
+	{126000,	6,	21,	15},
+	{127000,	6,	24,	17},
+	{127250,	6,	41,	29},
+	{128000,	6,	27,	19},
+	{129000,	6,	43,	30},
+	{129859,	4,	25,	26},
+	{130000,	6,	26,	18},
+	{130250,	6,	42,	29},
+	{131000,	6,	32,	22},
+	{131500,	6,	38,	26},
+	{131850,	6,	41,	28},
+	{132000,	6,	22,	15},
+	{132750,	6,	28,	19},
+	{133000,	6,	34,	23},
+	{133330,	6,	37,	25},
+	{134000,	6,	61,	41},
+	{135000,	6,	21,	14},
+	{135250,	6,	167,	111},
+	{136000,	6,	62,	41},
+	{137000,	6,	35,	23},
+	{138000,	6,	23,	15},
+	{138500,	6,	40,	26},
+	{138750,	6,	37,	24},
+	{139000,	6,	34,	22},
+	{139050,	6,	34,	22},
+	{139054,	6,	34,	22},
+	{140000,	6,	28,	18},
+	{141000,	6,	36,	23},
+	{141500,	6,	22,	14},
+	{142000,	6,	30,	19},
+	{143000,	6,	27,	17},
+	{143472,	4,	17,	16},
+	{144000,	6,	24,	15},
+	{145000,	6,	29,	18},
+	{146000,	6,	47,	29},
+	{146250,	6,	26,	16},
+	{147000,	6,	49,	30},
+	{147891,	6,	23,	14},
+	{148000,	6,	23,	14},
+	{148250,	6,	28,	17},
+	{148352,	4,	100,	91},
+	{148500,	6,	33,	20},
+	{149000,	6,	48,	29},
+	{150000,	6,	25,	15},
+	{151000,	4,	19,	17},
+	{152000,	6,	27,	16},
+	{152280,	6,	44,	26},
+	{153000,	6,	34,	20},
+	{154000,	6,	53,	31},
+	{155000,	6,	31,	18},
+	{155250,	6,	50,	29},
+	{155750,	6,	45,	26},
+	{156000,	6,	26,	15},
+	{157000,	6,	61,	35},
+	{157500,	6,	28,	16},
+	{158000,	6,	65,	37},
+	{158250,	6,	44,	25},
+	{159000,	6,	53,	30},
+	{159500,	6,	39,	22},
+	{160000,	6,	32,	18},
+	{161000,	4,	31,	26},
+	{162000,	4,	18,	15},
+	{162162,	4,	131,	109},
+	{162500,	4,	53,	44},
+	{163000,	4,	29,	24},
+	{164000,	4,	17,	14},
+	{165000,	4,	22,	18},
+	{166000,	4,	32,	26},
+	{167000,	4,	26,	21},
+	{168000,	4,	46,	37},
+	{169000,	4,	104,	83},
+	{169128,	4,	64,	51},
+	{169500,	4,	39,	31},
+	{170000,	4,	34,	27},
+	{171000,	4,	19,	15},
+	{172000,	4,	51,	40},
+	{172750,	4,	32,	25},
+	{172800,	4,	32,	25},
+	{173000,	4,	41,	32},
+	{174000,	4,	49,	38},
+	{174787,	4,	22,	17},
+	{175000,	4,	35,	27},
+	{176000,	4,	30,	23},
+	{177000,	4,	38,	29},
+	{178000,	4,	29,	22},
+	{178500,	4,	37,	28},
+	{179000,	4,	53,	40},
+	{179500,	4,	73,	55},
+	{180000,	4,	20,	15},
+	{181000,	4,	55,	41},
+	{182000,	4,	31,	23},
+	{183000,	4,	42,	31},
+	{184000,	4,	30,	22},
+	{184750,	4,	26,	19},
+	{185000,	4,	37,	27},
+	{186000,	4,	51,	37},
+	{187000,	4,	36,	26},
+	{188000,	4,	32,	23},
+	{189000,	4,	21,	15},
+	{190000,	4,	38,	27},
+	{190960,	4,	41,	29},
+	{191000,	4,	41,	29},
+	{192000,	4,	27,	19},
+	{192250,	4,	37,	26},
+	{193000,	4,	20,	14},
+	{193250,	4,	53,	37},
+	{194000,	4,	23,	16},
+	{194208,	4,	23,	16},
+	{195000,	4,	26,	18},
+	{196000,	4,	45,	31},
+	{197000,	4,	35,	24},
+	{197750,	4,	41,	28},
+	{198000,	4,	22,	15},
+	{198500,	4,	25,	17},
+	{199000,	4,	28,	19},
+	{200000,	4,	37,	25},
+	{201000,	4,	61,	41},
+	{202000,	4,	112,	75},
+	{202500,	4,	21,	14},
+	{203000,	4,	146,	97},
+	{204000,	4,	62,	41},
+	{204750,	4,	44,	29},
+	{205000,	4,	38,	25},
+	{206000,	4,	29,	19},
+	{207000,	4,	23,	15},
+	{207500,	4,	40,	26},
+	{208000,	4,	37,	24},
+	{208900,	4,	48,	31},
+	{209000,	4,	48,	31},
+	{209250,	4,	31,	20},
+	{210000,	4,	28,	18},
+	{211000,	4,	25,	16},
+	{212000,	4,	22,	14},
+	{213000,	4,	30,	19},
+	{213750,	4,	38,	24},
+	{214000,	4,	46,	29},
+	{214750,	4,	35,	22},
+	{215000,	4,	43,	27},
+	{216000,	4,	24,	15},
+	{217000,	4,	37,	23},
+	{218000,	4,	42,	26},
+	{218250,	4,	42,	26},
+	{218750,	4,	34,	21},
+	{219000,	4,	47,	29},
+	{219000,	4,	47,	29},
+	{220000,	4,	44,	27},
+	{220640,	4,	49,	30},
+	{220750,	4,	36,	22},
+	{221000,	4,	36,	22},
+	{222000,	4,	23,	14},
+	{222525,	4,	28,	17},
+	{222750,	4,	33,	20},
+	{227000,	4,	37,	22},
+	{230250,	4,	29,	17},
+	{233500,	4,	38,	22},
+	{235000,	4,	40,	23},
+	{238000,	4,	30,	17},
+	{241500,	2,	17,	19},
+	{245250,	2,	20,	22},
+	{247750,	2,	22,	24},
+	{253250,	2,	15,	16},
+	{256250,	2,	18,	19},
+	{262500,	2,	31,	32},
+	{267250,	2,	66,	67},
+	{268500,	2,	94,	95},
+	{270000,	2,	14,	14},
+	{272500,	2,	77,	76},
+	{273750,	2,	57,	56},
+	{280750,	2,	24,	23},
+	{281250,	2,	23,	22},
+	{286000,	2,	17,	16},
+	{291750,	2,	26,	24},
+	{296703,	2,	56,	51},
+	{297000,	2,	22,	20},
+	{298000,	2,	21,	19},
+};
+
+void intel_ddi_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	int port = intel_hdmi->ddi_port;
+	int pipe = intel_crtc->pipe;
+	int p, n2, r2, valid=0;
+	u32 temp, i;
+
+	/* On Haswell, we need to enable the clocks and prepare DDI function to
+	 * work in HDMI mode for this pipe.
+	 */
+	DRM_DEBUG_KMS("Preparing HDMI DDI mode for Haswell on port %c, pipe %c\n", port_name(port), pipe_name(pipe));
+
+	for (i=0; i < ARRAY_SIZE(wrpll_tmds_clock_table); i++) {
+		if (crtc->mode.clock == wrpll_tmds_clock_table[i].clock) {
+			p = wrpll_tmds_clock_table[i].p;
+			n2 = wrpll_tmds_clock_table[i].n2;
+			r2 = wrpll_tmds_clock_table[i].r2;
+
+			DRM_DEBUG_KMS("WR PLL clock: found settings for %dKHz refresh rate: p=%d, n2=%d, r2=%d\n",
+					crtc->mode.clock,
+					p, n2, r2);
+
+			valid = 1;
+			break;
+		}
+	}
+
+	if (!valid) {
+		DRM_ERROR("Unable to find WR PLL clock settings for %dKHz refresh rate\n",
+				crtc->mode.clock);
+		return;
+	}
+
+	/* Enable LCPLL if disabled */
+	temp = I915_READ(LCPLL_CTL);
+	if (temp & LCPLL_PLL_DISABLE)
+		I915_WRITE(LCPLL_CTL,
+				temp & ~LCPLL_PLL_DISABLE);
+
+	/* Configure WR PLL 1, program the correct divider values for
+	 * the desired frequency and wait for warmup */
+	I915_WRITE(WRPLL_CTL1,
+			WRPLL_PLL_ENABLE |
+			WRPLL_PLL_SELECT_LCPLL_2700 |
+			WRPLL_DIVIDER_REFERENCE(r2) |
+			WRPLL_DIVIDER_FEEDBACK(n2) |
+			WRPLL_DIVIDER_POST(p));
+
+	udelay(20);
+
+	/* Use WRPLL1 clock to drive the output to the port, and tell the pipe to use
+	 * this port for connection.
+	 */
+	I915_WRITE(PORT_CLK_SEL(port),
+			PORT_CLK_SEL_WRPLL1);
+	I915_WRITE(PIPE_CLK_SEL(pipe),
+			PIPE_CLK_SEL_PORT(port));
+
+	udelay(20);
+
+	if (intel_hdmi->has_audio) {
+		/* Proper support for digital audio needs a new logic and a new set
+		 * of registers, so we leave it for future patch bombing.
+		 */
+		DRM_DEBUG_DRIVER("HDMI audio on pipe %c not yet supported on DDI\n",
+				 pipe_name(intel_crtc->pipe));
+	}
+
+	/* Enable PIPE_DDI_FUNC_CTL for the pipe to work in HDMI mode */
+	temp = I915_READ(DDI_FUNC_CTL(pipe));
+	temp &= ~PIPE_DDI_PORT_MASK;
+	temp &= ~PIPE_DDI_BPC_12;
+	temp |= PIPE_DDI_SELECT_PORT(port) |
+			PIPE_DDI_MODE_SELECT_HDMI |
+			((intel_crtc->bpp > 24) ?
+				PIPE_DDI_BPC_12 :
+				PIPE_DDI_BPC_8) |
+			PIPE_DDI_FUNC_ENABLE;
+
+	I915_WRITE(DDI_FUNC_CTL(pipe), temp);
+
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
+	intel_hdmi_set_spd_infoframe(encoder);
+}
+
+void intel_ddi_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	int port = intel_hdmi->ddi_port;
+	u32 temp;
+
+	temp = I915_READ(DDI_BUF_CTL(port));
+
+	if (mode != DRM_MODE_DPMS_ON) {
+		temp &= ~DDI_BUF_CTL_ENABLE;
+	} else {
+		temp |= DDI_BUF_CTL_ENABLE;
+	}
+
+	/* Enable DDI_BUF_CTL. In HDMI/DVI mode, the port width,
+	 * and swing/emphasis values are ignored so nothing special needs
+	 * to be done besides enabling the port.
+	 */
+	I915_WRITE(DDI_BUF_CTL(port),
+			temp);
+}
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 1b1cf3b3ff51..ee61ad1e642b 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -24,7 +24,7 @@
  *	Eric Anholt <eric@anholt.net>
  */
 
-#include <linux/cpufreq.h>
+#include <linux/dmi.h>
 #include <linux/module.h>
 #include <linux/input.h>
 #include <linux/i2c.h>
@@ -44,7 +44,6 @@
 #define HAS_eDP (intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP))
 
 bool intel_pipe_has_type(struct drm_crtc *crtc, int type);
-static void intel_update_watermarks(struct drm_device *dev);
 static void intel_increase_pllclock(struct drm_crtc *crtc);
 static void intel_crtc_update_cursor(struct drm_crtc *crtc, bool on);
 
@@ -360,6 +359,88 @@ static const intel_limit_t intel_limits_ironlake_display_port = {
 	.find_pll = intel_find_pll_ironlake_dp,
 };
 
+u32 intel_dpio_read(struct drm_i915_private *dev_priv, int reg)
+{
+	unsigned long flags;
+	u32 val = 0;
+
+	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
+	if (wait_for_atomic_us((I915_READ(DPIO_PKT) & DPIO_BUSY) == 0, 100)) {
+		DRM_ERROR("DPIO idle wait timed out\n");
+		goto out_unlock;
+	}
+
+	I915_WRITE(DPIO_REG, reg);
+	I915_WRITE(DPIO_PKT, DPIO_RID | DPIO_OP_READ | DPIO_PORTID |
+		   DPIO_BYTE);
+	if (wait_for_atomic_us((I915_READ(DPIO_PKT) & DPIO_BUSY) == 0, 100)) {
+		DRM_ERROR("DPIO read wait timed out\n");
+		goto out_unlock;
+	}
+	val = I915_READ(DPIO_DATA);
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_priv->dpio_lock, flags);
+	return val;
+}
+
+static void vlv_init_dpio(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/* Reset the DPIO config */
+	I915_WRITE(DPIO_CTL, 0);
+	POSTING_READ(DPIO_CTL);
+	I915_WRITE(DPIO_CTL, 1);
+	POSTING_READ(DPIO_CTL);
+}
+
+static int intel_dual_link_lvds_callback(const struct dmi_system_id *id)
+{
+	DRM_INFO("Forcing lvds to dual link mode on %s\n", id->ident);
+	return 1;
+}
+
+static const struct dmi_system_id intel_dual_link_lvds[] = {
+	{
+		.callback = intel_dual_link_lvds_callback,
+		.ident = "Apple MacBook Pro (Core i5/i7 Series)",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro8,2"),
+		},
+	},
+	{ }	/* terminating entry */
+};
+
+static bool is_dual_link_lvds(struct drm_i915_private *dev_priv,
+			      unsigned int reg)
+{
+	unsigned int val;
+
+	/* use the module option value if specified */
+	if (i915_lvds_channel_mode > 0)
+		return i915_lvds_channel_mode == 2;
+
+	if (dmi_check_system(intel_dual_link_lvds))
+		return true;
+
+	if (dev_priv->lvds_val)
+		val = dev_priv->lvds_val;
+	else {
+		/* BIOS should set the proper LVDS register value at boot, but
+		 * in reality, it doesn't set the value when the lid is closed;
+		 * we need to check "the value to be set" in VBT when LVDS
+		 * register is uninitialized.
+		 */
+		val = I915_READ(reg);
+		if (!(val & ~LVDS_DETECTED))
+			val = dev_priv->bios_lvds_val;
+		dev_priv->lvds_val = val;
+	}
+	return (val & LVDS_CLKB_POWER_MASK) == LVDS_CLKB_POWER_UP;
+}
+
 static const intel_limit_t *intel_ironlake_limit(struct drm_crtc *crtc,
 						int refclk)
 {
@@ -368,8 +449,7 @@ static const intel_limit_t *intel_ironlake_limit(struct drm_crtc *crtc,
 	const intel_limit_t *limit;
 
 	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
-		if ((I915_READ(PCH_LVDS) & LVDS_CLKB_POWER_MASK) ==
-		    LVDS_CLKB_POWER_UP) {
+		if (is_dual_link_lvds(dev_priv, PCH_LVDS)) {
 			/* LVDS dual channel */
 			if (refclk == 100000)
 				limit = &intel_limits_ironlake_dual_lvds_100m;
@@ -397,8 +477,7 @@ static const intel_limit_t *intel_g4x_limit(struct drm_crtc *crtc)
 	const intel_limit_t *limit;
 
 	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
-		if ((I915_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
-		    LVDS_CLKB_POWER_UP)
+		if (is_dual_link_lvds(dev_priv, LVDS))
 			/* LVDS with dual channel */
 			limit = &intel_limits_g4x_dual_channel_lvds;
 		else
@@ -536,8 +615,7 @@ intel_find_best_PLL(const intel_limit_t *limit, struct drm_crtc *crtc,
 		 * reliably set up different single/dual channel state, if we
 		 * even can.
 		 */
-		if ((I915_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
-		    LVDS_CLKB_POWER_UP)
+		if (is_dual_link_lvds(dev_priv, LVDS))
 			clock.p2 = limit->p2.p2_fast;
 		else
 			clock.p2 = limit->p2.p2_slow;
@@ -706,6 +784,17 @@ intel_find_pll_g4x_dp(const intel_limit_t *limit, struct drm_crtc *crtc,
 	return true;
 }
 
+static void ironlake_wait_for_vblank(struct drm_device *dev, int pipe)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 frame, frame_reg = PIPEFRAME(pipe);
+
+	frame = I915_READ(frame_reg);
+
+	if (wait_for(I915_READ_NOTRACE(frame_reg) != frame, 50))
+		DRM_DEBUG_KMS("vblank wait timed out\n");
+}
+
 /**
  * intel_wait_for_vblank - wait for vblank on a given pipe
  * @dev: drm device
@@ -719,6 +808,11 @@ void intel_wait_for_vblank(struct drm_device *dev, int pipe)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipestat_reg = PIPESTAT(pipe);
 
+	if (INTEL_INFO(dev)->gen >= 5) {
+		ironlake_wait_for_vblank(dev, pipe);
+		return;
+	}
+
 	/* Clear existing vblank status. Note this will clear any other
 	 * sticky status fields as well.
 	 *
@@ -771,15 +865,20 @@ void intel_wait_for_pipe_off(struct drm_device *dev, int pipe)
 			     100))
 			DRM_DEBUG_KMS("pipe_off wait timed out\n");
 	} else {
-		u32 last_line;
+		u32 last_line, line_mask;
 		int reg = PIPEDSL(pipe);
 		unsigned long timeout = jiffies + msecs_to_jiffies(100);
 
+		if (IS_GEN2(dev))
+			line_mask = DSL_LINEMASK_GEN2;
+		else
+			line_mask = DSL_LINEMASK_GEN3;
+
 		/* Wait for the display line to settle */
 		do {
-			last_line = I915_READ(reg) & DSL_LINEMASK;
+			last_line = I915_READ(reg) & line_mask;
 			mdelay(5);
-		} while (((I915_READ(reg) & DSL_LINEMASK) != last_line) &&
+		} while (((I915_READ(reg) & line_mask) != last_line) &&
 			 time_after(timeout, jiffies));
 		if (time_after(jiffies, timeout))
 			DRM_DEBUG_KMS("pipe_off wait timed out\n");
@@ -811,26 +910,33 @@ static void assert_pll(struct drm_i915_private *dev_priv,
 
 /* For ILK+ */
 static void assert_pch_pll(struct drm_i915_private *dev_priv,
-			   enum pipe pipe, bool state)
+			   struct intel_crtc *intel_crtc, bool state)
 {
 	int reg;
 	u32 val;
 	bool cur_state;
 
+	if (HAS_PCH_LPT(dev_priv->dev)) {
+		DRM_DEBUG_DRIVER("LPT detected: skipping PCH PLL test\n");
+		return;
+	}
+
+	if (!intel_crtc->pch_pll) {
+		WARN(1, "asserting PCH PLL enabled with no PLL\n");
+		return;
+	}
+
 	if (HAS_PCH_CPT(dev_priv->dev)) {
 		u32 pch_dpll;
 
 		pch_dpll = I915_READ(PCH_DPLL_SEL);
 
 		/* Make sure the selected PLL is enabled to the transcoder */
-		WARN(!((pch_dpll >> (4 * pipe)) & 8),
-		     "transcoder %d PLL not enabled\n", pipe);
-
-		/* Convert the transcoder pipe number to a pll pipe number */
-		pipe = (pch_dpll >> (4 * pipe)) & 1;
+		WARN(!((pch_dpll >> (4 * intel_crtc->pipe)) & 8),
+		     "transcoder %d PLL not enabled\n", intel_crtc->pipe);
 	}
 
-	reg = PCH_DPLL(pipe);
+	reg = intel_crtc->pch_pll->pll_reg;
 	val = I915_READ(reg);
 	cur_state = !!(val & DPLL_VCO_ENABLE);
 	WARN(cur_state != state,
@@ -847,9 +953,16 @@ static void assert_fdi_tx(struct drm_i915_private *dev_priv,
 	u32 val;
 	bool cur_state;
 
-	reg = FDI_TX_CTL(pipe);
-	val = I915_READ(reg);
-	cur_state = !!(val & FDI_TX_ENABLE);
+	if (IS_HASWELL(dev_priv->dev)) {
+		/* On Haswell, DDI is used instead of FDI_TX_CTL */
+		reg = DDI_FUNC_CTL(pipe);
+		val = I915_READ(reg);
+		cur_state = !!(val & PIPE_DDI_FUNC_ENABLE);
+	} else {
+		reg = FDI_TX_CTL(pipe);
+		val = I915_READ(reg);
+		cur_state = !!(val & FDI_TX_ENABLE);
+	}
 	WARN(cur_state != state,
 	     "FDI TX state assertion failure (expected %s, current %s)\n",
 	     state_string(state), state_string(cur_state));
@@ -864,9 +977,14 @@ static void assert_fdi_rx(struct drm_i915_private *dev_priv,
 	u32 val;
 	bool cur_state;
 
-	reg = FDI_RX_CTL(pipe);
-	val = I915_READ(reg);
-	cur_state = !!(val & FDI_RX_ENABLE);
+	if (IS_HASWELL(dev_priv->dev) && pipe > 0) {
+			DRM_ERROR("Attempting to enable FDI_RX on Haswell pipe > 0\n");
+			return;
+	} else {
+		reg = FDI_RX_CTL(pipe);
+		val = I915_READ(reg);
+		cur_state = !!(val & FDI_RX_ENABLE);
+	}
 	WARN(cur_state != state,
 	     "FDI RX state assertion failure (expected %s, current %s)\n",
 	     state_string(state), state_string(cur_state));
@@ -884,6 +1002,10 @@ static void assert_fdi_tx_pll_enabled(struct drm_i915_private *dev_priv,
 	if (dev_priv->info->gen == 5)
 		return;
 
+	/* On Haswell, DDI ports are responsible for the FDI PLL setup */
+	if (IS_HASWELL(dev_priv->dev))
+		return;
+
 	reg = FDI_TX_CTL(pipe);
 	val = I915_READ(reg);
 	WARN(!(val & FDI_TX_PLL_ENABLE), "FDI TX PLL assertion failure, should be active but is disabled\n");
@@ -895,6 +1017,10 @@ static void assert_fdi_rx_pll_enabled(struct drm_i915_private *dev_priv,
 	int reg;
 	u32 val;
 
+	if (IS_HASWELL(dev_priv->dev) && pipe > 0) {
+		DRM_ERROR("Attempting to enable FDI on Haswell with pipe > 0\n");
+		return;
+	}
 	reg = FDI_RX_CTL(pipe);
 	val = I915_READ(reg);
 	WARN(!(val & FDI_RX_PLL_ENABLE), "FDI RX PLL assertion failure, should be active but is disabled\n");
@@ -1000,6 +1126,11 @@ static void assert_pch_refclk_enabled(struct drm_i915_private *dev_priv)
 	u32 val;
 	bool enabled;
 
+	if (HAS_PCH_LPT(dev_priv->dev)) {
+		DRM_DEBUG_DRIVER("LPT does not has PCH refclk, skipping check\n");
+		return;
+	}
+
 	val = I915_READ(PCH_DREF_CONTROL);
 	enabled = !!(val & (DREF_SSC_SOURCE_MASK | DREF_NONSPREAD_SOURCE_MASK |
 			    DREF_SUPERSPREAD_SOURCE_MASK));
@@ -1198,6 +1329,69 @@ static void intel_disable_pll(struct drm_i915_private *dev_priv, enum pipe pipe)
 	POSTING_READ(reg);
 }
 
+/* SBI access */
+static void
+intel_sbi_write(struct drm_i915_private *dev_priv, u16 reg, u32 value)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
+	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_READY) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to become ready\n");
+		goto out_unlock;
+	}
+
+	I915_WRITE(SBI_ADDR,
+			(reg << 16));
+	I915_WRITE(SBI_DATA,
+			value);
+	I915_WRITE(SBI_CTL_STAT,
+			SBI_BUSY |
+			SBI_CTL_OP_CRWR);
+
+	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_READY | SBI_RESPONSE_SUCCESS)) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to complete write transaction\n");
+		goto out_unlock;
+	}
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_priv->dpio_lock, flags);
+}
+
+static u32
+intel_sbi_read(struct drm_i915_private *dev_priv, u16 reg)
+{
+	unsigned long flags;
+	u32 value;
+
+	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
+	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_READY) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to become ready\n");
+		goto out_unlock;
+	}
+
+	I915_WRITE(SBI_ADDR,
+			(reg << 16));
+	I915_WRITE(SBI_CTL_STAT,
+			SBI_BUSY |
+			SBI_CTL_OP_CRRD);
+
+	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_READY | SBI_RESPONSE_SUCCESS)) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to complete read transaction\n");
+		goto out_unlock;
+	}
+
+	value = I915_READ(SBI_DATA);
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_priv->dpio_lock, flags);
+	return value;
+}
+
 /**
  * intel_enable_pch_pll - enable PCH PLL
  * @dev_priv: i915 private structure
@@ -1206,60 +1400,88 @@ static void intel_disable_pll(struct drm_i915_private *dev_priv, enum pipe pipe)
  * The PCH PLL needs to be enabled before the PCH transcoder, since it
  * drives the transcoder clock.
  */
-static void intel_enable_pch_pll(struct drm_i915_private *dev_priv,
-				 enum pipe pipe)
+static void intel_enable_pch_pll(struct intel_crtc *intel_crtc)
 {
+	struct drm_i915_private *dev_priv = intel_crtc->base.dev->dev_private;
+	struct intel_pch_pll *pll;
 	int reg;
 	u32 val;
 
-	if (pipe > 1)
+	/* PCH PLLs only available on ILK, SNB and IVB */
+	BUG_ON(dev_priv->info->gen < 5);
+	pll = intel_crtc->pch_pll;
+	if (pll == NULL)
 		return;
 
-	/* PCH only available on ILK+ */
-	BUG_ON(dev_priv->info->gen < 5);
+	if (WARN_ON(pll->refcount == 0))
+		return;
+
+	DRM_DEBUG_KMS("enable PCH PLL %x (active %d, on? %d)for crtc %d\n",
+		      pll->pll_reg, pll->active, pll->on,
+		      intel_crtc->base.base.id);
 
 	/* PCH refclock must be enabled first */
 	assert_pch_refclk_enabled(dev_priv);
 
-	reg = PCH_DPLL(pipe);
+	if (pll->active++ && pll->on) {
+		assert_pch_pll_enabled(dev_priv, intel_crtc);
+		return;
+	}
+
+	DRM_DEBUG_KMS("enabling PCH PLL %x\n", pll->pll_reg);
+
+	reg = pll->pll_reg;
 	val = I915_READ(reg);
 	val |= DPLL_VCO_ENABLE;
 	I915_WRITE(reg, val);
 	POSTING_READ(reg);
 	udelay(200);
+
+	pll->on = true;
 }
 
-static void intel_disable_pch_pll(struct drm_i915_private *dev_priv,
-				  enum pipe pipe)
+static void intel_disable_pch_pll(struct intel_crtc *intel_crtc)
 {
+	struct drm_i915_private *dev_priv = intel_crtc->base.dev->dev_private;
+	struct intel_pch_pll *pll = intel_crtc->pch_pll;
 	int reg;
-	u32 val, pll_mask = TRANSC_DPLL_ENABLE | TRANSC_DPLLB_SEL,
-		pll_sel = TRANSC_DPLL_ENABLE;
-
-	if (pipe > 1)
-		return;
+	u32 val;
 
 	/* PCH only available on ILK+ */
 	BUG_ON(dev_priv->info->gen < 5);
+	if (pll == NULL)
+	       return;
 
-	/* Make sure transcoder isn't still depending on us */
-	assert_transcoder_disabled(dev_priv, pipe);
+	if (WARN_ON(pll->refcount == 0))
+		return;
 
-	if (pipe == 0)
-		pll_sel |= TRANSC_DPLLA_SEL;
-	else if (pipe == 1)
-		pll_sel |= TRANSC_DPLLB_SEL;
+	DRM_DEBUG_KMS("disable PCH PLL %x (active %d, on? %d) for crtc %d\n",
+		      pll->pll_reg, pll->active, pll->on,
+		      intel_crtc->base.base.id);
 
+	if (WARN_ON(pll->active == 0)) {
+		assert_pch_pll_disabled(dev_priv, intel_crtc);
+		return;
+	}
 
-	if ((I915_READ(PCH_DPLL_SEL) & pll_mask) == pll_sel)
+	if (--pll->active) {
+		assert_pch_pll_enabled(dev_priv, intel_crtc);
 		return;
+	}
+
+	DRM_DEBUG_KMS("disabling PCH PLL %x\n", pll->pll_reg);
 
-	reg = PCH_DPLL(pipe);
+	/* Make sure transcoder isn't still depending on us */
+	assert_transcoder_disabled(dev_priv, intel_crtc->pipe);
+
+	reg = pll->pll_reg;
 	val = I915_READ(reg);
 	val &= ~DPLL_VCO_ENABLE;
 	I915_WRITE(reg, val);
 	POSTING_READ(reg);
 	udelay(200);
+
+	pll->on = false;
 }
 
 static void intel_enable_transcoder(struct drm_i915_private *dev_priv,
@@ -1273,12 +1495,16 @@ static void intel_enable_transcoder(struct drm_i915_private *dev_priv,
 	BUG_ON(dev_priv->info->gen < 5);
 
 	/* Make sure PCH DPLL is enabled */
-	assert_pch_pll_enabled(dev_priv, pipe);
+	assert_pch_pll_enabled(dev_priv, to_intel_crtc(crtc));
 
 	/* FDI must be feeding us bits for PCH ports */
 	assert_fdi_tx_enabled(dev_priv, pipe);
 	assert_fdi_rx_enabled(dev_priv, pipe);
 
+	if (IS_HASWELL(dev_priv->dev) && pipe > 0) {
+		DRM_ERROR("Attempting to enable transcoder on Haswell with pipe > 0\n");
+		return;
+	}
 	reg = TRANSCONF(pipe);
 	val = I915_READ(reg);
 	pipeconf_val = I915_READ(PIPECONF(pipe));
@@ -1415,7 +1641,7 @@ static void intel_disable_pipe(struct drm_i915_private *dev_priv,
  * Plane regs are double buffered, going from enabled->disabled needs a
  * trigger in order to latch.  The display address reg provides this.
  */
-static void intel_flush_display_plane(struct drm_i915_private *dev_priv,
+void intel_flush_display_plane(struct drm_i915_private *dev_priv,
 				      enum plane plane)
 {
 	I915_WRITE(DSPADDR(plane), I915_READ(DSPADDR(plane)));
@@ -1526,490 +1752,6 @@ static void intel_disable_pch_ports(struct drm_i915_private *dev_priv,
 	disable_pch_hdmi(dev_priv, pipe, HDMID);
 }
 
-static void i8xx_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 fbc_ctl;
-
-	/* Disable compression */
-	fbc_ctl = I915_READ(FBC_CONTROL);
-	if ((fbc_ctl & FBC_CTL_EN) == 0)
-		return;
-
-	fbc_ctl &= ~FBC_CTL_EN;
-	I915_WRITE(FBC_CONTROL, fbc_ctl);
-
-	/* Wait for compressing bit to clear */
-	if (wait_for((I915_READ(FBC_STATUS) & FBC_STAT_COMPRESSING) == 0, 10)) {
-		DRM_DEBUG_KMS("FBC idle timed out\n");
-		return;
-	}
-
-	DRM_DEBUG_KMS("disabled FBC\n");
-}
-
-static void i8xx_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_framebuffer *fb = crtc->fb;
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int cfb_pitch;
-	int plane, i;
-	u32 fbc_ctl, fbc_ctl2;
-
-	cfb_pitch = dev_priv->cfb_size / FBC_LL_SIZE;
-	if (fb->pitches[0] < cfb_pitch)
-		cfb_pitch = fb->pitches[0];
-
-	/* FBC_CTL wants 64B units */
-	cfb_pitch = (cfb_pitch / 64) - 1;
-	plane = intel_crtc->plane == 0 ? FBC_CTL_PLANEA : FBC_CTL_PLANEB;
-
-	/* Clear old tags */
-	for (i = 0; i < (FBC_LL_SIZE / 32) + 1; i++)
-		I915_WRITE(FBC_TAG + (i * 4), 0);
-
-	/* Set it up... */
-	fbc_ctl2 = FBC_CTL_FENCE_DBL | FBC_CTL_IDLE_IMM | FBC_CTL_CPU_FENCE;
-	fbc_ctl2 |= plane;
-	I915_WRITE(FBC_CONTROL2, fbc_ctl2);
-	I915_WRITE(FBC_FENCE_OFF, crtc->y);
-
-	/* enable it... */
-	fbc_ctl = FBC_CTL_EN | FBC_CTL_PERIODIC;
-	if (IS_I945GM(dev))
-		fbc_ctl |= FBC_CTL_C3_IDLE; /* 945 needs special SR handling */
-	fbc_ctl |= (cfb_pitch & 0xff) << FBC_CTL_STRIDE_SHIFT;
-	fbc_ctl |= (interval & 0x2fff) << FBC_CTL_INTERVAL_SHIFT;
-	fbc_ctl |= obj->fence_reg;
-	I915_WRITE(FBC_CONTROL, fbc_ctl);
-
-	DRM_DEBUG_KMS("enabled FBC, pitch %d, yoff %d, plane %d, ",
-		      cfb_pitch, crtc->y, intel_crtc->plane);
-}
-
-static bool i8xx_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	return I915_READ(FBC_CONTROL) & FBC_CTL_EN;
-}
-
-static void g4x_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_framebuffer *fb = crtc->fb;
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
-	unsigned long stall_watermark = 200;
-	u32 dpfc_ctl;
-
-	dpfc_ctl = plane | DPFC_SR_EN | DPFC_CTL_LIMIT_1X;
-	dpfc_ctl |= DPFC_CTL_FENCE_EN | obj->fence_reg;
-	I915_WRITE(DPFC_CHICKEN, DPFC_HT_MODIFY);
-
-	I915_WRITE(DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
-		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
-		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
-	I915_WRITE(DPFC_FENCE_YOFF, crtc->y);
-
-	/* enable it... */
-	I915_WRITE(DPFC_CONTROL, I915_READ(DPFC_CONTROL) | DPFC_CTL_EN);
-
-	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
-}
-
-static void g4x_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dpfc_ctl;
-
-	/* Disable compression */
-	dpfc_ctl = I915_READ(DPFC_CONTROL);
-	if (dpfc_ctl & DPFC_CTL_EN) {
-		dpfc_ctl &= ~DPFC_CTL_EN;
-		I915_WRITE(DPFC_CONTROL, dpfc_ctl);
-
-		DRM_DEBUG_KMS("disabled FBC\n");
-	}
-}
-
-static bool g4x_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	return I915_READ(DPFC_CONTROL) & DPFC_CTL_EN;
-}
-
-static void sandybridge_blit_fbc_update(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 blt_ecoskpd;
-
-	/* Make sure blitter notifies FBC of writes */
-	gen6_gt_force_wake_get(dev_priv);
-	blt_ecoskpd = I915_READ(GEN6_BLITTER_ECOSKPD);
-	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY <<
-		GEN6_BLITTER_LOCK_SHIFT;
-	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
-	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY;
-	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
-	blt_ecoskpd &= ~(GEN6_BLITTER_FBC_NOTIFY <<
-			 GEN6_BLITTER_LOCK_SHIFT);
-	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
-	POSTING_READ(GEN6_BLITTER_ECOSKPD);
-	gen6_gt_force_wake_put(dev_priv);
-}
-
-static void ironlake_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_framebuffer *fb = crtc->fb;
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
-	unsigned long stall_watermark = 200;
-	u32 dpfc_ctl;
-
-	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
-	dpfc_ctl &= DPFC_RESERVED;
-	dpfc_ctl |= (plane | DPFC_CTL_LIMIT_1X);
-	/* Set persistent mode for front-buffer rendering, ala X. */
-	dpfc_ctl |= DPFC_CTL_PERSISTENT_MODE;
-	dpfc_ctl |= (DPFC_CTL_FENCE_EN | obj->fence_reg);
-	I915_WRITE(ILK_DPFC_CHICKEN, DPFC_HT_MODIFY);
-
-	I915_WRITE(ILK_DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
-		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
-		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
-	I915_WRITE(ILK_DPFC_FENCE_YOFF, crtc->y);
-	I915_WRITE(ILK_FBC_RT_BASE, obj->gtt_offset | ILK_FBC_RT_VALID);
-	/* enable it... */
-	I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl | DPFC_CTL_EN);
-
-	if (IS_GEN6(dev)) {
-		I915_WRITE(SNB_DPFC_CTL_SA,
-			   SNB_CPU_FENCE_ENABLE | obj->fence_reg);
-		I915_WRITE(DPFC_CPU_FENCE_OFFSET, crtc->y);
-		sandybridge_blit_fbc_update(dev);
-	}
-
-	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
-}
-
-static void ironlake_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dpfc_ctl;
-
-	/* Disable compression */
-	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
-	if (dpfc_ctl & DPFC_CTL_EN) {
-		dpfc_ctl &= ~DPFC_CTL_EN;
-		I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl);
-
-		DRM_DEBUG_KMS("disabled FBC\n");
-	}
-}
-
-static bool ironlake_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	return I915_READ(ILK_DPFC_CONTROL) & DPFC_CTL_EN;
-}
-
-bool intel_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!dev_priv->display.fbc_enabled)
-		return false;
-
-	return dev_priv->display.fbc_enabled(dev);
-}
-
-static void intel_fbc_work_fn(struct work_struct *__work)
-{
-	struct intel_fbc_work *work =
-		container_of(to_delayed_work(__work),
-			     struct intel_fbc_work, work);
-	struct drm_device *dev = work->crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	mutex_lock(&dev->struct_mutex);
-	if (work == dev_priv->fbc_work) {
-		/* Double check that we haven't switched fb without cancelling
-		 * the prior work.
-		 */
-		if (work->crtc->fb == work->fb) {
-			dev_priv->display.enable_fbc(work->crtc,
-						     work->interval);
-
-			dev_priv->cfb_plane = to_intel_crtc(work->crtc)->plane;
-			dev_priv->cfb_fb = work->crtc->fb->base.id;
-			dev_priv->cfb_y = work->crtc->y;
-		}
-
-		dev_priv->fbc_work = NULL;
-	}
-	mutex_unlock(&dev->struct_mutex);
-
-	kfree(work);
-}
-
-static void intel_cancel_fbc_work(struct drm_i915_private *dev_priv)
-{
-	if (dev_priv->fbc_work == NULL)
-		return;
-
-	DRM_DEBUG_KMS("cancelling pending FBC enable\n");
-
-	/* Synchronisation is provided by struct_mutex and checking of
-	 * dev_priv->fbc_work, so we can perform the cancellation
-	 * entirely asynchronously.
-	 */
-	if (cancel_delayed_work(&dev_priv->fbc_work->work))
-		/* tasklet was killed before being run, clean up */
-		kfree(dev_priv->fbc_work);
-
-	/* Mark the work as no longer wanted so that if it does
-	 * wake-up (because the work was already running and waiting
-	 * for our mutex), it will discover that is no longer
-	 * necessary to run.
-	 */
-	dev_priv->fbc_work = NULL;
-}
-
-static void intel_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct intel_fbc_work *work;
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!dev_priv->display.enable_fbc)
-		return;
-
-	intel_cancel_fbc_work(dev_priv);
-
-	work = kzalloc(sizeof *work, GFP_KERNEL);
-	if (work == NULL) {
-		dev_priv->display.enable_fbc(crtc, interval);
-		return;
-	}
-
-	work->crtc = crtc;
-	work->fb = crtc->fb;
-	work->interval = interval;
-	INIT_DELAYED_WORK(&work->work, intel_fbc_work_fn);
-
-	dev_priv->fbc_work = work;
-
-	DRM_DEBUG_KMS("scheduling delayed FBC enable\n");
-
-	/* Delay the actual enabling to let pageflipping cease and the
-	 * display to settle before starting the compression. Note that
-	 * this delay also serves a second purpose: it allows for a
-	 * vblank to pass after disabling the FBC before we attempt
-	 * to modify the control registers.
-	 *
-	 * A more complicated solution would involve tracking vblanks
-	 * following the termination of the page-flipping sequence
-	 * and indeed performing the enable as a co-routine and not
-	 * waiting synchronously upon the vblank.
-	 */
-	schedule_delayed_work(&work->work, msecs_to_jiffies(50));
-}
-
-void intel_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	intel_cancel_fbc_work(dev_priv);
-
-	if (!dev_priv->display.disable_fbc)
-		return;
-
-	dev_priv->display.disable_fbc(dev);
-	dev_priv->cfb_plane = -1;
-}
-
-/**
- * intel_update_fbc - enable/disable FBC as needed
- * @dev: the drm_device
- *
- * Set up the framebuffer compression hardware at mode set time.  We
- * enable it if possible:
- *   - plane A only (on pre-965)
- *   - no pixel mulitply/line duplication
- *   - no alpha buffer discard
- *   - no dual wide
- *   - framebuffer <= 2048 in width, 1536 in height
- *
- * We can't assume that any compression will take place (worst case),
- * so the compressed buffer has to be the same size as the uncompressed
- * one.  It also must reside (along with the line length buffer) in
- * stolen memory.
- *
- * We need to enable/disable FBC on a global basis.
- */
-static void intel_update_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = NULL, *tmp_crtc;
-	struct intel_crtc *intel_crtc;
-	struct drm_framebuffer *fb;
-	struct intel_framebuffer *intel_fb;
-	struct drm_i915_gem_object *obj;
-	int enable_fbc;
-
-	DRM_DEBUG_KMS("\n");
-
-	if (!i915_powersave)
-		return;
-
-	if (!I915_HAS_FBC(dev))
-		return;
-
-	/*
-	 * If FBC is already on, we just have to verify that we can
-	 * keep it that way...
-	 * Need to disable if:
-	 *   - more than one pipe is active
-	 *   - changing FBC params (stride, fence, mode)
-	 *   - new fb is too large to fit in compressed buffer
-	 *   - going to an unsupported config (interlace, pixel multiply, etc.)
-	 */
-	list_for_each_entry(tmp_crtc, &dev->mode_config.crtc_list, head) {
-		if (tmp_crtc->enabled && tmp_crtc->fb) {
-			if (crtc) {
-				DRM_DEBUG_KMS("more than one pipe active, disabling compression\n");
-				dev_priv->no_fbc_reason = FBC_MULTIPLE_PIPES;
-				goto out_disable;
-			}
-			crtc = tmp_crtc;
-		}
-	}
-
-	if (!crtc || crtc->fb == NULL) {
-		DRM_DEBUG_KMS("no output, disabling\n");
-		dev_priv->no_fbc_reason = FBC_NO_OUTPUT;
-		goto out_disable;
-	}
-
-	intel_crtc = to_intel_crtc(crtc);
-	fb = crtc->fb;
-	intel_fb = to_intel_framebuffer(fb);
-	obj = intel_fb->obj;
-
-	enable_fbc = i915_enable_fbc;
-	if (enable_fbc < 0) {
-		DRM_DEBUG_KMS("fbc set to per-chip default\n");
-		enable_fbc = 1;
-		if (INTEL_INFO(dev)->gen <= 6)
-			enable_fbc = 0;
-	}
-	if (!enable_fbc) {
-		DRM_DEBUG_KMS("fbc disabled per module param\n");
-		dev_priv->no_fbc_reason = FBC_MODULE_PARAM;
-		goto out_disable;
-	}
-	if (intel_fb->obj->base.size > dev_priv->cfb_size) {
-		DRM_DEBUG_KMS("framebuffer too large, disabling "
-			      "compression\n");
-		dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
-		goto out_disable;
-	}
-	if ((crtc->mode.flags & DRM_MODE_FLAG_INTERLACE) ||
-	    (crtc->mode.flags & DRM_MODE_FLAG_DBLSCAN)) {
-		DRM_DEBUG_KMS("mode incompatible with compression, "
-			      "disabling\n");
-		dev_priv->no_fbc_reason = FBC_UNSUPPORTED_MODE;
-		goto out_disable;
-	}
-	if ((crtc->mode.hdisplay > 2048) ||
-	    (crtc->mode.vdisplay > 1536)) {
-		DRM_DEBUG_KMS("mode too large for compression, disabling\n");
-		dev_priv->no_fbc_reason = FBC_MODE_TOO_LARGE;
-		goto out_disable;
-	}
-	if ((IS_I915GM(dev) || IS_I945GM(dev)) && intel_crtc->plane != 0) {
-		DRM_DEBUG_KMS("plane not 0, disabling compression\n");
-		dev_priv->no_fbc_reason = FBC_BAD_PLANE;
-		goto out_disable;
-	}
-
-	/* The use of a CPU fence is mandatory in order to detect writes
-	 * by the CPU to the scanout and trigger updates to the FBC.
-	 */
-	if (obj->tiling_mode != I915_TILING_X ||
-	    obj->fence_reg == I915_FENCE_REG_NONE) {
-		DRM_DEBUG_KMS("framebuffer not tiled or fenced, disabling compression\n");
-		dev_priv->no_fbc_reason = FBC_NOT_TILED;
-		goto out_disable;
-	}
-
-	/* If the kernel debugger is active, always disable compression */
-	if (in_dbg_master())
-		goto out_disable;
-
-	/* If the scanout has not changed, don't modify the FBC settings.
-	 * Note that we make the fundamental assumption that the fb->obj
-	 * cannot be unpinned (and have its GTT offset and fence revoked)
-	 * without first being decoupled from the scanout and FBC disabled.
-	 */
-	if (dev_priv->cfb_plane == intel_crtc->plane &&
-	    dev_priv->cfb_fb == fb->base.id &&
-	    dev_priv->cfb_y == crtc->y)
-		return;
-
-	if (intel_fbc_enabled(dev)) {
-		/* We update FBC along two paths, after changing fb/crtc
-		 * configuration (modeswitching) and after page-flipping
-		 * finishes. For the latter, we know that not only did
-		 * we disable the FBC at the start of the page-flip
-		 * sequence, but also more than one vblank has passed.
-		 *
-		 * For the former case of modeswitching, it is possible
-		 * to switch between two FBC valid configurations
-		 * instantaneously so we do need to disable the FBC
-		 * before we can modify its control registers. We also
-		 * have to wait for the next vblank for that to take
-		 * effect. However, since we delay enabling FBC we can
-		 * assume that a vblank has passed since disabling and
-		 * that we can safely alter the registers in the deferred
-		 * callback.
-		 *
-		 * In the scenario that we go from a valid to invalid
-		 * and then back to valid FBC configuration we have
-		 * no strict enforcement that a vblank occurred since
-		 * disabling the FBC. However, along all current pipe
-		 * disabling paths we do need to wait for a vblank at
-		 * some point. And we wait before enabling FBC anyway.
-		 */
-		DRM_DEBUG_KMS("disabling active FBC for update\n");
-		intel_disable_fbc(dev);
-	}
-
-	intel_enable_fbc(crtc, 500);
-	return;
-
-out_disable:
-	/* Multiple disables should be harmless */
-	if (intel_fbc_enabled(dev)) {
-		DRM_DEBUG_KMS("unsupported config, disabling FBC\n");
-		intel_disable_fbc(dev);
-	}
-}
-
 int
 intel_pin_and_fence_fb_obj(struct drm_device *dev,
 			   struct drm_i915_gem_object *obj,
@@ -2050,13 +1792,11 @@ intel_pin_and_fence_fb_obj(struct drm_device *dev,
 	 * framebuffer compression.  For simplicity, we always install
 	 * a fence as the cost is not that onerous.
 	 */
-	if (obj->tiling_mode != I915_TILING_NONE) {
-		ret = i915_gem_object_get_fence(obj, pipelined);
-		if (ret)
-			goto err_unpin;
+	ret = i915_gem_object_get_fence(obj);
+	if (ret)
+		goto err_unpin;
 
-		i915_gem_object_pin_fence(obj);
-	}
+	i915_gem_object_pin_fence(obj);
 
 	dev_priv->mm.interruptible = true;
 	return 0;
@@ -2137,7 +1877,7 @@ static int i9xx_update_plane(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		      Start, Offset, x, y, fb->pitches[0]);
 	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
 	if (INTEL_INFO(dev)->gen >= 4) {
-		I915_WRITE(DSPSURF(plane), Start);
+		I915_MODIFY_DISPBASE(DSPSURF(plane), Start);
 		I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
 		I915_WRITE(DSPADDR(plane), Offset);
 	} else
@@ -2217,7 +1957,7 @@ static int ironlake_update_plane(struct drm_crtc *crtc,
 	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
 		      Start, Offset, x, y, fb->pitches[0]);
 	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
-	I915_WRITE(DSPSURF(plane), Start);
+	I915_MODIFY_DISPBASE(DSPSURF(plane), Start);
 	I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
 	I915_WRITE(DSPADDR(plane), Offset);
 	POSTING_READ(reg);
@@ -2232,16 +1972,12 @@ intel_pipe_set_base_atomic(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
-
-	ret = dev_priv->display.update_plane(crtc, fb, x, y);
-	if (ret)
-		return ret;
 
-	intel_update_fbc(dev);
+	if (dev_priv->display.disable_fbc)
+		dev_priv->display.disable_fbc(dev);
 	intel_increase_pllclock(crtc);
 
-	return 0;
+	return dev_priv->display.update_plane(crtc, fb, x, y);
 }
 
 static int
@@ -2276,6 +2012,7 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		    struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_master_private *master_priv;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int ret;
@@ -2286,16 +2023,10 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		return 0;
 	}
 
-	switch (intel_crtc->plane) {
-	case 0:
-	case 1:
-		break;
-	case 2:
-		if (IS_IVYBRIDGE(dev))
-			break;
-		/* fall through otherwise */
-	default:
-		DRM_ERROR("no plane for crtc\n");
+	if(intel_crtc->plane > dev_priv->num_pipe) {
+		DRM_ERROR("no plane for crtc: plane %d, num_pipes %d\n",
+				intel_crtc->plane,
+				dev_priv->num_pipe);
 		return -EINVAL;
 	}
 
@@ -2312,8 +2043,7 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	if (old_fb)
 		intel_finish_fb(old_fb);
 
-	ret = intel_pipe_set_base_atomic(crtc, crtc->fb, x, y,
-					 LEAVE_ATOMIC_MODE_SET);
+	ret = dev_priv->display.update_plane(crtc, crtc->fb, x, y);
 	if (ret) {
 		intel_unpin_fb_obj(to_intel_framebuffer(crtc->fb)->obj);
 		mutex_unlock(&dev->struct_mutex);
@@ -2326,6 +2056,7 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		intel_unpin_fb_obj(to_intel_framebuffer(old_fb)->obj);
 	}
 
+	intel_update_fbc(dev);
 	mutex_unlock(&dev->struct_mutex);
 
 	if (!dev->primary->master)
@@ -2547,7 +2278,7 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp, i;
+	u32 reg, temp, i, retry;
 
 	/* Train 1: umask FDI RX Interrupt symbol_lock and bit_lock bit
 	   for train result */
@@ -2599,15 +2330,19 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 		POSTING_READ(reg);
 		udelay(500);
 
-		reg = FDI_RX_IIR(pipe);
-		temp = I915_READ(reg);
-		DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
-
-		if (temp & FDI_RX_BIT_LOCK) {
-			I915_WRITE(reg, temp | FDI_RX_BIT_LOCK);
-			DRM_DEBUG_KMS("FDI train 1 done.\n");
-			break;
+		for (retry = 0; retry < 5; retry++) {
+			reg = FDI_RX_IIR(pipe);
+			temp = I915_READ(reg);
+			DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
+			if (temp & FDI_RX_BIT_LOCK) {
+				I915_WRITE(reg, temp | FDI_RX_BIT_LOCK);
+				DRM_DEBUG_KMS("FDI train 1 done.\n");
+				break;
+			}
+			udelay(50);
 		}
+		if (retry < 5)
+			break;
 	}
 	if (i == 4)
 		DRM_ERROR("FDI train 1 fail!\n");
@@ -2648,15 +2383,19 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 		POSTING_READ(reg);
 		udelay(500);
 
-		reg = FDI_RX_IIR(pipe);
-		temp = I915_READ(reg);
-		DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
-
-		if (temp & FDI_RX_SYMBOL_LOCK) {
-			I915_WRITE(reg, temp | FDI_RX_SYMBOL_LOCK);
-			DRM_DEBUG_KMS("FDI train 2 done.\n");
-			break;
+		for (retry = 0; retry < 5; retry++) {
+			reg = FDI_RX_IIR(pipe);
+			temp = I915_READ(reg);
+			DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
+			if (temp & FDI_RX_SYMBOL_LOCK) {
+				I915_WRITE(reg, temp | FDI_RX_SYMBOL_LOCK);
+				DRM_DEBUG_KMS("FDI train 2 done.\n");
+				break;
+			}
+			udelay(50);
 		}
+		if (retry < 5)
+			break;
 	}
 	if (i == 4)
 		DRM_ERROR("FDI train 2 fail!\n");
@@ -2808,14 +2547,18 @@ static void ironlake_fdi_pll_enable(struct drm_crtc *crtc)
 	POSTING_READ(reg);
 	udelay(200);
 
-	/* Enable CPU FDI TX PLL, always on for Ironlake */
-	reg = FDI_TX_CTL(pipe);
-	temp = I915_READ(reg);
-	if ((temp & FDI_TX_PLL_ENABLE) == 0) {
-		I915_WRITE(reg, temp | FDI_TX_PLL_ENABLE);
+	/* On Haswell, the PLL configuration for ports and pipes is handled
+	 * separately, as part of DDI setup */
+	if (!IS_HASWELL(dev)) {
+		/* Enable CPU FDI TX PLL, always on for Ironlake */
+		reg = FDI_TX_CTL(pipe);
+		temp = I915_READ(reg);
+		if ((temp & FDI_TX_PLL_ENABLE) == 0) {
+			I915_WRITE(reg, temp | FDI_TX_PLL_ENABLE);
 
-		POSTING_READ(reg);
-		udelay(100);
+			POSTING_READ(reg);
+			udelay(100);
+		}
 	}
 }
 
@@ -2888,38 +2631,16 @@ static void ironlake_fdi_disable(struct drm_crtc *crtc)
 	udelay(100);
 }
 
-/*
- * When we disable a pipe, we need to clear any pending scanline wait events
- * to avoid hanging the ring, which we assume we are waiting on.
- */
-static void intel_clear_scanline_wait(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
-	u32 tmp;
-
-	if (IS_GEN2(dev))
-		/* Can't break the hang on i8xx */
-		return;
-
-	ring = LP_RING(dev_priv);
-	tmp = I915_READ_CTL(ring);
-	if (tmp & RING_WAIT)
-		I915_WRITE_CTL(ring, tmp);
-}
-
 static void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 {
-	struct drm_i915_gem_object *obj;
-	struct drm_i915_private *dev_priv;
+	struct drm_device *dev = crtc->dev;
 
 	if (crtc->fb == NULL)
 		return;
 
-	obj = to_intel_framebuffer(crtc->fb)->obj;
-	dev_priv = crtc->dev->dev_private;
-	wait_event(dev_priv->pending_flip_queue,
-		   atomic_read(&obj->pending_flip) == 0);
+	mutex_lock(&dev->struct_mutex);
+	intel_finish_fb(crtc->fb);
+	mutex_unlock(&dev->struct_mutex);
 }
 
 static bool intel_crtc_driving_pch(struct drm_crtc *crtc)
@@ -2936,6 +2657,22 @@ static bool intel_crtc_driving_pch(struct drm_crtc *crtc)
 		if (encoder->base.crtc != crtc)
 			continue;
 
+		/* On Haswell, LPT PCH handles the VGA connection via FDI, and Haswell
+		 * CPU handles all others */
+		if (IS_HASWELL(dev)) {
+			/* It is still unclear how this will work on PPT, so throw up a warning */
+			WARN_ON(!HAS_PCH_LPT(dev));
+
+			if (encoder->type == DRM_MODE_ENCODER_DAC) {
+				DRM_DEBUG_KMS("Haswell detected DAC encoder, assuming is PCH\n");
+				return true;
+			} else {
+				DRM_DEBUG_KMS("Haswell detected encoder %d, assuming is CPU\n",
+						encoder->type);
+				return false;
+			}
+		}
+
 		switch (encoder->type) {
 		case INTEL_OUTPUT_EDP:
 			if (!intel_encoder_is_pch_edp(&encoder->base))
@@ -2947,6 +2684,97 @@ static bool intel_crtc_driving_pch(struct drm_crtc *crtc)
 	return true;
 }
 
+/* Program iCLKIP clock to the desired frequency */
+static void lpt_program_iclkip(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 divsel, phaseinc, auxdiv, phasedir = 0;
+	u32 temp;
+
+	/* It is necessary to ungate the pixclk gate prior to programming
+	 * the divisors, and gate it back when it is done.
+	 */
+	I915_WRITE(PIXCLK_GATE, PIXCLK_GATE_GATE);
+
+	/* Disable SSCCTL */
+	intel_sbi_write(dev_priv, SBI_SSCCTL6,
+				intel_sbi_read(dev_priv, SBI_SSCCTL6) |
+					SBI_SSCCTL_DISABLE);
+
+	/* 20MHz is a corner case which is out of range for the 7-bit divisor */
+	if (crtc->mode.clock == 20000) {
+		auxdiv = 1;
+		divsel = 0x41;
+		phaseinc = 0x20;
+	} else {
+		/* The iCLK virtual clock root frequency is in MHz,
+		 * but the crtc->mode.clock in in KHz. To get the divisors,
+		 * it is necessary to divide one by another, so we
+		 * convert the virtual clock precision to KHz here for higher
+		 * precision.
+		 */
+		u32 iclk_virtual_root_freq = 172800 * 1000;
+		u32 iclk_pi_range = 64;
+		u32 desired_divisor, msb_divisor_value, pi_value;
+
+		desired_divisor = (iclk_virtual_root_freq / crtc->mode.clock);
+		msb_divisor_value = desired_divisor / iclk_pi_range;
+		pi_value = desired_divisor % iclk_pi_range;
+
+		auxdiv = 0;
+		divsel = msb_divisor_value - 2;
+		phaseinc = pi_value;
+	}
+
+	/* This should not happen with any sane values */
+	WARN_ON(SBI_SSCDIVINTPHASE_DIVSEL(divsel) &
+		~SBI_SSCDIVINTPHASE_DIVSEL_MASK);
+	WARN_ON(SBI_SSCDIVINTPHASE_DIR(phasedir) &
+		~SBI_SSCDIVINTPHASE_INCVAL_MASK);
+
+	DRM_DEBUG_KMS("iCLKIP clock: found settings for %dKHz refresh rate: auxdiv=%x, divsel=%x, phasedir=%x, phaseinc=%x\n",
+			crtc->mode.clock,
+			auxdiv,
+			divsel,
+			phasedir,
+			phaseinc);
+
+	/* Program SSCDIVINTPHASE6 */
+	temp = intel_sbi_read(dev_priv, SBI_SSCDIVINTPHASE6);
+	temp &= ~SBI_SSCDIVINTPHASE_DIVSEL_MASK;
+	temp |= SBI_SSCDIVINTPHASE_DIVSEL(divsel);
+	temp &= ~SBI_SSCDIVINTPHASE_INCVAL_MASK;
+	temp |= SBI_SSCDIVINTPHASE_INCVAL(phaseinc);
+	temp |= SBI_SSCDIVINTPHASE_DIR(phasedir);
+	temp |= SBI_SSCDIVINTPHASE_PROPAGATE;
+
+	intel_sbi_write(dev_priv,
+			SBI_SSCDIVINTPHASE6,
+			temp);
+
+	/* Program SSCAUXDIV */
+	temp = intel_sbi_read(dev_priv, SBI_SSCAUXDIV6);
+	temp &= ~SBI_SSCAUXDIV_FINALDIV2SEL(1);
+	temp |= SBI_SSCAUXDIV_FINALDIV2SEL(auxdiv);
+	intel_sbi_write(dev_priv,
+			SBI_SSCAUXDIV6,
+			temp);
+
+
+	/* Enable modulator and associated divider */
+	temp = intel_sbi_read(dev_priv, SBI_SSCCTL6);
+	temp &= ~SBI_SSCCTL_DISABLE;
+	intel_sbi_write(dev_priv,
+			SBI_SSCCTL6,
+			temp);
+
+	/* Wait for initialization time */
+	udelay(24);
+
+	I915_WRITE(PIXCLK_GATE, PIXCLK_GATE_UNGATE);
+}
+
 /*
  * Enable PCH resources required for PCH ports:
  *   - PCH PLLs
@@ -2961,29 +2789,41 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp, transc_sel;
+	u32 reg, temp;
+
+	assert_transcoder_disabled(dev_priv, pipe);
 
 	/* For PCH output, training FDI link */
 	dev_priv->display.fdi_link_train(crtc);
 
-	intel_enable_pch_pll(dev_priv, pipe);
+	intel_enable_pch_pll(intel_crtc);
 
-	if (HAS_PCH_CPT(dev)) {
-		transc_sel = intel_crtc->use_pll_a ? TRANSC_DPLLA_SEL :
-			TRANSC_DPLLB_SEL;
+	if (HAS_PCH_LPT(dev)) {
+		DRM_DEBUG_KMS("LPT detected: programming iCLKIP\n");
+		lpt_program_iclkip(crtc);
+	} else if (HAS_PCH_CPT(dev)) {
+		u32 sel;
 
-		/* Be sure PCH DPLL SEL is set */
 		temp = I915_READ(PCH_DPLL_SEL);
-		if (pipe == 0) {
-			temp &= ~(TRANSA_DPLLB_SEL);
-			temp |= (TRANSA_DPLL_ENABLE | TRANSA_DPLLA_SEL);
-		} else if (pipe == 1) {
-			temp &= ~(TRANSB_DPLLB_SEL);
-			temp |= (TRANSB_DPLL_ENABLE | TRANSB_DPLLB_SEL);
-		} else if (pipe == 2) {
-			temp &= ~(TRANSC_DPLLB_SEL);
-			temp |= (TRANSC_DPLL_ENABLE | transc_sel);
+		switch (pipe) {
+		default:
+		case 0:
+			temp |= TRANSA_DPLL_ENABLE;
+			sel = TRANSA_DPLLB_SEL;
+			break;
+		case 1:
+			temp |= TRANSB_DPLL_ENABLE;
+			sel = TRANSB_DPLLB_SEL;
+			break;
+		case 2:
+			temp |= TRANSC_DPLL_ENABLE;
+			sel = TRANSC_DPLLB_SEL;
+			break;
 		}
+		if (intel_crtc->pch_pll->pll_reg == _PCH_DPLL_B)
+			temp |= sel;
+		else
+			temp &= ~sel;
 		I915_WRITE(PCH_DPLL_SEL, temp);
 	}
 
@@ -2998,7 +2838,8 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	I915_WRITE(TRANS_VSYNC(pipe),  I915_READ(VSYNC(pipe)));
 	I915_WRITE(TRANS_VSYNCSHIFT(pipe),  I915_READ(VSYNCSHIFT(pipe)));
 
-	intel_fdi_normal_train(crtc);
+	if (!IS_HASWELL(dev))
+		intel_fdi_normal_train(crtc);
 
 	/* For PCH DP, enable TRANS_DP_CTL */
 	if (HAS_PCH_CPT(dev) &&
@@ -3041,6 +2882,93 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	intel_enable_transcoder(dev_priv, pipe);
 }
 
+static void intel_put_pch_pll(struct intel_crtc *intel_crtc)
+{
+	struct intel_pch_pll *pll = intel_crtc->pch_pll;
+
+	if (pll == NULL)
+		return;
+
+	if (pll->refcount == 0) {
+		WARN(1, "bad PCH PLL refcount\n");
+		return;
+	}
+
+	--pll->refcount;
+	intel_crtc->pch_pll = NULL;
+}
+
+static struct intel_pch_pll *intel_get_pch_pll(struct intel_crtc *intel_crtc, u32 dpll, u32 fp)
+{
+	struct drm_i915_private *dev_priv = intel_crtc->base.dev->dev_private;
+	struct intel_pch_pll *pll;
+	int i;
+
+	pll = intel_crtc->pch_pll;
+	if (pll) {
+		DRM_DEBUG_KMS("CRTC:%d reusing existing PCH PLL %x\n",
+			      intel_crtc->base.base.id, pll->pll_reg);
+		goto prepare;
+	}
+
+	if (HAS_PCH_IBX(dev_priv->dev)) {
+		/* Ironlake PCH has a fixed PLL->PCH pipe mapping. */
+		i = intel_crtc->pipe;
+		pll = &dev_priv->pch_plls[i];
+
+		DRM_DEBUG_KMS("CRTC:%d using pre-allocated PCH PLL %x\n",
+			      intel_crtc->base.base.id, pll->pll_reg);
+
+		goto found;
+	}
+
+	for (i = 0; i < dev_priv->num_pch_pll; i++) {
+		pll = &dev_priv->pch_plls[i];
+
+		/* Only want to check enabled timings first */
+		if (pll->refcount == 0)
+			continue;
+
+		if (dpll == (I915_READ(pll->pll_reg) & 0x7fffffff) &&
+		    fp == I915_READ(pll->fp0_reg)) {
+			DRM_DEBUG_KMS("CRTC:%d sharing existing PCH PLL %x (refcount %d, ative %d)\n",
+				      intel_crtc->base.base.id,
+				      pll->pll_reg, pll->refcount, pll->active);
+
+			goto found;
+		}
+	}
+
+	/* Ok no matching timings, maybe there's a free one? */
+	for (i = 0; i < dev_priv->num_pch_pll; i++) {
+		pll = &dev_priv->pch_plls[i];
+		if (pll->refcount == 0) {
+			DRM_DEBUG_KMS("CRTC:%d allocated PCH PLL %x\n",
+				      intel_crtc->base.base.id, pll->pll_reg);
+			goto found;
+		}
+	}
+
+	return NULL;
+
+found:
+	intel_crtc->pch_pll = pll;
+	pll->refcount++;
+	DRM_DEBUG_DRIVER("using pll %d for pipe %d\n", i, intel_crtc->pipe);
+prepare: /* separate function? */
+	DRM_DEBUG_DRIVER("switching PLL %x off\n", pll->pll_reg);
+
+	/* Wait for the clocks to stabilize before rewriting the regs */
+	I915_WRITE(pll->pll_reg, dpll & ~DPLL_VCO_ENABLE);
+	POSTING_READ(pll->pll_reg);
+	udelay(150);
+
+	I915_WRITE(pll->fp0_reg, fp);
+	I915_WRITE(pll->pll_reg, dpll & ~DPLL_VCO_ENABLE);
+	pll->on = false;
+	return pll;
+}
+
 void intel_cpt_verify_modeset(struct drm_device *dev, int pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -3185,8 +3113,7 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 	}
 
 	/* disable PCH DPLL */
-	if (!intel_crtc->no_pll)
-		intel_disable_pch_pll(dev_priv, pipe);
+	intel_disable_pch_pll(intel_crtc);
 
 	/* Switch from PCDclk to Rawclk */
 	reg = FDI_RX_CTL(pipe);
@@ -3214,7 +3141,6 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 
 	mutex_lock(&dev->struct_mutex);
 	intel_update_fbc(dev);
-	intel_clear_scanline_wait(dev);
 	mutex_unlock(&dev->struct_mutex);
 }
 
@@ -3242,6 +3168,12 @@ static void ironlake_crtc_dpms(struct drm_crtc *crtc, int mode)
 	}
 }
 
+static void ironlake_crtc_off(struct drm_crtc *crtc)
+{
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	intel_put_pch_pll(intel_crtc);
+}
+
 static void intel_crtc_dpms_overlay(struct intel_crtc *intel_crtc, bool enable)
 {
 	if (!enable && intel_crtc->overlay) {
@@ -3313,7 +3245,6 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	intel_crtc->active = false;
 	intel_update_fbc(dev);
 	intel_update_watermarks(dev);
-	intel_clear_scanline_wait(dev);
 }
 
 static void i9xx_crtc_dpms(struct drm_crtc *crtc, int mode)
@@ -3333,6 +3264,10 @@ static void i9xx_crtc_dpms(struct drm_crtc *crtc, int mode)
 	}
 }
 
+static void i9xx_crtc_off(struct drm_crtc *crtc)
+{
+}
+
 /**
  * Sets the power management mode of the pipe and plane.
  */
@@ -3380,25 +3315,11 @@ static void intel_crtc_disable(struct drm_crtc *crtc)
 {
 	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
 	struct drm_device *dev = crtc->dev;
-
-	/* Flush any pending WAITs before we disable the pipe. Note that
-	 * we need to drop the struct_mutex in order to acquire it again
-	 * during the lowlevel dpms routines around a couple of the
-	 * operations. It does not look trivial nor desirable to move
-	 * that locking higher. So instead we leave a window for the
-	 * submission of further commands on the fb before we can actually
-	 * disable it. This race with userspace exists anyway, and we can
-	 * only rely on the pipe being disabled by userspace after it
-	 * receives the hotplug notification and has flushed any pending
-	 * batches.
-	 */
-	if (crtc->fb) {
-		mutex_lock(&dev->struct_mutex);
-		intel_finish_fb(crtc->fb);
-		mutex_unlock(&dev->struct_mutex);
-	}
+	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+	dev_priv->display.off(crtc);
+
 	assert_plane_disabled(dev->dev_private, to_intel_crtc(crtc)->plane);
 	assert_pipe_disabled(dev->dev_private, to_intel_crtc(crtc)->pipe);
 
@@ -3448,8 +3369,7 @@ void intel_encoder_commit(struct drm_encoder *encoder)
 {
 	struct drm_encoder_helper_funcs *encoder_funcs = encoder->helper_private;
 	struct drm_device *dev = encoder->dev;
-	struct intel_encoder *intel_encoder = to_intel_encoder(encoder);
-	struct intel_crtc *intel_crtc = to_intel_crtc(intel_encoder->base.crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 
 	/* lvds has its own version of commit see intel_lvds_commit */
 	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_ON);
@@ -3487,6 +3407,11 @@ static bool intel_crtc_mode_fixup(struct drm_crtc *crtc,
 	return true;
 }
 
+static int valleyview_get_display_clock_speed(struct drm_device *dev)
+{
+	return 400000; /* FIXME */
+}
+
 static int i945_get_display_clock_speed(struct drm_device *dev)
 {
 	return 400000;
@@ -3584,1342 +3509,6 @@ ironlake_compute_m_n(int bits_per_pixel, int nlanes, int pixel_clock,
 	fdi_reduce_ratio(&m_n->link_m, &m_n->link_n);
 }
 
-
-struct intel_watermark_params {
-	unsigned long fifo_size;
-	unsigned long max_wm;
-	unsigned long default_wm;
-	unsigned long guard_size;
-	unsigned long cacheline_size;
-};
-
-/* Pineview has different values for various configs */
-static const struct intel_watermark_params pineview_display_wm = {
-	PINEVIEW_DISPLAY_FIFO,
-	PINEVIEW_MAX_WM,
-	PINEVIEW_DFT_WM,
-	PINEVIEW_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params pineview_display_hplloff_wm = {
-	PINEVIEW_DISPLAY_FIFO,
-	PINEVIEW_MAX_WM,
-	PINEVIEW_DFT_HPLLOFF_WM,
-	PINEVIEW_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params pineview_cursor_wm = {
-	PINEVIEW_CURSOR_FIFO,
-	PINEVIEW_CURSOR_MAX_WM,
-	PINEVIEW_CURSOR_DFT_WM,
-	PINEVIEW_CURSOR_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params pineview_cursor_hplloff_wm = {
-	PINEVIEW_CURSOR_FIFO,
-	PINEVIEW_CURSOR_MAX_WM,
-	PINEVIEW_CURSOR_DFT_WM,
-	PINEVIEW_CURSOR_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params g4x_wm_info = {
-	G4X_FIFO_SIZE,
-	G4X_MAX_WM,
-	G4X_MAX_WM,
-	2,
-	G4X_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params g4x_cursor_wm_info = {
-	I965_CURSOR_FIFO,
-	I965_CURSOR_MAX_WM,
-	I965_CURSOR_DFT_WM,
-	2,
-	G4X_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params i965_cursor_wm_info = {
-	I965_CURSOR_FIFO,
-	I965_CURSOR_MAX_WM,
-	I965_CURSOR_DFT_WM,
-	2,
-	I915_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params i945_wm_info = {
-	I945_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I915_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params i915_wm_info = {
-	I915_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I915_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params i855_wm_info = {
-	I855GM_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I830_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params i830_wm_info = {
-	I830_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I830_FIFO_LINE_SIZE
-};
-
-static const struct intel_watermark_params ironlake_display_wm_info = {
-	ILK_DISPLAY_FIFO,
-	ILK_DISPLAY_MAXWM,
-	ILK_DISPLAY_DFTWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params ironlake_cursor_wm_info = {
-	ILK_CURSOR_FIFO,
-	ILK_CURSOR_MAXWM,
-	ILK_CURSOR_DFTWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params ironlake_display_srwm_info = {
-	ILK_DISPLAY_SR_FIFO,
-	ILK_DISPLAY_MAX_SRWM,
-	ILK_DISPLAY_DFT_SRWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params ironlake_cursor_srwm_info = {
-	ILK_CURSOR_SR_FIFO,
-	ILK_CURSOR_MAX_SRWM,
-	ILK_CURSOR_DFT_SRWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-
-static const struct intel_watermark_params sandybridge_display_wm_info = {
-	SNB_DISPLAY_FIFO,
-	SNB_DISPLAY_MAXWM,
-	SNB_DISPLAY_DFTWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params sandybridge_cursor_wm_info = {
-	SNB_CURSOR_FIFO,
-	SNB_CURSOR_MAXWM,
-	SNB_CURSOR_DFTWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params sandybridge_display_srwm_info = {
-	SNB_DISPLAY_SR_FIFO,
-	SNB_DISPLAY_MAX_SRWM,
-	SNB_DISPLAY_DFT_SRWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params sandybridge_cursor_srwm_info = {
-	SNB_CURSOR_SR_FIFO,
-	SNB_CURSOR_MAX_SRWM,
-	SNB_CURSOR_DFT_SRWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-
-
-/**
- * intel_calculate_wm - calculate watermark level
- * @clock_in_khz: pixel clock
- * @wm: chip FIFO params
- * @pixel_size: display pixel size
- * @latency_ns: memory latency for the platform
- *
- * Calculate the watermark level (the level at which the display plane will
- * start fetching from memory again).  Each chip has a different display
- * FIFO size and allocation, so the caller needs to figure that out and pass
- * in the correct intel_watermark_params structure.
- *
- * As the pixel clock runs, the FIFO will be drained at a rate that depends
- * on the pixel size.  When it reaches the watermark level, it'll start
- * fetching FIFO line sized based chunks from memory until the FIFO fills
- * past the watermark point.  If the FIFO drains completely, a FIFO underrun
- * will occur, and a display engine hang could result.
- */
-static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
-					const struct intel_watermark_params *wm,
-					int fifo_size,
-					int pixel_size,
-					unsigned long latency_ns)
-{
-	long entries_required, wm_size;
-
-	/*
-	 * Note: we need to make sure we don't overflow for various clock &
-	 * latency values.
-	 * clocks go from a few thousand to several hundred thousand.
-	 * latency is usually a few thousand
-	 */
-	entries_required = ((clock_in_khz / 1000) * pixel_size * latency_ns) /
-		1000;
-	entries_required = DIV_ROUND_UP(entries_required, wm->cacheline_size);
-
-	DRM_DEBUG_KMS("FIFO entries required for mode: %ld\n", entries_required);
-
-	wm_size = fifo_size - (entries_required + wm->guard_size);
-
-	DRM_DEBUG_KMS("FIFO watermark level: %ld\n", wm_size);
-
-	/* Don't promote wm_size to unsigned... */
-	if (wm_size > (long)wm->max_wm)
-		wm_size = wm->max_wm;
-	if (wm_size <= 0)
-		wm_size = wm->default_wm;
-	return wm_size;
-}
-
-struct cxsr_latency {
-	int is_desktop;
-	int is_ddr3;
-	unsigned long fsb_freq;
-	unsigned long mem_freq;
-	unsigned long display_sr;
-	unsigned long display_hpll_disable;
-	unsigned long cursor_sr;
-	unsigned long cursor_hpll_disable;
-};
-
-static const struct cxsr_latency cxsr_latency_table[] = {
-	{1, 0, 800, 400, 3382, 33382, 3983, 33983},    /* DDR2-400 SC */
-	{1, 0, 800, 667, 3354, 33354, 3807, 33807},    /* DDR2-667 SC */
-	{1, 0, 800, 800, 3347, 33347, 3763, 33763},    /* DDR2-800 SC */
-	{1, 1, 800, 667, 6420, 36420, 6873, 36873},    /* DDR3-667 SC */
-	{1, 1, 800, 800, 5902, 35902, 6318, 36318},    /* DDR3-800 SC */
-
-	{1, 0, 667, 400, 3400, 33400, 4021, 34021},    /* DDR2-400 SC */
-	{1, 0, 667, 667, 3372, 33372, 3845, 33845},    /* DDR2-667 SC */
-	{1, 0, 667, 800, 3386, 33386, 3822, 33822},    /* DDR2-800 SC */
-	{1, 1, 667, 667, 6438, 36438, 6911, 36911},    /* DDR3-667 SC */
-	{1, 1, 667, 800, 5941, 35941, 6377, 36377},    /* DDR3-800 SC */
-
-	{1, 0, 400, 400, 3472, 33472, 4173, 34173},    /* DDR2-400 SC */
-	{1, 0, 400, 667, 3443, 33443, 3996, 33996},    /* DDR2-667 SC */
-	{1, 0, 400, 800, 3430, 33430, 3946, 33946},    /* DDR2-800 SC */
-	{1, 1, 400, 667, 6509, 36509, 7062, 37062},    /* DDR3-667 SC */
-	{1, 1, 400, 800, 5985, 35985, 6501, 36501},    /* DDR3-800 SC */
-
-	{0, 0, 800, 400, 3438, 33438, 4065, 34065},    /* DDR2-400 SC */
-	{0, 0, 800, 667, 3410, 33410, 3889, 33889},    /* DDR2-667 SC */
-	{0, 0, 800, 800, 3403, 33403, 3845, 33845},    /* DDR2-800 SC */
-	{0, 1, 800, 667, 6476, 36476, 6955, 36955},    /* DDR3-667 SC */
-	{0, 1, 800, 800, 5958, 35958, 6400, 36400},    /* DDR3-800 SC */
-
-	{0, 0, 667, 400, 3456, 33456, 4103, 34106},    /* DDR2-400 SC */
-	{0, 0, 667, 667, 3428, 33428, 3927, 33927},    /* DDR2-667 SC */
-	{0, 0, 667, 800, 3443, 33443, 3905, 33905},    /* DDR2-800 SC */
-	{0, 1, 667, 667, 6494, 36494, 6993, 36993},    /* DDR3-667 SC */
-	{0, 1, 667, 800, 5998, 35998, 6460, 36460},    /* DDR3-800 SC */
-
-	{0, 0, 400, 400, 3528, 33528, 4255, 34255},    /* DDR2-400 SC */
-	{0, 0, 400, 667, 3500, 33500, 4079, 34079},    /* DDR2-667 SC */
-	{0, 0, 400, 800, 3487, 33487, 4029, 34029},    /* DDR2-800 SC */
-	{0, 1, 400, 667, 6566, 36566, 7145, 37145},    /* DDR3-667 SC */
-	{0, 1, 400, 800, 6042, 36042, 6584, 36584},    /* DDR3-800 SC */
-};
-
-static const struct cxsr_latency *intel_get_cxsr_latency(int is_desktop,
-							 int is_ddr3,
-							 int fsb,
-							 int mem)
-{
-	const struct cxsr_latency *latency;
-	int i;
-
-	if (fsb == 0 || mem == 0)
-		return NULL;
-
-	for (i = 0; i < ARRAY_SIZE(cxsr_latency_table); i++) {
-		latency = &cxsr_latency_table[i];
-		if (is_desktop == latency->is_desktop &&
-		    is_ddr3 == latency->is_ddr3 &&
-		    fsb == latency->fsb_freq && mem == latency->mem_freq)
-			return latency;
-	}
-
-	DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
-
-	return NULL;
-}
-
-static void pineview_disable_cxsr(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	/* deactivate cxsr */
-	I915_WRITE(DSPFW3, I915_READ(DSPFW3) & ~PINEVIEW_SELF_REFRESH_EN);
-}
-
-/*
- * Latency for FIFO fetches is dependent on several factors:
- *   - memory configuration (speed, channels)
- *   - chipset
- *   - current MCH state
- * It can be fairly high in some situations, so here we assume a fairly
- * pessimal value.  It's a tradeoff between extra memory fetches (if we
- * set this value too high, the FIFO will fetch frequently to stay full)
- * and power consumption (set it too low to save power and we might see
- * FIFO underruns and display "flicker").
- *
- * A value of 5us seems to be a good balance; safe for very low end
- * platforms but not overly aggressive on lower latency configs.
- */
-static const int latency_ns = 5000;
-
-static int i9xx_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x7f;
-	if (plane)
-		size = ((dsparb >> DSPARB_CSTART_SHIFT) & 0x7f) - size;
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A", size);
-
-	return size;
-}
-
-static int i85x_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x1ff;
-	if (plane)
-		size = ((dsparb >> DSPARB_BEND_SHIFT) & 0x1ff) - size;
-	size >>= 1; /* Convert to cachelines */
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A", size);
-
-	return size;
-}
-
-static int i845_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x7f;
-	size >>= 2; /* Convert to cachelines */
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A",
-		      size);
-
-	return size;
-}
-
-static int i830_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x7f;
-	size >>= 1; /* Convert to cachelines */
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A", size);
-
-	return size;
-}
-
-static struct drm_crtc *single_enabled_crtc(struct drm_device *dev)
-{
-	struct drm_crtc *crtc, *enabled = NULL;
-
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
-		if (crtc->enabled && crtc->fb) {
-			if (enabled)
-				return NULL;
-			enabled = crtc;
-		}
-	}
-
-	return enabled;
-}
-
-static void pineview_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	const struct cxsr_latency *latency;
-	u32 reg;
-	unsigned long wm;
-
-	latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev), dev_priv->is_ddr3,
-					 dev_priv->fsb_freq, dev_priv->mem_freq);
-	if (!latency) {
-		DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
-		pineview_disable_cxsr(dev);
-		return;
-	}
-
-	crtc = single_enabled_crtc(dev);
-	if (crtc) {
-		int clock = crtc->mode.clock;
-		int pixel_size = crtc->fb->bits_per_pixel / 8;
-
-		/* Display SR */
-		wm = intel_calculate_wm(clock, &pineview_display_wm,
-					pineview_display_wm.fifo_size,
-					pixel_size, latency->display_sr);
-		reg = I915_READ(DSPFW1);
-		reg &= ~DSPFW_SR_MASK;
-		reg |= wm << DSPFW_SR_SHIFT;
-		I915_WRITE(DSPFW1, reg);
-		DRM_DEBUG_KMS("DSPFW1 register is %x\n", reg);
-
-		/* cursor SR */
-		wm = intel_calculate_wm(clock, &pineview_cursor_wm,
-					pineview_display_wm.fifo_size,
-					pixel_size, latency->cursor_sr);
-		reg = I915_READ(DSPFW3);
-		reg &= ~DSPFW_CURSOR_SR_MASK;
-		reg |= (wm & 0x3f) << DSPFW_CURSOR_SR_SHIFT;
-		I915_WRITE(DSPFW3, reg);
-
-		/* Display HPLL off SR */
-		wm = intel_calculate_wm(clock, &pineview_display_hplloff_wm,
-					pineview_display_hplloff_wm.fifo_size,
-					pixel_size, latency->display_hpll_disable);
-		reg = I915_READ(DSPFW3);
-		reg &= ~DSPFW_HPLL_SR_MASK;
-		reg |= wm & DSPFW_HPLL_SR_MASK;
-		I915_WRITE(DSPFW3, reg);
-
-		/* cursor HPLL off SR */
-		wm = intel_calculate_wm(clock, &pineview_cursor_hplloff_wm,
-					pineview_display_hplloff_wm.fifo_size,
-					pixel_size, latency->cursor_hpll_disable);
-		reg = I915_READ(DSPFW3);
-		reg &= ~DSPFW_HPLL_CURSOR_MASK;
-		reg |= (wm & 0x3f) << DSPFW_HPLL_CURSOR_SHIFT;
-		I915_WRITE(DSPFW3, reg);
-		DRM_DEBUG_KMS("DSPFW3 register is %x\n", reg);
-
-		/* activate cxsr */
-		I915_WRITE(DSPFW3,
-			   I915_READ(DSPFW3) | PINEVIEW_SELF_REFRESH_EN);
-		DRM_DEBUG_KMS("Self-refresh is enabled\n");
-	} else {
-		pineview_disable_cxsr(dev);
-		DRM_DEBUG_KMS("Self-refresh is disabled\n");
-	}
-}
-
-static bool g4x_compute_wm0(struct drm_device *dev,
-			    int plane,
-			    const struct intel_watermark_params *display,
-			    int display_latency_ns,
-			    const struct intel_watermark_params *cursor,
-			    int cursor_latency_ns,
-			    int *plane_wm,
-			    int *cursor_wm)
-{
-	struct drm_crtc *crtc;
-	int htotal, hdisplay, clock, pixel_size;
-	int line_time_us, line_count;
-	int entries, tlb_miss;
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	if (crtc->fb == NULL || !crtc->enabled) {
-		*cursor_wm = cursor->guard_size;
-		*plane_wm = display->guard_size;
-		return false;
-	}
-
-	htotal = crtc->mode.htotal;
-	hdisplay = crtc->mode.hdisplay;
-	clock = crtc->mode.clock;
-	pixel_size = crtc->fb->bits_per_pixel / 8;
-
-	/* Use the small buffer method to calculate plane watermark */
-	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
-	tlb_miss = display->fifo_size*display->cacheline_size - hdisplay * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, display->cacheline_size);
-	*plane_wm = entries + display->guard_size;
-	if (*plane_wm > (int)display->max_wm)
-		*plane_wm = display->max_wm;
-
-	/* Use the large buffer method to calculate cursor watermark */
-	line_time_us = ((htotal * 1000) / clock);
-	line_count = (cursor_latency_ns / line_time_us + 1000) / 1000;
-	entries = line_count * 64 * pixel_size;
-	tlb_miss = cursor->fifo_size*cursor->cacheline_size - hdisplay * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
-	if (*cursor_wm > (int)cursor->max_wm)
-		*cursor_wm = (int)cursor->max_wm;
-
-	return true;
-}
-
-/*
- * Check the wm result.
- *
- * If any calculated watermark values is larger than the maximum value that
- * can be programmed into the associated watermark register, that watermark
- * must be disabled.
- */
-static bool g4x_check_srwm(struct drm_device *dev,
-			   int display_wm, int cursor_wm,
-			   const struct intel_watermark_params *display,
-			   const struct intel_watermark_params *cursor)
-{
-	DRM_DEBUG_KMS("SR watermark: display plane %d, cursor %d\n",
-		      display_wm, cursor_wm);
-
-	if (display_wm > display->max_wm) {
-		DRM_DEBUG_KMS("display watermark is too large(%d/%ld), disabling\n",
-			      display_wm, display->max_wm);
-		return false;
-	}
-
-	if (cursor_wm > cursor->max_wm) {
-		DRM_DEBUG_KMS("cursor watermark is too large(%d/%ld), disabling\n",
-			      cursor_wm, cursor->max_wm);
-		return false;
-	}
-
-	if (!(display_wm || cursor_wm)) {
-		DRM_DEBUG_KMS("SR latency is 0, disabling\n");
-		return false;
-	}
-
-	return true;
-}
-
-static bool g4x_compute_srwm(struct drm_device *dev,
-			     int plane,
-			     int latency_ns,
-			     const struct intel_watermark_params *display,
-			     const struct intel_watermark_params *cursor,
-			     int *display_wm, int *cursor_wm)
-{
-	struct drm_crtc *crtc;
-	int hdisplay, htotal, pixel_size, clock;
-	unsigned long line_time_us;
-	int line_count, line_size;
-	int small, large;
-	int entries;
-
-	if (!latency_ns) {
-		*display_wm = *cursor_wm = 0;
-		return false;
-	}
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	hdisplay = crtc->mode.hdisplay;
-	htotal = crtc->mode.htotal;
-	clock = crtc->mode.clock;
-	pixel_size = crtc->fb->bits_per_pixel / 8;
-
-	line_time_us = (htotal * 1000) / clock;
-	line_count = (latency_ns / line_time_us + 1000) / 1000;
-	line_size = hdisplay * pixel_size;
-
-	/* Use the minimum of the small and large buffer method for primary */
-	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
-	large = line_count * line_size;
-
-	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
-	*display_wm = entries + display->guard_size;
-
-	/* calculate the self-refresh watermark for display cursor */
-	entries = line_count * pixel_size * 64;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
-
-	return g4x_check_srwm(dev,
-			      *display_wm, *cursor_wm,
-			      display, cursor);
-}
-
-#define single_plane_enabled(mask) is_power_of_2(mask)
-
-static void g4x_update_wm(struct drm_device *dev)
-{
-	static const int sr_latency_ns = 12000;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
-	int plane_sr, cursor_sr;
-	unsigned int enabled = 0;
-
-	if (g4x_compute_wm0(dev, 0,
-			    &g4x_wm_info, latency_ns,
-			    &g4x_cursor_wm_info, latency_ns,
-			    &planea_wm, &cursora_wm))
-		enabled |= 1;
-
-	if (g4x_compute_wm0(dev, 1,
-			    &g4x_wm_info, latency_ns,
-			    &g4x_cursor_wm_info, latency_ns,
-			    &planeb_wm, &cursorb_wm))
-		enabled |= 2;
-
-	plane_sr = cursor_sr = 0;
-	if (single_plane_enabled(enabled) &&
-	    g4x_compute_srwm(dev, ffs(enabled) - 1,
-			     sr_latency_ns,
-			     &g4x_wm_info,
-			     &g4x_cursor_wm_info,
-			     &plane_sr, &cursor_sr))
-		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
-	else
-		I915_WRITE(FW_BLC_SELF,
-			   I915_READ(FW_BLC_SELF) & ~FW_BLC_SELF_EN);
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
-		      planea_wm, cursora_wm,
-		      planeb_wm, cursorb_wm,
-		      plane_sr, cursor_sr);
-
-	I915_WRITE(DSPFW1,
-		   (plane_sr << DSPFW_SR_SHIFT) |
-		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
-		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
-		   planea_wm);
-	I915_WRITE(DSPFW2,
-		   (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
-		   (cursora_wm << DSPFW_CURSORA_SHIFT));
-	/* HPLL off in SR has some issues on G4x... disable it */
-	I915_WRITE(DSPFW3,
-		   (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN) |
-		   (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
-}
-
-static void i965_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	int srwm = 1;
-	int cursor_sr = 16;
-
-	/* Calc sr entries for one plane configs */
-	crtc = single_enabled_crtc(dev);
-	if (crtc) {
-		/* self-refresh has much higher latency */
-		static const int sr_latency_ns = 12000;
-		int clock = crtc->mode.clock;
-		int htotal = crtc->mode.htotal;
-		int hdisplay = crtc->mode.hdisplay;
-		int pixel_size = crtc->fb->bits_per_pixel / 8;
-		unsigned long line_time_us;
-		int entries;
-
-		line_time_us = ((htotal * 1000) / clock);
-
-		/* Use ns/us then divide to preserve precision */
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			pixel_size * hdisplay;
-		entries = DIV_ROUND_UP(entries, I915_FIFO_LINE_SIZE);
-		srwm = I965_FIFO_SIZE - entries;
-		if (srwm < 0)
-			srwm = 1;
-		srwm &= 0x1ff;
-		DRM_DEBUG_KMS("self-refresh entries: %d, wm: %d\n",
-			      entries, srwm);
-
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			pixel_size * 64;
-		entries = DIV_ROUND_UP(entries,
-					  i965_cursor_wm_info.cacheline_size);
-		cursor_sr = i965_cursor_wm_info.fifo_size -
-			(entries + i965_cursor_wm_info.guard_size);
-
-		if (cursor_sr > i965_cursor_wm_info.max_wm)
-			cursor_sr = i965_cursor_wm_info.max_wm;
-
-		DRM_DEBUG_KMS("self-refresh watermark: display plane %d "
-			      "cursor %d\n", srwm, cursor_sr);
-
-		if (IS_CRESTLINE(dev))
-			I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
-	} else {
-		/* Turn off self refresh if both pipes are enabled */
-		if (IS_CRESTLINE(dev))
-			I915_WRITE(FW_BLC_SELF, I915_READ(FW_BLC_SELF)
-				   & ~FW_BLC_SELF_EN);
-	}
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: 8, B: 8, C: 8, SR %d\n",
-		      srwm);
-
-	/* 965 has limitations... */
-	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) |
-		   (8 << 16) | (8 << 8) | (8 << 0));
-	I915_WRITE(DSPFW2, (8 << 8) | (8 << 0));
-	/* update cursor SR watermark */
-	I915_WRITE(DSPFW3, (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
-}
-
-static void i9xx_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	const struct intel_watermark_params *wm_info;
-	uint32_t fwater_lo;
-	uint32_t fwater_hi;
-	int cwm, srwm = 1;
-	int fifo_size;
-	int planea_wm, planeb_wm;
-	struct drm_crtc *crtc, *enabled = NULL;
-
-	if (IS_I945GM(dev))
-		wm_info = &i945_wm_info;
-	else if (!IS_GEN2(dev))
-		wm_info = &i915_wm_info;
-	else
-		wm_info = &i855_wm_info;
-
-	fifo_size = dev_priv->display.get_fifo_size(dev, 0);
-	crtc = intel_get_crtc_for_plane(dev, 0);
-	if (crtc->enabled && crtc->fb) {
-		planea_wm = intel_calculate_wm(crtc->mode.clock,
-					       wm_info, fifo_size,
-					       crtc->fb->bits_per_pixel / 8,
-					       latency_ns);
-		enabled = crtc;
-	} else
-		planea_wm = fifo_size - wm_info->guard_size;
-
-	fifo_size = dev_priv->display.get_fifo_size(dev, 1);
-	crtc = intel_get_crtc_for_plane(dev, 1);
-	if (crtc->enabled && crtc->fb) {
-		planeb_wm = intel_calculate_wm(crtc->mode.clock,
-					       wm_info, fifo_size,
-					       crtc->fb->bits_per_pixel / 8,
-					       latency_ns);
-		if (enabled == NULL)
-			enabled = crtc;
-		else
-			enabled = NULL;
-	} else
-		planeb_wm = fifo_size - wm_info->guard_size;
-
-	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
-
-	/*
-	 * Overlay gets an aggressive default since video jitter is bad.
-	 */
-	cwm = 2;
-
-	/* Play safe and disable self-refresh before adjusting watermarks. */
-	if (IS_I945G(dev) || IS_I945GM(dev))
-		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN_MASK | 0);
-	else if (IS_I915GM(dev))
-		I915_WRITE(INSTPM, I915_READ(INSTPM) & ~INSTPM_SELF_EN);
-
-	/* Calc sr entries for one plane configs */
-	if (HAS_FW_BLC(dev) && enabled) {
-		/* self-refresh has much higher latency */
-		static const int sr_latency_ns = 6000;
-		int clock = enabled->mode.clock;
-		int htotal = enabled->mode.htotal;
-		int hdisplay = enabled->mode.hdisplay;
-		int pixel_size = enabled->fb->bits_per_pixel / 8;
-		unsigned long line_time_us;
-		int entries;
-
-		line_time_us = (htotal * 1000) / clock;
-
-		/* Use ns/us then divide to preserve precision */
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			pixel_size * hdisplay;
-		entries = DIV_ROUND_UP(entries, wm_info->cacheline_size);
-		DRM_DEBUG_KMS("self-refresh entries: %d\n", entries);
-		srwm = wm_info->fifo_size - entries;
-		if (srwm < 0)
-			srwm = 1;
-
-		if (IS_I945G(dev) || IS_I945GM(dev))
-			I915_WRITE(FW_BLC_SELF,
-				   FW_BLC_SELF_FIFO_MASK | (srwm & 0xff));
-		else if (IS_I915GM(dev))
-			I915_WRITE(FW_BLC_SELF, srwm & 0x3f);
-	}
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d, C: %d, SR %d\n",
-		      planea_wm, planeb_wm, cwm, srwm);
-
-	fwater_lo = ((planeb_wm & 0x3f) << 16) | (planea_wm & 0x3f);
-	fwater_hi = (cwm & 0x1f);
-
-	/* Set request length to 8 cachelines per fetch */
-	fwater_lo = fwater_lo | (1 << 24) | (1 << 8);
-	fwater_hi = fwater_hi | (1 << 8);
-
-	I915_WRITE(FW_BLC, fwater_lo);
-	I915_WRITE(FW_BLC2, fwater_hi);
-
-	if (HAS_FW_BLC(dev)) {
-		if (enabled) {
-			if (IS_I945G(dev) || IS_I945GM(dev))
-				I915_WRITE(FW_BLC_SELF,
-					   FW_BLC_SELF_EN_MASK | FW_BLC_SELF_EN);
-			else if (IS_I915GM(dev))
-				I915_WRITE(INSTPM, I915_READ(INSTPM) | INSTPM_SELF_EN);
-			DRM_DEBUG_KMS("memory self refresh enabled\n");
-		} else
-			DRM_DEBUG_KMS("memory self refresh disabled\n");
-	}
-}
-
-static void i830_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	uint32_t fwater_lo;
-	int planea_wm;
-
-	crtc = single_enabled_crtc(dev);
-	if (crtc == NULL)
-		return;
-
-	planea_wm = intel_calculate_wm(crtc->mode.clock, &i830_wm_info,
-				       dev_priv->display.get_fifo_size(dev, 0),
-				       crtc->fb->bits_per_pixel / 8,
-				       latency_ns);
-	fwater_lo = I915_READ(FW_BLC) & ~0xfff;
-	fwater_lo |= (3<<8) | planea_wm;
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d\n", planea_wm);
-
-	I915_WRITE(FW_BLC, fwater_lo);
-}
-
-#define ILK_LP0_PLANE_LATENCY		700
-#define ILK_LP0_CURSOR_LATENCY		1300
-
-/*
- * Check the wm result.
- *
- * If any calculated watermark values is larger than the maximum value that
- * can be programmed into the associated watermark register, that watermark
- * must be disabled.
- */
-static bool ironlake_check_srwm(struct drm_device *dev, int level,
-				int fbc_wm, int display_wm, int cursor_wm,
-				const struct intel_watermark_params *display,
-				const struct intel_watermark_params *cursor)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	DRM_DEBUG_KMS("watermark %d: display plane %d, fbc lines %d,"
-		      " cursor %d\n", level, display_wm, fbc_wm, cursor_wm);
-
-	if (fbc_wm > SNB_FBC_MAX_SRWM) {
-		DRM_DEBUG_KMS("fbc watermark(%d) is too large(%d), disabling wm%d+\n",
-			      fbc_wm, SNB_FBC_MAX_SRWM, level);
-
-		/* fbc has it's own way to disable FBC WM */
-		I915_WRITE(DISP_ARB_CTL,
-			   I915_READ(DISP_ARB_CTL) | DISP_FBC_WM_DIS);
-		return false;
-	}
-
-	if (display_wm > display->max_wm) {
-		DRM_DEBUG_KMS("display watermark(%d) is too large(%d), disabling wm%d+\n",
-			      display_wm, SNB_DISPLAY_MAX_SRWM, level);
-		return false;
-	}
-
-	if (cursor_wm > cursor->max_wm) {
-		DRM_DEBUG_KMS("cursor watermark(%d) is too large(%d), disabling wm%d+\n",
-			      cursor_wm, SNB_CURSOR_MAX_SRWM, level);
-		return false;
-	}
-
-	if (!(fbc_wm || display_wm || cursor_wm)) {
-		DRM_DEBUG_KMS("latency %d is 0, disabling wm%d+\n", level, level);
-		return false;
-	}
-
-	return true;
-}
-
-/*
- * Compute watermark values of WM[1-3],
- */
-static bool ironlake_compute_srwm(struct drm_device *dev, int level, int plane,
-				  int latency_ns,
-				  const struct intel_watermark_params *display,
-				  const struct intel_watermark_params *cursor,
-				  int *fbc_wm, int *display_wm, int *cursor_wm)
-{
-	struct drm_crtc *crtc;
-	unsigned long line_time_us;
-	int hdisplay, htotal, pixel_size, clock;
-	int line_count, line_size;
-	int small, large;
-	int entries;
-
-	if (!latency_ns) {
-		*fbc_wm = *display_wm = *cursor_wm = 0;
-		return false;
-	}
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	hdisplay = crtc->mode.hdisplay;
-	htotal = crtc->mode.htotal;
-	clock = crtc->mode.clock;
-	pixel_size = crtc->fb->bits_per_pixel / 8;
-
-	line_time_us = (htotal * 1000) / clock;
-	line_count = (latency_ns / line_time_us + 1000) / 1000;
-	line_size = hdisplay * pixel_size;
-
-	/* Use the minimum of the small and large buffer method for primary */
-	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
-	large = line_count * line_size;
-
-	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
-	*display_wm = entries + display->guard_size;
-
-	/*
-	 * Spec says:
-	 * FBC WM = ((Final Primary WM * 64) / number of bytes per line) + 2
-	 */
-	*fbc_wm = DIV_ROUND_UP(*display_wm * 64, line_size) + 2;
-
-	/* calculate the self-refresh watermark for display cursor */
-	entries = line_count * pixel_size * 64;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
-
-	return ironlake_check_srwm(dev, level,
-				   *fbc_wm, *display_wm, *cursor_wm,
-				   display, cursor);
-}
-
-static void ironlake_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int fbc_wm, plane_wm, cursor_wm;
-	unsigned int enabled;
-
-	enabled = 0;
-	if (g4x_compute_wm0(dev, 0,
-			    &ironlake_display_wm_info,
-			    ILK_LP0_PLANE_LATENCY,
-			    &ironlake_cursor_wm_info,
-			    ILK_LP0_CURSOR_LATENCY,
-			    &plane_wm, &cursor_wm)) {
-		I915_WRITE(WM0_PIPEA_ILK,
-			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
-		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
-			      " plane %d, " "cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 1;
-	}
-
-	if (g4x_compute_wm0(dev, 1,
-			    &ironlake_display_wm_info,
-			    ILK_LP0_PLANE_LATENCY,
-			    &ironlake_cursor_wm_info,
-			    ILK_LP0_CURSOR_LATENCY,
-			    &plane_wm, &cursor_wm)) {
-		I915_WRITE(WM0_PIPEB_ILK,
-			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
-		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
-			      " plane %d, cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 2;
-	}
-
-	/*
-	 * Calculate and update the self-refresh watermark only when one
-	 * display plane is used.
-	 */
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	if (!single_plane_enabled(enabled))
-		return;
-	enabled = ffs(enabled) - 1;
-
-	/* WM1 */
-	if (!ironlake_compute_srwm(dev, 1, enabled,
-				   ILK_READ_WM1_LATENCY() * 500,
-				   &ironlake_display_srwm_info,
-				   &ironlake_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM1_LP_ILK,
-		   WM1_LP_SR_EN |
-		   (ILK_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/* WM2 */
-	if (!ironlake_compute_srwm(dev, 2, enabled,
-				   ILK_READ_WM2_LATENCY() * 500,
-				   &ironlake_display_srwm_info,
-				   &ironlake_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM2_LP_ILK,
-		   WM2_LP_EN |
-		   (ILK_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/*
-	 * WM3 is unsupported on ILK, probably because we don't have latency
-	 * data for that power state
-	 */
-}
-
-void sandybridge_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
-	u32 val;
-	int fbc_wm, plane_wm, cursor_wm;
-	unsigned int enabled;
-
-	enabled = 0;
-	if (g4x_compute_wm0(dev, 0,
-			    &sandybridge_display_wm_info, latency,
-			    &sandybridge_cursor_wm_info, latency,
-			    &plane_wm, &cursor_wm)) {
-		val = I915_READ(WM0_PIPEA_ILK);
-		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
-		I915_WRITE(WM0_PIPEA_ILK, val |
-			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
-		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
-			      " plane %d, " "cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 1;
-	}
-
-	if (g4x_compute_wm0(dev, 1,
-			    &sandybridge_display_wm_info, latency,
-			    &sandybridge_cursor_wm_info, latency,
-			    &plane_wm, &cursor_wm)) {
-		val = I915_READ(WM0_PIPEB_ILK);
-		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
-		I915_WRITE(WM0_PIPEB_ILK, val |
-			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
-		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
-			      " plane %d, cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 2;
-	}
-
-	/* IVB has 3 pipes */
-	if (IS_IVYBRIDGE(dev) &&
-	    g4x_compute_wm0(dev, 2,
-			    &sandybridge_display_wm_info, latency,
-			    &sandybridge_cursor_wm_info, latency,
-			    &plane_wm, &cursor_wm)) {
-		val = I915_READ(WM0_PIPEC_IVB);
-		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
-		I915_WRITE(WM0_PIPEC_IVB, val |
-			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
-		DRM_DEBUG_KMS("FIFO watermarks For pipe C -"
-			      " plane %d, cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 3;
-	}
-
-	/*
-	 * Calculate and update the self-refresh watermark only when one
-	 * display plane is used.
-	 *
-	 * SNB support 3 levels of watermark.
-	 *
-	 * WM1/WM2/WM2 watermarks have to be enabled in the ascending order,
-	 * and disabled in the descending order
-	 *
-	 */
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	if (!single_plane_enabled(enabled) ||
-	    dev_priv->sprite_scaling_enabled)
-		return;
-	enabled = ffs(enabled) - 1;
-
-	/* WM1 */
-	if (!ironlake_compute_srwm(dev, 1, enabled,
-				   SNB_READ_WM1_LATENCY() * 500,
-				   &sandybridge_display_srwm_info,
-				   &sandybridge_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM1_LP_ILK,
-		   WM1_LP_SR_EN |
-		   (SNB_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/* WM2 */
-	if (!ironlake_compute_srwm(dev, 2, enabled,
-				   SNB_READ_WM2_LATENCY() * 500,
-				   &sandybridge_display_srwm_info,
-				   &sandybridge_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM2_LP_ILK,
-		   WM2_LP_EN |
-		   (SNB_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/* WM3 */
-	if (!ironlake_compute_srwm(dev, 3, enabled,
-				   SNB_READ_WM3_LATENCY() * 500,
-				   &sandybridge_display_srwm_info,
-				   &sandybridge_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM3_LP_ILK,
-		   WM3_LP_EN |
-		   (SNB_READ_WM3_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-}
-
-static bool
-sandybridge_compute_sprite_wm(struct drm_device *dev, int plane,
-			      uint32_t sprite_width, int pixel_size,
-			      const struct intel_watermark_params *display,
-			      int display_latency_ns, int *sprite_wm)
-{
-	struct drm_crtc *crtc;
-	int clock;
-	int entries, tlb_miss;
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	if (crtc->fb == NULL || !crtc->enabled) {
-		*sprite_wm = display->guard_size;
-		return false;
-	}
-
-	clock = crtc->mode.clock;
-
-	/* Use the small buffer method to calculate the sprite watermark */
-	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
-	tlb_miss = display->fifo_size*display->cacheline_size -
-		sprite_width * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, display->cacheline_size);
-	*sprite_wm = entries + display->guard_size;
-	if (*sprite_wm > (int)display->max_wm)
-		*sprite_wm = display->max_wm;
-
-	return true;
-}
-
-static bool
-sandybridge_compute_sprite_srwm(struct drm_device *dev, int plane,
-				uint32_t sprite_width, int pixel_size,
-				const struct intel_watermark_params *display,
-				int latency_ns, int *sprite_wm)
-{
-	struct drm_crtc *crtc;
-	unsigned long line_time_us;
-	int clock;
-	int line_count, line_size;
-	int small, large;
-	int entries;
-
-	if (!latency_ns) {
-		*sprite_wm = 0;
-		return false;
-	}
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	clock = crtc->mode.clock;
-	if (!clock) {
-		*sprite_wm = 0;
-		return false;
-	}
-
-	line_time_us = (sprite_width * 1000) / clock;
-	if (!line_time_us) {
-		*sprite_wm = 0;
-		return false;
-	}
-
-	line_count = (latency_ns / line_time_us + 1000) / 1000;
-	line_size = sprite_width * pixel_size;
-
-	/* Use the minimum of the small and large buffer method for primary */
-	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
-	large = line_count * line_size;
-
-	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
-	*sprite_wm = entries + display->guard_size;
-
-	return *sprite_wm > 0x3ff ? false : true;
-}
-
-static void sandybridge_update_sprite_wm(struct drm_device *dev, int pipe,
-					 uint32_t sprite_width, int pixel_size)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
-	u32 val;
-	int sprite_wm, reg;
-	int ret;
-
-	switch (pipe) {
-	case 0:
-		reg = WM0_PIPEA_ILK;
-		break;
-	case 1:
-		reg = WM0_PIPEB_ILK;
-		break;
-	case 2:
-		reg = WM0_PIPEC_IVB;
-		break;
-	default:
-		return; /* bad pipe */
-	}
-
-	ret = sandybridge_compute_sprite_wm(dev, pipe, sprite_width, pixel_size,
-					    &sandybridge_display_wm_info,
-					    latency, &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite wm for pipe %d\n",
-			      pipe);
-		return;
-	}
-
-	val = I915_READ(reg);
-	val &= ~WM0_PIPE_SPRITE_MASK;
-	I915_WRITE(reg, val | (sprite_wm << WM0_PIPE_SPRITE_SHIFT));
-	DRM_DEBUG_KMS("sprite watermarks For pipe %d - %d\n", pipe, sprite_wm);
-
-
-	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
-					      pixel_size,
-					      &sandybridge_display_srwm_info,
-					      SNB_READ_WM1_LATENCY() * 500,
-					      &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite lp1 wm on pipe %d\n",
-			      pipe);
-		return;
-	}
-	I915_WRITE(WM1S_LP_ILK, sprite_wm);
-
-	/* Only IVB has two more LP watermarks for sprite */
-	if (!IS_IVYBRIDGE(dev))
-		return;
-
-	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
-					      pixel_size,
-					      &sandybridge_display_srwm_info,
-					      SNB_READ_WM2_LATENCY() * 500,
-					      &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite lp2 wm on pipe %d\n",
-			      pipe);
-		return;
-	}
-	I915_WRITE(WM2S_LP_IVB, sprite_wm);
-
-	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
-					      pixel_size,
-					      &sandybridge_display_srwm_info,
-					      SNB_READ_WM3_LATENCY() * 500,
-					      &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite lp3 wm on pipe %d\n",
-			      pipe);
-		return;
-	}
-	I915_WRITE(WM3S_LP_IVB, sprite_wm);
-}
-
-/**
- * intel_update_watermarks - update FIFO watermark values based on current modes
- *
- * Calculate watermark values for the various WM regs based on current mode
- * and plane configuration.
- *
- * There are several cases to deal with here:
- *   - normal (i.e. non-self-refresh)
- *   - self-refresh (SR) mode
- *   - lines are large relative to FIFO size (buffer can hold up to 2)
- *   - lines are small relative to FIFO size (buffer can hold more than 2
- *     lines), so need to account for TLB latency
- *
- *   The normal calculation is:
- *     watermark = dotclock * bytes per pixel * latency
- *   where latency is platform & configuration dependent (we assume pessimal
- *   values here).
- *
- *   The SR calculation is:
- *     watermark = (trunc(latency/line time)+1) * surface width *
- *       bytes per pixel
- *   where
- *     line time = htotal / dotclock
- *     surface width = hdisplay for normal plane and 64 for cursor
- *   and latency is assumed to be high, as above.
- *
- * The final value programmed to the register should always be rounded up,
- * and include an extra 2 entries to account for clock crossings.
- *
- * We don't use the sprite, so we can ignore that.  And on Crestline we have
- * to set the non-SR watermarks to 8.
- */
-static void intel_update_watermarks(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->display.update_wm)
-		dev_priv->display.update_wm(dev);
-}
-
-void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
-				    uint32_t sprite_width, int pixel_size)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->display.update_sprite_wm)
-		dev_priv->display.update_sprite_wm(dev, pipe, sprite_width,
-						   pixel_size);
-}
-
 static inline bool intel_panel_use_ssc(struct drm_i915_private *dev_priv)
 {
 	if (i915_panel_use_ssc >= 0)
@@ -5143,6 +3732,222 @@ static void i9xx_update_pll_dividers(struct drm_crtc *crtc,
 	}
 }
 
+static void intel_update_lvds(struct drm_crtc *crtc, intel_clock_t *clock,
+			      struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 temp;
+
+	temp = I915_READ(LVDS);
+	temp |= LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP;
+	if (pipe == 1) {
+		temp |= LVDS_PIPEB_SELECT;
+	} else {
+		temp &= ~LVDS_PIPEB_SELECT;
+	}
+	/* set the corresponsding LVDS_BORDER bit */
+	temp |= dev_priv->lvds_border_bits;
+	/* Set the B0-B3 data pairs corresponding to whether we're going to
+	 * set the DPLLs for dual-channel mode or not.
+	 */
+	if (clock->p2 == 7)
+		temp |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
+	else
+		temp &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
+
+	/* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
+	 * appropriately here, but we need to look more thoroughly into how
+	 * panels behave in the two modes.
+	 */
+	/* set the dithering flag on LVDS as needed */
+	if (INTEL_INFO(dev)->gen >= 4) {
+		if (dev_priv->lvds_dither)
+			temp |= LVDS_ENABLE_DITHER;
+		else
+			temp &= ~LVDS_ENABLE_DITHER;
+	}
+	temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
+	if (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC)
+		temp |= LVDS_HSYNC_POLARITY;
+	if (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC)
+		temp |= LVDS_VSYNC_POLARITY;
+	I915_WRITE(LVDS, temp);
+}
+
+static void i9xx_update_pll(struct drm_crtc *crtc,
+			    struct drm_display_mode *mode,
+			    struct drm_display_mode *adjusted_mode,
+			    intel_clock_t *clock, intel_clock_t *reduced_clock,
+			    int num_connectors)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 dpll;
+	bool is_sdvo;
+
+	is_sdvo = intel_pipe_has_type(crtc, INTEL_OUTPUT_SDVO) ||
+		intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI);
+
+	dpll = DPLL_VGA_MODE_DIS;
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		dpll |= DPLLB_MODE_LVDS;
+	else
+		dpll |= DPLLB_MODE_DAC_SERIAL;
+	if (is_sdvo) {
+		int pixel_multiplier = intel_mode_get_pixel_multiplier(adjusted_mode);
+		if (pixel_multiplier > 1) {
+			if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev))
+				dpll |= (pixel_multiplier - 1) << SDVO_MULTIPLIER_SHIFT_HIRES;
+		}
+		dpll |= DPLL_DVO_HIGH_SPEED;
+	}
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
+		dpll |= DPLL_DVO_HIGH_SPEED;
+
+	/* compute bitmask from p1 value */
+	if (IS_PINEVIEW(dev))
+		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT_PINEVIEW;
+	else {
+		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
+		if (IS_G4X(dev) && reduced_clock)
+			dpll |= (1 << (reduced_clock->p1 - 1)) << DPLL_FPA1_P1_POST_DIV_SHIFT;
+	}
+	switch (clock->p2) {
+	case 5:
+		dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_5;
+		break;
+	case 7:
+		dpll |= DPLLB_LVDS_P2_CLOCK_DIV_7;
+		break;
+	case 10:
+		dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_10;
+		break;
+	case 14:
+		dpll |= DPLLB_LVDS_P2_CLOCK_DIV_14;
+		break;
+	}
+	if (INTEL_INFO(dev)->gen >= 4)
+		dpll |= (6 << PLL_LOAD_PULSE_PHASE_SHIFT);
+
+	if (is_sdvo && intel_pipe_has_type(crtc, INTEL_OUTPUT_TVOUT))
+		dpll |= PLL_REF_INPUT_TVCLKINBC;
+	else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_TVOUT))
+		/* XXX: just matching BIOS for now */
+		/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
+		dpll |= 3;
+	else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+		 intel_panel_use_ssc(dev_priv) && num_connectors < 2)
+		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
+	else
+		dpll |= PLL_REF_INPUT_DREFCLK;
+
+	dpll |= DPLL_VCO_ENABLE;
+	I915_WRITE(DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
+	 * This is an exception to the general rule that mode_set doesn't turn
+	 * things on.
+	 */
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		intel_update_lvds(crtc, clock, adjusted_mode);
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
+		intel_dp_set_m_n(crtc, mode, adjusted_mode);
+
+	I915_WRITE(DPLL(pipe), dpll);
+
+	/* Wait for the clocks to stabilize. */
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	if (INTEL_INFO(dev)->gen >= 4) {
+		u32 temp = 0;
+		if (is_sdvo) {
+			temp = intel_mode_get_pixel_multiplier(adjusted_mode);
+			if (temp > 1)
+				temp = (temp - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
+			else
+				temp = 0;
+		}
+		I915_WRITE(DPLL_MD(pipe), temp);
+	} else {
+		/* The pixel multiplier can only be updated once the
+		 * DPLL is enabled and the clocks are stable.
+		 *
+		 * So write it again.
+		 */
+		I915_WRITE(DPLL(pipe), dpll);
+	}
+}
+
+static void i8xx_update_pll(struct drm_crtc *crtc,
+			    struct drm_display_mode *adjusted_mode,
+			    intel_clock_t *clock,
+			    int num_connectors)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 dpll;
+
+	dpll = DPLL_VGA_MODE_DIS;
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
+	} else {
+		if (clock->p1 == 2)
+			dpll |= PLL_P1_DIVIDE_BY_TWO;
+		else
+			dpll |= (clock->p1 - 2) << DPLL_FPA01_P1_POST_DIV_SHIFT;
+		if (clock->p2 == 4)
+			dpll |= PLL_P2_DIVIDE_BY_4;
+	}
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_TVOUT))
+		/* XXX: just matching BIOS for now */
+		/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
+		dpll |= 3;
+	else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+		 intel_panel_use_ssc(dev_priv) && num_connectors < 2)
+		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
+	else
+		dpll |= PLL_REF_INPUT_DREFCLK;
+
+	dpll |= DPLL_VCO_ENABLE;
+	I915_WRITE(DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	I915_WRITE(DPLL(pipe), dpll);
+
+	/* Wait for the clocks to stabilize. */
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
+	 * This is an exception to the general rule that mode_set doesn't turn
+	 * things on.
+	 */
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		intel_update_lvds(crtc, clock, adjusted_mode);
+
+	/* The pixel multiplier can only be updated once the
+	 * DPLL is enabled and the clocks are stable.
+	 *
+	 * So write it again.
+	 */
+	I915_WRITE(DPLL(pipe), dpll);
+}
+
 static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 			      struct drm_display_mode *mode,
 			      struct drm_display_mode *adjusted_mode,
@@ -5156,15 +3961,13 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	int plane = intel_crtc->plane;
 	int refclk, num_connectors = 0;
 	intel_clock_t clock, reduced_clock;
-	u32 dpll, dspcntr, pipeconf, vsyncshift;
-	bool ok, has_reduced_clock = false, is_sdvo = false, is_dvo = false;
-	bool is_crt = false, is_lvds = false, is_tv = false, is_dp = false;
+	u32 dspcntr, pipeconf, vsyncshift;
+	bool ok, has_reduced_clock = false, is_sdvo = false;
+	bool is_lvds = false, is_tv = false, is_dp = false;
 	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
 	const intel_limit_t *limit;
 	int ret;
-	u32 temp;
-	u32 lvds_sync = 0;
 
 	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
 		if (encoder->base.crtc != crtc)
@@ -5180,15 +3983,9 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 			if (encoder->needs_tv_clock)
 				is_tv = true;
 			break;
-		case INTEL_OUTPUT_DVO:
-			is_dvo = true;
-			break;
 		case INTEL_OUTPUT_TVOUT:
 			is_tv = true;
 			break;
-		case INTEL_OUTPUT_ANALOG:
-			is_crt = true;
-			break;
 		case INTEL_OUTPUT_DISPLAYPORT:
 			is_dp = true;
 			break;
@@ -5235,71 +4032,12 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	i9xx_update_pll_dividers(crtc, &clock, has_reduced_clock ?
 				 &reduced_clock : NULL);
 
-	dpll = DPLL_VGA_MODE_DIS;
-
-	if (!IS_GEN2(dev)) {
-		if (is_lvds)
-			dpll |= DPLLB_MODE_LVDS;
-		else
-			dpll |= DPLLB_MODE_DAC_SERIAL;
-		if (is_sdvo) {
-			int pixel_multiplier = intel_mode_get_pixel_multiplier(adjusted_mode);
-			if (pixel_multiplier > 1) {
-				if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev))
-					dpll |= (pixel_multiplier - 1) << SDVO_MULTIPLIER_SHIFT_HIRES;
-			}
-			dpll |= DPLL_DVO_HIGH_SPEED;
-		}
-		if (is_dp)
-			dpll |= DPLL_DVO_HIGH_SPEED;
-
-		/* compute bitmask from p1 value */
-		if (IS_PINEVIEW(dev))
-			dpll |= (1 << (clock.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT_PINEVIEW;
-		else {
-			dpll |= (1 << (clock.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
-			if (IS_G4X(dev) && has_reduced_clock)
-				dpll |= (1 << (reduced_clock.p1 - 1)) << DPLL_FPA1_P1_POST_DIV_SHIFT;
-		}
-		switch (clock.p2) {
-		case 5:
-			dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_5;
-			break;
-		case 7:
-			dpll |= DPLLB_LVDS_P2_CLOCK_DIV_7;
-			break;
-		case 10:
-			dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_10;
-			break;
-		case 14:
-			dpll |= DPLLB_LVDS_P2_CLOCK_DIV_14;
-			break;
-		}
-		if (INTEL_INFO(dev)->gen >= 4)
-			dpll |= (6 << PLL_LOAD_PULSE_PHASE_SHIFT);
-	} else {
-		if (is_lvds) {
-			dpll |= (1 << (clock.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
-		} else {
-			if (clock.p1 == 2)
-				dpll |= PLL_P1_DIVIDE_BY_TWO;
-			else
-				dpll |= (clock.p1 - 2) << DPLL_FPA01_P1_POST_DIV_SHIFT;
-			if (clock.p2 == 4)
-				dpll |= PLL_P2_DIVIDE_BY_4;
-		}
-	}
-
-	if (is_sdvo && is_tv)
-		dpll |= PLL_REF_INPUT_TVCLKINBC;
-	else if (is_tv)
-		/* XXX: just matching BIOS for now */
-		/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
-		dpll |= 3;
-	else if (is_lvds && intel_panel_use_ssc(dev_priv) && num_connectors < 2)
-		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
+	if (IS_GEN2(dev))
+		i8xx_update_pll(crtc, adjusted_mode, &clock, num_connectors);
 	else
-		dpll |= PLL_REF_INPUT_DREFCLK;
+		i9xx_update_pll(crtc, mode, adjusted_mode, &clock,
+				has_reduced_clock ? &reduced_clock : NULL,
+				num_connectors);
 
 	/* setup pipeconf */
 	pipeconf = I915_READ(PIPECONF(pipe));
@@ -5336,97 +4074,9 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 		}
 	}
 
-	dpll |= DPLL_VCO_ENABLE;
-
 	DRM_DEBUG_KMS("Mode for pipe %c:\n", pipe == 0 ? 'A' : 'B');
 	drm_mode_debug_printmodeline(mode);
 
-	I915_WRITE(DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
-
-	POSTING_READ(DPLL(pipe));
-	udelay(150);
-
-	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
-	 * This is an exception to the general rule that mode_set doesn't turn
-	 * things on.
-	 */
-	if (is_lvds) {
-		temp = I915_READ(LVDS);
-		temp |= LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP;
-		if (pipe == 1) {
-			temp |= LVDS_PIPEB_SELECT;
-		} else {
-			temp &= ~LVDS_PIPEB_SELECT;
-		}
-		/* set the corresponsding LVDS_BORDER bit */
-		temp |= dev_priv->lvds_border_bits;
-		/* Set the B0-B3 data pairs corresponding to whether we're going to
-		 * set the DPLLs for dual-channel mode or not.
-		 */
-		if (clock.p2 == 7)
-			temp |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
-		else
-			temp &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
-
-		/* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
-		 * appropriately here, but we need to look more thoroughly into how
-		 * panels behave in the two modes.
-		 */
-		/* set the dithering flag on LVDS as needed */
-		if (INTEL_INFO(dev)->gen >= 4) {
-			if (dev_priv->lvds_dither)
-				temp |= LVDS_ENABLE_DITHER;
-			else
-				temp &= ~LVDS_ENABLE_DITHER;
-		}
-		if (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC)
-			lvds_sync |= LVDS_HSYNC_POLARITY;
-		if (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC)
-			lvds_sync |= LVDS_VSYNC_POLARITY;
-		if ((temp & (LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY))
-		    != lvds_sync) {
-			char flags[2] = "-+";
-			DRM_INFO("Changing LVDS panel from "
-				 "(%chsync, %cvsync) to (%chsync, %cvsync)\n",
-				 flags[!(temp & LVDS_HSYNC_POLARITY)],
-				 flags[!(temp & LVDS_VSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_HSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_VSYNC_POLARITY)]);
-			temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
-			temp |= lvds_sync;
-		}
-		I915_WRITE(LVDS, temp);
-	}
-
-	if (is_dp) {
-		intel_dp_set_m_n(crtc, mode, adjusted_mode);
-	}
-
-	I915_WRITE(DPLL(pipe), dpll);
-
-	/* Wait for the clocks to stabilize. */
-	POSTING_READ(DPLL(pipe));
-	udelay(150);
-
-	if (INTEL_INFO(dev)->gen >= 4) {
-		temp = 0;
-		if (is_sdvo) {
-			temp = intel_mode_get_pixel_multiplier(adjusted_mode);
-			if (temp > 1)
-				temp = (temp - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
-			else
-				temp = 0;
-		}
-		I915_WRITE(DPLL_MD(pipe), temp);
-	} else {
-		/* The pixel multiplier can only be updated once the
-		 * DPLL is enabled and the clocks are stable.
-		 *
-		 * So write it again.
-		 */
-		I915_WRITE(DPLL(pipe), dpll);
-	}
-
 	if (HAS_PIPE_CXSR(dev)) {
 		if (intel_crtc->lowfreq_avail) {
 			DRM_DEBUG_KMS("enabling CxSR downclocking\n");
@@ -5492,7 +4142,6 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 
 	I915_WRITE(DSPCNTR(plane), dspcntr);
 	POSTING_READ(DSPCNTR(plane));
-	intel_enable_plane(dev_priv, plane, pipe);
 
 	ret = intel_pipe_set_base(crtc, x, y, old_fb);
 
@@ -5668,17 +4317,16 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	u32 dpll, fp = 0, fp2 = 0, dspcntr, pipeconf;
 	bool ok, has_reduced_clock = false, is_sdvo = false;
 	bool is_crt = false, is_lvds = false, is_tv = false, is_dp = false;
-	struct intel_encoder *has_edp_encoder = NULL;
 	struct drm_mode_config *mode_config = &dev->mode_config;
-	struct intel_encoder *encoder;
+	struct intel_encoder *encoder, *edp_encoder = NULL;
 	const intel_limit_t *limit;
 	int ret;
 	struct fdi_m_n m_n = {0};
 	u32 temp;
-	u32 lvds_sync = 0;
 	int target_clock, pixel_multiplier, lane, link_bw, factor;
 	unsigned int pipe_bpp;
 	bool dither;
+	bool is_cpu_edp = false, is_pch_edp = false;
 
 	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
 		if (encoder->base.crtc != crtc)
@@ -5704,7 +4352,12 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 			is_dp = true;
 			break;
 		case INTEL_OUTPUT_EDP:
-			has_edp_encoder = encoder;
+			is_dp = true;
+			if (intel_encoder_is_pch_edp(&encoder->base))
+				is_pch_edp = true;
+			else
+				is_cpu_edp = true;
+			edp_encoder = encoder;
 			break;
 		}
 
@@ -5767,15 +4420,13 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	lane = 0;
 	/* CPU eDP doesn't require FDI link, so just set DP M/N
 	   according to current link config */
-	if (has_edp_encoder &&
-	    !intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
+	if (is_cpu_edp) {
 		target_clock = mode->clock;
-		intel_edp_link_config(has_edp_encoder,
-				      &lane, &link_bw);
+		intel_edp_link_config(edp_encoder, &lane, &link_bw);
 	} else {
 		/* [e]DP over FDI requires target mode clock
 		   instead of link clock */
-		if (is_dp || intel_encoder_is_pch_edp(&has_edp_encoder->base))
+		if (is_dp)
 			target_clock = mode->clock;
 		else
 			target_clock = adjusted_mode->clock;
@@ -5866,7 +4517,7 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		}
 		dpll |= DPLL_DVO_HIGH_SPEED;
 	}
-	if (is_dp || intel_encoder_is_pch_edp(&has_edp_encoder->base))
+	if (is_dp && !is_cpu_edp)
 		dpll |= DPLL_DVO_HIGH_SPEED;
 
 	/* compute bitmask from p1 value */
@@ -5909,30 +4560,22 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	DRM_DEBUG_KMS("Mode for pipe %d:\n", pipe);
 	drm_mode_debug_printmodeline(mode);
 
-	/* PCH eDP needs FDI, but CPU eDP does not */
-	if (!intel_crtc->no_pll) {
-		if (!has_edp_encoder ||
-		    intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
-			I915_WRITE(PCH_FP0(pipe), fp);
-			I915_WRITE(PCH_DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
-
-			POSTING_READ(PCH_DPLL(pipe));
-			udelay(150);
-		}
-	} else {
-		if (dpll == (I915_READ(PCH_DPLL(0)) & 0x7fffffff) &&
-		    fp == I915_READ(PCH_FP0(0))) {
-			intel_crtc->use_pll_a = true;
-			DRM_DEBUG_KMS("using pipe a dpll\n");
-		} else if (dpll == (I915_READ(PCH_DPLL(1)) & 0x7fffffff) &&
-			   fp == I915_READ(PCH_FP0(1))) {
-			intel_crtc->use_pll_a = false;
-			DRM_DEBUG_KMS("using pipe b dpll\n");
-		} else {
-			DRM_DEBUG_KMS("no matching PLL configuration for pipe 2\n");
+	/* CPU eDP is the only output that doesn't need a PCH PLL of its own on
+	 * pre-Haswell/LPT generation */
+	if (HAS_PCH_LPT(dev)) {
+		DRM_DEBUG_KMS("LPT detected: no PLL for pipe %d necessary\n",
+				pipe);
+	} else if (!is_cpu_edp) {
+		struct intel_pch_pll *pll;
+
+		pll = intel_get_pch_pll(intel_crtc, dpll, fp);
+		if (pll == NULL) {
+			DRM_DEBUG_DRIVER("failed to find PLL for pipe %d\n",
+					 pipe);
 			return -EINVAL;
 		}
-	}
+	} else
+		intel_put_pch_pll(intel_crtc);
 
 	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
 	 * This is an exception to the general rule that mode_set doesn't turn
@@ -5965,22 +4608,11 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		 * appropriately here, but we need to look more thoroughly into how
 		 * panels behave in the two modes.
 		 */
+		temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
 		if (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC)
-			lvds_sync |= LVDS_HSYNC_POLARITY;
+			temp |= LVDS_HSYNC_POLARITY;
 		if (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC)
-			lvds_sync |= LVDS_VSYNC_POLARITY;
-		if ((temp & (LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY))
-		    != lvds_sync) {
-			char flags[2] = "-+";
-			DRM_INFO("Changing LVDS panel from "
-				 "(%chsync, %cvsync) to (%chsync, %cvsync)\n",
-				 flags[!(temp & LVDS_HSYNC_POLARITY)],
-				 flags[!(temp & LVDS_VSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_HSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_VSYNC_POLARITY)]);
-			temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
-			temp |= lvds_sync;
-		}
+			temp |= LVDS_VSYNC_POLARITY;
 		I915_WRITE(PCH_LVDS, temp);
 	}
 
@@ -5990,7 +4622,7 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		pipeconf |= PIPECONF_DITHER_EN;
 		pipeconf |= PIPECONF_DITHER_TYPE_SP;
 	}
-	if (is_dp || intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
+	if (is_dp && !is_cpu_edp) {
 		intel_dp_set_m_n(crtc, mode, adjusted_mode);
 	} else {
 		/* For non-DP output, clear any trans DP clock recovery setting.*/
@@ -6000,13 +4632,11 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		I915_WRITE(TRANSDPLINK_N1(pipe), 0);
 	}
 
-	if (!intel_crtc->no_pll &&
-	    (!has_edp_encoder ||
-	     intel_encoder_is_pch_edp(&has_edp_encoder->base))) {
-		I915_WRITE(PCH_DPLL(pipe), dpll);
+	if (intel_crtc->pch_pll) {
+		I915_WRITE(intel_crtc->pch_pll->pll_reg, dpll);
 
 		/* Wait for the clocks to stabilize. */
-		POSTING_READ(PCH_DPLL(pipe));
+		POSTING_READ(intel_crtc->pch_pll->pll_reg);
 		udelay(150);
 
 		/* The pixel multiplier can only be updated once the
@@ -6014,20 +4644,20 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		 *
 		 * So write it again.
 		 */
-		I915_WRITE(PCH_DPLL(pipe), dpll);
+		I915_WRITE(intel_crtc->pch_pll->pll_reg, dpll);
 	}
 
 	intel_crtc->lowfreq_avail = false;
-	if (!intel_crtc->no_pll) {
+	if (intel_crtc->pch_pll) {
 		if (is_lvds && has_reduced_clock && i915_powersave) {
-			I915_WRITE(PCH_FP1(pipe), fp2);
+			I915_WRITE(intel_crtc->pch_pll->fp1_reg, fp2);
 			intel_crtc->lowfreq_avail = true;
 			if (HAS_PIPE_CXSR(dev)) {
 				DRM_DEBUG_KMS("enabling CxSR downclocking\n");
 				pipeconf |= PIPECONF_CXSR_DOWNCLOCK;
 			}
 		} else {
-			I915_WRITE(PCH_FP1(pipe), fp);
+			I915_WRITE(intel_crtc->pch_pll->fp1_reg, fp);
 			if (HAS_PIPE_CXSR(dev)) {
 				DRM_DEBUG_KMS("disabling CxSR downclocking\n");
 				pipeconf &= ~PIPECONF_CXSR_DOWNCLOCK;
@@ -6080,10 +4710,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	I915_WRITE(PIPE_LINK_M1(pipe), m_n.link_m);
 	I915_WRITE(PIPE_LINK_N1(pipe), m_n.link_n);
 
-	if (has_edp_encoder &&
-	    !intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
+	if (is_cpu_edp)
 		ironlake_set_pll_edp(crtc, adjusted_mode->clock);
-	}
 
 	I915_WRITE(PIPECONF(pipe), pipeconf);
 	POSTING_READ(PIPECONF(pipe));
@@ -6097,6 +4725,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 
 	intel_update_watermarks(dev);
 
+	intel_update_linetime_watermarks(dev, pipe, adjusted_mode);
+
 	return ret;
 }
 
@@ -6451,7 +5081,7 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 	if (!visible && !intel_crtc->cursor_visible)
 		return;
 
-	if (IS_IVYBRIDGE(dev)) {
+	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
 		I915_WRITE(CURPOS_IVB(pipe), pos);
 		ivb_update_cursor(crtc, base);
 	} else {
@@ -6461,9 +5091,6 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 		else
 			i9xx_update_cursor(crtc, base);
 	}
-
-	if (visible)
-		intel_mark_busy(dev, to_intel_framebuffer(crtc->fb)->obj);
 }
 
 static int intel_crtc_cursor_set(struct drm_crtc *crtc,
@@ -6987,7 +5614,6 @@ struct drm_display_mode *intel_crtc_mode_get(struct drm_device *dev,
 	mode->vsync_end = ((vsync & 0xffff0000) >> 16) + 1;
 
 	drm_mode_set_name(mode);
-	drm_mode_set_crtcinfo(mode, 0);
 
 	return mode;
 }
@@ -7086,7 +5712,7 @@ static void intel_decrease_pllclock(struct drm_crtc *crtc)
 	if (!HAS_PIPE_CXSR(dev) && intel_crtc->lowfreq_avail) {
 		int pipe = intel_crtc->pipe;
 		int dpll_reg = DPLL(pipe);
-		u32 dpll;
+		int dpll;
 
 		DRM_DEBUG_DRIVER("downclocking LVDS\n");
 
@@ -7100,6 +5726,7 @@ static void intel_decrease_pllclock(struct drm_crtc *crtc)
 		if (!(dpll & DISPLAY_RATE_SELECT_FPA1))
 			DRM_DEBUG_DRIVER("failed to downclock LVDS!\n");
 	}
+
 }
 
 /**
@@ -7158,12 +5785,16 @@ void intel_mark_busy(struct drm_device *dev, struct drm_i915_gem_object *obj)
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return;
 
-	if (!dev_priv->busy)
+	if (!dev_priv->busy) {
+		intel_sanitize_pm(dev);
 		dev_priv->busy = true;
-	else
+	} else
 		mod_timer(&dev_priv->idle_timer, jiffies +
 			  msecs_to_jiffies(GPU_IDLE_TIMEOUT));
 
+	if (obj == NULL)
+		return;
+
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		if (!crtc->fb)
 			continue;
@@ -7336,18 +5967,19 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	unsigned long offset;
 	u32 flip_mask;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
 	/* Offset into the new buffer for cases of shared fbs between CRTCs */
 	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
 
-	ret = BEGIN_LP_RING(6);
+	ret = intel_ring_begin(ring, 6);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	/* Can't queue multiple flips, so wait for the previous
 	 * one to finish before executing the next.
@@ -7356,15 +5988,19 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 		flip_mask = MI_WAIT_FOR_PLANE_B_FLIP;
 	else
 		flip_mask = MI_WAIT_FOR_PLANE_A_FLIP;
-	OUT_RING(MI_WAIT_FOR_EVENT | flip_mask);
-	OUT_RING(MI_NOOP);
-	OUT_RING(MI_DISPLAY_FLIP |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0]);
-	OUT_RING(obj->gtt_offset + offset);
-	OUT_RING(0); /* aux display base address, unused */
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | flip_mask);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0]);
+	intel_ring_emit(ring, obj->gtt_offset + offset);
+	intel_ring_emit(ring, 0); /* aux display base address, unused */
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7377,33 +6013,38 @@ static int intel_gen3_queue_flip(struct drm_device *dev,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	unsigned long offset;
 	u32 flip_mask;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
 	/* Offset into the new buffer for cases of shared fbs between CRTCs */
 	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
 
-	ret = BEGIN_LP_RING(6);
+	ret = intel_ring_begin(ring, 6);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	if (intel_crtc->plane)
 		flip_mask = MI_WAIT_FOR_PLANE_B_FLIP;
 	else
 		flip_mask = MI_WAIT_FOR_PLANE_A_FLIP;
-	OUT_RING(MI_WAIT_FOR_EVENT | flip_mask);
-	OUT_RING(MI_NOOP);
-	OUT_RING(MI_DISPLAY_FLIP_I915 |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0]);
-	OUT_RING(obj->gtt_offset + offset);
-	OUT_RING(MI_NOOP);
-
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | flip_mask);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0]);
+	intel_ring_emit(ring, obj->gtt_offset + offset);
+	intel_ring_emit(ring, MI_NOOP);
+
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7415,24 +6056,25 @@ static int intel_gen4_queue_flip(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	uint32_t pf, pipesrc;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
-	ret = BEGIN_LP_RING(4);
+	ret = intel_ring_begin(ring, 4);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	/* i965+ uses the linear or tiled offsets from the
 	 * Display Registers (which do not change across a page-flip)
 	 * so we need only reprogram the base address.
 	 */
-	OUT_RING(MI_DISPLAY_FLIP |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0]);
-	OUT_RING(obj->gtt_offset | obj->tiling_mode);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0]);
+	intel_ring_emit(ring, obj->gtt_offset | obj->tiling_mode);
 
 	/* XXX Enabling the panel-fitter across page-flip is so far
 	 * untested on non-native modes, so ignore it for now.
@@ -7440,9 +6082,13 @@ static int intel_gen4_queue_flip(struct drm_device *dev,
 	 */
 	pf = 0;
 	pipesrc = I915_READ(PIPESRC(intel_crtc->pipe)) & 0x0fff0fff;
-	OUT_RING(pf | pipesrc);
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, pf | pipesrc);
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7453,21 +6099,22 @@ static int intel_gen6_queue_flip(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	uint32_t pf, pipesrc;
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
-	ret = BEGIN_LP_RING(4);
+	ret = intel_ring_begin(ring, 4);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
-	OUT_RING(MI_DISPLAY_FLIP |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0] | obj->tiling_mode);
-	OUT_RING(obj->gtt_offset);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0] | obj->tiling_mode);
+	intel_ring_emit(ring, obj->gtt_offset);
 
 	/* Contrary to the suggestions in the documentation,
 	 * "Enable Panel Fitter" does not seem to be required when page
@@ -7477,9 +6124,13 @@ static int intel_gen6_queue_flip(struct drm_device *dev,
 	 */
 	pf = 0;
 	pipesrc = I915_READ(PIPESRC(intel_crtc->pipe)) & 0x0fff0fff;
-	OUT_RING(pf | pipesrc);
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, pf | pipesrc);
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7501,18 +6152,22 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 
 	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
 	ret = intel_ring_begin(ring, 4);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 | (intel_crtc->plane << 19));
 	intel_ring_emit(ring, (fb->pitches[0] | obj->tiling_mode));
 	intel_ring_emit(ring, (obj->gtt_offset));
 	intel_ring_emit(ring, (MI_NOOP));
 	intel_ring_advance(ring);
-out:
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7589,6 +6244,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 		goto cleanup_pending;
 
 	intel_disable_fbc(dev);
+	intel_mark_busy(dev, obj);
 	mutex_unlock(&dev->struct_mutex);
 
 	trace_i915_flip_request(intel_crtc->plane, obj);
@@ -7617,10 +6273,11 @@ static void intel_sanitize_modesetting(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 reg, val;
+	int i;
 
 	/* Clear any frame start delays used for debugging left by the BIOS */
-	for_each_pipe(pipe) {
-		reg = PIPECONF(pipe);
+	for_each_pipe(i) {
+		reg = PIPECONF(i);
 		I915_WRITE(reg, I915_READ(reg) & ~PIPECONF_FRAME_START_DELAY_MASK);
 	}
 
@@ -7690,6 +6347,23 @@ static const struct drm_crtc_funcs intel_crtc_funcs = {
 	.page_flip = intel_crtc_page_flip,
 };
 
+static void intel_pch_pll_init(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	int i;
+
+	if (dev_priv->num_pch_pll == 0) {
+		DRM_DEBUG_KMS("No PCH PLLs on this hardware, skipping initialisation\n");
+		return;
+	}
+
+	for (i = 0; i < dev_priv->num_pch_pll; i++) {
+		dev_priv->pch_plls[i].pll_reg = _PCH_DPLL(i);
+		dev_priv->pch_plls[i].fp0_reg = _PCH_FP0(i);
+		dev_priv->pch_plls[i].fp1_reg = _PCH_FP1(i);
+	}
+}
+
 static void intel_crtc_init(struct drm_device *dev, int pipe)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
@@ -7727,8 +6401,6 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 	intel_crtc->bpp = 24; /* default for pre-Ironlake */
 
 	if (HAS_PCH_SPLIT(dev)) {
-		if (pipe == 2 && IS_IVYBRIDGE(dev))
-			intel_crtc->no_pll = true;
 		intel_helper_funcs.prepare = ironlake_crtc_prepare;
 		intel_helper_funcs.commit = ironlake_crtc_commit;
 	} else {
@@ -7747,15 +6419,12 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 int intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
 				struct drm_file *file)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct drm_i915_get_pipe_from_crtc_id *pipe_from_crtc_id = data;
 	struct drm_mode_object *drmmode_obj;
 	struct intel_crtc *crtc;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
 
 	drmmode_obj = drm_mode_object_find(dev, pipe_from_crtc_id->crtc_id,
 			DRM_MODE_OBJECT_CRTC);
@@ -7828,12 +6497,31 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 	intel_crt_init(dev);
 
-	if (HAS_PCH_SPLIT(dev)) {
+	if (IS_HASWELL(dev)) {
+		int found;
+
+		/* Haswell uses DDI functions to detect digital outputs */
+		found = I915_READ(DDI_BUF_CTL_A) & DDI_INIT_DISPLAY_DETECTED;
+		/* DDI A only supports eDP */
+		if (found)
+			intel_ddi_init(dev, PORT_A);
+
+		/* DDI B, C and D detection is indicated by the SFUSE_STRAP
+		 * register */
+		found = I915_READ(SFUSE_STRAP);
+
+		if (found & SFUSE_STRAP_DDIB_DETECTED)
+			intel_ddi_init(dev, PORT_B);
+		if (found & SFUSE_STRAP_DDIC_DETECTED)
+			intel_ddi_init(dev, PORT_C);
+		if (found & SFUSE_STRAP_DDID_DETECTED)
+			intel_ddi_init(dev, PORT_D);
+	} else if (HAS_PCH_SPLIT(dev)) {
 		int found;
 
 		if (I915_READ(HDMIB) & PORT_DETECTED) {
 			/* PCH SDVOB multiplex with HDMIB */
-			found = intel_sdvo_init(dev, PCH_SDVOB);
+			found = intel_sdvo_init(dev, PCH_SDVOB, true);
 			if (!found)
 				intel_hdmi_init(dev, HDMIB);
 			if (!found && (I915_READ(PCH_DP_B) & DP_DETECTED))
@@ -7857,7 +6545,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOB\n");
-			found = intel_sdvo_init(dev, SDVOB);
+			found = intel_sdvo_init(dev, SDVOB, true);
 			if (!found && SUPPORTS_INTEGRATED_HDMI(dev)) {
 				DRM_DEBUG_KMS("probing HDMI on SDVOB\n");
 				intel_hdmi_init(dev, SDVOB);
@@ -7873,7 +6561,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOC\n");
-			found = intel_sdvo_init(dev, SDVOC);
+			found = intel_sdvo_init(dev, SDVOC, false);
 		}
 
 		if (!found && (I915_READ(SDVOC) & SDVO_DETECTED)) {
@@ -8002,882 +6690,6 @@ static const struct drm_mode_config_funcs intel_mode_funcs = {
 	.output_poll_changed = intel_fb_output_poll_changed,
 };
 
-static struct drm_i915_gem_object *
-intel_alloc_context_page(struct drm_device *dev)
-{
-	struct drm_i915_gem_object *ctx;
-	int ret;
-
-	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
-
-	ctx = i915_gem_alloc_object(dev, 4096);
-	if (!ctx) {
-		DRM_DEBUG("failed to alloc power context, RC6 disabled\n");
-		return NULL;
-	}
-
-	ret = i915_gem_object_pin(ctx, 4096, true);
-	if (ret) {
-		DRM_ERROR("failed to pin power context: %d\n", ret);
-		goto err_unref;
-	}
-
-	ret = i915_gem_object_set_to_gtt_domain(ctx, 1);
-	if (ret) {
-		DRM_ERROR("failed to set-domain on power context: %d\n", ret);
-		goto err_unpin;
-	}
-
-	return ctx;
-
-err_unpin:
-	i915_gem_object_unpin(ctx);
-err_unref:
-	drm_gem_object_unreference(&ctx->base);
-	mutex_unlock(&dev->struct_mutex);
-	return NULL;
-}
-
-bool ironlake_set_drps(struct drm_device *dev, u8 val)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u16 rgvswctl;
-
-	rgvswctl = I915_READ16(MEMSWCTL);
-	if (rgvswctl & MEMCTL_CMD_STS) {
-		DRM_DEBUG("gpu busy, RCS change rejected\n");
-		return false; /* still busy with another command */
-	}
-
-	rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
-		(val << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
-	I915_WRITE16(MEMSWCTL, rgvswctl);
-	POSTING_READ16(MEMSWCTL);
-
-	rgvswctl |= MEMCTL_CMD_STS;
-	I915_WRITE16(MEMSWCTL, rgvswctl);
-
-	return true;
-}
-
-void ironlake_enable_drps(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 rgvmodectl = I915_READ(MEMMODECTL);
-	u8 fmax, fmin, fstart, vstart;
-
-	/* Enable temp reporting */
-	I915_WRITE16(PMMISC, I915_READ(PMMISC) | MCPPCE_EN);
-	I915_WRITE16(TSC1, I915_READ(TSC1) | TSE);
-
-	/* 100ms RC evaluation intervals */
-	I915_WRITE(RCUPEI, 100000);
-	I915_WRITE(RCDNEI, 100000);
-
-	/* Set max/min thresholds to 90ms and 80ms respectively */
-	I915_WRITE(RCBMAXAVG, 90000);
-	I915_WRITE(RCBMINAVG, 80000);
-
-	I915_WRITE(MEMIHYST, 1);
-
-	/* Set up min, max, and cur for interrupt handling */
-	fmax = (rgvmodectl & MEMMODE_FMAX_MASK) >> MEMMODE_FMAX_SHIFT;
-	fmin = (rgvmodectl & MEMMODE_FMIN_MASK);
-	fstart = (rgvmodectl & MEMMODE_FSTART_MASK) >>
-		MEMMODE_FSTART_SHIFT;
-
-	vstart = (I915_READ(PXVFREQ_BASE + (fstart * 4)) & PXVFREQ_PX_MASK) >>
-		PXVFREQ_PX_SHIFT;
-
-	dev_priv->fmax = fmax; /* IPS callback will increase this */
-	dev_priv->fstart = fstart;
-
-	dev_priv->max_delay = fstart;
-	dev_priv->min_delay = fmin;
-	dev_priv->cur_delay = fstart;
-
-	DRM_DEBUG_DRIVER("fmax: %d, fmin: %d, fstart: %d\n",
-			 fmax, fmin, fstart);
-
-	I915_WRITE(MEMINTREN, MEMINT_CX_SUPR_EN | MEMINT_EVAL_CHG_EN);
-
-	/*
-	 * Interrupts will be enabled in ironlake_irq_postinstall
-	 */
-
-	I915_WRITE(VIDSTART, vstart);
-	POSTING_READ(VIDSTART);
-
-	rgvmodectl |= MEMMODE_SWMODE_EN;
-	I915_WRITE(MEMMODECTL, rgvmodectl);
-
-	if (wait_for((I915_READ(MEMSWCTL) & MEMCTL_CMD_STS) == 0, 10))
-		DRM_ERROR("stuck trying to change perf mode\n");
-	msleep(1);
-
-	ironlake_set_drps(dev, fstart);
-
-	dev_priv->last_count1 = I915_READ(0x112e4) + I915_READ(0x112e8) +
-		I915_READ(0x112e0);
-	dev_priv->last_time1 = jiffies_to_msecs(jiffies);
-	dev_priv->last_count2 = I915_READ(0x112f4);
-	getrawmonotonic(&dev_priv->last_time2);
-}
-
-void ironlake_disable_drps(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u16 rgvswctl = I915_READ16(MEMSWCTL);
-
-	/* Ack interrupts, disable EFC interrupt */
-	I915_WRITE(MEMINTREN, I915_READ(MEMINTREN) & ~MEMINT_EVAL_CHG_EN);
-	I915_WRITE(MEMINTRSTS, MEMINT_EVAL_CHG);
-	I915_WRITE(DEIER, I915_READ(DEIER) & ~DE_PCU_EVENT);
-	I915_WRITE(DEIIR, DE_PCU_EVENT);
-	I915_WRITE(DEIMR, I915_READ(DEIMR) | DE_PCU_EVENT);
-
-	/* Go back to the starting frequency */
-	ironlake_set_drps(dev, dev_priv->fstart);
-	msleep(1);
-	rgvswctl |= MEMCTL_CMD_STS;
-	I915_WRITE(MEMSWCTL, rgvswctl);
-	msleep(1);
-
-}
-
-void gen6_set_rps(struct drm_device *dev, u8 val)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 swreq;
-
-	swreq = (val & 0x3ff) << 25;
-	I915_WRITE(GEN6_RPNSWREQ, swreq);
-}
-
-void gen6_disable_rps(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(GEN6_RPNSWREQ, 1 << 31);
-	I915_WRITE(GEN6_PMINTRMSK, 0xffffffff);
-	I915_WRITE(GEN6_PMIER, 0);
-	/* Complete PM interrupt masking here doesn't race with the rps work
-	 * item again unmasking PM interrupts because that is using a different
-	 * register (PMIMR) to mask PM interrupts. The only risk is in leaving
-	 * stale bits in PMIIR and PMIMR which gen6_enable_rps will clean up. */
-
-	spin_lock_irq(&dev_priv->rps_lock);
-	dev_priv->pm_iir = 0;
-	spin_unlock_irq(&dev_priv->rps_lock);
-
-	I915_WRITE(GEN6_PMIIR, I915_READ(GEN6_PMIIR));
-}
-
-static unsigned long intel_pxfreq(u32 vidfreq)
-{
-	unsigned long freq;
-	int div = (vidfreq & 0x3f0000) >> 16;
-	int post = (vidfreq & 0x3000) >> 12;
-	int pre = (vidfreq & 0x7);
-
-	if (!pre)
-		return 0;
-
-	freq = ((div * 133333) / ((1<<post) * pre));
-
-	return freq;
-}
-
-void intel_init_emon(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 lcfuse;
-	u8 pxw[16];
-	int i;
-
-	/* Disable to program */
-	I915_WRITE(ECR, 0);
-	POSTING_READ(ECR);
-
-	/* Program energy weights for various events */
-	I915_WRITE(SDEW, 0x15040d00);
-	I915_WRITE(CSIEW0, 0x007f0000);
-	I915_WRITE(CSIEW1, 0x1e220004);
-	I915_WRITE(CSIEW2, 0x04000004);
-
-	for (i = 0; i < 5; i++)
-		I915_WRITE(PEW + (i * 4), 0);
-	for (i = 0; i < 3; i++)
-		I915_WRITE(DEW + (i * 4), 0);
-
-	/* Program P-state weights to account for frequency power adjustment */
-	for (i = 0; i < 16; i++) {
-		u32 pxvidfreq = I915_READ(PXVFREQ_BASE + (i * 4));
-		unsigned long freq = intel_pxfreq(pxvidfreq);
-		unsigned long vid = (pxvidfreq & PXVFREQ_PX_MASK) >>
-			PXVFREQ_PX_SHIFT;
-		unsigned long val;
-
-		val = vid * vid;
-		val *= (freq / 1000);
-		val *= 255;
-		val /= (127*127*900);
-		if (val > 0xff)
-			DRM_ERROR("bad pxval: %ld\n", val);
-		pxw[i] = val;
-	}
-	/* Render standby states get 0 weight */
-	pxw[14] = 0;
-	pxw[15] = 0;
-
-	for (i = 0; i < 4; i++) {
-		u32 val = (pxw[i*4] << 24) | (pxw[(i*4)+1] << 16) |
-			(pxw[(i*4)+2] << 8) | (pxw[(i*4)+3]);
-		I915_WRITE(PXW + (i * 4), val);
-	}
-
-	/* Adjust magic regs to magic values (more experimental results) */
-	I915_WRITE(OGW0, 0);
-	I915_WRITE(OGW1, 0);
-	I915_WRITE(EG0, 0x00007f00);
-	I915_WRITE(EG1, 0x0000000e);
-	I915_WRITE(EG2, 0x000e0000);
-	I915_WRITE(EG3, 0x68000300);
-	I915_WRITE(EG4, 0x42000000);
-	I915_WRITE(EG5, 0x00140031);
-	I915_WRITE(EG6, 0);
-	I915_WRITE(EG7, 0);
-
-	for (i = 0; i < 8; i++)
-		I915_WRITE(PXWL + (i * 4), 0);
-
-	/* Enable PMON + select events */
-	I915_WRITE(ECR, 0x80000019);
-
-	lcfuse = I915_READ(LCFUSE02);
-
-	dev_priv->corr = (lcfuse & LCFUSE_HIV_MASK);
-}
-
-static int intel_enable_rc6(struct drm_device *dev)
-{
-	/*
-	 * Respect the kernel parameter if it is set
-	 */
-	if (i915_enable_rc6 >= 0)
-		return i915_enable_rc6;
-
-	/*
-	 * Disable RC6 on Ironlake
-	 */
-	if (INTEL_INFO(dev)->gen == 5)
-		return 0;
-
-	/*
-	 * Disable rc6 on Sandybridge
-	 */
-	if (INTEL_INFO(dev)->gen == 6) {
-		DRM_DEBUG_DRIVER("Sandybridge: deep RC6 disabled\n");
-		return INTEL_RC6_ENABLE;
-	}
-	DRM_DEBUG_DRIVER("RC6 and deep RC6 enabled\n");
-	return (INTEL_RC6_ENABLE | INTEL_RC6p_ENABLE);
-}
-
-void gen6_enable_rps(struct drm_i915_private *dev_priv)
-{
-	u32 rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
-	u32 gt_perf_status = I915_READ(GEN6_GT_PERF_STATUS);
-	u32 pcu_mbox, rc6_mask = 0;
-	u32 gtfifodbg;
-	int cur_freq, min_freq, max_freq;
-	int rc6_mode;
-	int i;
-
-	/* Here begins a magic sequence of register writes to enable
-	 * auto-downclocking.
-	 *
-	 * Perhaps there might be some value in exposing these to
-	 * userspace...
-	 */
-	I915_WRITE(GEN6_RC_STATE, 0);
-	mutex_lock(&dev_priv->dev->struct_mutex);
-
-	/* Clear the DBG now so we don't confuse earlier errors */
-	if ((gtfifodbg = I915_READ(GTFIFODBG))) {
-		DRM_ERROR("GT fifo had a previous error %x\n", gtfifodbg);
-		I915_WRITE(GTFIFODBG, gtfifodbg);
-	}
-
-	gen6_gt_force_wake_get(dev_priv);
-
-	/* disable the counters and set deterministic thresholds */
-	I915_WRITE(GEN6_RC_CONTROL, 0);
-
-	I915_WRITE(GEN6_RC1_WAKE_RATE_LIMIT, 1000 << 16);
-	I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 40 << 16 | 30);
-	I915_WRITE(GEN6_RC6pp_WAKE_RATE_LIMIT, 30);
-	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000);
-	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25);
-
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		I915_WRITE(RING_MAX_IDLE(dev_priv->ring[i].mmio_base), 10);
-
-	I915_WRITE(GEN6_RC_SLEEP, 0);
-	I915_WRITE(GEN6_RC1e_THRESHOLD, 1000);
-	I915_WRITE(GEN6_RC6_THRESHOLD, 50000);
-	I915_WRITE(GEN6_RC6p_THRESHOLD, 100000);
-	I915_WRITE(GEN6_RC6pp_THRESHOLD, 64000); /* unused */
-
-	rc6_mode = intel_enable_rc6(dev_priv->dev);
-	if (rc6_mode & INTEL_RC6_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6_ENABLE;
-
-	if (rc6_mode & INTEL_RC6p_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6p_ENABLE;
-
-	if (rc6_mode & INTEL_RC6pp_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6pp_ENABLE;
-
-	DRM_INFO("Enabling RC6 states: RC6 %s, RC6p %s, RC6pp %s\n",
-			(rc6_mode & INTEL_RC6_ENABLE) ? "on" : "off",
-			(rc6_mode & INTEL_RC6p_ENABLE) ? "on" : "off",
-			(rc6_mode & INTEL_RC6pp_ENABLE) ? "on" : "off");
-
-	I915_WRITE(GEN6_RC_CONTROL,
-		   rc6_mask |
-		   GEN6_RC_CTL_EI_MODE(1) |
-		   GEN6_RC_CTL_HW_ENABLE);
-
-	I915_WRITE(GEN6_RPNSWREQ,
-		   GEN6_FREQUENCY(10) |
-		   GEN6_OFFSET(0) |
-		   GEN6_AGGRESSIVE_TURBO);
-	I915_WRITE(GEN6_RC_VIDEO_FREQ,
-		   GEN6_FREQUENCY(12));
-
-	I915_WRITE(GEN6_RP_DOWN_TIMEOUT, 1000000);
-	I915_WRITE(GEN6_RP_INTERRUPT_LIMITS,
-		   18 << 24 |
-		   6 << 16);
-	I915_WRITE(GEN6_RP_UP_THRESHOLD, 10000);
-	I915_WRITE(GEN6_RP_DOWN_THRESHOLD, 1000000);
-	I915_WRITE(GEN6_RP_UP_EI, 100000);
-	I915_WRITE(GEN6_RP_DOWN_EI, 5000000);
-	I915_WRITE(GEN6_RP_IDLE_HYSTERSIS, 10);
-	I915_WRITE(GEN6_RP_CONTROL,
-		   GEN6_RP_MEDIA_TURBO |
-		   GEN6_RP_MEDIA_HW_MODE |
-		   GEN6_RP_MEDIA_IS_GFX |
-		   GEN6_RP_ENABLE |
-		   GEN6_RP_UP_BUSY_AVG |
-		   GEN6_RP_DOWN_IDLE_CONT);
-
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
-
-	I915_WRITE(GEN6_PCODE_DATA, 0);
-	I915_WRITE(GEN6_PCODE_MAILBOX,
-		   GEN6_PCODE_READY |
-		   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
-
-	min_freq = (rp_state_cap & 0xff0000) >> 16;
-	max_freq = rp_state_cap & 0xff;
-	cur_freq = (gt_perf_status & 0xff00) >> 8;
-
-	/* Check for overclock support */
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
-	I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_READ_OC_PARAMS);
-	pcu_mbox = I915_READ(GEN6_PCODE_DATA);
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
-	if (pcu_mbox & (1<<31)) { /* OC supported */
-		max_freq = pcu_mbox & 0xff;
-		DRM_DEBUG_DRIVER("overclocking supported, adjusting frequency max to %dMHz\n", pcu_mbox * 50);
-	}
-
-	/* In units of 100MHz */
-	dev_priv->max_delay = max_freq;
-	dev_priv->min_delay = min_freq;
-	dev_priv->cur_delay = cur_freq;
-
-	/* requires MSI enabled */
-	I915_WRITE(GEN6_PMIER,
-		   GEN6_PM_MBOX_EVENT |
-		   GEN6_PM_THERMAL_EVENT |
-		   GEN6_PM_RP_DOWN_TIMEOUT |
-		   GEN6_PM_RP_UP_THRESHOLD |
-		   GEN6_PM_RP_DOWN_THRESHOLD |
-		   GEN6_PM_RP_UP_EI_EXPIRED |
-		   GEN6_PM_RP_DOWN_EI_EXPIRED);
-	spin_lock_irq(&dev_priv->rps_lock);
-	WARN_ON(dev_priv->pm_iir != 0);
-	I915_WRITE(GEN6_PMIMR, 0);
-	spin_unlock_irq(&dev_priv->rps_lock);
-	/* enable all PM interrupts */
-	I915_WRITE(GEN6_PMINTRMSK, 0);
-
-	gen6_gt_force_wake_put(dev_priv);
-	mutex_unlock(&dev_priv->dev->struct_mutex);
-}
-
-void gen6_update_ring_freq(struct drm_i915_private *dev_priv)
-{
-	int min_freq = 15;
-	int gpu_freq, ia_freq, max_ia_freq;
-	int scaling_factor = 180;
-
-	max_ia_freq = cpufreq_quick_get_max(0);
-	/*
-	 * Default to measured freq if none found, PCU will ensure we don't go
-	 * over
-	 */
-	if (!max_ia_freq)
-		max_ia_freq = tsc_khz;
-
-	/* Convert from kHz to MHz */
-	max_ia_freq /= 1000;
-
-	mutex_lock(&dev_priv->dev->struct_mutex);
-
-	/*
-	 * For each potential GPU frequency, load a ring frequency we'd like
-	 * to use for memory access.  We do this by specifying the IA frequency
-	 * the PCU should use as a reference to determine the ring frequency.
-	 */
-	for (gpu_freq = dev_priv->max_delay; gpu_freq >= dev_priv->min_delay;
-	     gpu_freq--) {
-		int diff = dev_priv->max_delay - gpu_freq;
-
-		/*
-		 * For GPU frequencies less than 750MHz, just use the lowest
-		 * ring freq.
-		 */
-		if (gpu_freq < min_freq)
-			ia_freq = 800;
-		else
-			ia_freq = max_ia_freq - ((diff * scaling_factor) / 2);
-		ia_freq = DIV_ROUND_CLOSEST(ia_freq, 100);
-
-		I915_WRITE(GEN6_PCODE_DATA,
-			   (ia_freq << GEN6_PCODE_FREQ_IA_RATIO_SHIFT) |
-			   gpu_freq);
-		I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_PCODE_READY |
-			   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
-		if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) &
-			      GEN6_PCODE_READY) == 0, 10)) {
-			DRM_ERROR("pcode write of freq table timed out\n");
-			continue;
-		}
-	}
-
-	mutex_unlock(&dev_priv->dev->struct_mutex);
-}
-
-static void ironlake_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
-
-	/* Required for FBC */
-	dspclk_gate |= DPFCUNIT_CLOCK_GATE_DISABLE |
-		DPFCRUNIT_CLOCK_GATE_DISABLE |
-		DPFDUNIT_CLOCK_GATE_DISABLE;
-	/* Required for CxSR */
-	dspclk_gate |= DPARBUNIT_CLOCK_GATE_DISABLE;
-
-	I915_WRITE(PCH_3DCGDIS0,
-		   MARIUNIT_CLOCK_GATE_DISABLE |
-		   SVSMUNIT_CLOCK_GATE_DISABLE);
-	I915_WRITE(PCH_3DCGDIS1,
-		   VFMUNIT_CLOCK_GATE_DISABLE);
-
-	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
-
-	/*
-	 * According to the spec the following bits should be set in
-	 * order to enable memory self-refresh
-	 * The bit 22/21 of 0x42004
-	 * The bit 5 of 0x42020
-	 * The bit 15 of 0x45000
-	 */
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   (I915_READ(ILK_DISPLAY_CHICKEN2) |
-		    ILK_DPARB_GATE | ILK_VSDPFD_FULL));
-	I915_WRITE(ILK_DSPCLK_GATE,
-		   (I915_READ(ILK_DSPCLK_GATE) |
-		    ILK_DPARB_CLK_GATE));
-	I915_WRITE(DISP_ARB_CTL,
-		   (I915_READ(DISP_ARB_CTL) |
-		    DISP_FBC_WM_DIS));
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	/*
-	 * Based on the document from hardware guys the following bits
-	 * should be set unconditionally in order to enable FBC.
-	 * The bit 22 of 0x42000
-	 * The bit 22 of 0x42004
-	 * The bit 7,8,9 of 0x42020.
-	 */
-	if (IS_IRONLAKE_M(dev)) {
-		I915_WRITE(ILK_DISPLAY_CHICKEN1,
-			   I915_READ(ILK_DISPLAY_CHICKEN1) |
-			   ILK_FBCQ_DIS);
-		I915_WRITE(ILK_DISPLAY_CHICKEN2,
-			   I915_READ(ILK_DISPLAY_CHICKEN2) |
-			   ILK_DPARB_GATE);
-		I915_WRITE(ILK_DSPCLK_GATE,
-			   I915_READ(ILK_DSPCLK_GATE) |
-			   ILK_DPFC_DIS1 |
-			   ILK_DPFC_DIS2 |
-			   ILK_CLK_FBC);
-	}
-
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   I915_READ(ILK_DISPLAY_CHICKEN2) |
-		   ILK_ELPIN_409_SELECT);
-	I915_WRITE(_3D_CHICKEN2,
-		   _3D_CHICKEN2_WM_READ_PIPELINED << 16 |
-		   _3D_CHICKEN2_WM_READ_PIPELINED);
-}
-
-static void gen6_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
-	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
-
-	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
-
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   I915_READ(ILK_DISPLAY_CHICKEN2) |
-		   ILK_ELPIN_409_SELECT);
-
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	I915_WRITE(GEN6_UCGCTL1,
-		   I915_READ(GEN6_UCGCTL1) |
-		   GEN6_BLBUNIT_CLOCK_GATE_DISABLE);
-
-	/* According to the BSpec vol1g, bit 12 (RCPBUNIT) clock
-	 * gating disable must be set.  Failure to set it results in
-	 * flickering pixels due to Z write ordering failures after
-	 * some amount of runtime in the Mesa "fire" demo, and Unigine
-	 * Sanctuary and Tropics, and apparently anything else with
-	 * alpha test or pixel discard.
-	 *
-	 * According to the spec, bit 11 (RCCUNIT) must also be set,
-	 * but we didn't debug actual testcases to find it out.
-	 */
-	I915_WRITE(GEN6_UCGCTL2,
-		   GEN6_RCPBUNIT_CLOCK_GATE_DISABLE |
-		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
-
-	/*
-	 * According to the spec the following bits should be
-	 * set in order to enable memory self-refresh and fbc:
-	 * The bit21 and bit22 of 0x42000
-	 * The bit21 and bit22 of 0x42004
-	 * The bit5 and bit7 of 0x42020
-	 * The bit14 of 0x70180
-	 * The bit14 of 0x71180
-	 */
-	I915_WRITE(ILK_DISPLAY_CHICKEN1,
-		   I915_READ(ILK_DISPLAY_CHICKEN1) |
-		   ILK_FBCQ_DIS | ILK_PABSTRETCH_DIS);
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   I915_READ(ILK_DISPLAY_CHICKEN2) |
-		   ILK_DPARB_GATE | ILK_VSDPFD_FULL);
-	I915_WRITE(ILK_DSPCLK_GATE,
-		   I915_READ(ILK_DSPCLK_GATE) |
-		   ILK_DPARB_CLK_GATE  |
-		   ILK_DPFD_CLK_GATE);
-
-	for_each_pipe(pipe) {
-		I915_WRITE(DSPCNTR(pipe),
-			   I915_READ(DSPCNTR(pipe)) |
-			   DISPPLANE_TRICKLE_FEED_DISABLE);
-		intel_flush_display_plane(dev_priv, pipe);
-	}
-}
-
-static void ivybridge_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
-	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
-
-	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
-
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	/* According to the spec, bit 13 (RCZUNIT) must be set on IVB.
-	 * This implements the WaDisableRCZUnitClockGating workaround.
-	 */
-	I915_WRITE(GEN6_UCGCTL2, GEN6_RCZUNIT_CLOCK_GATE_DISABLE);
-
-	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
-
-	I915_WRITE(IVB_CHICKEN3,
-		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
-		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
-
-	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
-	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
-		   GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC);
-
-	/* WaApplyL3ControlAndL3ChickenMode requires those two on Ivy Bridge */
-	I915_WRITE(GEN7_L3CNTLREG1,
-			GEN7_WA_FOR_GEN7_L3_CONTROL);
-	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER,
-			GEN7_WA_L3_CHICKEN_MODE);
-
-	/* This is required by WaCatErrorRejectionIssue */
-	I915_WRITE(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG,
-			I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
-			GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
-
-	for_each_pipe(pipe) {
-		I915_WRITE(DSPCNTR(pipe),
-			   I915_READ(DSPCNTR(pipe)) |
-			   DISPPLANE_TRICKLE_FEED_DISABLE);
-		intel_flush_display_plane(dev_priv, pipe);
-	}
-}
-
-static void g4x_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dspclk_gate;
-
-	I915_WRITE(RENCLK_GATE_D1, 0);
-	I915_WRITE(RENCLK_GATE_D2, VF_UNIT_CLOCK_GATE_DISABLE |
-		   GS_UNIT_CLOCK_GATE_DISABLE |
-		   CL_UNIT_CLOCK_GATE_DISABLE);
-	I915_WRITE(RAMCLK_GATE_D, 0);
-	dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE |
-		OVRUNIT_CLOCK_GATE_DISABLE |
-		OVCUNIT_CLOCK_GATE_DISABLE;
-	if (IS_GM45(dev))
-		dspclk_gate |= DSSUNIT_CLOCK_GATE_DISABLE;
-	I915_WRITE(DSPCLK_GATE_D, dspclk_gate);
-}
-
-static void crestline_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(RENCLK_GATE_D1, I965_RCC_CLOCK_GATE_DISABLE);
-	I915_WRITE(RENCLK_GATE_D2, 0);
-	I915_WRITE(DSPCLK_GATE_D, 0);
-	I915_WRITE(RAMCLK_GATE_D, 0);
-	I915_WRITE16(DEUC, 0);
-}
-
-static void broadwater_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(RENCLK_GATE_D1, I965_RCZ_CLOCK_GATE_DISABLE |
-		   I965_RCC_CLOCK_GATE_DISABLE |
-		   I965_RCPB_CLOCK_GATE_DISABLE |
-		   I965_ISC_CLOCK_GATE_DISABLE |
-		   I965_FBC_CLOCK_GATE_DISABLE);
-	I915_WRITE(RENCLK_GATE_D2, 0);
-}
-
-static void gen3_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dstate = I915_READ(D_STATE);
-
-	dstate |= DSTATE_PLL_D3_OFF | DSTATE_GFX_CLOCK_GATING |
-		DSTATE_DOT_CLOCK_GATING;
-	I915_WRITE(D_STATE, dstate);
-}
-
-static void i85x_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(RENCLK_GATE_D1, SV_CLOCK_GATE_DISABLE);
-}
-
-static void i830_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(DSPCLK_GATE_D, OVRUNIT_CLOCK_GATE_DISABLE);
-}
-
-static void ibx_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	/*
-	 * On Ibex Peak and Cougar Point, we need to disable clock
-	 * gating for the panel power sequencer or it will fail to
-	 * start up when no ports are active.
-	 */
-	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
-}
-
-static void cpt_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
-
-	/*
-	 * On Ibex Peak and Cougar Point, we need to disable clock
-	 * gating for the panel power sequencer or it will fail to
-	 * start up when no ports are active.
-	 */
-	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
-	I915_WRITE(SOUTH_CHICKEN2, I915_READ(SOUTH_CHICKEN2) |
-		   DPLS_EDP_PPS_FIX_DIS);
-	/* Without this, mode sets may fail silently on FDI */
-	for_each_pipe(pipe)
-		I915_WRITE(TRANS_CHICKEN2(pipe), TRANS_AUTOTRAIN_GEN_STALL_DIS);
-}
-
-static void ironlake_teardown_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->renderctx) {
-		i915_gem_object_unpin(dev_priv->renderctx);
-		drm_gem_object_unreference(&dev_priv->renderctx->base);
-		dev_priv->renderctx = NULL;
-	}
-
-	if (dev_priv->pwrctx) {
-		i915_gem_object_unpin(dev_priv->pwrctx);
-		drm_gem_object_unreference(&dev_priv->pwrctx->base);
-		dev_priv->pwrctx = NULL;
-	}
-}
-
-static void ironlake_disable_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (I915_READ(PWRCTXA)) {
-		/* Wake the GPU, prevent RC6, then restore RSTDBYCTL */
-		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) | RCX_SW_EXIT);
-		wait_for(((I915_READ(RSTDBYCTL) & RSX_STATUS_MASK) == RSX_STATUS_ON),
-			 50);
-
-		I915_WRITE(PWRCTXA, 0);
-		POSTING_READ(PWRCTXA);
-
-		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
-		POSTING_READ(RSTDBYCTL);
-	}
-
-	ironlake_teardown_rc6(dev);
-}
-
-static int ironlake_setup_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->renderctx == NULL)
-		dev_priv->renderctx = intel_alloc_context_page(dev);
-	if (!dev_priv->renderctx)
-		return -ENOMEM;
-
-	if (dev_priv->pwrctx == NULL)
-		dev_priv->pwrctx = intel_alloc_context_page(dev);
-	if (!dev_priv->pwrctx) {
-		ironlake_teardown_rc6(dev);
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
-void ironlake_enable_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
-
-	/* rc6 disabled by default due to repeated reports of hanging during
-	 * boot and resume.
-	 */
-	if (!intel_enable_rc6(dev))
-		return;
-
-	mutex_lock(&dev->struct_mutex);
-	ret = ironlake_setup_rc6(dev);
-	if (ret) {
-		mutex_unlock(&dev->struct_mutex);
-		return;
-	}
-
-	/*
-	 * GPU can automatically power down the render unit if given a page
-	 * to save state.
-	 */
-	ret = BEGIN_LP_RING(6);
-	if (ret) {
-		ironlake_teardown_rc6(dev);
-		mutex_unlock(&dev->struct_mutex);
-		return;
-	}
-
-	OUT_RING(MI_SUSPEND_FLUSH | MI_SUSPEND_FLUSH_EN);
-	OUT_RING(MI_SET_CONTEXT);
-	OUT_RING(dev_priv->renderctx->gtt_offset |
-		 MI_MM_SPACE_GTT |
-		 MI_SAVE_EXT_STATE_EN |
-		 MI_RESTORE_EXT_STATE_EN |
-		 MI_RESTORE_INHIBIT);
-	OUT_RING(MI_SUSPEND_FLUSH);
-	OUT_RING(MI_NOOP);
-	OUT_RING(MI_FLUSH);
-	ADVANCE_LP_RING();
-
-	/*
-	 * Wait for the command parser to advance past MI_SET_CONTEXT. The HW
-	 * does an implicit flush, combined with MI_FLUSH above, it should be
-	 * safe to assume that renderctx is valid
-	 */
-	ret = intel_wait_ring_idle(LP_RING(dev_priv));
-	if (ret) {
-		DRM_ERROR("failed to enable ironlake power power savings\n");
-		ironlake_teardown_rc6(dev);
-		mutex_unlock(&dev->struct_mutex);
-		return;
-	}
-
-	I915_WRITE(PWRCTXA, dev_priv->pwrctx->gtt_offset | PWRCTX_EN);
-	I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
-	mutex_unlock(&dev->struct_mutex);
-}
-
-void intel_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	dev_priv->display.init_clock_gating(dev);
-
-	if (dev_priv->display.init_pch_clock_gating)
-		dev_priv->display.init_pch_clock_gating(dev);
-}
-
 /* Set up chip specific display functions */
 static void intel_init_display(struct drm_device *dev)
 {
@@ -8887,32 +6699,20 @@ static void intel_init_display(struct drm_device *dev)
 	if (HAS_PCH_SPLIT(dev)) {
 		dev_priv->display.dpms = ironlake_crtc_dpms;
 		dev_priv->display.crtc_mode_set = ironlake_crtc_mode_set;
+		dev_priv->display.off = ironlake_crtc_off;
 		dev_priv->display.update_plane = ironlake_update_plane;
 	} else {
 		dev_priv->display.dpms = i9xx_crtc_dpms;
 		dev_priv->display.crtc_mode_set = i9xx_crtc_mode_set;
+		dev_priv->display.off = i9xx_crtc_off;
 		dev_priv->display.update_plane = i9xx_update_plane;
 	}
 
-	if (I915_HAS_FBC(dev)) {
-		if (HAS_PCH_SPLIT(dev)) {
-			dev_priv->display.fbc_enabled = ironlake_fbc_enabled;
-			dev_priv->display.enable_fbc = ironlake_enable_fbc;
-			dev_priv->display.disable_fbc = ironlake_disable_fbc;
-		} else if (IS_GM45(dev)) {
-			dev_priv->display.fbc_enabled = g4x_fbc_enabled;
-			dev_priv->display.enable_fbc = g4x_enable_fbc;
-			dev_priv->display.disable_fbc = g4x_disable_fbc;
-		} else if (IS_CRESTLINE(dev)) {
-			dev_priv->display.fbc_enabled = i8xx_fbc_enabled;
-			dev_priv->display.enable_fbc = i8xx_enable_fbc;
-			dev_priv->display.disable_fbc = i8xx_disable_fbc;
-		}
-		/* 855GM needs testing */
-	}
-
 	/* Returns the core display clock speed */
-	if (IS_I945G(dev) || (IS_G33(dev) && !IS_PINEVIEW_M(dev)))
+	if (IS_VALLEYVIEW(dev))
+		dev_priv->display.get_display_clock_speed =
+			valleyview_get_display_clock_speed;
+	else if (IS_I945G(dev) || (IS_G33(dev) && !IS_PINEVIEW_M(dev)))
 		dev_priv->display.get_display_clock_speed =
 			i945_get_display_clock_speed;
 	else if (IS_I915G(dev))
@@ -8934,124 +6734,27 @@ static void intel_init_display(struct drm_device *dev)
 		dev_priv->display.get_display_clock_speed =
 			i830_get_display_clock_speed;
 
-	/* For FIFO watermark updates */
 	if (HAS_PCH_SPLIT(dev)) {
-		dev_priv->display.force_wake_get = __gen6_gt_force_wake_get;
-		dev_priv->display.force_wake_put = __gen6_gt_force_wake_put;
-
-		/* IVB configs may use multi-threaded forcewake */
-		if (IS_IVYBRIDGE(dev)) {
-			u32	ecobus;
-
-			/* A small trick here - if the bios hasn't configured MT forcewake,
-			 * and if the device is in RC6, then force_wake_mt_get will not wake
-			 * the device and the ECOBUS read will return zero. Which will be
-			 * (correctly) interpreted by the test below as MT forcewake being
-			 * disabled.
-			 */
-			mutex_lock(&dev->struct_mutex);
-			__gen6_gt_force_wake_mt_get(dev_priv);
-			ecobus = I915_READ_NOTRACE(ECOBUS);
-			__gen6_gt_force_wake_mt_put(dev_priv);
-			mutex_unlock(&dev->struct_mutex);
-
-			if (ecobus & FORCEWAKE_MT_ENABLE) {
-				DRM_DEBUG_KMS("Using MT version of forcewake\n");
-				dev_priv->display.force_wake_get =
-					__gen6_gt_force_wake_mt_get;
-				dev_priv->display.force_wake_put =
-					__gen6_gt_force_wake_mt_put;
-			}
-		}
-
-		if (HAS_PCH_IBX(dev))
-			dev_priv->display.init_pch_clock_gating = ibx_init_clock_gating;
-		else if (HAS_PCH_CPT(dev))
-			dev_priv->display.init_pch_clock_gating = cpt_init_clock_gating;
-
 		if (IS_GEN5(dev)) {
-			if (I915_READ(MLTR_ILK) & ILK_SRLT_MASK)
-				dev_priv->display.update_wm = ironlake_update_wm;
-			else {
-				DRM_DEBUG_KMS("Failed to get proper latency. "
-					      "Disable CxSR\n");
-				dev_priv->display.update_wm = NULL;
-			}
 			dev_priv->display.fdi_link_train = ironlake_fdi_link_train;
-			dev_priv->display.init_clock_gating = ironlake_init_clock_gating;
 			dev_priv->display.write_eld = ironlake_write_eld;
 		} else if (IS_GEN6(dev)) {
-			if (SNB_READ_WM0_LATENCY()) {
-				dev_priv->display.update_wm = sandybridge_update_wm;
-				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
-			} else {
-				DRM_DEBUG_KMS("Failed to read display plane latency. "
-					      "Disable CxSR\n");
-				dev_priv->display.update_wm = NULL;
-			}
 			dev_priv->display.fdi_link_train = gen6_fdi_link_train;
-			dev_priv->display.init_clock_gating = gen6_init_clock_gating;
 			dev_priv->display.write_eld = ironlake_write_eld;
 		} else if (IS_IVYBRIDGE(dev)) {
 			/* FIXME: detect B0+ stepping and use auto training */
 			dev_priv->display.fdi_link_train = ivb_manual_fdi_link_train;
-			if (SNB_READ_WM0_LATENCY()) {
-				dev_priv->display.update_wm = sandybridge_update_wm;
-				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
-			} else {
-				DRM_DEBUG_KMS("Failed to read display plane latency. "
-					      "Disable CxSR\n");
-				dev_priv->display.update_wm = NULL;
-			}
-			dev_priv->display.init_clock_gating = ivybridge_init_clock_gating;
+			dev_priv->display.write_eld = ironlake_write_eld;
+		} else if (IS_HASWELL(dev)) {
+			dev_priv->display.fdi_link_train = hsw_fdi_link_train;
 			dev_priv->display.write_eld = ironlake_write_eld;
 		} else
 			dev_priv->display.update_wm = NULL;
-	} else if (IS_PINEVIEW(dev)) {
-		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev),
-					    dev_priv->is_ddr3,
-					    dev_priv->fsb_freq,
-					    dev_priv->mem_freq)) {
-			DRM_INFO("failed to find known CxSR latency "
-				 "(found ddr%s fsb freq %d, mem freq %d), "
-				 "disabling CxSR\n",
-				 (dev_priv->is_ddr3 == 1) ? "3" : "2",
-				 dev_priv->fsb_freq, dev_priv->mem_freq);
-			/* Disable CxSR and never update its watermark again */
-			pineview_disable_cxsr(dev);
-			dev_priv->display.update_wm = NULL;
-		} else
-			dev_priv->display.update_wm = pineview_update_wm;
-		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
+	} else if (IS_VALLEYVIEW(dev)) {
+		dev_priv->display.force_wake_get = vlv_force_wake_get;
+		dev_priv->display.force_wake_put = vlv_force_wake_put;
 	} else if (IS_G4X(dev)) {
 		dev_priv->display.write_eld = g4x_write_eld;
-		dev_priv->display.update_wm = g4x_update_wm;
-		dev_priv->display.init_clock_gating = g4x_init_clock_gating;
-	} else if (IS_GEN4(dev)) {
-		dev_priv->display.update_wm = i965_update_wm;
-		if (IS_CRESTLINE(dev))
-			dev_priv->display.init_clock_gating = crestline_init_clock_gating;
-		else if (IS_BROADWATER(dev))
-			dev_priv->display.init_clock_gating = broadwater_init_clock_gating;
-	} else if (IS_GEN3(dev)) {
-		dev_priv->display.update_wm = i9xx_update_wm;
-		dev_priv->display.get_fifo_size = i9xx_get_fifo_size;
-		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
-	} else if (IS_I865G(dev)) {
-		dev_priv->display.update_wm = i830_update_wm;
-		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
-		dev_priv->display.get_fifo_size = i830_get_fifo_size;
-	} else if (IS_I85X(dev)) {
-		dev_priv->display.update_wm = i9xx_update_wm;
-		dev_priv->display.get_fifo_size = i85x_get_fifo_size;
-		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
-	} else {
-		dev_priv->display.update_wm = i830_update_wm;
-		dev_priv->display.init_clock_gating = i830_init_clock_gating;
-		if (IS_845G(dev))
-			dev_priv->display.get_fifo_size = i845_get_fifo_size;
-		else
-			dev_priv->display.get_fifo_size = i830_get_fifo_size;
 	}
 
 	/* Default just returns -ENODEV to indicate unsupported */
@@ -9090,7 +6793,7 @@ static void quirk_pipea_force(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	dev_priv->quirks |= QUIRK_PIPEA_FORCE;
-	DRM_DEBUG_DRIVER("applying pipe a force quirk\n");
+	DRM_INFO("applying pipe a force quirk\n");
 }
 
 /*
@@ -9100,6 +6803,18 @@ static void quirk_ssc_force_disable(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	dev_priv->quirks |= QUIRK_LVDS_SSC_DISABLE;
+	DRM_INFO("applying lvds SSC disable quirk\n");
+}
+
+/*
+ * A machine (e.g. Acer Aspire 5734Z) may need to invert the panel backlight
+ * brightness value
+ */
+static void quirk_invert_brightness(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	dev_priv->quirks |= QUIRK_INVERT_BRIGHTNESS;
+	DRM_INFO("applying inverted panel brightness quirk\n");
 }
 
 struct intel_quirk {
@@ -9109,7 +6824,7 @@ struct intel_quirk {
 	void (*hook)(struct drm_device *dev);
 };
 
-struct intel_quirk intel_quirks[] = {
+static struct intel_quirk intel_quirks[] = {
 	/* HP Mini needs pipe A force quirk (LP: #322104) */
 	{ 0x27ae, 0x103c, 0x361a, quirk_pipea_force },
 
@@ -9134,6 +6849,9 @@ struct intel_quirk intel_quirks[] = {
 
 	/* Sony Vaio Y cannot use SSC on LVDS */
 	{ 0x0046, 0x104d, 0x9076, quirk_ssc_force_disable },
+
+	/* Acer Aspire 5734Z must invert backlight brightness */
+	{ 0x2a42, 0x1025, 0x0459, quirk_invert_brightness },
 };
 
 static void intel_init_quirks(struct drm_device *dev)
@@ -9166,7 +6884,7 @@ static void i915_disable_vga(struct drm_device *dev)
 		vga_reg = VGACNTRL;
 
 	vga_get_uninterruptible(dev->pdev, VGA_RSRC_LEGACY_IO);
-	outb(1, VGA_SR_INDEX);
+	outb(SR01, VGA_SR_INDEX);
 	sr1 = inb(VGA_SR_DATA);
 	outb(sr1 | 1<<5, VGA_SR_DATA);
 	vga_put(dev->pdev, VGA_RSRC_LEGACY_IO);
@@ -9176,6 +6894,40 @@ static void i915_disable_vga(struct drm_device *dev)
 	POSTING_READ(vga_reg);
 }
 
+static void ivb_pch_pwm_override(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/*
+	 * IVB has CPU eDP backlight regs too, set things up to let the
+	 * PCH regs control the backlight
+	 */
+	I915_WRITE(BLC_PWM_CPU_CTL2, PWM_ENABLE);
+	I915_WRITE(BLC_PWM_CPU_CTL, 0);
+	I915_WRITE(BLC_PWM_PCH_CTL1, PWM_ENABLE | (1<<30));
+}
+
+void intel_modeset_init_hw(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	intel_init_clock_gating(dev);
+
+	if (IS_IRONLAKE_M(dev)) {
+		ironlake_enable_drps(dev);
+		ironlake_enable_rc6(dev);
+		intel_init_emon(dev);
+	}
+
+	if ((IS_GEN6(dev) || IS_GEN7(dev)) && !IS_VALLEYVIEW(dev)) {
+		gen6_enable_rps(dev_priv);
+		gen6_update_ring_freq(dev_priv);
+	}
+
+	if (IS_IVYBRIDGE(dev))
+		ivb_pch_pwm_override(dev);
+}
+
 void intel_modeset_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -9189,10 +6941,14 @@ void intel_modeset_init(struct drm_device *dev)
 	dev->mode_config.preferred_depth = 24;
 	dev->mode_config.prefer_shadow = 1;
 
-	dev->mode_config.funcs = (void *)&intel_mode_funcs;
+	dev->mode_config.funcs = &intel_mode_funcs;
 
 	intel_init_quirks(dev);
 
+	intel_init_pm(dev);
+
+	intel_prepare_ddi(dev);
+
 	intel_init_display(dev);
 
 	if (IS_GEN2(dev)) {
@@ -9217,22 +6973,12 @@ void intel_modeset_init(struct drm_device *dev)
 			DRM_DEBUG_KMS("plane %d init failed: %d\n", i, ret);
 	}
 
+	intel_pch_pll_init(dev);
+
 	/* Just disable it once at startup */
 	i915_disable_vga(dev);
 	intel_setup_outputs(dev);
 
-	intel_init_clock_gating(dev);
-
-	if (IS_IRONLAKE_M(dev)) {
-		ironlake_enable_drps(dev);
-		intel_init_emon(dev);
-	}
-
-	if (IS_GEN6(dev) || IS_GEN7(dev)) {
-		gen6_enable_rps(dev_priv);
-		gen6_update_ring_freq(dev_priv);
-	}
-
 	INIT_WORK(&dev_priv->idle_work, intel_idle_update);
 	setup_timer(&dev_priv->idle_timer, intel_gpu_idle_timer,
 		    (unsigned long)dev);
@@ -9240,8 +6986,7 @@ void intel_modeset_init(struct drm_device *dev)
 
 void intel_modeset_gem_init(struct drm_device *dev)
 {
-	if (IS_IRONLAKE_M(dev))
-		ironlake_enable_rc6(dev);
+	intel_modeset_init_hw(dev);
 
 	intel_setup_overlay(dev);
 }
@@ -9271,12 +7016,15 @@ void intel_modeset_cleanup(struct drm_device *dev)
 
 	if (IS_IRONLAKE_M(dev))
 		ironlake_disable_drps(dev);
-	if (IS_GEN6(dev) || IS_GEN7(dev))
+	if ((IS_GEN6(dev) || IS_GEN7(dev)) && !IS_VALLEYVIEW(dev))
 		gen6_disable_rps(dev);
 
 	if (IS_IRONLAKE_M(dev))
 		ironlake_disable_rc6(dev);
 
+	if (IS_VALLEYVIEW(dev))
+		vlv_init_dpio(dev);
+
 	mutex_unlock(&dev->struct_mutex);
 
 	/* Disable the irq before mode object teardown, for the irq might
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 4b637919f74f..71c7096e3869 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -688,7 +688,7 @@ intel_dp_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
 	int lane_count, clock;
 	int max_lane_count = intel_dp_max_lane_count(intel_dp);
 	int max_clock = intel_dp_max_link_bw(intel_dp) == DP_LINK_BW_2_7 ? 1 : 0;
-	int bpp;
+	int bpp, mode_rate;
 	static int bws[2] = { DP_LINK_BW_1_62, DP_LINK_BW_2_7 };
 
 	if (is_edp(intel_dp) && intel_dp->panel_fixed_mode) {
@@ -702,24 +702,30 @@ intel_dp_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		mode->clock = intel_dp->panel_fixed_mode->clock;
 	}
 
+	DRM_DEBUG_KMS("DP link computation with max lane count %i "
+		      "max bw %02x pixel clock %iKHz\n",
+		      max_lane_count, bws[max_clock], mode->clock);
+
 	if (!intel_dp_adjust_dithering(intel_dp, mode, adjusted_mode))
 		return false;
 
 	bpp = adjusted_mode->private_flags & INTEL_MODE_DP_FORCE_6BPC ? 18 : 24;
+	mode_rate = intel_dp_link_required(mode->clock, bpp);
 
 	for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
 		for (clock = 0; clock <= max_clock; clock++) {
 			int link_avail = intel_dp_max_data_rate(intel_dp_link_clock(bws[clock]), lane_count);
 
-			if (intel_dp_link_required(mode->clock, bpp)
-					<= link_avail) {
+			if (mode_rate <= link_avail) {
 				intel_dp->link_bw = bws[clock];
 				intel_dp->lane_count = lane_count;
 				adjusted_mode->clock = intel_dp_link_clock(intel_dp->link_bw);
-				DRM_DEBUG_KMS("Display port link bw %02x lane "
-						"count %d clock %d\n",
+				DRM_DEBUG_KMS("DP link bw %02x lane "
+						"count %d clock %d bpp %d\n",
 				       intel_dp->link_bw, intel_dp->lane_count,
-				       adjusted_mode->clock);
+				       adjusted_mode->clock, bpp);
+				DRM_DEBUG_KMS("DP link bw required %i available %i\n",
+					      mode_rate, link_avail);
 				return true;
 			}
 		}
@@ -1149,6 +1155,7 @@ static void ironlake_edp_panel_off(struct intel_dp *intel_dp)
 	DRM_DEBUG_KMS("Turn eDP power off\n");
 
 	WARN(intel_dp->want_panel_vdd, "Cannot turn power off while VDD is on\n");
+	ironlake_panel_vdd_off_sync(intel_dp); /* finish any pending work */
 
 	pp = ironlake_get_pp_control(dev_priv);
 	pp &= ~(POWER_TARGET_ON | EDP_FORCE_VDD | PANEL_POWER_RESET | EDP_BLC_ENABLE);
@@ -1954,6 +1961,23 @@ intel_dp_get_dpcd(struct intel_dp *intel_dp)
 	return false;
 }
 
+static void
+intel_dp_probe_oui(struct intel_dp *intel_dp)
+{
+	u8 buf[3];
+
+	if (!(intel_dp->dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
+		return;
+
+	if (intel_dp_aux_native_read_retry(intel_dp, DP_SINK_OUI, buf, 3))
+		DRM_DEBUG_KMS("Sink OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+
+	if (intel_dp_aux_native_read_retry(intel_dp, DP_BRANCH_OUI, buf, 3))
+		DRM_DEBUG_KMS("Branch OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+}
+
 static bool
 intel_dp_get_sink_irq(struct intel_dp *intel_dp, u8 *sink_irq_vector)
 {
@@ -2137,6 +2161,8 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 	if (status != connector_status_connected)
 		return status;
 
+	intel_dp_probe_oui(intel_dp);
+
 	if (intel_dp->force_audio != HDMI_AUDIO_AUTO) {
 		intel_dp->has_audio = (intel_dp->force_audio == HDMI_AUDIO_ON);
 	} else {
@@ -2438,6 +2464,7 @@ intel_dp_init(struct drm_device *dev, int output_reg)
 	}
 
 	intel_encoder->crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
+
 	connector->interlace_allowed = true;
 	connector->doublescan_allowed = 0;
 
@@ -2483,6 +2510,13 @@ intel_dp_init(struct drm_device *dev, int output_reg)
 		pp_off = I915_READ(PCH_PP_OFF_DELAYS);
 		pp_div = I915_READ(PCH_PP_DIVISOR);
 
+		if (!pp_on || !pp_off || !pp_div) {
+			DRM_INFO("bad panel power sequencing delays, disabling panel\n");
+			intel_dp_encoder_destroy(&intel_dp->base.base);
+			intel_dp_destroy(&intel_connector->base);
+			return;
+		}
+
 		/* Pull timing values out of registers */
 		cur.t1_t3 = (pp_on & PANEL_POWER_UP_DELAY_MASK) >>
 			PANEL_POWER_UP_DELAY_SHIFT;
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 715afa153025..3e0918834e7e 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -45,6 +45,18 @@
 	ret__;								\
 })
 
+#define wait_for_atomic_us(COND, US) ({ \
+	int i, ret__ = -ETIMEDOUT;	\
+	for (i = 0; i < (US); i++) {	\
+		if ((COND)) {		\
+			ret__ = 0;	\
+			break;		\
+		}			\
+		udelay(1);		\
+	}				\
+	ret__;				\
+})
+
 #define wait_for(COND, MS) _wait_for(COND, MS, 1)
 #define wait_for_atomic(COND, MS) _wait_for(COND, MS, 0)
 
@@ -171,8 +183,8 @@ struct intel_crtc {
 	bool cursor_visible;
 	unsigned int bpp;
 
-	bool no_pll; /* tertiary pipe for IVB */
-	bool use_pll_a;
+	/* We can share PLLs across outputs if the timings match */
+	struct intel_pch_pll *pch_pll;
 };
 
 struct intel_plane {
@@ -196,6 +208,25 @@ struct intel_plane {
 			     struct drm_intel_sprite_colorkey *key);
 };
 
+struct intel_watermark_params {
+	unsigned long fifo_size;
+	unsigned long max_wm;
+	unsigned long default_wm;
+	unsigned long guard_size;
+	unsigned long cacheline_size;
+};
+
+struct cxsr_latency {
+	int is_desktop;
+	int is_ddr3;
+	unsigned long fsb_freq;
+	unsigned long mem_freq;
+	unsigned long display_sr;
+	unsigned long display_hpll_disable;
+	unsigned long cursor_sr;
+	unsigned long cursor_hpll_disable;
+};
+
 #define to_intel_crtc(x) container_of(x, struct intel_crtc, base)
 #define to_intel_connector(x) container_of(x, struct intel_connector, base)
 #define to_intel_encoder(x) container_of(x, struct intel_encoder, base)
@@ -207,6 +238,8 @@ struct intel_plane {
 #define DIP_TYPE_AVI    0x82
 #define DIP_VERSION_AVI 0x2
 #define DIP_LEN_AVI     13
+#define DIP_AVI_PR_1    0
+#define DIP_AVI_PR_2    1
 
 #define DIP_TYPE_SPD	0x83
 #define DIP_VERSION_SPD	0x1
@@ -240,23 +273,36 @@ struct dip_infoframe {
 			uint8_t ITC_EC_Q_SC;
 			/* PB4 - VIC 6:0 */
 			uint8_t VIC;
-			/* PB5 - PR 3:0 */
-			uint8_t PR;
+			/* PB5 - YQ 7:6, CN 5:4, PR 3:0 */
+			uint8_t YQ_CN_PR;
 			/* PB6 to PB13 */
 			uint16_t top_bar_end;
 			uint16_t bottom_bar_start;
 			uint16_t left_bar_end;
 			uint16_t right_bar_start;
-		} avi;
+		} __attribute__ ((packed)) avi;
 		struct {
 			uint8_t vn[8];
 			uint8_t pd[16];
 			uint8_t sdi;
-		} spd;
+		} __attribute__ ((packed)) spd;
 		uint8_t payload[27];
 	} __attribute__ ((packed)) body;
 } __attribute__((packed));
 
+struct intel_hdmi {
+	struct intel_encoder base;
+	u32 sdvox_reg;
+	int ddc_bus;
+	int ddi_port;
+	uint32_t color_range;
+	bool has_hdmi_sink;
+	bool has_audio;
+	enum hdmi_force_audio force_audio;
+	void (*write_infoframe)(struct drm_encoder *encoder,
+				struct dip_infoframe *frame);
+};
+
 static inline struct drm_crtc *
 intel_get_crtc_for_pipe(struct drm_device *dev, int pipe)
 {
@@ -296,8 +342,13 @@ extern void intel_attach_broadcast_rgb_property(struct drm_connector *connector)
 
 extern void intel_crt_init(struct drm_device *dev);
 extern void intel_hdmi_init(struct drm_device *dev, int sdvox_reg);
-void intel_dip_infoframe_csum(struct dip_infoframe *avi_if);
-extern bool intel_sdvo_init(struct drm_device *dev, int output_device);
+extern struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder);
+extern void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
+			    struct drm_display_mode *adjusted_mode);
+extern void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder);
+extern void intel_dip_infoframe_csum(struct dip_infoframe *avi_if);
+extern bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg,
+			    bool is_sdvob);
 extern void intel_dvo_init(struct drm_device *dev);
 extern void intel_tv_init(struct drm_device *dev);
 extern void intel_mark_busy(struct drm_device *dev,
@@ -311,6 +362,10 @@ extern bool intel_dpd_is_edp(struct drm_device *dev);
 extern void intel_edp_link_config(struct intel_encoder *, int *, int *);
 extern bool intel_encoder_is_pch_edp(struct drm_encoder *encoder);
 extern int intel_plane_init(struct drm_device *dev, enum pipe pipe);
+extern void intel_flush_display_plane(struct drm_i915_private *dev_priv,
+				      enum plane plane);
+
+void intel_sanitize_pm(struct drm_device *dev);
 
 /* intel_panel.c */
 extern void intel_fixed_panel_mode(struct drm_display_mode *fixed_mode,
@@ -368,12 +423,9 @@ extern void intel_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
 extern void intel_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
 				    u16 *blue, int regno);
 extern void intel_enable_clock_gating(struct drm_device *dev);
+extern void ironlake_disable_rc6(struct drm_device *dev);
 extern void ironlake_enable_drps(struct drm_device *dev);
 extern void ironlake_disable_drps(struct drm_device *dev);
-extern void gen6_enable_rps(struct drm_i915_private *dev_priv);
-extern void gen6_update_ring_freq(struct drm_i915_private *dev_priv);
-extern void gen6_disable_rps(struct drm_device *dev);
-extern void intel_init_emon(struct drm_device *dev);
 
 extern int intel_pin_and_fence_fb_obj(struct drm_device *dev,
 				      struct drm_i915_gem_object *obj,
@@ -411,16 +463,43 @@ extern void intel_init_clock_gating(struct drm_device *dev);
 extern void intel_write_eld(struct drm_encoder *encoder,
 			    struct drm_display_mode *mode);
 extern void intel_cpt_verify_modeset(struct drm_device *dev, int pipe);
+extern void intel_prepare_ddi(struct drm_device *dev);
+extern void hsw_fdi_link_train(struct drm_crtc *crtc);
+extern void intel_ddi_init(struct drm_device *dev, enum port port);
 
 /* For use by IVB LP watermark workaround in intel_sprite.c */
-extern void sandybridge_update_wm(struct drm_device *dev);
+extern void intel_update_watermarks(struct drm_device *dev);
 extern void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
 					   uint32_t sprite_width,
 					   int pixel_size);
+extern void intel_update_linetime_watermarks(struct drm_device *dev, int pipe,
+			 struct drm_display_mode *mode);
 
 extern int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 				     struct drm_file *file_priv);
 extern int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
 				     struct drm_file *file_priv);
 
+extern u32 intel_dpio_read(struct drm_i915_private *dev_priv, int reg);
+
+/* Power-related functions, located in intel_pm.c */
+extern void intel_init_pm(struct drm_device *dev);
+/* FBC */
+extern bool intel_fbc_enabled(struct drm_device *dev);
+extern void intel_enable_fbc(struct drm_crtc *crtc, unsigned long interval);
+extern void intel_update_fbc(struct drm_device *dev);
+/* IPS */
+extern void intel_gpu_ips_init(struct drm_i915_private *dev_priv);
+extern void intel_gpu_ips_teardown(void);
+
+extern void gen6_enable_rps(struct drm_i915_private *dev_priv);
+extern void gen6_update_ring_freq(struct drm_i915_private *dev_priv);
+extern void gen6_disable_rps(struct drm_device *dev);
+extern void intel_init_emon(struct drm_device *dev);
+
+extern void intel_ddi_dpms(struct drm_encoder *encoder, int mode);
+extern void intel_ddi_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode);
+
 #endif /* __INTEL_DRV_H__ */
diff --git a/drivers/gpu/drm/i915/intel_dvo.c b/drivers/gpu/drm/i915/intel_dvo.c
index 020a7d7f744d..60ba50b956f2 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -243,7 +243,7 @@ static int intel_dvo_get_modes(struct drm_connector *connector)
 	 * that's not the case.
 	 */
 	intel_ddc_get_modes(connector,
-			    &dev_priv->gmbus[GMBUS_PORT_DPC].adapter);
+			    intel_gmbus_get_adapter(dev_priv, GMBUS_PORT_DPC));
 	if (!list_empty(&connector->probed_modes))
 		return 1;
 
@@ -375,7 +375,7 @@ void intel_dvo_init(struct drm_device *dev)
 		 * special cases, but otherwise default to what's defined
 		 * in the spec.
 		 */
-		if (dvo->gpio != 0)
+		if (intel_gmbus_is_port_valid(dvo->gpio))
 			gpio = dvo->gpio;
 		else if (dvo->type == INTEL_DVO_CHIP_LVDS)
 			gpio = GMBUS_PORT_SSC;
@@ -386,7 +386,7 @@ void intel_dvo_init(struct drm_device *dev)
 		 * It appears that everything is on GPIOE except for panels
 		 * on i830 laptops, which are on GPIOB (DVOA).
 		 */
-		i2c = &dev_priv->gmbus[gpio].adapter;
+		i2c = intel_gmbus_get_adapter(dev_priv, gpio);
 
 		intel_dvo->dev = *dvo;
 		if (!dvo->dev_ops->init(&intel_dvo->dev, i2c))
diff --git a/drivers/gpu/drm/i915/intel_fb.c b/drivers/gpu/drm/i915/intel_fb.c
index 6e9ee33fd412..bf8690720a0c 100644
--- a/drivers/gpu/drm/i915/intel_fb.c
+++ b/drivers/gpu/drm/i915/intel_fb.c
@@ -94,7 +94,7 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
 	mutex_lock(&dev->struct_mutex);
 
 	/* Flush everything out, we'll be doing GTT only from now on */
-	ret = intel_pin_and_fence_fb_obj(dev, obj, false);
+	ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
 	if (ret) {
 		DRM_ERROR("failed to pin fb: %d\n", ret);
 		goto out_unref;
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index 2d7f47b56b6a..2ead3bf7c21d 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -37,19 +37,7 @@
 #include "i915_drm.h"
 #include "i915_drv.h"
 
-struct intel_hdmi {
-	struct intel_encoder base;
-	u32 sdvox_reg;
-	int ddc_bus;
-	uint32_t color_range;
-	bool has_hdmi_sink;
-	bool has_audio;
-	enum hdmi_force_audio force_audio;
-	void (*write_infoframe)(struct drm_encoder *encoder,
-				struct dip_infoframe *frame);
-};
-
-static struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder)
+struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder)
 {
 	return container_of(encoder, struct intel_hdmi, base.base);
 }
@@ -75,108 +63,246 @@ void intel_dip_infoframe_csum(struct dip_infoframe *frame)
 	frame->checksum = 0x100 - sum;
 }
 
-static u32 intel_infoframe_index(struct dip_infoframe *frame)
+static u32 g4x_infoframe_index(struct dip_infoframe *frame)
 {
-	u32 flags = 0;
-
 	switch (frame->type) {
 	case DIP_TYPE_AVI:
-		flags |= VIDEO_DIP_SELECT_AVI;
-		break;
+		return VIDEO_DIP_SELECT_AVI;
 	case DIP_TYPE_SPD:
-		flags |= VIDEO_DIP_SELECT_SPD;
-		break;
+		return VIDEO_DIP_SELECT_SPD;
 	default:
 		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
-		break;
+		return 0;
 	}
-
-	return flags;
 }
 
-static u32 intel_infoframe_flags(struct dip_infoframe *frame)
+static u32 g4x_infoframe_enable(struct dip_infoframe *frame)
 {
-	u32 flags = 0;
+	switch (frame->type) {
+	case DIP_TYPE_AVI:
+		return VIDEO_DIP_ENABLE_AVI;
+	case DIP_TYPE_SPD:
+		return VIDEO_DIP_ENABLE_SPD;
+	default:
+		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
+		return 0;
+	}
+}
 
+static u32 hsw_infoframe_enable(struct dip_infoframe *frame)
+{
 	switch (frame->type) {
 	case DIP_TYPE_AVI:
-		flags |= VIDEO_DIP_ENABLE_AVI | VIDEO_DIP_FREQ_VSYNC;
-		break;
+		return VIDEO_DIP_ENABLE_AVI_HSW;
 	case DIP_TYPE_SPD:
-		flags |= VIDEO_DIP_ENABLE_SPD | VIDEO_DIP_FREQ_VSYNC;
-		break;
+		return VIDEO_DIP_ENABLE_SPD_HSW;
 	default:
 		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
-		break;
+		return 0;
 	}
+}
 
-	return flags;
+static u32 hsw_infoframe_data_reg(struct dip_infoframe *frame, enum pipe pipe)
+{
+	switch (frame->type) {
+	case DIP_TYPE_AVI:
+		return HSW_TVIDEO_DIP_AVI_DATA(pipe);
+	case DIP_TYPE_SPD:
+		return HSW_TVIDEO_DIP_SPD_DATA(pipe);
+	default:
+		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
+		return 0;
+	}
 }
 
-static void i9xx_write_infoframe(struct drm_encoder *encoder,
-				 struct dip_infoframe *frame)
+static void g4x_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
 {
 	uint32_t *data = (uint32_t *)frame;
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
-	u32 port, flags, val = I915_READ(VIDEO_DIP_CTL);
+	u32 val = I915_READ(VIDEO_DIP_CTL);
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
 
-
-	/* XXX first guess at handling video port, is this corrent? */
+	val &= ~VIDEO_DIP_PORT_MASK;
 	if (intel_hdmi->sdvox_reg == SDVOB)
-		port = VIDEO_DIP_PORT_B;
+		val |= VIDEO_DIP_PORT_B;
 	else if (intel_hdmi->sdvox_reg == SDVOC)
-		port = VIDEO_DIP_PORT_C;
+		val |= VIDEO_DIP_PORT_C;
 	else
 		return;
 
-	flags = intel_infoframe_index(frame);
+	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
 
-	val &= ~VIDEO_DIP_SELECT_MASK;
+	val &= ~g4x_infoframe_enable(frame);
+	val |= VIDEO_DIP_ENABLE;
 
-	I915_WRITE(VIDEO_DIP_CTL, VIDEO_DIP_ENABLE | val | port | flags);
+	I915_WRITE(VIDEO_DIP_CTL, val);
 
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(VIDEO_DIP_DATA, *data);
 		data++;
 	}
 
-	flags |= intel_infoframe_flags(frame);
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
 
-	I915_WRITE(VIDEO_DIP_CTL, VIDEO_DIP_ENABLE | val | port | flags);
+	I915_WRITE(VIDEO_DIP_CTL, val);
 }
 
-static void ironlake_write_infoframe(struct drm_encoder *encoder,
-				     struct dip_infoframe *frame)
+static void ibx_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
 {
 	uint32_t *data = (uint32_t *)frame;
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = encoder->crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
-	u32 flags, val = I915_READ(reg);
+	u32 val = I915_READ(reg);
+
+	val &= ~VIDEO_DIP_PORT_MASK;
+	switch (intel_hdmi->sdvox_reg) {
+	case HDMIB:
+		val |= VIDEO_DIP_PORT_B;
+		break;
+	case HDMIC:
+		val |= VIDEO_DIP_PORT_C;
+		break;
+	case HDMID:
+		val |= VIDEO_DIP_PORT_D;
+		break;
+	default:
+		return;
+	}
 
 	intel_wait_for_vblank(dev, intel_crtc->pipe);
 
-	flags = intel_infoframe_index(frame);
+	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
+
+	val &= ~g4x_infoframe_enable(frame);
+	val |= VIDEO_DIP_ENABLE;
+
+	I915_WRITE(reg, val);
+
+	for (i = 0; i < len; i += 4) {
+		I915_WRITE(TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
+		data++;
+	}
+
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
+
+	I915_WRITE(reg, val);
+}
+
+static void cpt_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
+{
+	uint32_t *data = (uint32_t *)frame;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	unsigned i, len = DIP_HEADER_SIZE + frame->len;
+	u32 val = I915_READ(reg);
+
+	intel_wait_for_vblank(dev, intel_crtc->pipe);
 
 	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
+
+	/* The DIP control register spec says that we need to update the AVI
+	 * infoframe without clearing its enable bit */
+	if (frame->type == DIP_TYPE_AVI)
+		val |= VIDEO_DIP_ENABLE_AVI;
+	else
+		val &= ~g4x_infoframe_enable(frame);
+
+	val |= VIDEO_DIP_ENABLE;
 
-	I915_WRITE(reg, VIDEO_DIP_ENABLE | val | flags);
+	I915_WRITE(reg, val);
 
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
 		data++;
 	}
 
-	flags |= intel_infoframe_flags(frame);
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
 
-	I915_WRITE(reg, VIDEO_DIP_ENABLE | val | flags);
+	I915_WRITE(reg, val);
 }
+
+static void vlv_write_infoframe(struct drm_encoder *encoder,
+				     struct dip_infoframe *frame)
+{
+	uint32_t *data = (uint32_t *)frame;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	int reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	unsigned i, len = DIP_HEADER_SIZE + frame->len;
+	u32 val = I915_READ(reg);
+
+	intel_wait_for_vblank(dev, intel_crtc->pipe);
+
+	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
+
+	val &= ~g4x_infoframe_enable(frame);
+	val |= VIDEO_DIP_ENABLE;
+
+	I915_WRITE(reg, val);
+
+	for (i = 0; i < len; i += 4) {
+		I915_WRITE(VLV_TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
+		data++;
+	}
+
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
+
+	I915_WRITE(reg, val);
+}
+
+static void hsw_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
+{
+	uint32_t *data = (uint32_t *)frame;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 data_reg = hsw_infoframe_data_reg(frame, intel_crtc->pipe);
+	unsigned int i, len = DIP_HEADER_SIZE + frame->len;
+	u32 val = I915_READ(ctl_reg);
+
+	if (data_reg == 0)
+		return;
+
+	intel_wait_for_vblank(dev, intel_crtc->pipe);
+
+	val &= ~hsw_infoframe_enable(frame);
+	I915_WRITE(ctl_reg, val);
+
+	for (i = 0; i < len; i += 4) {
+		I915_WRITE(data_reg + i, *data);
+		data++;
+	}
+
+	val |= hsw_infoframe_enable(frame);
+	I915_WRITE(ctl_reg, val);
+}
+
 static void intel_set_infoframe(struct drm_encoder *encoder,
 				struct dip_infoframe *frame)
 {
@@ -189,7 +315,8 @@ static void intel_set_infoframe(struct drm_encoder *encoder,
 	intel_hdmi->write_infoframe(encoder, frame);
 }
 
-static void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder)
+void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
+					 struct drm_display_mode *adjusted_mode)
 {
 	struct dip_infoframe avi_if = {
 		.type = DIP_TYPE_AVI,
@@ -197,10 +324,13 @@ static void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder)
 		.len = DIP_LEN_AVI,
 	};
 
+	if (adjusted_mode->flags & DRM_MODE_FLAG_DBLCLK)
+		avi_if.body.avi.YQ_CN_PR |= DIP_AVI_PR_2;
+
 	intel_set_infoframe(encoder, &avi_if);
 }
 
-static void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
+void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
 {
 	struct dip_infoframe spd_if;
 
@@ -221,8 +351,7 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = encoder->crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	u32 sdvox;
 
@@ -259,7 +388,7 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 	I915_WRITE(intel_hdmi->sdvox_reg, sdvox);
 	POSTING_READ(intel_hdmi->sdvox_reg);
 
-	intel_hdmi_set_avi_infoframe(encoder);
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
 	intel_hdmi_set_spd_infoframe(encoder);
 }
 
@@ -334,7 +463,8 @@ intel_hdmi_detect(struct drm_connector *connector, bool force)
 	intel_hdmi->has_hdmi_sink = false;
 	intel_hdmi->has_audio = false;
 	edid = drm_get_edid(connector,
-			    &dev_priv->gmbus[intel_hdmi->ddc_bus].adapter);
+			    intel_gmbus_get_adapter(dev_priv,
+						    intel_hdmi->ddc_bus));
 
 	if (edid) {
 		if (edid->input & DRM_EDID_INPUT_DIGITAL) {
@@ -367,7 +497,8 @@ static int intel_hdmi_get_modes(struct drm_connector *connector)
 	 */
 
 	return intel_ddc_get_modes(connector,
-				   &dev_priv->gmbus[intel_hdmi->ddc_bus].adapter);
+				   intel_gmbus_get_adapter(dev_priv,
+							   intel_hdmi->ddc_bus));
 }
 
 static bool
@@ -379,7 +510,8 @@ intel_hdmi_detect_audio(struct drm_connector *connector)
 	bool has_audio = false;
 
 	edid = drm_get_edid(connector,
-			    &dev_priv->gmbus[intel_hdmi->ddc_bus].adapter);
+			    intel_gmbus_get_adapter(dev_priv,
+						    intel_hdmi->ddc_bus));
 	if (edid) {
 		if (edid->input & DRM_EDID_INPUT_DIGITAL)
 			has_audio = drm_detect_monitor_audio(edid);
@@ -393,8 +525,8 @@ intel_hdmi_detect_audio(struct drm_connector *connector)
 
 static int
 intel_hdmi_set_property(struct drm_connector *connector,
-		      struct drm_property *property,
-		      uint64_t val)
+			struct drm_property *property,
+			uint64_t val)
 {
 	struct intel_hdmi *intel_hdmi = intel_attached_hdmi(connector);
 	struct drm_i915_private *dev_priv = connector->dev->dev_private;
@@ -453,6 +585,14 @@ static void intel_hdmi_destroy(struct drm_connector *connector)
 	kfree(connector);
 }
 
+static const struct drm_encoder_helper_funcs intel_hdmi_helper_funcs_hsw = {
+	.dpms = intel_ddi_dpms,
+	.mode_fixup = intel_hdmi_mode_fixup,
+	.prepare = intel_encoder_prepare,
+	.mode_set = intel_ddi_mode_set,
+	.commit = intel_encoder_commit,
+};
+
 static const struct drm_encoder_helper_funcs intel_hdmi_helper_funcs = {
 	.dpms = intel_hdmi_dpms,
 	.mode_fixup = intel_hdmi_mode_fixup,
@@ -542,20 +682,60 @@ void intel_hdmi_init(struct drm_device *dev, int sdvox_reg)
 		intel_encoder->clone_mask = (1 << INTEL_HDMIF_CLONE_BIT);
 		intel_hdmi->ddc_bus = GMBUS_PORT_DPD;
 		dev_priv->hotplug_supported_mask |= HDMID_HOTPLUG_INT_STATUS;
+	} else if (sdvox_reg == DDI_BUF_CTL(PORT_B)) {
+		DRM_DEBUG_DRIVER("LPT: detected output on DDI B\n");
+		intel_encoder->clone_mask = (1 << INTEL_HDMIB_CLONE_BIT);
+		intel_hdmi->ddc_bus = GMBUS_PORT_DPB;
+		intel_hdmi->ddi_port = PORT_B;
+		dev_priv->hotplug_supported_mask |= HDMIB_HOTPLUG_INT_STATUS;
+	} else if (sdvox_reg == DDI_BUF_CTL(PORT_C)) {
+		DRM_DEBUG_DRIVER("LPT: detected output on DDI C\n");
+		intel_encoder->clone_mask = (1 << INTEL_HDMIC_CLONE_BIT);
+		intel_hdmi->ddc_bus = GMBUS_PORT_DPC;
+		intel_hdmi->ddi_port = PORT_C;
+		dev_priv->hotplug_supported_mask |= HDMIC_HOTPLUG_INT_STATUS;
+	} else if (sdvox_reg == DDI_BUF_CTL(PORT_D)) {
+		DRM_DEBUG_DRIVER("LPT: detected output on DDI D\n");
+		intel_encoder->clone_mask = (1 << INTEL_HDMID_CLONE_BIT);
+		intel_hdmi->ddc_bus = GMBUS_PORT_DPD;
+		intel_hdmi->ddi_port = PORT_D;
+		dev_priv->hotplug_supported_mask |= HDMID_HOTPLUG_INT_STATUS;
+	} else {
+		/* If we got an unknown sdvox_reg, things are pretty much broken
+		 * in a way that we should let the kernel know about it */
+		BUG();
 	}
 
 	intel_hdmi->sdvox_reg = sdvox_reg;
 
 	if (!HAS_PCH_SPLIT(dev)) {
-		intel_hdmi->write_infoframe = i9xx_write_infoframe;
+		intel_hdmi->write_infoframe = g4x_write_infoframe;
 		I915_WRITE(VIDEO_DIP_CTL, 0);
+	} else if (IS_VALLEYVIEW(dev)) {
+		intel_hdmi->write_infoframe = vlv_write_infoframe;
+		for_each_pipe(i)
+			I915_WRITE(VLV_TVIDEO_DIP_CTL(i), 0);
+	} else if (IS_HASWELL(dev)) {
+		/* FIXME: Haswell has a new set of DIP frame registers, but we are
+		 * just doing the minimal required for HDMI to work at this stage.
+		 */
+		intel_hdmi->write_infoframe = hsw_write_infoframe;
+		for_each_pipe(i)
+			I915_WRITE(HSW_TVIDEO_DIP_CTL(i), 0);
+	} else if (HAS_PCH_IBX(dev)) {
+		intel_hdmi->write_infoframe = ibx_write_infoframe;
+		for_each_pipe(i)
+			I915_WRITE(TVIDEO_DIP_CTL(i), 0);
 	} else {
-		intel_hdmi->write_infoframe = ironlake_write_infoframe;
+		intel_hdmi->write_infoframe = cpt_write_infoframe;
 		for_each_pipe(i)
 			I915_WRITE(TVIDEO_DIP_CTL(i), 0);
 	}
 
-	drm_encoder_helper_add(&intel_encoder->base, &intel_hdmi_helper_funcs);
+	if (IS_HASWELL(dev))
+		drm_encoder_helper_add(&intel_encoder->base, &intel_hdmi_helper_funcs_hsw);
+	else
+		drm_encoder_helper_add(&intel_encoder->base, &intel_hdmi_helper_funcs);
 
 	intel_hdmi_add_properties(intel_hdmi, connector);
 
diff --git a/drivers/gpu/drm/i915/intel_i2c.c b/drivers/gpu/drm/i915/intel_i2c.c
index 8fdc95700218..4a9707dd0f9c 100644
--- a/drivers/gpu/drm/i915/intel_i2c.c
+++ b/drivers/gpu/drm/i915/intel_i2c.c
@@ -35,6 +35,20 @@
 #include "i915_drm.h"
 #include "i915_drv.h"
 
+struct gmbus_port {
+	const char *name;
+	int reg;
+};
+
+static const struct gmbus_port gmbus_ports[] = {
+	{ "ssc", GPIOB },
+	{ "vga", GPIOA },
+	{ "panel", GPIOC },
+	{ "dpc", GPIOD },
+	{ "dpb", GPIOE },
+	{ "dpd", GPIOF },
+};
+
 /* Intel GPIO access functions */
 
 #define I2C_RISEFALL_TIME 10
@@ -49,10 +63,7 @@ void
 intel_i2c_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	if (HAS_PCH_SPLIT(dev))
-		I915_WRITE(PCH_GMBUS0, 0);
-	else
-		I915_WRITE(GMBUS0, 0);
+	I915_WRITE(dev_priv->gpio_mmio_base + GMBUS0, 0);
 }
 
 static void intel_i2c_quirk_set(struct drm_i915_private *dev_priv, bool enable)
@@ -140,63 +151,173 @@ static void set_data(void *data, int state_high)
 	POSTING_READ(bus->gpio_reg);
 }
 
-static bool
+static int
+intel_gpio_pre_xfer(struct i2c_adapter *adapter)
+{
+	struct intel_gmbus *bus = container_of(adapter,
+					       struct intel_gmbus,
+					       adapter);
+	struct drm_i915_private *dev_priv = bus->dev_priv;
+
+	intel_i2c_reset(dev_priv->dev);
+	intel_i2c_quirk_set(dev_priv, true);
+	set_data(bus, 1);
+	set_clock(bus, 1);
+	udelay(I2C_RISEFALL_TIME);
+	return 0;
+}
+
+static void
+intel_gpio_post_xfer(struct i2c_adapter *adapter)
+{
+	struct intel_gmbus *bus = container_of(adapter,
+					       struct intel_gmbus,
+					       adapter);
+	struct drm_i915_private *dev_priv = bus->dev_priv;
+
+	set_data(bus, 1);
+	set_clock(bus, 1);
+	intel_i2c_quirk_set(dev_priv, false);
+}
+
+static void
 intel_gpio_setup(struct intel_gmbus *bus, u32 pin)
 {
 	struct drm_i915_private *dev_priv = bus->dev_priv;
-	static const int map_pin_to_reg[] = {
-		0,
-		GPIOB,
-		GPIOA,
-		GPIOC,
-		GPIOD,
-		GPIOE,
-		0,
-		GPIOF,
-	};
 	struct i2c_algo_bit_data *algo;
 
-	if (pin >= ARRAY_SIZE(map_pin_to_reg) || !map_pin_to_reg[pin])
-		return false;
-
 	algo = &bus->bit_algo;
 
-	bus->gpio_reg = map_pin_to_reg[pin];
-	if (HAS_PCH_SPLIT(dev_priv->dev))
-		bus->gpio_reg += PCH_GPIOA - GPIOA;
+	/* -1 to map pin pair to gmbus index */
+	bus->gpio_reg = dev_priv->gpio_mmio_base + gmbus_ports[pin - 1].reg;
 
 	bus->adapter.algo_data = algo;
 	algo->setsda = set_data;
 	algo->setscl = set_clock;
 	algo->getsda = get_data;
 	algo->getscl = get_clock;
+	algo->pre_xfer = intel_gpio_pre_xfer;
+	algo->post_xfer = intel_gpio_post_xfer;
 	algo->udelay = I2C_RISEFALL_TIME;
 	algo->timeout = usecs_to_jiffies(2200);
 	algo->data = bus;
+}
+
+static int
+gmbus_xfer_read(struct drm_i915_private *dev_priv, struct i2c_msg *msg,
+		u32 gmbus1_index)
+{
+	int reg_offset = dev_priv->gpio_mmio_base;
+	u16 len = msg->len;
+	u8 *buf = msg->buf;
+
+	I915_WRITE(GMBUS1 + reg_offset,
+		   gmbus1_index |
+		   GMBUS_CYCLE_WAIT |
+		   (len << GMBUS_BYTE_COUNT_SHIFT) |
+		   (msg->addr << GMBUS_SLAVE_ADDR_SHIFT) |
+		   GMBUS_SLAVE_READ | GMBUS_SW_RDY);
+	while (len) {
+		int ret;
+		u32 val, loop = 0;
+		u32 gmbus2;
+
+		ret = wait_for((gmbus2 = I915_READ(GMBUS2 + reg_offset)) &
+			       (GMBUS_SATOER | GMBUS_HW_RDY),
+			       50);
+		if (ret)
+			return -ETIMEDOUT;
+		if (gmbus2 & GMBUS_SATOER)
+			return -ENXIO;
+
+		val = I915_READ(GMBUS3 + reg_offset);
+		do {
+			*buf++ = val & 0xff;
+			val >>= 8;
+		} while (--len && ++loop < 4);
+	}
 
-	return true;
+	return 0;
 }
 
 static int
-intel_i2c_quirk_xfer(struct intel_gmbus *bus,
-		     struct i2c_msg *msgs,
-		     int num)
+gmbus_xfer_write(struct drm_i915_private *dev_priv, struct i2c_msg *msg)
 {
-	struct drm_i915_private *dev_priv = bus->dev_priv;
+	int reg_offset = dev_priv->gpio_mmio_base;
+	u16 len = msg->len;
+	u8 *buf = msg->buf;
+	u32 val, loop;
+
+	val = loop = 0;
+	while (len && loop < 4) {
+		val |= *buf++ << (8 * loop++);
+		len -= 1;
+	}
+
+	I915_WRITE(GMBUS3 + reg_offset, val);
+	I915_WRITE(GMBUS1 + reg_offset,
+		   GMBUS_CYCLE_WAIT |
+		   (msg->len << GMBUS_BYTE_COUNT_SHIFT) |
+		   (msg->addr << GMBUS_SLAVE_ADDR_SHIFT) |
+		   GMBUS_SLAVE_WRITE | GMBUS_SW_RDY);
+	while (len) {
+		int ret;
+		u32 gmbus2;
+
+		val = loop = 0;
+		do {
+			val |= *buf++ << (8 * loop);
+		} while (--len && ++loop < 4);
+
+		I915_WRITE(GMBUS3 + reg_offset, val);
+
+		ret = wait_for((gmbus2 = I915_READ(GMBUS2 + reg_offset)) &
+			       (GMBUS_SATOER | GMBUS_HW_RDY),
+			       50);
+		if (ret)
+			return -ETIMEDOUT;
+		if (gmbus2 & GMBUS_SATOER)
+			return -ENXIO;
+	}
+	return 0;
+}
+
+/*
+ * The gmbus controller can combine a 1 or 2 byte write with a read that
+ * immediately follows it by using an "INDEX" cycle.
+ */
+static bool
+gmbus_is_index_read(struct i2c_msg *msgs, int i, int num)
+{
+	return (i + 1 < num &&
+		!(msgs[i].flags & I2C_M_RD) && msgs[i].len <= 2 &&
+		(msgs[i + 1].flags & I2C_M_RD));
+}
+
+static int
+gmbus_xfer_index_read(struct drm_i915_private *dev_priv, struct i2c_msg *msgs)
+{
+	int reg_offset = dev_priv->gpio_mmio_base;
+	u32 gmbus1_index = 0;
+	u32 gmbus5 = 0;
 	int ret;
 
-	intel_i2c_reset(dev_priv->dev);
+	if (msgs[0].len == 2)
+		gmbus5 = GMBUS_2BYTE_INDEX_EN |
+			 msgs[0].buf[1] | (msgs[0].buf[0] << 8);
+	if (msgs[0].len == 1)
+		gmbus1_index = GMBUS_CYCLE_INDEX |
+			       (msgs[0].buf[0] << GMBUS_SLAVE_INDEX_SHIFT);
 
-	intel_i2c_quirk_set(dev_priv, true);
-	set_data(bus, 1);
-	set_clock(bus, 1);
-	udelay(I2C_RISEFALL_TIME);
+	/* GMBUS5 holds 16-bit index */
+	if (gmbus5)
+		I915_WRITE(GMBUS5 + reg_offset, gmbus5);
 
-	ret = i2c_bit_algo.master_xfer(&bus->adapter, msgs, num);
+	ret = gmbus_xfer_read(dev_priv, &msgs[1], gmbus1_index);
 
-	set_data(bus, 1);
-	set_clock(bus, 1);
-	intel_i2c_quirk_set(dev_priv, false);
+	/* Clear GMBUS5 after each index transfer */
+	if (gmbus5)
+		I915_WRITE(GMBUS5 + reg_offset, 0);
 
 	return ret;
 }
@@ -210,117 +331,108 @@ gmbus_xfer(struct i2c_adapter *adapter,
 					       struct intel_gmbus,
 					       adapter);
 	struct drm_i915_private *dev_priv = bus->dev_priv;
-	int i, reg_offset, ret;
+	int i, reg_offset;
+	int ret = 0;
 
 	mutex_lock(&dev_priv->gmbus_mutex);
 
 	if (bus->force_bit) {
-		ret = intel_i2c_quirk_xfer(bus, msgs, num);
+		ret = i2c_bit_algo.master_xfer(adapter, msgs, num);
 		goto out;
 	}
 
-	reg_offset = HAS_PCH_SPLIT(dev_priv->dev) ? PCH_GMBUS0 - GMBUS0 : 0;
+	reg_offset = dev_priv->gpio_mmio_base;
 
 	I915_WRITE(GMBUS0 + reg_offset, bus->reg0);
 
 	for (i = 0; i < num; i++) {
-		u16 len = msgs[i].len;
-		u8 *buf = msgs[i].buf;
-
-		if (msgs[i].flags & I2C_M_RD) {
-			I915_WRITE(GMBUS1 + reg_offset,
-				   GMBUS_CYCLE_WAIT |
-				   (i + 1 == num ? GMBUS_CYCLE_STOP : 0) |
-				   (len << GMBUS_BYTE_COUNT_SHIFT) |
-				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
-				   GMBUS_SLAVE_READ | GMBUS_SW_RDY);
-			POSTING_READ(GMBUS2+reg_offset);
-			do {
-				u32 val, loop = 0;
-
-				if (wait_for(I915_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
-					goto timeout;
-				if (I915_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
-					goto clear_err;
-
-				val = I915_READ(GMBUS3 + reg_offset);
-				do {
-					*buf++ = val & 0xff;
-					val >>= 8;
-				} while (--len && ++loop < 4);
-			} while (len);
+		u32 gmbus2;
+
+		if (gmbus_is_index_read(msgs, i, num)) {
+			ret = gmbus_xfer_index_read(dev_priv, &msgs[i]);
+			i += 1;  /* set i to the index of the read xfer */
+		} else if (msgs[i].flags & I2C_M_RD) {
+			ret = gmbus_xfer_read(dev_priv, &msgs[i], 0);
 		} else {
-			u32 val, loop;
-
-			val = loop = 0;
-			do {
-				val |= *buf++ << (8 * loop);
-			} while (--len && ++loop < 4);
-
-			I915_WRITE(GMBUS3 + reg_offset, val);
-			I915_WRITE(GMBUS1 + reg_offset,
-				   GMBUS_CYCLE_WAIT |
-				   (i + 1 == num ? GMBUS_CYCLE_STOP : 0) |
-				   (msgs[i].len << GMBUS_BYTE_COUNT_SHIFT) |
-				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
-				   GMBUS_SLAVE_WRITE | GMBUS_SW_RDY);
-			POSTING_READ(GMBUS2+reg_offset);
-
-			while (len) {
-				if (wait_for(I915_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
-					goto timeout;
-				if (I915_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
-					goto clear_err;
-
-				val = loop = 0;
-				do {
-					val |= *buf++ << (8 * loop);
-				} while (--len && ++loop < 4);
-
-				I915_WRITE(GMBUS3 + reg_offset, val);
-				POSTING_READ(GMBUS2+reg_offset);
-			}
+			ret = gmbus_xfer_write(dev_priv, &msgs[i]);
 		}
 
-		if (i + 1 < num && wait_for(I915_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_WAIT_PHASE), 50))
+		if (ret == -ETIMEDOUT)
+			goto timeout;
+		if (ret == -ENXIO)
+			goto clear_err;
+
+		ret = wait_for((gmbus2 = I915_READ(GMBUS2 + reg_offset)) &
+			       (GMBUS_SATOER | GMBUS_HW_WAIT_PHASE),
+			       50);
+		if (ret)
 			goto timeout;
-		if (I915_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+		if (gmbus2 & GMBUS_SATOER)
 			goto clear_err;
 	}
 
-	goto done;
+	/* Generate a STOP condition on the bus. Note that gmbus can't generata
+	 * a STOP on the very first cycle. To simplify the code we
+	 * unconditionally generate the STOP condition with an additional gmbus
+	 * cycle. */
+	I915_WRITE(GMBUS1 + reg_offset, GMBUS_CYCLE_STOP | GMBUS_SW_RDY);
+
+	/* Mark the GMBUS interface as disabled after waiting for idle.
+	 * We will re-enable it at the start of the next xfer,
+	 * till then let it sleep.
+	 */
+	if (wait_for((I915_READ(GMBUS2 + reg_offset) & GMBUS_ACTIVE) == 0,
+		     10)) {
+		DRM_DEBUG_KMS("GMBUS [%s] timed out waiting for idle\n",
+			 adapter->name);
+		ret = -ETIMEDOUT;
+	}
+	I915_WRITE(GMBUS0 + reg_offset, 0);
+	ret = ret ?: i;
+	goto out;
 
 clear_err:
+	/*
+	 * Wait for bus to IDLE before clearing NAK.
+	 * If we clear the NAK while bus is still active, then it will stay
+	 * active and the next transaction may fail.
+	 */
+	if (wait_for((I915_READ(GMBUS2 + reg_offset) & GMBUS_ACTIVE) == 0,
+		     10))
+		DRM_DEBUG_KMS("GMBUS [%s] timed out after NAK\n",
+			      adapter->name);
+
 	/* Toggle the Software Clear Interrupt bit. This has the effect
 	 * of resetting the GMBUS controller and so clearing the
 	 * BUS_ERROR raised by the slave's NAK.
 	 */
 	I915_WRITE(GMBUS1 + reg_offset, GMBUS_SW_CLR_INT);
 	I915_WRITE(GMBUS1 + reg_offset, 0);
+	I915_WRITE(GMBUS0 + reg_offset, 0);
 
-done:
-	/* Mark the GMBUS interface as disabled after waiting for idle.
-	 * We will re-enable it at the start of the next xfer,
-	 * till then let it sleep.
+	DRM_DEBUG_KMS("GMBUS [%s] NAK for addr: %04x %c(%d)\n",
+			 adapter->name, msgs[i].addr,
+			 (msgs[i].flags & I2C_M_RD) ? 'r' : 'w', msgs[i].len);
+
+	/*
+	 * If no ACK is received during the address phase of a transaction,
+	 * the adapter must report -ENXIO.
+	 * It is not clear what to return if no ACK is received at other times.
+	 * So, we always return -ENXIO in all NAK cases, to ensure we send
+	 * it at least during the one case that is specified.
 	 */
-	if (wait_for((I915_READ(GMBUS2 + reg_offset) & GMBUS_ACTIVE) == 0, 10))
-		DRM_INFO("GMBUS timed out waiting for idle\n");
-	I915_WRITE(GMBUS0 + reg_offset, 0);
-	ret = i;
+	ret = -ENXIO;
 	goto out;
 
 timeout:
-	DRM_INFO("GMBUS timed out, falling back to bit banging on pin %d [%s]\n",
-		 bus->reg0 & 0xff, bus->adapter.name);
+	DRM_INFO("GMBUS [%s] timed out, falling back to bit banging on pin %d\n",
+		 bus->adapter.name, bus->reg0 & 0xff);
 	I915_WRITE(GMBUS0 + reg_offset, 0);
 
 	/* Hardware may not support GMBUS over these pins? Try GPIO bitbanging instead. */
-	if (!bus->has_gpio) {
-		ret = -EIO;
-	} else {
-		bus->force_bit = true;
-		ret = intel_i2c_quirk_xfer(bus, msgs, num);
-	}
+	bus->force_bit = true;
+	ret = i2c_bit_algo.master_xfer(adapter, msgs, num);
+
 out:
 	mutex_unlock(&dev_priv->gmbus_mutex);
 	return ret;
@@ -346,35 +458,26 @@ static const struct i2c_algorithm gmbus_algorithm = {
  */
 int intel_setup_gmbus(struct drm_device *dev)
 {
-	static const char *names[GMBUS_NUM_PORTS] = {
-		"disabled",
-		"ssc",
-		"vga",
-		"panel",
-		"dpc",
-		"dpb",
-		"reserved",
-		"dpd",
-	};
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret, i;
 
-	dev_priv->gmbus = kcalloc(GMBUS_NUM_PORTS, sizeof(struct intel_gmbus),
-				  GFP_KERNEL);
-	if (dev_priv->gmbus == NULL)
-		return -ENOMEM;
+	if (HAS_PCH_SPLIT(dev))
+		dev_priv->gpio_mmio_base = PCH_GPIOA - GPIOA;
+	else
+		dev_priv->gpio_mmio_base = 0;
 
 	mutex_init(&dev_priv->gmbus_mutex);
 
 	for (i = 0; i < GMBUS_NUM_PORTS; i++) {
 		struct intel_gmbus *bus = &dev_priv->gmbus[i];
+		u32 port = i + 1; /* +1 to map gmbus index to pin pair */
 
 		bus->adapter.owner = THIS_MODULE;
 		bus->adapter.class = I2C_CLASS_DDC;
 		snprintf(bus->adapter.name,
 			 sizeof(bus->adapter.name),
 			 "i915 gmbus %s",
-			 names[i]);
+			 gmbus_ports[i].name);
 
 		bus->adapter.dev.parent = &dev->pdev->dev;
 		bus->dev_priv = dev_priv;
@@ -385,13 +488,13 @@ int intel_setup_gmbus(struct drm_device *dev)
 			goto err;
 
 		/* By default use a conservative clock rate */
-		bus->reg0 = i | GMBUS_RATE_100KHZ;
+		bus->reg0 = port | GMBUS_RATE_100KHZ;
 
-		bus->has_gpio = intel_gpio_setup(bus, i);
-
-		/* XXX force bit banging until GMBUS is fully debugged */
-		if (bus->has_gpio)
+		/* gmbus seems to be broken on i830 */
+		if (IS_I830(dev))
 			bus->force_bit = true;
+
+		intel_gpio_setup(bus, port);
 	}
 
 	intel_i2c_reset(dev_priv->dev);
@@ -403,11 +506,18 @@ err:
 		struct intel_gmbus *bus = &dev_priv->gmbus[i];
 		i2c_del_adapter(&bus->adapter);
 	}
-	kfree(dev_priv->gmbus);
-	dev_priv->gmbus = NULL;
 	return ret;
 }
 
+struct i2c_adapter *intel_gmbus_get_adapter(struct drm_i915_private *dev_priv,
+					    unsigned port)
+{
+	WARN_ON(!intel_gmbus_is_port_valid(port));
+	/* -1 to map pin pair to gmbus index */
+	return (intel_gmbus_is_port_valid(port)) ?
+		&dev_priv->gmbus[port - 1].adapter : NULL;
+}
+
 void intel_gmbus_set_speed(struct i2c_adapter *adapter, int speed)
 {
 	struct intel_gmbus *bus = to_intel_gmbus(adapter);
@@ -419,8 +529,7 @@ void intel_gmbus_force_bit(struct i2c_adapter *adapter, bool force_bit)
 {
 	struct intel_gmbus *bus = to_intel_gmbus(adapter);
 
-	if (bus->has_gpio)
-		bus->force_bit = force_bit;
+	bus->force_bit = force_bit;
 }
 
 void intel_teardown_gmbus(struct drm_device *dev)
@@ -435,7 +544,4 @@ void intel_teardown_gmbus(struct drm_device *dev)
 		struct intel_gmbus *bus = &dev_priv->gmbus[i];
 		i2c_del_adapter(&bus->adapter);
 	}
-
-	kfree(dev_priv->gmbus);
-	dev_priv->gmbus = NULL;
 }
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index 9c71183629c2..9dee82350def 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -480,7 +480,7 @@ static int intel_lvds_get_modes(struct drm_connector *connector)
 
 static int intel_no_modeset_on_lid_dmi_callback(const struct dmi_system_id *id)
 {
-	DRM_DEBUG_KMS("Skipping forced modeset for %s\n", id->ident);
+	DRM_INFO("Skipping forced modeset for %s\n", id->ident);
 	return 1;
 }
 
@@ -628,7 +628,7 @@ static const struct drm_encoder_funcs intel_lvds_enc_funcs = {
 
 static int __init intel_no_lvds_dmi_callback(const struct dmi_system_id *id)
 {
-	DRM_DEBUG_KMS("Skipping LVDS initialization for %s\n", id->ident);
+	DRM_INFO("Skipping LVDS initialization for %s\n", id->ident);
 	return 1;
 }
 
@@ -851,8 +851,8 @@ static bool lvds_is_present_in_vbt(struct drm_device *dev,
 		    child->device_type != DEVICE_TYPE_LFP)
 			continue;
 
-		if (child->i2c_pin)
-		    *i2c_pin = child->i2c_pin;
+		if (intel_gmbus_is_port_valid(child->i2c_pin))
+			*i2c_pin = child->i2c_pin;
 
 		/* However, we cannot trust the BIOS writers to populate
 		 * the VBT correctly.  Since LVDS requires additional
@@ -993,7 +993,8 @@ bool intel_lvds_init(struct drm_device *dev)
 	 * preferred mode is the right one.
 	 */
 	intel_lvds->edid = drm_get_edid(connector,
-					&dev_priv->gmbus[pin].adapter);
+					intel_gmbus_get_adapter(dev_priv,
+								pin));
 	if (intel_lvds->edid) {
 		if (drm_add_edid_modes(connector,
 				       intel_lvds->edid)) {
diff --git a/drivers/gpu/drm/i915/intel_modes.c b/drivers/gpu/drm/i915/intel_modes.c
index d1928e79d9b6..d67ec3a51e42 100644
--- a/drivers/gpu/drm/i915/intel_modes.c
+++ b/drivers/gpu/drm/i915/intel_modes.c
@@ -56,7 +56,8 @@ bool intel_ddc_probe(struct intel_encoder *intel_encoder, int ddc_bus)
 		}
 	};
 
-	return i2c_transfer(&dev_priv->gmbus[ddc_bus].adapter, msgs, 2) == 2;
+	return i2c_transfer(intel_gmbus_get_adapter(dev_priv, ddc_bus),
+			    msgs, 2) == 2;
 }
 
 /**
diff --git a/drivers/gpu/drm/i915/intel_opregion.c b/drivers/gpu/drm/i915/intel_opregion.c
index 289140bc83cb..18bd0af855dc 100644
--- a/drivers/gpu/drm/i915/intel_opregion.c
+++ b/drivers/gpu/drm/i915/intel_opregion.c
@@ -25,6 +25,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/acpi.h>
 #include <linux/acpi_io.h>
 #include <acpi/video.h>
@@ -149,7 +151,7 @@ struct opregion_asle {
 static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 	u32 max;
 
 	if (!(bclp & ASLE_BCLP_VALID))
@@ -161,7 +163,7 @@ static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 
 	max = intel_panel_get_max_backlight(dev);
 	intel_panel_set_backlight(dev, bclp * max / 255);
-	asle->cblv = (bclp*0x64)/0xff | ASLE_CBLV_VALID;
+	iowrite32((bclp*0x64)/0xff | ASLE_CBLV_VALID, &asle->cblv);
 
 	return 0;
 }
@@ -198,14 +200,14 @@ static u32 asle_set_pfit(struct drm_device *dev, u32 pfit)
 void intel_opregion_asle_intr(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 	u32 asle_stat = 0;
 	u32 asle_req;
 
 	if (!asle)
 		return;
 
-	asle_req = asle->aslc & ASLE_REQ_MSK;
+	asle_req = ioread32(&asle->aslc) & ASLE_REQ_MSK;
 
 	if (!asle_req) {
 		DRM_DEBUG_DRIVER("non asle set request??\n");
@@ -213,31 +215,31 @@ void intel_opregion_asle_intr(struct drm_device *dev)
 	}
 
 	if (asle_req & ASLE_SET_ALS_ILLUM)
-		asle_stat |= asle_set_als_illum(dev, asle->alsi);
+		asle_stat |= asle_set_als_illum(dev, ioread32(&asle->alsi));
 
 	if (asle_req & ASLE_SET_BACKLIGHT)
-		asle_stat |= asle_set_backlight(dev, asle->bclp);
+		asle_stat |= asle_set_backlight(dev, ioread32(&asle->bclp));
 
 	if (asle_req & ASLE_SET_PFIT)
-		asle_stat |= asle_set_pfit(dev, asle->pfit);
+		asle_stat |= asle_set_pfit(dev, ioread32(&asle->pfit));
 
 	if (asle_req & ASLE_SET_PWM_FREQ)
-		asle_stat |= asle_set_pwm_freq(dev, asle->pfmb);
+		asle_stat |= asle_set_pwm_freq(dev, ioread32(&asle->pfmb));
 
-	asle->aslc = asle_stat;
+	iowrite32(asle_stat, &asle->aslc);
 }
 
 void intel_opregion_gse_intr(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 	u32 asle_stat = 0;
 	u32 asle_req;
 
 	if (!asle)
 		return;
 
-	asle_req = asle->aslc & ASLE_REQ_MSK;
+	asle_req = ioread32(&asle->aslc) & ASLE_REQ_MSK;
 
 	if (!asle_req) {
 		DRM_DEBUG_DRIVER("non asle set request??\n");
@@ -250,7 +252,7 @@ void intel_opregion_gse_intr(struct drm_device *dev)
 	}
 
 	if (asle_req & ASLE_SET_BACKLIGHT)
-		asle_stat |= asle_set_backlight(dev, asle->bclp);
+		asle_stat |= asle_set_backlight(dev, ioread32(&asle->bclp));
 
 	if (asle_req & ASLE_SET_PFIT) {
 		DRM_DEBUG_DRIVER("Pfit is not supported\n");
@@ -262,7 +264,7 @@ void intel_opregion_gse_intr(struct drm_device *dev)
 		asle_stat |= ASLE_PWM_FREQ_FAILED;
 	}
 
-	asle->aslc = asle_stat;
+	iowrite32(asle_stat, &asle->aslc);
 }
 #define ASLE_ALS_EN    (1<<0)
 #define ASLE_BLC_EN    (1<<1)
@@ -272,15 +274,16 @@ void intel_opregion_gse_intr(struct drm_device *dev)
 void intel_opregion_enable_asle(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 
 	if (asle) {
 		if (IS_MOBILE(dev))
 			intel_enable_asle(dev);
 
-		asle->tche = ASLE_ALS_EN | ASLE_BLC_EN | ASLE_PFIT_EN |
-			ASLE_PFMB_EN;
-		asle->ardy = 1;
+		iowrite32(ASLE_ALS_EN | ASLE_BLC_EN | ASLE_PFIT_EN |
+			  ASLE_PFMB_EN,
+			  &asle->tche);
+		iowrite32(1, &asle->ardy);
 	}
 }
 
@@ -298,7 +301,7 @@ static int intel_opregion_video_event(struct notifier_block *nb,
 	   Linux, these are handled by the dock, button and video drivers.
 	*/
 
-	struct opregion_acpi *acpi;
+	struct opregion_acpi __iomem *acpi;
 	struct acpi_bus_event *event = data;
 	int ret = NOTIFY_OK;
 
@@ -310,10 +313,11 @@ static int intel_opregion_video_event(struct notifier_block *nb,
 
 	acpi = system_opregion->acpi;
 
-	if (event->type == 0x80 && !(acpi->cevt & 0x1))
+	if (event->type == 0x80 &&
+	    (ioread32(&acpi->cevt) & 1) == 0)
 		ret = NOTIFY_BAD;
 
-	acpi->csts = 0;
+	iowrite32(0, &acpi->csts);
 
 	return ret;
 }
@@ -337,6 +341,7 @@ static void intel_didl_outputs(struct drm_device *dev)
 	struct acpi_device *acpi_dev, *acpi_cdev, *acpi_video_bus = NULL;
 	unsigned long long device_id;
 	acpi_status status;
+	u32 temp;
 	int i = 0;
 
 	handle = DEVICE_ACPI_HANDLE(&dev->pdev->dev);
@@ -355,7 +360,7 @@ static void intel_didl_outputs(struct drm_device *dev)
 	}
 
 	if (!acpi_video_bus) {
-		printk(KERN_WARNING "No ACPI video bus found\n");
+		pr_warn("No ACPI video bus found\n");
 		return;
 	}
 
@@ -371,7 +376,8 @@ static void intel_didl_outputs(struct drm_device *dev)
 		if (ACPI_SUCCESS(status)) {
 			if (!device_id)
 				goto blind_set;
-			opregion->acpi->didl[i] = (u32)(device_id & 0x0f0f);
+			iowrite32((u32)(device_id & 0x0f0f),
+				  &opregion->acpi->didl[i]);
 			i++;
 		}
 	}
@@ -379,7 +385,7 @@ static void intel_didl_outputs(struct drm_device *dev)
 end:
 	/* If fewer than 8 outputs, the list must be null terminated */
 	if (i < 8)
-		opregion->acpi->didl[i] = 0;
+		iowrite32(0, &opregion->acpi->didl[i]);
 	return;
 
 blind_set:
@@ -413,7 +419,9 @@ blind_set:
 			output_type = ACPI_LVDS_OUTPUT;
 			break;
 		}
-		opregion->acpi->didl[i] |= (1<<31) | output_type | i;
+		temp = ioread32(&opregion->acpi->didl[i]);
+		iowrite32(temp | (1<<31) | output_type | i,
+			  &opregion->acpi->didl[i]);
 		i++;
 	}
 	goto end;
@@ -434,8 +442,8 @@ void intel_opregion_init(struct drm_device *dev)
 		/* Notify BIOS we are ready to handle ACPI video ext notifs.
 		 * Right now, all the events are handled by the ACPI video module.
 		 * We don't actually need to do anything with them. */
-		opregion->acpi->csts = 0;
-		opregion->acpi->drdy = 1;
+		iowrite32(0, &opregion->acpi->csts);
+		iowrite32(1, &opregion->acpi->drdy);
 
 		system_opregion = opregion;
 		register_acpi_notifier(&intel_opregion_notifier);
@@ -454,7 +462,7 @@ void intel_opregion_fini(struct drm_device *dev)
 		return;
 
 	if (opregion->acpi) {
-		opregion->acpi->drdy = 0;
+		iowrite32(0, &opregion->acpi->drdy);
 
 		system_opregion = NULL;
 		unregister_acpi_notifier(&intel_opregion_notifier);
@@ -474,8 +482,9 @@ int intel_opregion_setup(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_opregion *opregion = &dev_priv->opregion;
-	void *base;
+	void __iomem *base;
 	u32 asls, mboxes;
+	char buf[sizeof(OPREGION_SIGNATURE)];
 	int err = 0;
 
 	pci_read_config_dword(dev->pdev, PCI_ASLS, &asls);
@@ -489,7 +498,9 @@ int intel_opregion_setup(struct drm_device *dev)
 	if (!base)
 		return -ENOMEM;
 
-	if (memcmp(base, OPREGION_SIGNATURE, 16)) {
+	memcpy_fromio(buf, base, sizeof(buf));
+
+	if (memcmp(buf, OPREGION_SIGNATURE, 16)) {
 		DRM_DEBUG_DRIVER("opregion signature mismatch\n");
 		err = -EINVAL;
 		goto err_out;
@@ -499,7 +510,7 @@ int intel_opregion_setup(struct drm_device *dev)
 
 	opregion->lid_state = base + ACPI_CLID;
 
-	mboxes = opregion->header->mboxes;
+	mboxes = ioread32(&opregion->header->mboxes);
 	if (mboxes & MBOX_ACPI) {
 		DRM_DEBUG_DRIVER("Public ACPI methods supported\n");
 		opregion->acpi = base + OPREGION_ACPI_OFFSET;
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index 80b331c322fb..458743da3774 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -187,14 +187,14 @@ struct intel_overlay {
 	void (*flip_tail)(struct intel_overlay *);
 };
 
-static struct overlay_registers *
+static struct overlay_registers __iomem *
 intel_overlay_map_regs(struct intel_overlay *overlay)
 {
 	drm_i915_private_t *dev_priv = overlay->dev->dev_private;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		regs = overlay->reg_bo->phys_obj->handle->vaddr;
+		regs = (struct overlay_registers __iomem *)overlay->reg_bo->phys_obj->handle->vaddr;
 	else
 		regs = io_mapping_map_wc(dev_priv->mm.gtt_mapping,
 					 overlay->reg_bo->gtt_offset);
@@ -203,7 +203,7 @@ intel_overlay_map_regs(struct intel_overlay *overlay)
 }
 
 static void intel_overlay_unmap_regs(struct intel_overlay *overlay,
-				     struct overlay_registers *regs)
+				     struct overlay_registers __iomem *regs)
 {
 	if (!OVERLAY_NEEDS_PHYSICAL(overlay->dev))
 		io_mapping_unmap(regs);
@@ -215,20 +215,21 @@ static int intel_overlay_do_wait_request(struct intel_overlay *overlay,
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
 	BUG_ON(overlay->last_flip_req);
-	ret = i915_add_request(LP_RING(dev_priv), NULL, request);
+	ret = i915_add_request(ring, NULL, request);
 	if (ret) {
 	    kfree(request);
 	    return ret;
 	}
 	overlay->last_flip_req = request->seqno;
 	overlay->flip_tail = tail;
-	ret = i915_wait_request(LP_RING(dev_priv), overlay->last_flip_req,
-				true);
+	ret = i915_wait_request(ring, overlay->last_flip_req);
 	if (ret)
 		return ret;
+	i915_gem_retire_requests(dev);
 
 	overlay->last_flip_req = 0;
 	return 0;
@@ -262,7 +263,7 @@ i830_activate_pipe_a(struct drm_device *dev)
 	DRM_DEBUG_DRIVER("Enabling pipe A in order to enable overlay\n");
 
 	mode = drm_mode_duplicate(dev, &vesa_640x480);
-	drm_mode_set_crtcinfo(mode, 0);
+
 	if (!drm_crtc_helper_set_mode(&crtc->base, mode,
 				       crtc->base.x, crtc->base.y,
 				       crtc->base.fb))
@@ -287,6 +288,7 @@ static int intel_overlay_on(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	struct drm_i915_gem_request *request;
 	int pipe_a_quirk = 0;
 	int ret;
@@ -306,17 +308,17 @@ static int intel_overlay_on(struct intel_overlay *overlay)
 		goto out;
 	}
 
-	ret = BEGIN_LP_RING(4);
+	ret = intel_ring_begin(ring, 4);
 	if (ret) {
 		kfree(request);
 		goto out;
 	}
 
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_ON);
-	OUT_RING(overlay->flip_addr | OFC_UPDATE);
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
-	OUT_RING(MI_NOOP);
-	ADVANCE_LP_RING();
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_ON);
+	intel_ring_emit(ring, overlay->flip_addr | OFC_UPDATE);
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_advance(ring);
 
 	ret = intel_overlay_do_wait_request(overlay, request, NULL);
 out:
@@ -332,6 +334,7 @@ static int intel_overlay_continue(struct intel_overlay *overlay,
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	struct drm_i915_gem_request *request;
 	u32 flip_addr = overlay->flip_addr;
 	u32 tmp;
@@ -351,16 +354,16 @@ static int intel_overlay_continue(struct intel_overlay *overlay,
 	if (tmp & (1 << 17))
 		DRM_DEBUG("overlay underrun, DOVSTA: %x\n", tmp);
 
-	ret = BEGIN_LP_RING(2);
+	ret = intel_ring_begin(ring, 2);
 	if (ret) {
 		kfree(request);
 		return ret;
 	}
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
-	OUT_RING(flip_addr);
-	ADVANCE_LP_RING();
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
+	intel_ring_emit(ring, flip_addr);
+	intel_ring_advance(ring);
 
-	ret = i915_add_request(LP_RING(dev_priv), NULL, request);
+	ret = i915_add_request(ring, NULL, request);
 	if (ret) {
 		kfree(request);
 		return ret;
@@ -401,6 +404,7 @@ static int intel_overlay_off(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	u32 flip_addr = overlay->flip_addr;
 	struct drm_i915_gem_request *request;
 	int ret;
@@ -417,20 +421,20 @@ static int intel_overlay_off(struct intel_overlay *overlay)
 	 * of the hw. Do it in both cases */
 	flip_addr |= OFC_UPDATE;
 
-	ret = BEGIN_LP_RING(6);
+	ret = intel_ring_begin(ring, 6);
 	if (ret) {
 		kfree(request);
 		return ret;
 	}
 	/* wait for overlay to go idle */
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
-	OUT_RING(flip_addr);
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
+	intel_ring_emit(ring, flip_addr);
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
 	/* turn overlay off */
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_OFF);
-	OUT_RING(flip_addr);
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
-	ADVANCE_LP_RING();
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_OFF);
+	intel_ring_emit(ring, flip_addr);
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+	intel_ring_advance(ring);
 
 	return intel_overlay_do_wait_request(overlay, request,
 					     intel_overlay_off_tail);
@@ -442,15 +446,16 @@ static int intel_overlay_recover_from_interrupt(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
 	if (overlay->last_flip_req == 0)
 		return 0;
 
-	ret = i915_wait_request(LP_RING(dev_priv), overlay->last_flip_req,
-				true);
+	ret = i915_wait_request(ring, overlay->last_flip_req);
 	if (ret)
 		return ret;
+	i915_gem_retire_requests(dev);
 
 	if (overlay->flip_tail)
 		overlay->flip_tail(overlay);
@@ -467,6 +472,7 @@ static int intel_overlay_release_old_vid(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
 	/* Only wait if there is actually an old frame to release to
@@ -483,15 +489,15 @@ static int intel_overlay_release_old_vid(struct intel_overlay *overlay)
 		if (request == NULL)
 			return -ENOMEM;
 
-		ret = BEGIN_LP_RING(2);
+		ret = intel_ring_begin(ring, 2);
 		if (ret) {
 			kfree(request);
 			return ret;
 		}
 
-		OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
-		OUT_RING(MI_NOOP);
-		ADVANCE_LP_RING();
+		intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+		intel_ring_emit(ring, MI_NOOP);
+		intel_ring_advance(ring);
 
 		ret = intel_overlay_do_wait_request(overlay, request,
 						    intel_overlay_release_old_vid_tail);
@@ -619,14 +625,15 @@ static const u16 uv_static_hcoeffs[N_HORIZ_UV_TAPS * N_PHASES] = {
 	0x3000, 0x0800, 0x3000
 };
 
-static void update_polyphase_filter(struct overlay_registers *regs)
+static void update_polyphase_filter(struct overlay_registers __iomem *regs)
 {
-	memcpy(regs->Y_HCOEFS, y_static_hcoeffs, sizeof(y_static_hcoeffs));
-	memcpy(regs->UV_HCOEFS, uv_static_hcoeffs, sizeof(uv_static_hcoeffs));
+	memcpy_toio(regs->Y_HCOEFS, y_static_hcoeffs, sizeof(y_static_hcoeffs));
+	memcpy_toio(regs->UV_HCOEFS, uv_static_hcoeffs,
+		    sizeof(uv_static_hcoeffs));
 }
 
 static bool update_scaling_factors(struct intel_overlay *overlay,
-				   struct overlay_registers *regs,
+				   struct overlay_registers __iomem *regs,
 				   struct put_image_params *params)
 {
 	/* fixed point with a 12 bit shift */
@@ -665,16 +672,19 @@ static bool update_scaling_factors(struct intel_overlay *overlay,
 	overlay->old_xscale = xscale;
 	overlay->old_yscale = yscale;
 
-	regs->YRGBSCALE = (((yscale & FRACT_MASK) << 20) |
-			   ((xscale >> FP_SHIFT)  << 16) |
-			   ((xscale & FRACT_MASK) << 3));
+	iowrite32(((yscale & FRACT_MASK) << 20) |
+		  ((xscale >> FP_SHIFT)  << 16) |
+		  ((xscale & FRACT_MASK) << 3),
+		 &regs->YRGBSCALE);
 
-	regs->UVSCALE = (((yscale_UV & FRACT_MASK) << 20) |
-			 ((xscale_UV >> FP_SHIFT)  << 16) |
-			 ((xscale_UV & FRACT_MASK) << 3));
+	iowrite32(((yscale_UV & FRACT_MASK) << 20) |
+		  ((xscale_UV >> FP_SHIFT)  << 16) |
+		  ((xscale_UV & FRACT_MASK) << 3),
+		 &regs->UVSCALE);
 
-	regs->UVSCALEV = ((((yscale    >> FP_SHIFT) << 16) |
-			   ((yscale_UV >> FP_SHIFT) << 0)));
+	iowrite32((((yscale    >> FP_SHIFT) << 16) |
+		   ((yscale_UV >> FP_SHIFT) << 0)),
+		 &regs->UVSCALEV);
 
 	if (scale_changed)
 		update_polyphase_filter(regs);
@@ -683,30 +693,32 @@ static bool update_scaling_factors(struct intel_overlay *overlay,
 }
 
 static void update_colorkey(struct intel_overlay *overlay,
-			    struct overlay_registers *regs)
+			    struct overlay_registers __iomem *regs)
 {
 	u32 key = overlay->color_key;
 
 	switch (overlay->crtc->base.fb->bits_per_pixel) {
 	case 8:
-		regs->DCLRKV = 0;
-		regs->DCLRKM = CLK_RGB8I_MASK | DST_KEY_ENABLE;
+		iowrite32(0, &regs->DCLRKV);
+		iowrite32(CLK_RGB8I_MASK | DST_KEY_ENABLE, &regs->DCLRKM);
 		break;
 
 	case 16:
 		if (overlay->crtc->base.fb->depth == 15) {
-			regs->DCLRKV = RGB15_TO_COLORKEY(key);
-			regs->DCLRKM = CLK_RGB15_MASK | DST_KEY_ENABLE;
+			iowrite32(RGB15_TO_COLORKEY(key), &regs->DCLRKV);
+			iowrite32(CLK_RGB15_MASK | DST_KEY_ENABLE,
+				  &regs->DCLRKM);
 		} else {
-			regs->DCLRKV = RGB16_TO_COLORKEY(key);
-			regs->DCLRKM = CLK_RGB16_MASK | DST_KEY_ENABLE;
+			iowrite32(RGB16_TO_COLORKEY(key), &regs->DCLRKV);
+			iowrite32(CLK_RGB16_MASK | DST_KEY_ENABLE,
+				  &regs->DCLRKM);
 		}
 		break;
 
 	case 24:
 	case 32:
-		regs->DCLRKV = key;
-		regs->DCLRKM = CLK_RGB24_MASK | DST_KEY_ENABLE;
+		iowrite32(key, &regs->DCLRKV);
+		iowrite32(CLK_RGB24_MASK | DST_KEY_ENABLE, &regs->DCLRKM);
 		break;
 	}
 }
@@ -761,9 +773,10 @@ static int intel_overlay_do_put_image(struct intel_overlay *overlay,
 				      struct put_image_params *params)
 {
 	int ret, tmp_width;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	bool scale_changed = false;
 	struct drm_device *dev = overlay->dev;
+	u32 swidth, swidthsw, sheight, ostride;
 
 	BUG_ON(!mutex_is_locked(&dev->struct_mutex));
 	BUG_ON(!mutex_is_locked(&dev->mode_config.mutex));
@@ -782,16 +795,18 @@ static int intel_overlay_do_put_image(struct intel_overlay *overlay,
 		goto out_unpin;
 
 	if (!overlay->active) {
+		u32 oconfig;
 		regs = intel_overlay_map_regs(overlay);
 		if (!regs) {
 			ret = -ENOMEM;
 			goto out_unpin;
 		}
-		regs->OCONFIG = OCONF_CC_OUT_8BIT;
+		oconfig = OCONF_CC_OUT_8BIT;
 		if (IS_GEN4(overlay->dev))
-			regs->OCONFIG |= OCONF_CSC_MODE_BT709;
-		regs->OCONFIG |= overlay->crtc->pipe == 0 ?
+			oconfig |= OCONF_CSC_MODE_BT709;
+		oconfig |= overlay->crtc->pipe == 0 ?
 			OCONF_PIPE_A : OCONF_PIPE_B;
+		iowrite32(oconfig, &regs->OCONFIG);
 		intel_overlay_unmap_regs(overlay, regs);
 
 		ret = intel_overlay_on(overlay);
@@ -805,42 +820,46 @@ static int intel_overlay_do_put_image(struct intel_overlay *overlay,
 		goto out_unpin;
 	}
 
-	regs->DWINPOS = (params->dst_y << 16) | params->dst_x;
-	regs->DWINSZ = (params->dst_h << 16) | params->dst_w;
+	iowrite32((params->dst_y << 16) | params->dst_x, &regs->DWINPOS);
+	iowrite32((params->dst_h << 16) | params->dst_w, &regs->DWINSZ);
 
 	if (params->format & I915_OVERLAY_YUV_PACKED)
 		tmp_width = packed_width_bytes(params->format, params->src_w);
 	else
 		tmp_width = params->src_w;
 
-	regs->SWIDTH = params->src_w;
-	regs->SWIDTHSW = calc_swidthsw(overlay->dev,
-				       params->offset_Y, tmp_width);
-	regs->SHEIGHT = params->src_h;
-	regs->OBUF_0Y = new_bo->gtt_offset + params->offset_Y;
-	regs->OSTRIDE = params->stride_Y;
+	swidth = params->src_w;
+	swidthsw = calc_swidthsw(overlay->dev, params->offset_Y, tmp_width);
+	sheight = params->src_h;
+	iowrite32(new_bo->gtt_offset + params->offset_Y, &regs->OBUF_0Y);
+	ostride = params->stride_Y;
 
 	if (params->format & I915_OVERLAY_YUV_PLANAR) {
 		int uv_hscale = uv_hsubsampling(params->format);
 		int uv_vscale = uv_vsubsampling(params->format);
 		u32 tmp_U, tmp_V;
-		regs->SWIDTH |= (params->src_w/uv_hscale) << 16;
+		swidth |= (params->src_w/uv_hscale) << 16;
 		tmp_U = calc_swidthsw(overlay->dev, params->offset_U,
 				      params->src_w/uv_hscale);
 		tmp_V = calc_swidthsw(overlay->dev, params->offset_V,
 				      params->src_w/uv_hscale);
-		regs->SWIDTHSW |= max_t(u32, tmp_U, tmp_V) << 16;
-		regs->SHEIGHT |= (params->src_h/uv_vscale) << 16;
-		regs->OBUF_0U = new_bo->gtt_offset + params->offset_U;
-		regs->OBUF_0V = new_bo->gtt_offset + params->offset_V;
-		regs->OSTRIDE |= params->stride_UV << 16;
+		swidthsw |= max_t(u32, tmp_U, tmp_V) << 16;
+		sheight |= (params->src_h/uv_vscale) << 16;
+		iowrite32(new_bo->gtt_offset + params->offset_U, &regs->OBUF_0U);
+		iowrite32(new_bo->gtt_offset + params->offset_V, &regs->OBUF_0V);
+		ostride |= params->stride_UV << 16;
 	}
 
+	iowrite32(swidth, &regs->SWIDTH);
+	iowrite32(swidthsw, &regs->SWIDTHSW);
+	iowrite32(sheight, &regs->SHEIGHT);
+	iowrite32(ostride, &regs->OSTRIDE);
+
 	scale_changed = update_scaling_factors(overlay, regs, params);
 
 	update_colorkey(overlay, regs);
 
-	regs->OCMD = overlay_cmd_reg(params);
+	iowrite32(overlay_cmd_reg(params), &regs->OCMD);
 
 	intel_overlay_unmap_regs(overlay, regs);
 
@@ -860,7 +879,7 @@ out_unpin:
 
 int intel_overlay_switch_off(struct intel_overlay *overlay)
 {
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	struct drm_device *dev = overlay->dev;
 	int ret;
 
@@ -879,7 +898,7 @@ int intel_overlay_switch_off(struct intel_overlay *overlay)
 		return ret;
 
 	regs = intel_overlay_map_regs(overlay);
-	regs->OCMD = 0;
+	iowrite32(0, &regs->OCMD);
 	intel_overlay_unmap_regs(overlay, regs);
 
 	ret = intel_overlay_off(overlay);
@@ -1109,11 +1128,7 @@ int intel_overlay_put_image(struct drm_device *dev, void *data,
 	struct put_image_params *params;
 	int ret;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
+	/* No need to check for DRIVER_MODESET - we don't set it up then. */
 	overlay = dev_priv->overlay;
 	if (!overlay) {
 		DRM_DEBUG("userspace bug: no overlay\n");
@@ -1250,10 +1265,11 @@ out_free:
 }
 
 static void update_reg_attrs(struct intel_overlay *overlay,
-			     struct overlay_registers *regs)
+			     struct overlay_registers __iomem *regs)
 {
-	regs->OCLRC0 = (overlay->contrast << 18) | (overlay->brightness & 0xff);
-	regs->OCLRC1 = overlay->saturation;
+	iowrite32((overlay->contrast << 18) | (overlay->brightness & 0xff),
+		  &regs->OCLRC0);
+	iowrite32(overlay->saturation, &regs->OCLRC1);
 }
 
 static bool check_gamma_bounds(u32 gamma1, u32 gamma2)
@@ -1306,14 +1322,10 @@ int intel_overlay_attrs(struct drm_device *dev, void *data,
 	struct drm_intel_overlay_attrs *attrs = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_overlay *overlay;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	int ret;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
+	/* No need to check for DRIVER_MODESET - we don't set it up then. */
 	overlay = dev_priv->overlay;
 	if (!overlay) {
 		DRM_DEBUG("userspace bug: no overlay\n");
@@ -1396,7 +1408,7 @@ void intel_setup_overlay(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_overlay *overlay;
 	struct drm_i915_gem_object *reg_bo;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	int ret;
 
 	if (!HAS_OVERLAY(dev))
@@ -1451,7 +1463,7 @@ void intel_setup_overlay(struct drm_device *dev)
 	if (!regs)
 		goto out_unpin_bo;
 
-	memset(regs, 0, sizeof(struct overlay_registers));
+	memset_io(regs, 0, sizeof(struct overlay_registers));
 	update_polyphase_filter(regs);
 	update_reg_attrs(overlay, regs);
 
@@ -1499,14 +1511,17 @@ struct intel_overlay_error_state {
 	u32 isr;
 };
 
-static struct overlay_registers *
+static struct overlay_registers __iomem *
 intel_overlay_map_regs_atomic(struct intel_overlay *overlay)
 {
 	drm_i915_private_t *dev_priv = overlay->dev->dev_private;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		regs = overlay->reg_bo->phys_obj->handle->vaddr;
+		/* Cast to make sparse happy, but it's wc memory anyway, so
+		 * equivalent to the wc io mapping on X86. */
+		regs = (struct overlay_registers __iomem *)
+			overlay->reg_bo->phys_obj->handle->vaddr;
 	else
 		regs = io_mapping_map_atomic_wc(dev_priv->mm.gtt_mapping,
 						overlay->reg_bo->gtt_offset);
@@ -1515,7 +1530,7 @@ intel_overlay_map_regs_atomic(struct intel_overlay *overlay)
 }
 
 static void intel_overlay_unmap_regs_atomic(struct intel_overlay *overlay,
-					    struct overlay_registers *regs)
+					struct overlay_registers __iomem *regs)
 {
 	if (!OVERLAY_NEEDS_PHYSICAL(overlay->dev))
 		io_mapping_unmap_atomic(regs);
@@ -1540,9 +1555,9 @@ intel_overlay_capture_error_state(struct drm_device *dev)
 	error->dovsta = I915_READ(DOVSTA);
 	error->isr = I915_READ(ISR);
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		error->base = (long) overlay->reg_bo->phys_obj->handle->vaddr;
+		error->base = (__force long)overlay->reg_bo->phys_obj->handle->vaddr;
 	else
-		error->base = (long) overlay->reg_bo->gtt_offset;
+		error->base = overlay->reg_bo->gtt_offset;
 
 	regs = intel_overlay_map_regs_atomic(overlay);
 	if (!regs)
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index 48177ec4720e..2a1625d84a69 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -28,6 +28,9 @@
  *      Chris Wilson <chris@chris-wilson.co.uk>
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/moduleparam.h>
 #include "intel_drv.h"
 
 #define PCI_LBPC 0xf4 /* legacy/combination backlight modes */
@@ -169,7 +172,7 @@ u32 intel_panel_get_max_backlight(struct drm_device *dev)
 		/* XXX add code here to query mode clock or hardware clock
 		 * and program max PWM appropriately.
 		 */
-		printk_once(KERN_WARNING "fixme: max PWM is zero.\n");
+		pr_warn_once("fixme: max PWM is zero\n");
 		return 1;
 	}
 
@@ -189,6 +192,27 @@ u32 intel_panel_get_max_backlight(struct drm_device *dev)
 	return max;
 }
 
+static int i915_panel_invert_brightness;
+MODULE_PARM_DESC(invert_brightness, "Invert backlight brightness "
+	"(-1 force normal, 0 machine defaults, 1 force inversion), please "
+	"report PCI device ID, subsystem vendor and subsystem device ID "
+	"to dri-devel@lists.freedesktop.org, if your machine needs it. "
+	"It will then be included in an upcoming module version.");
+module_param_named(invert_brightness, i915_panel_invert_brightness, int, 0600);
+static u32 intel_panel_compute_brightness(struct drm_device *dev, u32 val)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (i915_panel_invert_brightness < 0)
+		return val;
+
+	if (i915_panel_invert_brightness > 0 ||
+	    dev_priv->quirks & QUIRK_INVERT_BRIGHTNESS)
+		return intel_panel_get_max_backlight(dev) - val;
+
+	return val;
+}
+
 u32 intel_panel_get_backlight(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -209,6 +233,7 @@ u32 intel_panel_get_backlight(struct drm_device *dev)
 		}
 	}
 
+	val = intel_panel_compute_brightness(dev, val);
 	DRM_DEBUG_DRIVER("get backlight PWM = %d\n", val);
 	return val;
 }
@@ -226,6 +251,7 @@ static void intel_panel_actually_set_backlight(struct drm_device *dev, u32 level
 	u32 tmp;
 
 	DRM_DEBUG_DRIVER("set backlight PWM = %d\n", level);
+	level = intel_panel_compute_brightness(dev, level);
 
 	if (HAS_PCH_SPLIT(dev))
 		return intel_pch_panel_set_backlight(dev, level);
@@ -342,6 +368,7 @@ int intel_panel_setup_backlight(struct drm_device *dev)
 	else
 		return -ENODEV;
 
+	memset(&props, 0, sizeof(props));
 	props.type = BACKLIGHT_RAW;
 	props.max_brightness = intel_panel_get_max_backlight(dev);
 	dev_priv->backlight =
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
new file mode 100644
index 000000000000..8e79ff67ec98
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -0,0 +1,3796 @@
+/*
+ * Copyright © 2012 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Eugeni Dodonov <eugeni.dodonov@intel.com>
+ *
+ */
+
+#include <linux/cpufreq.h>
+#include "i915_drv.h"
+#include "intel_drv.h"
+#include "../../../platform/x86/intel_ips.h"
+#include <linux/module.h>
+
+/* FBC, or Frame Buffer Compression, is a technique employed to compress the
+ * framebuffer contents in-memory, aiming at reducing the required bandwidth
+ * during in-memory transfers and, therefore, reduce the power packet.
+ *
+ * The benefits of FBC are mostly visible with solid backgrounds and
+ * variation-less patterns.
+ *
+ * FBC-related functionality can be enabled by the means of the
+ * i915.i915_enable_fbc parameter
+ */
+
+static void i8xx_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 fbc_ctl;
+
+	/* Disable compression */
+	fbc_ctl = I915_READ(FBC_CONTROL);
+	if ((fbc_ctl & FBC_CTL_EN) == 0)
+		return;
+
+	fbc_ctl &= ~FBC_CTL_EN;
+	I915_WRITE(FBC_CONTROL, fbc_ctl);
+
+	/* Wait for compressing bit to clear */
+	if (wait_for((I915_READ(FBC_STATUS) & FBC_STAT_COMPRESSING) == 0, 10)) {
+		DRM_DEBUG_KMS("FBC idle timed out\n");
+		return;
+	}
+
+	DRM_DEBUG_KMS("disabled FBC\n");
+}
+
+static void i8xx_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_framebuffer *fb = crtc->fb;
+	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int cfb_pitch;
+	int plane, i;
+	u32 fbc_ctl, fbc_ctl2;
+
+	cfb_pitch = dev_priv->cfb_size / FBC_LL_SIZE;
+	if (fb->pitches[0] < cfb_pitch)
+		cfb_pitch = fb->pitches[0];
+
+	/* FBC_CTL wants 64B units */
+	cfb_pitch = (cfb_pitch / 64) - 1;
+	plane = intel_crtc->plane == 0 ? FBC_CTL_PLANEA : FBC_CTL_PLANEB;
+
+	/* Clear old tags */
+	for (i = 0; i < (FBC_LL_SIZE / 32) + 1; i++)
+		I915_WRITE(FBC_TAG + (i * 4), 0);
+
+	/* Set it up... */
+	fbc_ctl2 = FBC_CTL_FENCE_DBL | FBC_CTL_IDLE_IMM | FBC_CTL_CPU_FENCE;
+	fbc_ctl2 |= plane;
+	I915_WRITE(FBC_CONTROL2, fbc_ctl2);
+	I915_WRITE(FBC_FENCE_OFF, crtc->y);
+
+	/* enable it... */
+	fbc_ctl = FBC_CTL_EN | FBC_CTL_PERIODIC;
+	if (IS_I945GM(dev))
+		fbc_ctl |= FBC_CTL_C3_IDLE; /* 945 needs special SR handling */
+	fbc_ctl |= (cfb_pitch & 0xff) << FBC_CTL_STRIDE_SHIFT;
+	fbc_ctl |= (interval & 0x2fff) << FBC_CTL_INTERVAL_SHIFT;
+	fbc_ctl |= obj->fence_reg;
+	I915_WRITE(FBC_CONTROL, fbc_ctl);
+
+	DRM_DEBUG_KMS("enabled FBC, pitch %d, yoff %d, plane %d, ",
+		      cfb_pitch, crtc->y, intel_crtc->plane);
+}
+
+static bool i8xx_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	return I915_READ(FBC_CONTROL) & FBC_CTL_EN;
+}
+
+static void g4x_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_framebuffer *fb = crtc->fb;
+	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
+	unsigned long stall_watermark = 200;
+	u32 dpfc_ctl;
+
+	dpfc_ctl = plane | DPFC_SR_EN | DPFC_CTL_LIMIT_1X;
+	dpfc_ctl |= DPFC_CTL_FENCE_EN | obj->fence_reg;
+	I915_WRITE(DPFC_CHICKEN, DPFC_HT_MODIFY);
+
+	I915_WRITE(DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
+		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
+		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
+	I915_WRITE(DPFC_FENCE_YOFF, crtc->y);
+
+	/* enable it... */
+	I915_WRITE(DPFC_CONTROL, I915_READ(DPFC_CONTROL) | DPFC_CTL_EN);
+
+	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
+}
+
+static void g4x_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 dpfc_ctl;
+
+	/* Disable compression */
+	dpfc_ctl = I915_READ(DPFC_CONTROL);
+	if (dpfc_ctl & DPFC_CTL_EN) {
+		dpfc_ctl &= ~DPFC_CTL_EN;
+		I915_WRITE(DPFC_CONTROL, dpfc_ctl);
+
+		DRM_DEBUG_KMS("disabled FBC\n");
+	}
+}
+
+static bool g4x_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	return I915_READ(DPFC_CONTROL) & DPFC_CTL_EN;
+}
+
+static void sandybridge_blit_fbc_update(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 blt_ecoskpd;
+
+	/* Make sure blitter notifies FBC of writes */
+	gen6_gt_force_wake_get(dev_priv);
+	blt_ecoskpd = I915_READ(GEN6_BLITTER_ECOSKPD);
+	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY <<
+		GEN6_BLITTER_LOCK_SHIFT;
+	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
+	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY;
+	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
+	blt_ecoskpd &= ~(GEN6_BLITTER_FBC_NOTIFY <<
+			 GEN6_BLITTER_LOCK_SHIFT);
+	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
+	POSTING_READ(GEN6_BLITTER_ECOSKPD);
+	gen6_gt_force_wake_put(dev_priv);
+}
+
+static void ironlake_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_framebuffer *fb = crtc->fb;
+	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
+	unsigned long stall_watermark = 200;
+	u32 dpfc_ctl;
+
+	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
+	dpfc_ctl &= DPFC_RESERVED;
+	dpfc_ctl |= (plane | DPFC_CTL_LIMIT_1X);
+	/* Set persistent mode for front-buffer rendering, ala X. */
+	dpfc_ctl |= DPFC_CTL_PERSISTENT_MODE;
+	dpfc_ctl |= (DPFC_CTL_FENCE_EN | obj->fence_reg);
+	I915_WRITE(ILK_DPFC_CHICKEN, DPFC_HT_MODIFY);
+
+	I915_WRITE(ILK_DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
+		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
+		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
+	I915_WRITE(ILK_DPFC_FENCE_YOFF, crtc->y);
+	I915_WRITE(ILK_FBC_RT_BASE, obj->gtt_offset | ILK_FBC_RT_VALID);
+	/* enable it... */
+	I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl | DPFC_CTL_EN);
+
+	if (IS_GEN6(dev)) {
+		I915_WRITE(SNB_DPFC_CTL_SA,
+			   SNB_CPU_FENCE_ENABLE | obj->fence_reg);
+		I915_WRITE(DPFC_CPU_FENCE_OFFSET, crtc->y);
+		sandybridge_blit_fbc_update(dev);
+	}
+
+	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
+}
+
+static void ironlake_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 dpfc_ctl;
+
+	/* Disable compression */
+	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
+	if (dpfc_ctl & DPFC_CTL_EN) {
+		dpfc_ctl &= ~DPFC_CTL_EN;
+		I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl);
+
+		DRM_DEBUG_KMS("disabled FBC\n");
+	}
+}
+
+static bool ironlake_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	return I915_READ(ILK_DPFC_CONTROL) & DPFC_CTL_EN;
+}
+
+bool intel_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->display.fbc_enabled)
+		return false;
+
+	return dev_priv->display.fbc_enabled(dev);
+}
+
+static void intel_fbc_work_fn(struct work_struct *__work)
+{
+	struct intel_fbc_work *work =
+		container_of(to_delayed_work(__work),
+			     struct intel_fbc_work, work);
+	struct drm_device *dev = work->crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&dev->struct_mutex);
+	if (work == dev_priv->fbc_work) {
+		/* Double check that we haven't switched fb without cancelling
+		 * the prior work.
+		 */
+		if (work->crtc->fb == work->fb) {
+			dev_priv->display.enable_fbc(work->crtc,
+						     work->interval);
+
+			dev_priv->cfb_plane = to_intel_crtc(work->crtc)->plane;
+			dev_priv->cfb_fb = work->crtc->fb->base.id;
+			dev_priv->cfb_y = work->crtc->y;
+		}
+
+		dev_priv->fbc_work = NULL;
+	}
+	mutex_unlock(&dev->struct_mutex);
+
+	kfree(work);
+}
+
+static void intel_cancel_fbc_work(struct drm_i915_private *dev_priv)
+{
+	if (dev_priv->fbc_work == NULL)
+		return;
+
+	DRM_DEBUG_KMS("cancelling pending FBC enable\n");
+
+	/* Synchronisation is provided by struct_mutex and checking of
+	 * dev_priv->fbc_work, so we can perform the cancellation
+	 * entirely asynchronously.
+	 */
+	if (cancel_delayed_work(&dev_priv->fbc_work->work))
+		/* tasklet was killed before being run, clean up */
+		kfree(dev_priv->fbc_work);
+
+	/* Mark the work as no longer wanted so that if it does
+	 * wake-up (because the work was already running and waiting
+	 * for our mutex), it will discover that is no longer
+	 * necessary to run.
+	 */
+	dev_priv->fbc_work = NULL;
+}
+
+void intel_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct intel_fbc_work *work;
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->display.enable_fbc)
+		return;
+
+	intel_cancel_fbc_work(dev_priv);
+
+	work = kzalloc(sizeof *work, GFP_KERNEL);
+	if (work == NULL) {
+		dev_priv->display.enable_fbc(crtc, interval);
+		return;
+	}
+
+	work->crtc = crtc;
+	work->fb = crtc->fb;
+	work->interval = interval;
+	INIT_DELAYED_WORK(&work->work, intel_fbc_work_fn);
+
+	dev_priv->fbc_work = work;
+
+	DRM_DEBUG_KMS("scheduling delayed FBC enable\n");
+
+	/* Delay the actual enabling to let pageflipping cease and the
+	 * display to settle before starting the compression. Note that
+	 * this delay also serves a second purpose: it allows for a
+	 * vblank to pass after disabling the FBC before we attempt
+	 * to modify the control registers.
+	 *
+	 * A more complicated solution would involve tracking vblanks
+	 * following the termination of the page-flipping sequence
+	 * and indeed performing the enable as a co-routine and not
+	 * waiting synchronously upon the vblank.
+	 */
+	schedule_delayed_work(&work->work, msecs_to_jiffies(50));
+}
+
+void intel_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	intel_cancel_fbc_work(dev_priv);
+
+	if (!dev_priv->display.disable_fbc)
+		return;
+
+	dev_priv->display.disable_fbc(dev);
+	dev_priv->cfb_plane = -1;
+}
+
+/**
+ * intel_update_fbc - enable/disable FBC as needed
+ * @dev: the drm_device
+ *
+ * Set up the framebuffer compression hardware at mode set time.  We
+ * enable it if possible:
+ *   - plane A only (on pre-965)
+ *   - no pixel mulitply/line duplication
+ *   - no alpha buffer discard
+ *   - no dual wide
+ *   - framebuffer <= 2048 in width, 1536 in height
+ *
+ * We can't assume that any compression will take place (worst case),
+ * so the compressed buffer has to be the same size as the uncompressed
+ * one.  It also must reside (along with the line length buffer) in
+ * stolen memory.
+ *
+ * We need to enable/disable FBC on a global basis.
+ */
+void intel_update_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = NULL, *tmp_crtc;
+	struct intel_crtc *intel_crtc;
+	struct drm_framebuffer *fb;
+	struct intel_framebuffer *intel_fb;
+	struct drm_i915_gem_object *obj;
+	int enable_fbc;
+
+	DRM_DEBUG_KMS("\n");
+
+	if (!i915_powersave)
+		return;
+
+	if (!I915_HAS_FBC(dev))
+		return;
+
+	/*
+	 * If FBC is already on, we just have to verify that we can
+	 * keep it that way...
+	 * Need to disable if:
+	 *   - more than one pipe is active
+	 *   - changing FBC params (stride, fence, mode)
+	 *   - new fb is too large to fit in compressed buffer
+	 *   - going to an unsupported config (interlace, pixel multiply, etc.)
+	 */
+	list_for_each_entry(tmp_crtc, &dev->mode_config.crtc_list, head) {
+		if (tmp_crtc->enabled && tmp_crtc->fb) {
+			if (crtc) {
+				DRM_DEBUG_KMS("more than one pipe active, disabling compression\n");
+				dev_priv->no_fbc_reason = FBC_MULTIPLE_PIPES;
+				goto out_disable;
+			}
+			crtc = tmp_crtc;
+		}
+	}
+
+	if (!crtc || crtc->fb == NULL) {
+		DRM_DEBUG_KMS("no output, disabling\n");
+		dev_priv->no_fbc_reason = FBC_NO_OUTPUT;
+		goto out_disable;
+	}
+
+	intel_crtc = to_intel_crtc(crtc);
+	fb = crtc->fb;
+	intel_fb = to_intel_framebuffer(fb);
+	obj = intel_fb->obj;
+
+	enable_fbc = i915_enable_fbc;
+	if (enable_fbc < 0) {
+		DRM_DEBUG_KMS("fbc set to per-chip default\n");
+		enable_fbc = 1;
+		if (INTEL_INFO(dev)->gen <= 6)
+			enable_fbc = 0;
+	}
+	if (!enable_fbc) {
+		DRM_DEBUG_KMS("fbc disabled per module param\n");
+		dev_priv->no_fbc_reason = FBC_MODULE_PARAM;
+		goto out_disable;
+	}
+	if (intel_fb->obj->base.size > dev_priv->cfb_size) {
+		DRM_DEBUG_KMS("framebuffer too large, disabling "
+			      "compression\n");
+		dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
+		goto out_disable;
+	}
+	if ((crtc->mode.flags & DRM_MODE_FLAG_INTERLACE) ||
+	    (crtc->mode.flags & DRM_MODE_FLAG_DBLSCAN)) {
+		DRM_DEBUG_KMS("mode incompatible with compression, "
+			      "disabling\n");
+		dev_priv->no_fbc_reason = FBC_UNSUPPORTED_MODE;
+		goto out_disable;
+	}
+	if ((crtc->mode.hdisplay > 2048) ||
+	    (crtc->mode.vdisplay > 1536)) {
+		DRM_DEBUG_KMS("mode too large for compression, disabling\n");
+		dev_priv->no_fbc_reason = FBC_MODE_TOO_LARGE;
+		goto out_disable;
+	}
+	if ((IS_I915GM(dev) || IS_I945GM(dev)) && intel_crtc->plane != 0) {
+		DRM_DEBUG_KMS("plane not 0, disabling compression\n");
+		dev_priv->no_fbc_reason = FBC_BAD_PLANE;
+		goto out_disable;
+	}
+
+	/* The use of a CPU fence is mandatory in order to detect writes
+	 * by the CPU to the scanout and trigger updates to the FBC.
+	 */
+	if (obj->tiling_mode != I915_TILING_X ||
+	    obj->fence_reg == I915_FENCE_REG_NONE) {
+		DRM_DEBUG_KMS("framebuffer not tiled or fenced, disabling compression\n");
+		dev_priv->no_fbc_reason = FBC_NOT_TILED;
+		goto out_disable;
+	}
+
+	/* If the kernel debugger is active, always disable compression */
+	if (in_dbg_master())
+		goto out_disable;
+
+	/* If the scanout has not changed, don't modify the FBC settings.
+	 * Note that we make the fundamental assumption that the fb->obj
+	 * cannot be unpinned (and have its GTT offset and fence revoked)
+	 * without first being decoupled from the scanout and FBC disabled.
+	 */
+	if (dev_priv->cfb_plane == intel_crtc->plane &&
+	    dev_priv->cfb_fb == fb->base.id &&
+	    dev_priv->cfb_y == crtc->y)
+		return;
+
+	if (intel_fbc_enabled(dev)) {
+		/* We update FBC along two paths, after changing fb/crtc
+		 * configuration (modeswitching) and after page-flipping
+		 * finishes. For the latter, we know that not only did
+		 * we disable the FBC at the start of the page-flip
+		 * sequence, but also more than one vblank has passed.
+		 *
+		 * For the former case of modeswitching, it is possible
+		 * to switch between two FBC valid configurations
+		 * instantaneously so we do need to disable the FBC
+		 * before we can modify its control registers. We also
+		 * have to wait for the next vblank for that to take
+		 * effect. However, since we delay enabling FBC we can
+		 * assume that a vblank has passed since disabling and
+		 * that we can safely alter the registers in the deferred
+		 * callback.
+		 *
+		 * In the scenario that we go from a valid to invalid
+		 * and then back to valid FBC configuration we have
+		 * no strict enforcement that a vblank occurred since
+		 * disabling the FBC. However, along all current pipe
+		 * disabling paths we do need to wait for a vblank at
+		 * some point. And we wait before enabling FBC anyway.
+		 */
+		DRM_DEBUG_KMS("disabling active FBC for update\n");
+		intel_disable_fbc(dev);
+	}
+
+	intel_enable_fbc(crtc, 500);
+	return;
+
+out_disable:
+	/* Multiple disables should be harmless */
+	if (intel_fbc_enabled(dev)) {
+		DRM_DEBUG_KMS("unsupported config, disabling FBC\n");
+		intel_disable_fbc(dev);
+	}
+}
+
+static void i915_pineview_get_mem_freq(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	u32 tmp;
+
+	tmp = I915_READ(CLKCFG);
+
+	switch (tmp & CLKCFG_FSB_MASK) {
+	case CLKCFG_FSB_533:
+		dev_priv->fsb_freq = 533; /* 133*4 */
+		break;
+	case CLKCFG_FSB_800:
+		dev_priv->fsb_freq = 800; /* 200*4 */
+		break;
+	case CLKCFG_FSB_667:
+		dev_priv->fsb_freq =  667; /* 167*4 */
+		break;
+	case CLKCFG_FSB_400:
+		dev_priv->fsb_freq = 400; /* 100*4 */
+		break;
+	}
+
+	switch (tmp & CLKCFG_MEM_MASK) {
+	case CLKCFG_MEM_533:
+		dev_priv->mem_freq = 533;
+		break;
+	case CLKCFG_MEM_667:
+		dev_priv->mem_freq = 667;
+		break;
+	case CLKCFG_MEM_800:
+		dev_priv->mem_freq = 800;
+		break;
+	}
+
+	/* detect pineview DDR3 setting */
+	tmp = I915_READ(CSHRDDR3CTL);
+	dev_priv->is_ddr3 = (tmp & CSHRDDR3CTL_DDR3) ? 1 : 0;
+}
+
+static void i915_ironlake_get_mem_freq(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	u16 ddrpll, csipll;
+
+	ddrpll = I915_READ16(DDRMPLL1);
+	csipll = I915_READ16(CSIPLL0);
+
+	switch (ddrpll & 0xff) {
+	case 0xc:
+		dev_priv->mem_freq = 800;
+		break;
+	case 0x10:
+		dev_priv->mem_freq = 1066;
+		break;
+	case 0x14:
+		dev_priv->mem_freq = 1333;
+		break;
+	case 0x18:
+		dev_priv->mem_freq = 1600;
+		break;
+	default:
+		DRM_DEBUG_DRIVER("unknown memory frequency 0x%02x\n",
+				 ddrpll & 0xff);
+		dev_priv->mem_freq = 0;
+		break;
+	}
+
+	dev_priv->r_t = dev_priv->mem_freq;
+
+	switch (csipll & 0x3ff) {
+	case 0x00c:
+		dev_priv->fsb_freq = 3200;
+		break;
+	case 0x00e:
+		dev_priv->fsb_freq = 3733;
+		break;
+	case 0x010:
+		dev_priv->fsb_freq = 4266;
+		break;
+	case 0x012:
+		dev_priv->fsb_freq = 4800;
+		break;
+	case 0x014:
+		dev_priv->fsb_freq = 5333;
+		break;
+	case 0x016:
+		dev_priv->fsb_freq = 5866;
+		break;
+	case 0x018:
+		dev_priv->fsb_freq = 6400;
+		break;
+	default:
+		DRM_DEBUG_DRIVER("unknown fsb frequency 0x%04x\n",
+				 csipll & 0x3ff);
+		dev_priv->fsb_freq = 0;
+		break;
+	}
+
+	if (dev_priv->fsb_freq == 3200) {
+		dev_priv->c_m = 0;
+	} else if (dev_priv->fsb_freq > 3200 && dev_priv->fsb_freq <= 4800) {
+		dev_priv->c_m = 1;
+	} else {
+		dev_priv->c_m = 2;
+	}
+}
+
+static const struct cxsr_latency cxsr_latency_table[] = {
+	{1, 0, 800, 400, 3382, 33382, 3983, 33983},    /* DDR2-400 SC */
+	{1, 0, 800, 667, 3354, 33354, 3807, 33807},    /* DDR2-667 SC */
+	{1, 0, 800, 800, 3347, 33347, 3763, 33763},    /* DDR2-800 SC */
+	{1, 1, 800, 667, 6420, 36420, 6873, 36873},    /* DDR3-667 SC */
+	{1, 1, 800, 800, 5902, 35902, 6318, 36318},    /* DDR3-800 SC */
+
+	{1, 0, 667, 400, 3400, 33400, 4021, 34021},    /* DDR2-400 SC */
+	{1, 0, 667, 667, 3372, 33372, 3845, 33845},    /* DDR2-667 SC */
+	{1, 0, 667, 800, 3386, 33386, 3822, 33822},    /* DDR2-800 SC */
+	{1, 1, 667, 667, 6438, 36438, 6911, 36911},    /* DDR3-667 SC */
+	{1, 1, 667, 800, 5941, 35941, 6377, 36377},    /* DDR3-800 SC */
+
+	{1, 0, 400, 400, 3472, 33472, 4173, 34173},    /* DDR2-400 SC */
+	{1, 0, 400, 667, 3443, 33443, 3996, 33996},    /* DDR2-667 SC */
+	{1, 0, 400, 800, 3430, 33430, 3946, 33946},    /* DDR2-800 SC */
+	{1, 1, 400, 667, 6509, 36509, 7062, 37062},    /* DDR3-667 SC */
+	{1, 1, 400, 800, 5985, 35985, 6501, 36501},    /* DDR3-800 SC */
+
+	{0, 0, 800, 400, 3438, 33438, 4065, 34065},    /* DDR2-400 SC */
+	{0, 0, 800, 667, 3410, 33410, 3889, 33889},    /* DDR2-667 SC */
+	{0, 0, 800, 800, 3403, 33403, 3845, 33845},    /* DDR2-800 SC */
+	{0, 1, 800, 667, 6476, 36476, 6955, 36955},    /* DDR3-667 SC */
+	{0, 1, 800, 800, 5958, 35958, 6400, 36400},    /* DDR3-800 SC */
+
+	{0, 0, 667, 400, 3456, 33456, 4103, 34106},    /* DDR2-400 SC */
+	{0, 0, 667, 667, 3428, 33428, 3927, 33927},    /* DDR2-667 SC */
+	{0, 0, 667, 800, 3443, 33443, 3905, 33905},    /* DDR2-800 SC */
+	{0, 1, 667, 667, 6494, 36494, 6993, 36993},    /* DDR3-667 SC */
+	{0, 1, 667, 800, 5998, 35998, 6460, 36460},    /* DDR3-800 SC */
+
+	{0, 0, 400, 400, 3528, 33528, 4255, 34255},    /* DDR2-400 SC */
+	{0, 0, 400, 667, 3500, 33500, 4079, 34079},    /* DDR2-667 SC */
+	{0, 0, 400, 800, 3487, 33487, 4029, 34029},    /* DDR2-800 SC */
+	{0, 1, 400, 667, 6566, 36566, 7145, 37145},    /* DDR3-667 SC */
+	{0, 1, 400, 800, 6042, 36042, 6584, 36584},    /* DDR3-800 SC */
+};
+
+static const struct cxsr_latency *intel_get_cxsr_latency(int is_desktop,
+							 int is_ddr3,
+							 int fsb,
+							 int mem)
+{
+	const struct cxsr_latency *latency;
+	int i;
+
+	if (fsb == 0 || mem == 0)
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(cxsr_latency_table); i++) {
+		latency = &cxsr_latency_table[i];
+		if (is_desktop == latency->is_desktop &&
+		    is_ddr3 == latency->is_ddr3 &&
+		    fsb == latency->fsb_freq && mem == latency->mem_freq)
+			return latency;
+	}
+
+	DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
+
+	return NULL;
+}
+
+static void pineview_disable_cxsr(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/* deactivate cxsr */
+	I915_WRITE(DSPFW3, I915_READ(DSPFW3) & ~PINEVIEW_SELF_REFRESH_EN);
+}
+
+/*
+ * Latency for FIFO fetches is dependent on several factors:
+ *   - memory configuration (speed, channels)
+ *   - chipset
+ *   - current MCH state
+ * It can be fairly high in some situations, so here we assume a fairly
+ * pessimal value.  It's a tradeoff between extra memory fetches (if we
+ * set this value too high, the FIFO will fetch frequently to stay full)
+ * and power consumption (set it too low to save power and we might see
+ * FIFO underruns and display "flicker").
+ *
+ * A value of 5us seems to be a good balance; safe for very low end
+ * platforms but not overly aggressive on lower latency configs.
+ */
+static const int latency_ns = 5000;
+
+static int i9xx_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x7f;
+	if (plane)
+		size = ((dsparb >> DSPARB_CSTART_SHIFT) & 0x7f) - size;
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A", size);
+
+	return size;
+}
+
+static int i85x_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x1ff;
+	if (plane)
+		size = ((dsparb >> DSPARB_BEND_SHIFT) & 0x1ff) - size;
+	size >>= 1; /* Convert to cachelines */
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A", size);
+
+	return size;
+}
+
+static int i845_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x7f;
+	size >>= 2; /* Convert to cachelines */
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A",
+		      size);
+
+	return size;
+}
+
+static int i830_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x7f;
+	size >>= 1; /* Convert to cachelines */
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A", size);
+
+	return size;
+}
+
+/* Pineview has different values for various configs */
+static const struct intel_watermark_params pineview_display_wm = {
+	PINEVIEW_DISPLAY_FIFO,
+	PINEVIEW_MAX_WM,
+	PINEVIEW_DFT_WM,
+	PINEVIEW_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params pineview_display_hplloff_wm = {
+	PINEVIEW_DISPLAY_FIFO,
+	PINEVIEW_MAX_WM,
+	PINEVIEW_DFT_HPLLOFF_WM,
+	PINEVIEW_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params pineview_cursor_wm = {
+	PINEVIEW_CURSOR_FIFO,
+	PINEVIEW_CURSOR_MAX_WM,
+	PINEVIEW_CURSOR_DFT_WM,
+	PINEVIEW_CURSOR_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params pineview_cursor_hplloff_wm = {
+	PINEVIEW_CURSOR_FIFO,
+	PINEVIEW_CURSOR_MAX_WM,
+	PINEVIEW_CURSOR_DFT_WM,
+	PINEVIEW_CURSOR_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params g4x_wm_info = {
+	G4X_FIFO_SIZE,
+	G4X_MAX_WM,
+	G4X_MAX_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params g4x_cursor_wm_info = {
+	I965_CURSOR_FIFO,
+	I965_CURSOR_MAX_WM,
+	I965_CURSOR_DFT_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params valleyview_wm_info = {
+	VALLEYVIEW_FIFO_SIZE,
+	VALLEYVIEW_MAX_WM,
+	VALLEYVIEW_MAX_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params valleyview_cursor_wm_info = {
+	I965_CURSOR_FIFO,
+	VALLEYVIEW_CURSOR_MAX_WM,
+	I965_CURSOR_DFT_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params i965_cursor_wm_info = {
+	I965_CURSOR_FIFO,
+	I965_CURSOR_MAX_WM,
+	I965_CURSOR_DFT_WM,
+	2,
+	I915_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params i945_wm_info = {
+	I945_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I915_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params i915_wm_info = {
+	I915_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I915_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params i855_wm_info = {
+	I855GM_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I830_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params i830_wm_info = {
+	I830_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I830_FIFO_LINE_SIZE
+};
+
+static const struct intel_watermark_params ironlake_display_wm_info = {
+	ILK_DISPLAY_FIFO,
+	ILK_DISPLAY_MAXWM,
+	ILK_DISPLAY_DFTWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params ironlake_cursor_wm_info = {
+	ILK_CURSOR_FIFO,
+	ILK_CURSOR_MAXWM,
+	ILK_CURSOR_DFTWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params ironlake_display_srwm_info = {
+	ILK_DISPLAY_SR_FIFO,
+	ILK_DISPLAY_MAX_SRWM,
+	ILK_DISPLAY_DFT_SRWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params ironlake_cursor_srwm_info = {
+	ILK_CURSOR_SR_FIFO,
+	ILK_CURSOR_MAX_SRWM,
+	ILK_CURSOR_DFT_SRWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+
+static const struct intel_watermark_params sandybridge_display_wm_info = {
+	SNB_DISPLAY_FIFO,
+	SNB_DISPLAY_MAXWM,
+	SNB_DISPLAY_DFTWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params sandybridge_cursor_wm_info = {
+	SNB_CURSOR_FIFO,
+	SNB_CURSOR_MAXWM,
+	SNB_CURSOR_DFTWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params sandybridge_display_srwm_info = {
+	SNB_DISPLAY_SR_FIFO,
+	SNB_DISPLAY_MAX_SRWM,
+	SNB_DISPLAY_DFT_SRWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params sandybridge_cursor_srwm_info = {
+	SNB_CURSOR_SR_FIFO,
+	SNB_CURSOR_MAX_SRWM,
+	SNB_CURSOR_DFT_SRWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+
+
+/**
+ * intel_calculate_wm - calculate watermark level
+ * @clock_in_khz: pixel clock
+ * @wm: chip FIFO params
+ * @pixel_size: display pixel size
+ * @latency_ns: memory latency for the platform
+ *
+ * Calculate the watermark level (the level at which the display plane will
+ * start fetching from memory again).  Each chip has a different display
+ * FIFO size and allocation, so the caller needs to figure that out and pass
+ * in the correct intel_watermark_params structure.
+ *
+ * As the pixel clock runs, the FIFO will be drained at a rate that depends
+ * on the pixel size.  When it reaches the watermark level, it'll start
+ * fetching FIFO line sized based chunks from memory until the FIFO fills
+ * past the watermark point.  If the FIFO drains completely, a FIFO underrun
+ * will occur, and a display engine hang could result.
+ */
+static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
+					const struct intel_watermark_params *wm,
+					int fifo_size,
+					int pixel_size,
+					unsigned long latency_ns)
+{
+	long entries_required, wm_size;
+
+	/*
+	 * Note: we need to make sure we don't overflow for various clock &
+	 * latency values.
+	 * clocks go from a few thousand to several hundred thousand.
+	 * latency is usually a few thousand
+	 */
+	entries_required = ((clock_in_khz / 1000) * pixel_size * latency_ns) /
+		1000;
+	entries_required = DIV_ROUND_UP(entries_required, wm->cacheline_size);
+
+	DRM_DEBUG_KMS("FIFO entries required for mode: %ld\n", entries_required);
+
+	wm_size = fifo_size - (entries_required + wm->guard_size);
+
+	DRM_DEBUG_KMS("FIFO watermark level: %ld\n", wm_size);
+
+	/* Don't promote wm_size to unsigned... */
+	if (wm_size > (long)wm->max_wm)
+		wm_size = wm->max_wm;
+	if (wm_size <= 0)
+		wm_size = wm->default_wm;
+	return wm_size;
+}
+
+static struct drm_crtc *single_enabled_crtc(struct drm_device *dev)
+{
+	struct drm_crtc *crtc, *enabled = NULL;
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		if (crtc->enabled && crtc->fb) {
+			if (enabled)
+				return NULL;
+			enabled = crtc;
+		}
+	}
+
+	return enabled;
+}
+
+static void pineview_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	const struct cxsr_latency *latency;
+	u32 reg;
+	unsigned long wm;
+
+	latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev), dev_priv->is_ddr3,
+					 dev_priv->fsb_freq, dev_priv->mem_freq);
+	if (!latency) {
+		DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
+		pineview_disable_cxsr(dev);
+		return;
+	}
+
+	crtc = single_enabled_crtc(dev);
+	if (crtc) {
+		int clock = crtc->mode.clock;
+		int pixel_size = crtc->fb->bits_per_pixel / 8;
+
+		/* Display SR */
+		wm = intel_calculate_wm(clock, &pineview_display_wm,
+					pineview_display_wm.fifo_size,
+					pixel_size, latency->display_sr);
+		reg = I915_READ(DSPFW1);
+		reg &= ~DSPFW_SR_MASK;
+		reg |= wm << DSPFW_SR_SHIFT;
+		I915_WRITE(DSPFW1, reg);
+		DRM_DEBUG_KMS("DSPFW1 register is %x\n", reg);
+
+		/* cursor SR */
+		wm = intel_calculate_wm(clock, &pineview_cursor_wm,
+					pineview_display_wm.fifo_size,
+					pixel_size, latency->cursor_sr);
+		reg = I915_READ(DSPFW3);
+		reg &= ~DSPFW_CURSOR_SR_MASK;
+		reg |= (wm & 0x3f) << DSPFW_CURSOR_SR_SHIFT;
+		I915_WRITE(DSPFW3, reg);
+
+		/* Display HPLL off SR */
+		wm = intel_calculate_wm(clock, &pineview_display_hplloff_wm,
+					pineview_display_hplloff_wm.fifo_size,
+					pixel_size, latency->display_hpll_disable);
+		reg = I915_READ(DSPFW3);
+		reg &= ~DSPFW_HPLL_SR_MASK;
+		reg |= wm & DSPFW_HPLL_SR_MASK;
+		I915_WRITE(DSPFW3, reg);
+
+		/* cursor HPLL off SR */
+		wm = intel_calculate_wm(clock, &pineview_cursor_hplloff_wm,
+					pineview_display_hplloff_wm.fifo_size,
+					pixel_size, latency->cursor_hpll_disable);
+		reg = I915_READ(DSPFW3);
+		reg &= ~DSPFW_HPLL_CURSOR_MASK;
+		reg |= (wm & 0x3f) << DSPFW_HPLL_CURSOR_SHIFT;
+		I915_WRITE(DSPFW3, reg);
+		DRM_DEBUG_KMS("DSPFW3 register is %x\n", reg);
+
+		/* activate cxsr */
+		I915_WRITE(DSPFW3,
+			   I915_READ(DSPFW3) | PINEVIEW_SELF_REFRESH_EN);
+		DRM_DEBUG_KMS("Self-refresh is enabled\n");
+	} else {
+		pineview_disable_cxsr(dev);
+		DRM_DEBUG_KMS("Self-refresh is disabled\n");
+	}
+}
+
+static bool g4x_compute_wm0(struct drm_device *dev,
+			    int plane,
+			    const struct intel_watermark_params *display,
+			    int display_latency_ns,
+			    const struct intel_watermark_params *cursor,
+			    int cursor_latency_ns,
+			    int *plane_wm,
+			    int *cursor_wm)
+{
+	struct drm_crtc *crtc;
+	int htotal, hdisplay, clock, pixel_size;
+	int line_time_us, line_count;
+	int entries, tlb_miss;
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	if (crtc->fb == NULL || !crtc->enabled) {
+		*cursor_wm = cursor->guard_size;
+		*plane_wm = display->guard_size;
+		return false;
+	}
+
+	htotal = crtc->mode.htotal;
+	hdisplay = crtc->mode.hdisplay;
+	clock = crtc->mode.clock;
+	pixel_size = crtc->fb->bits_per_pixel / 8;
+
+	/* Use the small buffer method to calculate plane watermark */
+	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
+	tlb_miss = display->fifo_size*display->cacheline_size - hdisplay * 8;
+	if (tlb_miss > 0)
+		entries += tlb_miss;
+	entries = DIV_ROUND_UP(entries, display->cacheline_size);
+	*plane_wm = entries + display->guard_size;
+	if (*plane_wm > (int)display->max_wm)
+		*plane_wm = display->max_wm;
+
+	/* Use the large buffer method to calculate cursor watermark */
+	line_time_us = ((htotal * 1000) / clock);
+	line_count = (cursor_latency_ns / line_time_us + 1000) / 1000;
+	entries = line_count * 64 * pixel_size;
+	tlb_miss = cursor->fifo_size*cursor->cacheline_size - hdisplay * 8;
+	if (tlb_miss > 0)
+		entries += tlb_miss;
+	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
+	*cursor_wm = entries + cursor->guard_size;
+	if (*cursor_wm > (int)cursor->max_wm)
+		*cursor_wm = (int)cursor->max_wm;
+
+	return true;
+}
+
+/*
+ * Check the wm result.
+ *
+ * If any calculated watermark values is larger than the maximum value that
+ * can be programmed into the associated watermark register, that watermark
+ * must be disabled.
+ */
+static bool g4x_check_srwm(struct drm_device *dev,
+			   int display_wm, int cursor_wm,
+			   const struct intel_watermark_params *display,
+			   const struct intel_watermark_params *cursor)
+{
+	DRM_DEBUG_KMS("SR watermark: display plane %d, cursor %d\n",
+		      display_wm, cursor_wm);
+
+	if (display_wm > display->max_wm) {
+		DRM_DEBUG_KMS("display watermark is too large(%d/%ld), disabling\n",
+			      display_wm, display->max_wm);
+		return false;
+	}
+
+	if (cursor_wm > cursor->max_wm) {
+		DRM_DEBUG_KMS("cursor watermark is too large(%d/%ld), disabling\n",
+			      cursor_wm, cursor->max_wm);
+		return false;
+	}
+
+	if (!(display_wm || cursor_wm)) {
+		DRM_DEBUG_KMS("SR latency is 0, disabling\n");
+		return false;
+	}
+
+	return true;
+}
+
+static bool g4x_compute_srwm(struct drm_device *dev,
+			     int plane,
+			     int latency_ns,
+			     const struct intel_watermark_params *display,
+			     const struct intel_watermark_params *cursor,
+			     int *display_wm, int *cursor_wm)
+{
+	struct drm_crtc *crtc;
+	int hdisplay, htotal, pixel_size, clock;
+	unsigned long line_time_us;
+	int line_count, line_size;
+	int small, large;
+	int entries;
+
+	if (!latency_ns) {
+		*display_wm = *cursor_wm = 0;
+		return false;
+	}
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	hdisplay = crtc->mode.hdisplay;
+	htotal = crtc->mode.htotal;
+	clock = crtc->mode.clock;
+	pixel_size = crtc->fb->bits_per_pixel / 8;
+
+	line_time_us = (htotal * 1000) / clock;
+	line_count = (latency_ns / line_time_us + 1000) / 1000;
+	line_size = hdisplay * pixel_size;
+
+	/* Use the minimum of the small and large buffer method for primary */
+	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
+	large = line_count * line_size;
+
+	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
+	*display_wm = entries + display->guard_size;
+
+	/* calculate the self-refresh watermark for display cursor */
+	entries = line_count * pixel_size * 64;
+	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
+	*cursor_wm = entries + cursor->guard_size;
+
+	return g4x_check_srwm(dev,
+			      *display_wm, *cursor_wm,
+			      display, cursor);
+}
+
+static bool vlv_compute_drain_latency(struct drm_device *dev,
+				     int plane,
+				     int *plane_prec_mult,
+				     int *plane_dl,
+				     int *cursor_prec_mult,
+				     int *cursor_dl)
+{
+	struct drm_crtc *crtc;
+	int clock, pixel_size;
+	int entries;
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	if (crtc->fb == NULL || !crtc->enabled)
+		return false;
+
+	clock = crtc->mode.clock;	/* VESA DOT Clock */
+	pixel_size = crtc->fb->bits_per_pixel / 8;	/* BPP */
+
+	entries = (clock / 1000) * pixel_size;
+	*plane_prec_mult = (entries > 256) ?
+		DRAIN_LATENCY_PRECISION_32 : DRAIN_LATENCY_PRECISION_16;
+	*plane_dl = (64 * (*plane_prec_mult) * 4) / ((clock / 1000) *
+						     pixel_size);
+
+	entries = (clock / 1000) * 4;	/* BPP is always 4 for cursor */
+	*cursor_prec_mult = (entries > 256) ?
+		DRAIN_LATENCY_PRECISION_32 : DRAIN_LATENCY_PRECISION_16;
+	*cursor_dl = (64 * (*cursor_prec_mult) * 4) / ((clock / 1000) * 4);
+
+	return true;
+}
+
+/*
+ * Update drain latency registers of memory arbiter
+ *
+ * Valleyview SoC has a new memory arbiter and needs drain latency registers
+ * to be programmed. Each plane has a drain latency multiplier and a drain
+ * latency value.
+ */
+
+static void vlv_update_drain_latency(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int planea_prec, planea_dl, planeb_prec, planeb_dl;
+	int cursora_prec, cursora_dl, cursorb_prec, cursorb_dl;
+	int plane_prec_mult, cursor_prec_mult; /* Precision multiplier is
+							either 16 or 32 */
+
+	/* For plane A, Cursor A */
+	if (vlv_compute_drain_latency(dev, 0, &plane_prec_mult, &planea_dl,
+				      &cursor_prec_mult, &cursora_dl)) {
+		cursora_prec = (cursor_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_CURSORA_PRECISION_32 : DDL_CURSORA_PRECISION_16;
+		planea_prec = (plane_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_PLANEA_PRECISION_32 : DDL_PLANEA_PRECISION_16;
+
+		I915_WRITE(VLV_DDL1, cursora_prec |
+				(cursora_dl << DDL_CURSORA_SHIFT) |
+				planea_prec | planea_dl);
+	}
+
+	/* For plane B, Cursor B */
+	if (vlv_compute_drain_latency(dev, 1, &plane_prec_mult, &planeb_dl,
+				      &cursor_prec_mult, &cursorb_dl)) {
+		cursorb_prec = (cursor_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_CURSORB_PRECISION_32 : DDL_CURSORB_PRECISION_16;
+		planeb_prec = (plane_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_PLANEB_PRECISION_32 : DDL_PLANEB_PRECISION_16;
+
+		I915_WRITE(VLV_DDL2, cursorb_prec |
+				(cursorb_dl << DDL_CURSORB_SHIFT) |
+				planeb_prec | planeb_dl);
+	}
+}
+
+#define single_plane_enabled(mask) is_power_of_2(mask)
+
+static void valleyview_update_wm(struct drm_device *dev)
+{
+	static const int sr_latency_ns = 12000;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
+	int plane_sr, cursor_sr;
+	unsigned int enabled = 0;
+
+	vlv_update_drain_latency(dev);
+
+	if (g4x_compute_wm0(dev, 0,
+			    &valleyview_wm_info, latency_ns,
+			    &valleyview_cursor_wm_info, latency_ns,
+			    &planea_wm, &cursora_wm))
+		enabled |= 1;
+
+	if (g4x_compute_wm0(dev, 1,
+			    &valleyview_wm_info, latency_ns,
+			    &valleyview_cursor_wm_info, latency_ns,
+			    &planeb_wm, &cursorb_wm))
+		enabled |= 2;
+
+	plane_sr = cursor_sr = 0;
+	if (single_plane_enabled(enabled) &&
+	    g4x_compute_srwm(dev, ffs(enabled) - 1,
+			     sr_latency_ns,
+			     &valleyview_wm_info,
+			     &valleyview_cursor_wm_info,
+			     &plane_sr, &cursor_sr))
+		I915_WRITE(FW_BLC_SELF_VLV, FW_CSPWRDWNEN);
+	else
+		I915_WRITE(FW_BLC_SELF_VLV,
+			   I915_READ(FW_BLC_SELF_VLV) & ~FW_CSPWRDWNEN);
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
+		      planea_wm, cursora_wm,
+		      planeb_wm, cursorb_wm,
+		      plane_sr, cursor_sr);
+
+	I915_WRITE(DSPFW1,
+		   (plane_sr << DSPFW_SR_SHIFT) |
+		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
+		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
+		   planea_wm);
+	I915_WRITE(DSPFW2,
+		   (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
+		   (cursora_wm << DSPFW_CURSORA_SHIFT));
+	I915_WRITE(DSPFW3,
+		   (I915_READ(DSPFW3) | (cursor_sr << DSPFW_CURSOR_SR_SHIFT)));
+}
+
+static void g4x_update_wm(struct drm_device *dev)
+{
+	static const int sr_latency_ns = 12000;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
+	int plane_sr, cursor_sr;
+	unsigned int enabled = 0;
+
+	if (g4x_compute_wm0(dev, 0,
+			    &g4x_wm_info, latency_ns,
+			    &g4x_cursor_wm_info, latency_ns,
+			    &planea_wm, &cursora_wm))
+		enabled |= 1;
+
+	if (g4x_compute_wm0(dev, 1,
+			    &g4x_wm_info, latency_ns,
+			    &g4x_cursor_wm_info, latency_ns,
+			    &planeb_wm, &cursorb_wm))
+		enabled |= 2;
+
+	plane_sr = cursor_sr = 0;
+	if (single_plane_enabled(enabled) &&
+	    g4x_compute_srwm(dev, ffs(enabled) - 1,
+			     sr_latency_ns,
+			     &g4x_wm_info,
+			     &g4x_cursor_wm_info,
+			     &plane_sr, &cursor_sr))
+		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
+	else
+		I915_WRITE(FW_BLC_SELF,
+			   I915_READ(FW_BLC_SELF) & ~FW_BLC_SELF_EN);
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
+		      planea_wm, cursora_wm,
+		      planeb_wm, cursorb_wm,
+		      plane_sr, cursor_sr);
+
+	I915_WRITE(DSPFW1,
+		   (plane_sr << DSPFW_SR_SHIFT) |
+		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
+		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
+		   planea_wm);
+	I915_WRITE(DSPFW2,
+		   (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
+		   (cursora_wm << DSPFW_CURSORA_SHIFT));
+	/* HPLL off in SR has some issues on G4x... disable it */
+	I915_WRITE(DSPFW3,
+		   (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN) |
+		   (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+}
+
+static void i965_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	int srwm = 1;
+	int cursor_sr = 16;
+
+	/* Calc sr entries for one plane configs */
+	crtc = single_enabled_crtc(dev);
+	if (crtc) {
+		/* self-refresh has much higher latency */
+		static const int sr_latency_ns = 12000;
+		int clock = crtc->mode.clock;
+		int htotal = crtc->mode.htotal;
+		int hdisplay = crtc->mode.hdisplay;
+		int pixel_size = crtc->fb->bits_per_pixel / 8;
+		unsigned long line_time_us;
+		int entries;
+
+		line_time_us = ((htotal * 1000) / clock);
+
+		/* Use ns/us then divide to preserve precision */
+		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
+			pixel_size * hdisplay;
+		entries = DIV_ROUND_UP(entries, I915_FIFO_LINE_SIZE);
+		srwm = I965_FIFO_SIZE - entries;
+		if (srwm < 0)
+			srwm = 1;
+		srwm &= 0x1ff;
+		DRM_DEBUG_KMS("self-refresh entries: %d, wm: %d\n",
+			      entries, srwm);
+
+		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
+			pixel_size * 64;
+		entries = DIV_ROUND_UP(entries,
+					  i965_cursor_wm_info.cacheline_size);
+		cursor_sr = i965_cursor_wm_info.fifo_size -
+			(entries + i965_cursor_wm_info.guard_size);
+
+		if (cursor_sr > i965_cursor_wm_info.max_wm)
+			cursor_sr = i965_cursor_wm_info.max_wm;
+
+		DRM_DEBUG_KMS("self-refresh watermark: display plane %d "
+			      "cursor %d\n", srwm, cursor_sr);
+
+		if (IS_CRESTLINE(dev))
+			I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
+	} else {
+		/* Turn off self refresh if both pipes are enabled */
+		if (IS_CRESTLINE(dev))
+			I915_WRITE(FW_BLC_SELF, I915_READ(FW_BLC_SELF)
+				   & ~FW_BLC_SELF_EN);
+	}
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: 8, B: 8, C: 8, SR %d\n",
+		      srwm);
+
+	/* 965 has limitations... */
+	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) |
+		   (8 << 16) | (8 << 8) | (8 << 0));
+	I915_WRITE(DSPFW2, (8 << 8) | (8 << 0));
+	/* update cursor SR watermark */
+	I915_WRITE(DSPFW3, (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+}
+
+static void i9xx_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	const struct intel_watermark_params *wm_info;
+	uint32_t fwater_lo;
+	uint32_t fwater_hi;
+	int cwm, srwm = 1;
+	int fifo_size;
+	int planea_wm, planeb_wm;
+	struct drm_crtc *crtc, *enabled = NULL;
+
+	if (IS_I945GM(dev))
+		wm_info = &i945_wm_info;
+	else if (!IS_GEN2(dev))
+		wm_info = &i915_wm_info;
+	else
+		wm_info = &i855_wm_info;
+
+	fifo_size = dev_priv->display.get_fifo_size(dev, 0);
+	crtc = intel_get_crtc_for_plane(dev, 0);
+	if (crtc->enabled && crtc->fb) {
+		planea_wm = intel_calculate_wm(crtc->mode.clock,
+					       wm_info, fifo_size,
+					       crtc->fb->bits_per_pixel / 8,
+					       latency_ns);
+		enabled = crtc;
+	} else
+		planea_wm = fifo_size - wm_info->guard_size;
+
+	fifo_size = dev_priv->display.get_fifo_size(dev, 1);
+	crtc = intel_get_crtc_for_plane(dev, 1);
+	if (crtc->enabled && crtc->fb) {
+		planeb_wm = intel_calculate_wm(crtc->mode.clock,
+					       wm_info, fifo_size,
+					       crtc->fb->bits_per_pixel / 8,
+					       latency_ns);
+		if (enabled == NULL)
+			enabled = crtc;
+		else
+			enabled = NULL;
+	} else
+		planeb_wm = fifo_size - wm_info->guard_size;
+
+	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
+
+	/*
+	 * Overlay gets an aggressive default since video jitter is bad.
+	 */
+	cwm = 2;
+
+	/* Play safe and disable self-refresh before adjusting watermarks. */
+	if (IS_I945G(dev) || IS_I945GM(dev))
+		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN_MASK | 0);
+	else if (IS_I915GM(dev))
+		I915_WRITE(INSTPM, I915_READ(INSTPM) & ~INSTPM_SELF_EN);
+
+	/* Calc sr entries for one plane configs */
+	if (HAS_FW_BLC(dev) && enabled) {
+		/* self-refresh has much higher latency */
+		static const int sr_latency_ns = 6000;
+		int clock = enabled->mode.clock;
+		int htotal = enabled->mode.htotal;
+		int hdisplay = enabled->mode.hdisplay;
+		int pixel_size = enabled->fb->bits_per_pixel / 8;
+		unsigned long line_time_us;
+		int entries;
+
+		line_time_us = (htotal * 1000) / clock;
+
+		/* Use ns/us then divide to preserve precision */
+		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
+			pixel_size * hdisplay;
+		entries = DIV_ROUND_UP(entries, wm_info->cacheline_size);
+		DRM_DEBUG_KMS("self-refresh entries: %d\n", entries);
+		srwm = wm_info->fifo_size - entries;
+		if (srwm < 0)
+			srwm = 1;
+
+		if (IS_I945G(dev) || IS_I945GM(dev))
+			I915_WRITE(FW_BLC_SELF,
+				   FW_BLC_SELF_FIFO_MASK | (srwm & 0xff));
+		else if (IS_I915GM(dev))
+			I915_WRITE(FW_BLC_SELF, srwm & 0x3f);
+	}
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d, C: %d, SR %d\n",
+		      planea_wm, planeb_wm, cwm, srwm);
+
+	fwater_lo = ((planeb_wm & 0x3f) << 16) | (planea_wm & 0x3f);
+	fwater_hi = (cwm & 0x1f);
+
+	/* Set request length to 8 cachelines per fetch */
+	fwater_lo = fwater_lo | (1 << 24) | (1 << 8);
+	fwater_hi = fwater_hi | (1 << 8);
+
+	I915_WRITE(FW_BLC, fwater_lo);
+	I915_WRITE(FW_BLC2, fwater_hi);
+
+	if (HAS_FW_BLC(dev)) {
+		if (enabled) {
+			if (IS_I945G(dev) || IS_I945GM(dev))
+				I915_WRITE(FW_BLC_SELF,
+					   FW_BLC_SELF_EN_MASK | FW_BLC_SELF_EN);
+			else if (IS_I915GM(dev))
+				I915_WRITE(INSTPM, I915_READ(INSTPM) | INSTPM_SELF_EN);
+			DRM_DEBUG_KMS("memory self refresh enabled\n");
+		} else
+			DRM_DEBUG_KMS("memory self refresh disabled\n");
+	}
+}
+
+static void i830_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	uint32_t fwater_lo;
+	int planea_wm;
+
+	crtc = single_enabled_crtc(dev);
+	if (crtc == NULL)
+		return;
+
+	planea_wm = intel_calculate_wm(crtc->mode.clock, &i830_wm_info,
+				       dev_priv->display.get_fifo_size(dev, 0),
+				       crtc->fb->bits_per_pixel / 8,
+				       latency_ns);
+	fwater_lo = I915_READ(FW_BLC) & ~0xfff;
+	fwater_lo |= (3<<8) | planea_wm;
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d\n", planea_wm);
+
+	I915_WRITE(FW_BLC, fwater_lo);
+}
+
+#define ILK_LP0_PLANE_LATENCY		700
+#define ILK_LP0_CURSOR_LATENCY		1300
+
+/*
+ * Check the wm result.
+ *
+ * If any calculated watermark values is larger than the maximum value that
+ * can be programmed into the associated watermark register, that watermark
+ * must be disabled.
+ */
+static bool ironlake_check_srwm(struct drm_device *dev, int level,
+				int fbc_wm, int display_wm, int cursor_wm,
+				const struct intel_watermark_params *display,
+				const struct intel_watermark_params *cursor)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	DRM_DEBUG_KMS("watermark %d: display plane %d, fbc lines %d,"
+		      " cursor %d\n", level, display_wm, fbc_wm, cursor_wm);
+
+	if (fbc_wm > SNB_FBC_MAX_SRWM) {
+		DRM_DEBUG_KMS("fbc watermark(%d) is too large(%d), disabling wm%d+\n",
+			      fbc_wm, SNB_FBC_MAX_SRWM, level);
+
+		/* fbc has it's own way to disable FBC WM */
+		I915_WRITE(DISP_ARB_CTL,
+			   I915_READ(DISP_ARB_CTL) | DISP_FBC_WM_DIS);
+		return false;
+	}
+
+	if (display_wm > display->max_wm) {
+		DRM_DEBUG_KMS("display watermark(%d) is too large(%d), disabling wm%d+\n",
+			      display_wm, SNB_DISPLAY_MAX_SRWM, level);
+		return false;
+	}
+
+	if (cursor_wm > cursor->max_wm) {
+		DRM_DEBUG_KMS("cursor watermark(%d) is too large(%d), disabling wm%d+\n",
+			      cursor_wm, SNB_CURSOR_MAX_SRWM, level);
+		return false;
+	}
+
+	if (!(fbc_wm || display_wm || cursor_wm)) {
+		DRM_DEBUG_KMS("latency %d is 0, disabling wm%d+\n", level, level);
+		return false;
+	}
+
+	return true;
+}
+
+/*
+ * Compute watermark values of WM[1-3],
+ */
+static bool ironlake_compute_srwm(struct drm_device *dev, int level, int plane,
+				  int latency_ns,
+				  const struct intel_watermark_params *display,
+				  const struct intel_watermark_params *cursor,
+				  int *fbc_wm, int *display_wm, int *cursor_wm)
+{
+	struct drm_crtc *crtc;
+	unsigned long line_time_us;
+	int hdisplay, htotal, pixel_size, clock;
+	int line_count, line_size;
+	int small, large;
+	int entries;
+
+	if (!latency_ns) {
+		*fbc_wm = *display_wm = *cursor_wm = 0;
+		return false;
+	}
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	hdisplay = crtc->mode.hdisplay;
+	htotal = crtc->mode.htotal;
+	clock = crtc->mode.clock;
+	pixel_size = crtc->fb->bits_per_pixel / 8;
+
+	line_time_us = (htotal * 1000) / clock;
+	line_count = (latency_ns / line_time_us + 1000) / 1000;
+	line_size = hdisplay * pixel_size;
+
+	/* Use the minimum of the small and large buffer method for primary */
+	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
+	large = line_count * line_size;
+
+	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
+	*display_wm = entries + display->guard_size;
+
+	/*
+	 * Spec says:
+	 * FBC WM = ((Final Primary WM * 64) / number of bytes per line) + 2
+	 */
+	*fbc_wm = DIV_ROUND_UP(*display_wm * 64, line_size) + 2;
+
+	/* calculate the self-refresh watermark for display cursor */
+	entries = line_count * pixel_size * 64;
+	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
+	*cursor_wm = entries + cursor->guard_size;
+
+	return ironlake_check_srwm(dev, level,
+				   *fbc_wm, *display_wm, *cursor_wm,
+				   display, cursor);
+}
+
+static void ironlake_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int fbc_wm, plane_wm, cursor_wm;
+	unsigned int enabled;
+
+	enabled = 0;
+	if (g4x_compute_wm0(dev, 0,
+			    &ironlake_display_wm_info,
+			    ILK_LP0_PLANE_LATENCY,
+			    &ironlake_cursor_wm_info,
+			    ILK_LP0_CURSOR_LATENCY,
+			    &plane_wm, &cursor_wm)) {
+		I915_WRITE(WM0_PIPEA_ILK,
+			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
+		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
+			      " plane %d, " "cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 1;
+	}
+
+	if (g4x_compute_wm0(dev, 1,
+			    &ironlake_display_wm_info,
+			    ILK_LP0_PLANE_LATENCY,
+			    &ironlake_cursor_wm_info,
+			    ILK_LP0_CURSOR_LATENCY,
+			    &plane_wm, &cursor_wm)) {
+		I915_WRITE(WM0_PIPEB_ILK,
+			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
+		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
+			      " plane %d, cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 2;
+	}
+
+	/*
+	 * Calculate and update the self-refresh watermark only when one
+	 * display plane is used.
+	 */
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	if (!single_plane_enabled(enabled))
+		return;
+	enabled = ffs(enabled) - 1;
+
+	/* WM1 */
+	if (!ironlake_compute_srwm(dev, 1, enabled,
+				   ILK_READ_WM1_LATENCY() * 500,
+				   &ironlake_display_srwm_info,
+				   &ironlake_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM1_LP_ILK,
+		   WM1_LP_SR_EN |
+		   (ILK_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/* WM2 */
+	if (!ironlake_compute_srwm(dev, 2, enabled,
+				   ILK_READ_WM2_LATENCY() * 500,
+				   &ironlake_display_srwm_info,
+				   &ironlake_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM2_LP_ILK,
+		   WM2_LP_EN |
+		   (ILK_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/*
+	 * WM3 is unsupported on ILK, probably because we don't have latency
+	 * data for that power state
+	 */
+}
+
+static void sandybridge_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
+	u32 val;
+	int fbc_wm, plane_wm, cursor_wm;
+	unsigned int enabled;
+
+	enabled = 0;
+	if (g4x_compute_wm0(dev, 0,
+			    &sandybridge_display_wm_info, latency,
+			    &sandybridge_cursor_wm_info, latency,
+			    &plane_wm, &cursor_wm)) {
+		val = I915_READ(WM0_PIPEA_ILK);
+		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
+		I915_WRITE(WM0_PIPEA_ILK, val |
+			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
+		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
+			      " plane %d, " "cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 1;
+	}
+
+	if (g4x_compute_wm0(dev, 1,
+			    &sandybridge_display_wm_info, latency,
+			    &sandybridge_cursor_wm_info, latency,
+			    &plane_wm, &cursor_wm)) {
+		val = I915_READ(WM0_PIPEB_ILK);
+		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
+		I915_WRITE(WM0_PIPEB_ILK, val |
+			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
+		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
+			      " plane %d, cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 2;
+	}
+
+	if ((dev_priv->num_pipe == 3) &&
+	    g4x_compute_wm0(dev, 2,
+			    &sandybridge_display_wm_info, latency,
+			    &sandybridge_cursor_wm_info, latency,
+			    &plane_wm, &cursor_wm)) {
+		val = I915_READ(WM0_PIPEC_IVB);
+		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
+		I915_WRITE(WM0_PIPEC_IVB, val |
+			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
+		DRM_DEBUG_KMS("FIFO watermarks For pipe C -"
+			      " plane %d, cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 3;
+	}
+
+	/*
+	 * Calculate and update the self-refresh watermark only when one
+	 * display plane is used.
+	 *
+	 * SNB support 3 levels of watermark.
+	 *
+	 * WM1/WM2/WM2 watermarks have to be enabled in the ascending order,
+	 * and disabled in the descending order
+	 *
+	 */
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	if (!single_plane_enabled(enabled) ||
+	    dev_priv->sprite_scaling_enabled)
+		return;
+	enabled = ffs(enabled) - 1;
+
+	/* WM1 */
+	if (!ironlake_compute_srwm(dev, 1, enabled,
+				   SNB_READ_WM1_LATENCY() * 500,
+				   &sandybridge_display_srwm_info,
+				   &sandybridge_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM1_LP_ILK,
+		   WM1_LP_SR_EN |
+		   (SNB_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/* WM2 */
+	if (!ironlake_compute_srwm(dev, 2, enabled,
+				   SNB_READ_WM2_LATENCY() * 500,
+				   &sandybridge_display_srwm_info,
+				   &sandybridge_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM2_LP_ILK,
+		   WM2_LP_EN |
+		   (SNB_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/* WM3 */
+	if (!ironlake_compute_srwm(dev, 3, enabled,
+				   SNB_READ_WM3_LATENCY() * 500,
+				   &sandybridge_display_srwm_info,
+				   &sandybridge_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM3_LP_ILK,
+		   WM3_LP_EN |
+		   (SNB_READ_WM3_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+}
+
+static void
+haswell_update_linetime_wm(struct drm_device *dev, int pipe,
+				 struct drm_display_mode *mode)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 temp;
+
+	temp = I915_READ(PIPE_WM_LINETIME(pipe));
+	temp &= ~PIPE_WM_LINETIME_MASK;
+
+	/* The WM are computed with base on how long it takes to fill a single
+	 * row at the given clock rate, multiplied by 8.
+	 * */
+	temp |= PIPE_WM_LINETIME_TIME(
+		((mode->crtc_hdisplay * 1000) / mode->clock) * 8);
+
+	/* IPS watermarks are only used by pipe A, and are ignored by
+	 * pipes B and C.  They are calculated similarly to the common
+	 * linetime values, except that we are using CD clock frequency
+	 * in MHz instead of pixel rate for the division.
+	 *
+	 * This is a placeholder for the IPS watermark calculation code.
+	 */
+
+	I915_WRITE(PIPE_WM_LINETIME(pipe), temp);
+}
+
+static bool
+sandybridge_compute_sprite_wm(struct drm_device *dev, int plane,
+			      uint32_t sprite_width, int pixel_size,
+			      const struct intel_watermark_params *display,
+			      int display_latency_ns, int *sprite_wm)
+{
+	struct drm_crtc *crtc;
+	int clock;
+	int entries, tlb_miss;
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	if (crtc->fb == NULL || !crtc->enabled) {
+		*sprite_wm = display->guard_size;
+		return false;
+	}
+
+	clock = crtc->mode.clock;
+
+	/* Use the small buffer method to calculate the sprite watermark */
+	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
+	tlb_miss = display->fifo_size*display->cacheline_size -
+		sprite_width * 8;
+	if (tlb_miss > 0)
+		entries += tlb_miss;
+	entries = DIV_ROUND_UP(entries, display->cacheline_size);
+	*sprite_wm = entries + display->guard_size;
+	if (*sprite_wm > (int)display->max_wm)
+		*sprite_wm = display->max_wm;
+
+	return true;
+}
+
+static bool
+sandybridge_compute_sprite_srwm(struct drm_device *dev, int plane,
+				uint32_t sprite_width, int pixel_size,
+				const struct intel_watermark_params *display,
+				int latency_ns, int *sprite_wm)
+{
+	struct drm_crtc *crtc;
+	unsigned long line_time_us;
+	int clock;
+	int line_count, line_size;
+	int small, large;
+	int entries;
+
+	if (!latency_ns) {
+		*sprite_wm = 0;
+		return false;
+	}
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	clock = crtc->mode.clock;
+	if (!clock) {
+		*sprite_wm = 0;
+		return false;
+	}
+
+	line_time_us = (sprite_width * 1000) / clock;
+	if (!line_time_us) {
+		*sprite_wm = 0;
+		return false;
+	}
+
+	line_count = (latency_ns / line_time_us + 1000) / 1000;
+	line_size = sprite_width * pixel_size;
+
+	/* Use the minimum of the small and large buffer method for primary */
+	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
+	large = line_count * line_size;
+
+	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
+	*sprite_wm = entries + display->guard_size;
+
+	return *sprite_wm > 0x3ff ? false : true;
+}
+
+static void sandybridge_update_sprite_wm(struct drm_device *dev, int pipe,
+					 uint32_t sprite_width, int pixel_size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
+	u32 val;
+	int sprite_wm, reg;
+	int ret;
+
+	switch (pipe) {
+	case 0:
+		reg = WM0_PIPEA_ILK;
+		break;
+	case 1:
+		reg = WM0_PIPEB_ILK;
+		break;
+	case 2:
+		reg = WM0_PIPEC_IVB;
+		break;
+	default:
+		return; /* bad pipe */
+	}
+
+	ret = sandybridge_compute_sprite_wm(dev, pipe, sprite_width, pixel_size,
+					    &sandybridge_display_wm_info,
+					    latency, &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite wm for pipe %d\n",
+			      pipe);
+		return;
+	}
+
+	val = I915_READ(reg);
+	val &= ~WM0_PIPE_SPRITE_MASK;
+	I915_WRITE(reg, val | (sprite_wm << WM0_PIPE_SPRITE_SHIFT));
+	DRM_DEBUG_KMS("sprite watermarks For pipe %d - %d\n", pipe, sprite_wm);
+
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM1_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp1 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM1S_LP_ILK, sprite_wm);
+
+	/* Only IVB has two more LP watermarks for sprite */
+	if (!IS_IVYBRIDGE(dev))
+		return;
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM2_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp2 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM2S_LP_IVB, sprite_wm);
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM3_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp3 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM3S_LP_IVB, sprite_wm);
+}
+
+/**
+ * intel_update_watermarks - update FIFO watermark values based on current modes
+ *
+ * Calculate watermark values for the various WM regs based on current mode
+ * and plane configuration.
+ *
+ * There are several cases to deal with here:
+ *   - normal (i.e. non-self-refresh)
+ *   - self-refresh (SR) mode
+ *   - lines are large relative to FIFO size (buffer can hold up to 2)
+ *   - lines are small relative to FIFO size (buffer can hold more than 2
+ *     lines), so need to account for TLB latency
+ *
+ *   The normal calculation is:
+ *     watermark = dotclock * bytes per pixel * latency
+ *   where latency is platform & configuration dependent (we assume pessimal
+ *   values here).
+ *
+ *   The SR calculation is:
+ *     watermark = (trunc(latency/line time)+1) * surface width *
+ *       bytes per pixel
+ *   where
+ *     line time = htotal / dotclock
+ *     surface width = hdisplay for normal plane and 64 for cursor
+ *   and latency is assumed to be high, as above.
+ *
+ * The final value programmed to the register should always be rounded up,
+ * and include an extra 2 entries to account for clock crossings.
+ *
+ * We don't use the sprite, so we can ignore that.  And on Crestline we have
+ * to set the non-SR watermarks to 8.
+ */
+void intel_update_watermarks(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.update_wm)
+		dev_priv->display.update_wm(dev);
+}
+
+void intel_update_linetime_watermarks(struct drm_device *dev,
+		int pipe, struct drm_display_mode *mode)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.update_linetime_wm)
+		dev_priv->display.update_linetime_wm(dev, pipe, mode);
+}
+
+void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
+				    uint32_t sprite_width, int pixel_size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.update_sprite_wm)
+		dev_priv->display.update_sprite_wm(dev, pipe, sprite_width,
+						   pixel_size);
+}
+
+static struct drm_i915_gem_object *
+intel_alloc_context_page(struct drm_device *dev)
+{
+	struct drm_i915_gem_object *ctx;
+	int ret;
+
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
+	ctx = i915_gem_alloc_object(dev, 4096);
+	if (!ctx) {
+		DRM_DEBUG("failed to alloc power context, RC6 disabled\n");
+		return NULL;
+	}
+
+	ret = i915_gem_object_pin(ctx, 4096, true);
+	if (ret) {
+		DRM_ERROR("failed to pin power context: %d\n", ret);
+		goto err_unref;
+	}
+
+	ret = i915_gem_object_set_to_gtt_domain(ctx, 1);
+	if (ret) {
+		DRM_ERROR("failed to set-domain on power context: %d\n", ret);
+		goto err_unpin;
+	}
+
+	return ctx;
+
+err_unpin:
+	i915_gem_object_unpin(ctx);
+err_unref:
+	drm_gem_object_unreference(&ctx->base);
+	mutex_unlock(&dev->struct_mutex);
+	return NULL;
+}
+
+bool ironlake_set_drps(struct drm_device *dev, u8 val)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u16 rgvswctl;
+
+	rgvswctl = I915_READ16(MEMSWCTL);
+	if (rgvswctl & MEMCTL_CMD_STS) {
+		DRM_DEBUG("gpu busy, RCS change rejected\n");
+		return false; /* still busy with another command */
+	}
+
+	rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
+		(val << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
+	I915_WRITE16(MEMSWCTL, rgvswctl);
+	POSTING_READ16(MEMSWCTL);
+
+	rgvswctl |= MEMCTL_CMD_STS;
+	I915_WRITE16(MEMSWCTL, rgvswctl);
+
+	return true;
+}
+
+void ironlake_enable_drps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 rgvmodectl = I915_READ(MEMMODECTL);
+	u8 fmax, fmin, fstart, vstart;
+
+	/* Enable temp reporting */
+	I915_WRITE16(PMMISC, I915_READ(PMMISC) | MCPPCE_EN);
+	I915_WRITE16(TSC1, I915_READ(TSC1) | TSE);
+
+	/* 100ms RC evaluation intervals */
+	I915_WRITE(RCUPEI, 100000);
+	I915_WRITE(RCDNEI, 100000);
+
+	/* Set max/min thresholds to 90ms and 80ms respectively */
+	I915_WRITE(RCBMAXAVG, 90000);
+	I915_WRITE(RCBMINAVG, 80000);
+
+	I915_WRITE(MEMIHYST, 1);
+
+	/* Set up min, max, and cur for interrupt handling */
+	fmax = (rgvmodectl & MEMMODE_FMAX_MASK) >> MEMMODE_FMAX_SHIFT;
+	fmin = (rgvmodectl & MEMMODE_FMIN_MASK);
+	fstart = (rgvmodectl & MEMMODE_FSTART_MASK) >>
+		MEMMODE_FSTART_SHIFT;
+
+	vstart = (I915_READ(PXVFREQ_BASE + (fstart * 4)) & PXVFREQ_PX_MASK) >>
+		PXVFREQ_PX_SHIFT;
+
+	dev_priv->fmax = fmax; /* IPS callback will increase this */
+	dev_priv->fstart = fstart;
+
+	dev_priv->max_delay = fstart;
+	dev_priv->min_delay = fmin;
+	dev_priv->cur_delay = fstart;
+
+	DRM_DEBUG_DRIVER("fmax: %d, fmin: %d, fstart: %d\n",
+			 fmax, fmin, fstart);
+
+	I915_WRITE(MEMINTREN, MEMINT_CX_SUPR_EN | MEMINT_EVAL_CHG_EN);
+
+	/*
+	 * Interrupts will be enabled in ironlake_irq_postinstall
+	 */
+
+	I915_WRITE(VIDSTART, vstart);
+	POSTING_READ(VIDSTART);
+
+	rgvmodectl |= MEMMODE_SWMODE_EN;
+	I915_WRITE(MEMMODECTL, rgvmodectl);
+
+	if (wait_for((I915_READ(MEMSWCTL) & MEMCTL_CMD_STS) == 0, 10))
+		DRM_ERROR("stuck trying to change perf mode\n");
+	msleep(1);
+
+	ironlake_set_drps(dev, fstart);
+
+	dev_priv->last_count1 = I915_READ(0x112e4) + I915_READ(0x112e8) +
+		I915_READ(0x112e0);
+	dev_priv->last_time1 = jiffies_to_msecs(jiffies);
+	dev_priv->last_count2 = I915_READ(0x112f4);
+	getrawmonotonic(&dev_priv->last_time2);
+}
+
+void ironlake_disable_drps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u16 rgvswctl = I915_READ16(MEMSWCTL);
+
+	/* Ack interrupts, disable EFC interrupt */
+	I915_WRITE(MEMINTREN, I915_READ(MEMINTREN) & ~MEMINT_EVAL_CHG_EN);
+	I915_WRITE(MEMINTRSTS, MEMINT_EVAL_CHG);
+	I915_WRITE(DEIER, I915_READ(DEIER) & ~DE_PCU_EVENT);
+	I915_WRITE(DEIIR, DE_PCU_EVENT);
+	I915_WRITE(DEIMR, I915_READ(DEIMR) | DE_PCU_EVENT);
+
+	/* Go back to the starting frequency */
+	ironlake_set_drps(dev, dev_priv->fstart);
+	msleep(1);
+	rgvswctl |= MEMCTL_CMD_STS;
+	I915_WRITE(MEMSWCTL, rgvswctl);
+	msleep(1);
+
+}
+
+void gen6_set_rps(struct drm_device *dev, u8 val)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 swreq;
+
+	swreq = (val & 0x3ff) << 25;
+	I915_WRITE(GEN6_RPNSWREQ, swreq);
+}
+
+void gen6_disable_rps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(GEN6_RPNSWREQ, 1 << 31);
+	I915_WRITE(GEN6_PMINTRMSK, 0xffffffff);
+	I915_WRITE(GEN6_PMIER, 0);
+	/* Complete PM interrupt masking here doesn't race with the rps work
+	 * item again unmasking PM interrupts because that is using a different
+	 * register (PMIMR) to mask PM interrupts. The only risk is in leaving
+	 * stale bits in PMIIR and PMIMR which gen6_enable_rps will clean up. */
+
+	spin_lock_irq(&dev_priv->rps_lock);
+	dev_priv->pm_iir = 0;
+	spin_unlock_irq(&dev_priv->rps_lock);
+
+	I915_WRITE(GEN6_PMIIR, I915_READ(GEN6_PMIIR));
+}
+
+int intel_enable_rc6(const struct drm_device *dev)
+{
+	/*
+	 * Respect the kernel parameter if it is set
+	 */
+	if (i915_enable_rc6 >= 0)
+		return i915_enable_rc6;
+
+	/*
+	 * Disable RC6 on Ironlake
+	 */
+	if (INTEL_INFO(dev)->gen == 5)
+		return 0;
+
+	/* Sorry Haswell, no RC6 for you for now. */
+	if (IS_HASWELL(dev))
+		return 0;
+
+	/*
+	 * Disable rc6 on Sandybridge
+	 */
+	if (INTEL_INFO(dev)->gen == 6) {
+		DRM_DEBUG_DRIVER("Sandybridge: deep RC6 disabled\n");
+		return INTEL_RC6_ENABLE;
+	}
+	DRM_DEBUG_DRIVER("RC6 and deep RC6 enabled\n");
+	return (INTEL_RC6_ENABLE | INTEL_RC6p_ENABLE);
+}
+
+void gen6_enable_rps(struct drm_i915_private *dev_priv)
+{
+	struct intel_ring_buffer *ring;
+	u32 rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
+	u32 gt_perf_status = I915_READ(GEN6_GT_PERF_STATUS);
+	u32 pcu_mbox, rc6_mask = 0;
+	u32 gtfifodbg;
+	int cur_freq, min_freq, max_freq;
+	int rc6_mode;
+	int i;
+
+	/* Here begins a magic sequence of register writes to enable
+	 * auto-downclocking.
+	 *
+	 * Perhaps there might be some value in exposing these to
+	 * userspace...
+	 */
+	I915_WRITE(GEN6_RC_STATE, 0);
+	mutex_lock(&dev_priv->dev->struct_mutex);
+
+	/* Clear the DBG now so we don't confuse earlier errors */
+	if ((gtfifodbg = I915_READ(GTFIFODBG))) {
+		DRM_ERROR("GT fifo had a previous error %x\n", gtfifodbg);
+		I915_WRITE(GTFIFODBG, gtfifodbg);
+	}
+
+	gen6_gt_force_wake_get(dev_priv);
+
+	/* disable the counters and set deterministic thresholds */
+	I915_WRITE(GEN6_RC_CONTROL, 0);
+
+	I915_WRITE(GEN6_RC1_WAKE_RATE_LIMIT, 1000 << 16);
+	I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 40 << 16 | 30);
+	I915_WRITE(GEN6_RC6pp_WAKE_RATE_LIMIT, 30);
+	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000);
+	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25);
+
+	for_each_ring(ring, dev_priv, i)
+		I915_WRITE(RING_MAX_IDLE(ring->mmio_base), 10);
+
+	I915_WRITE(GEN6_RC_SLEEP, 0);
+	I915_WRITE(GEN6_RC1e_THRESHOLD, 1000);
+	I915_WRITE(GEN6_RC6_THRESHOLD, 50000);
+	I915_WRITE(GEN6_RC6p_THRESHOLD, 100000);
+	I915_WRITE(GEN6_RC6pp_THRESHOLD, 64000); /* unused */
+
+	rc6_mode = intel_enable_rc6(dev_priv->dev);
+	if (rc6_mode & INTEL_RC6_ENABLE)
+		rc6_mask |= GEN6_RC_CTL_RC6_ENABLE;
+
+	if (rc6_mode & INTEL_RC6p_ENABLE)
+		rc6_mask |= GEN6_RC_CTL_RC6p_ENABLE;
+
+	if (rc6_mode & INTEL_RC6pp_ENABLE)
+		rc6_mask |= GEN6_RC_CTL_RC6pp_ENABLE;
+
+	DRM_INFO("Enabling RC6 states: RC6 %s, RC6p %s, RC6pp %s\n",
+			(rc6_mode & INTEL_RC6_ENABLE) ? "on" : "off",
+			(rc6_mode & INTEL_RC6p_ENABLE) ? "on" : "off",
+			(rc6_mode & INTEL_RC6pp_ENABLE) ? "on" : "off");
+
+	I915_WRITE(GEN6_RC_CONTROL,
+		   rc6_mask |
+		   GEN6_RC_CTL_EI_MODE(1) |
+		   GEN6_RC_CTL_HW_ENABLE);
+
+	I915_WRITE(GEN6_RPNSWREQ,
+		   GEN6_FREQUENCY(10) |
+		   GEN6_OFFSET(0) |
+		   GEN6_AGGRESSIVE_TURBO);
+	I915_WRITE(GEN6_RC_VIDEO_FREQ,
+		   GEN6_FREQUENCY(12));
+
+	I915_WRITE(GEN6_RP_DOWN_TIMEOUT, 1000000);
+	I915_WRITE(GEN6_RP_INTERRUPT_LIMITS,
+		   18 << 24 |
+		   6 << 16);
+	I915_WRITE(GEN6_RP_UP_THRESHOLD, 10000);
+	I915_WRITE(GEN6_RP_DOWN_THRESHOLD, 1000000);
+	I915_WRITE(GEN6_RP_UP_EI, 100000);
+	I915_WRITE(GEN6_RP_DOWN_EI, 5000000);
+	I915_WRITE(GEN6_RP_IDLE_HYSTERSIS, 10);
+	I915_WRITE(GEN6_RP_CONTROL,
+		   GEN6_RP_MEDIA_TURBO |
+		   GEN6_RP_MEDIA_HW_MODE |
+		   GEN6_RP_MEDIA_IS_GFX |
+		   GEN6_RP_ENABLE |
+		   GEN6_RP_UP_BUSY_AVG |
+		   GEN6_RP_DOWN_IDLE_CONT);
+
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
+
+	I915_WRITE(GEN6_PCODE_DATA, 0);
+	I915_WRITE(GEN6_PCODE_MAILBOX,
+		   GEN6_PCODE_READY |
+		   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
+
+	min_freq = (rp_state_cap & 0xff0000) >> 16;
+	max_freq = rp_state_cap & 0xff;
+	cur_freq = (gt_perf_status & 0xff00) >> 8;
+
+	/* Check for overclock support */
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
+	I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_READ_OC_PARAMS);
+	pcu_mbox = I915_READ(GEN6_PCODE_DATA);
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
+	if (pcu_mbox & (1<<31)) { /* OC supported */
+		max_freq = pcu_mbox & 0xff;
+		DRM_DEBUG_DRIVER("overclocking supported, adjusting frequency max to %dMHz\n", pcu_mbox * 50);
+	}
+
+	/* In units of 100MHz */
+	dev_priv->max_delay = max_freq;
+	dev_priv->min_delay = min_freq;
+	dev_priv->cur_delay = cur_freq;
+
+	/* requires MSI enabled */
+	I915_WRITE(GEN6_PMIER,
+		   GEN6_PM_MBOX_EVENT |
+		   GEN6_PM_THERMAL_EVENT |
+		   GEN6_PM_RP_DOWN_TIMEOUT |
+		   GEN6_PM_RP_UP_THRESHOLD |
+		   GEN6_PM_RP_DOWN_THRESHOLD |
+		   GEN6_PM_RP_UP_EI_EXPIRED |
+		   GEN6_PM_RP_DOWN_EI_EXPIRED);
+	spin_lock_irq(&dev_priv->rps_lock);
+	WARN_ON(dev_priv->pm_iir != 0);
+	I915_WRITE(GEN6_PMIMR, 0);
+	spin_unlock_irq(&dev_priv->rps_lock);
+	/* enable all PM interrupts */
+	I915_WRITE(GEN6_PMINTRMSK, 0);
+
+	gen6_gt_force_wake_put(dev_priv);
+	mutex_unlock(&dev_priv->dev->struct_mutex);
+}
+
+void gen6_update_ring_freq(struct drm_i915_private *dev_priv)
+{
+	int min_freq = 15;
+	int gpu_freq, ia_freq, max_ia_freq;
+	int scaling_factor = 180;
+
+	max_ia_freq = cpufreq_quick_get_max(0);
+	/*
+	 * Default to measured freq if none found, PCU will ensure we don't go
+	 * over
+	 */
+	if (!max_ia_freq)
+		max_ia_freq = tsc_khz;
+
+	/* Convert from kHz to MHz */
+	max_ia_freq /= 1000;
+
+	mutex_lock(&dev_priv->dev->struct_mutex);
+
+	/*
+	 * For each potential GPU frequency, load a ring frequency we'd like
+	 * to use for memory access.  We do this by specifying the IA frequency
+	 * the PCU should use as a reference to determine the ring frequency.
+	 */
+	for (gpu_freq = dev_priv->max_delay; gpu_freq >= dev_priv->min_delay;
+	     gpu_freq--) {
+		int diff = dev_priv->max_delay - gpu_freq;
+
+		/*
+		 * For GPU frequencies less than 750MHz, just use the lowest
+		 * ring freq.
+		 */
+		if (gpu_freq < min_freq)
+			ia_freq = 800;
+		else
+			ia_freq = max_ia_freq - ((diff * scaling_factor) / 2);
+		ia_freq = DIV_ROUND_CLOSEST(ia_freq, 100);
+
+		I915_WRITE(GEN6_PCODE_DATA,
+			   (ia_freq << GEN6_PCODE_FREQ_IA_RATIO_SHIFT) |
+			   gpu_freq);
+		I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_PCODE_READY |
+			   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
+		if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) &
+			      GEN6_PCODE_READY) == 0, 10)) {
+			DRM_ERROR("pcode write of freq table timed out\n");
+			continue;
+		}
+	}
+
+	mutex_unlock(&dev_priv->dev->struct_mutex);
+}
+
+static void ironlake_teardown_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->renderctx) {
+		i915_gem_object_unpin(dev_priv->renderctx);
+		drm_gem_object_unreference(&dev_priv->renderctx->base);
+		dev_priv->renderctx = NULL;
+	}
+
+	if (dev_priv->pwrctx) {
+		i915_gem_object_unpin(dev_priv->pwrctx);
+		drm_gem_object_unreference(&dev_priv->pwrctx->base);
+		dev_priv->pwrctx = NULL;
+	}
+}
+
+void ironlake_disable_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (I915_READ(PWRCTXA)) {
+		/* Wake the GPU, prevent RC6, then restore RSTDBYCTL */
+		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) | RCX_SW_EXIT);
+		wait_for(((I915_READ(RSTDBYCTL) & RSX_STATUS_MASK) == RSX_STATUS_ON),
+			 50);
+
+		I915_WRITE(PWRCTXA, 0);
+		POSTING_READ(PWRCTXA);
+
+		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
+		POSTING_READ(RSTDBYCTL);
+	}
+
+	ironlake_teardown_rc6(dev);
+}
+
+static int ironlake_setup_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->renderctx == NULL)
+		dev_priv->renderctx = intel_alloc_context_page(dev);
+	if (!dev_priv->renderctx)
+		return -ENOMEM;
+
+	if (dev_priv->pwrctx == NULL)
+		dev_priv->pwrctx = intel_alloc_context_page(dev);
+	if (!dev_priv->pwrctx) {
+		ironlake_teardown_rc6(dev);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void ironlake_enable_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
+	int ret;
+
+	/* rc6 disabled by default due to repeated reports of hanging during
+	 * boot and resume.
+	 */
+	if (!intel_enable_rc6(dev))
+		return;
+
+	mutex_lock(&dev->struct_mutex);
+	ret = ironlake_setup_rc6(dev);
+	if (ret) {
+		mutex_unlock(&dev->struct_mutex);
+		return;
+	}
+
+	/*
+	 * GPU can automatically power down the render unit if given a page
+	 * to save state.
+	 */
+	ret = intel_ring_begin(ring, 6);
+	if (ret) {
+		ironlake_teardown_rc6(dev);
+		mutex_unlock(&dev->struct_mutex);
+		return;
+	}
+
+	intel_ring_emit(ring, MI_SUSPEND_FLUSH | MI_SUSPEND_FLUSH_EN);
+	intel_ring_emit(ring, MI_SET_CONTEXT);
+	intel_ring_emit(ring, dev_priv->renderctx->gtt_offset |
+			MI_MM_SPACE_GTT |
+			MI_SAVE_EXT_STATE_EN |
+			MI_RESTORE_EXT_STATE_EN |
+			MI_RESTORE_INHIBIT);
+	intel_ring_emit(ring, MI_SUSPEND_FLUSH);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_emit(ring, MI_FLUSH);
+	intel_ring_advance(ring);
+
+	/*
+	 * Wait for the command parser to advance past MI_SET_CONTEXT. The HW
+	 * does an implicit flush, combined with MI_FLUSH above, it should be
+	 * safe to assume that renderctx is valid
+	 */
+	ret = intel_wait_ring_idle(ring);
+	if (ret) {
+		DRM_ERROR("failed to enable ironlake power power savings\n");
+		ironlake_teardown_rc6(dev);
+		mutex_unlock(&dev->struct_mutex);
+		return;
+	}
+
+	I915_WRITE(PWRCTXA, dev_priv->pwrctx->gtt_offset | PWRCTX_EN);
+	I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
+	mutex_unlock(&dev->struct_mutex);
+}
+
+static unsigned long intel_pxfreq(u32 vidfreq)
+{
+	unsigned long freq;
+	int div = (vidfreq & 0x3f0000) >> 16;
+	int post = (vidfreq & 0x3000) >> 12;
+	int pre = (vidfreq & 0x7);
+
+	if (!pre)
+		return 0;
+
+	freq = ((div * 133333) / ((1<<post) * pre));
+
+	return freq;
+}
+
+static const struct cparams {
+	u16 i;
+	u16 t;
+	u16 m;
+	u16 c;
+} cparams[] = {
+	{ 1, 1333, 301, 28664 },
+	{ 1, 1066, 294, 24460 },
+	{ 1, 800, 294, 25192 },
+	{ 0, 1333, 276, 27605 },
+	{ 0, 1066, 276, 27605 },
+	{ 0, 800, 231, 23784 },
+};
+
+unsigned long i915_chipset_val(struct drm_i915_private *dev_priv)
+{
+	u64 total_count, diff, ret;
+	u32 count1, count2, count3, m = 0, c = 0;
+	unsigned long now = jiffies_to_msecs(jiffies), diff1;
+	int i;
+
+	diff1 = now - dev_priv->last_time1;
+
+	/* Prevent division-by-zero if we are asking too fast.
+	 * Also, we don't get interesting results if we are polling
+	 * faster than once in 10ms, so just return the saved value
+	 * in such cases.
+	 */
+	if (diff1 <= 10)
+		return dev_priv->chipset_power;
+
+	count1 = I915_READ(DMIEC);
+	count2 = I915_READ(DDREC);
+	count3 = I915_READ(CSIEC);
+
+	total_count = count1 + count2 + count3;
+
+	/* FIXME: handle per-counter overflow */
+	if (total_count < dev_priv->last_count1) {
+		diff = ~0UL - dev_priv->last_count1;
+		diff += total_count;
+	} else {
+		diff = total_count - dev_priv->last_count1;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(cparams); i++) {
+		if (cparams[i].i == dev_priv->c_m &&
+		    cparams[i].t == dev_priv->r_t) {
+			m = cparams[i].m;
+			c = cparams[i].c;
+			break;
+		}
+	}
+
+	diff = div_u64(diff, diff1);
+	ret = ((m * diff) + c);
+	ret = div_u64(ret, 10);
+
+	dev_priv->last_count1 = total_count;
+	dev_priv->last_time1 = now;
+
+	dev_priv->chipset_power = ret;
+
+	return ret;
+}
+
+unsigned long i915_mch_val(struct drm_i915_private *dev_priv)
+{
+	unsigned long m, x, b;
+	u32 tsfs;
+
+	tsfs = I915_READ(TSFS);
+
+	m = ((tsfs & TSFS_SLOPE_MASK) >> TSFS_SLOPE_SHIFT);
+	x = I915_READ8(TR1);
+
+	b = tsfs & TSFS_INTR_MASK;
+
+	return ((m * x) / 127) - b;
+}
+
+static u16 pvid_to_extvid(struct drm_i915_private *dev_priv, u8 pxvid)
+{
+	static const struct v_table {
+		u16 vd; /* in .1 mil */
+		u16 vm; /* in .1 mil */
+	} v_table[] = {
+		{ 0, 0, },
+		{ 375, 0, },
+		{ 500, 0, },
+		{ 625, 0, },
+		{ 750, 0, },
+		{ 875, 0, },
+		{ 1000, 0, },
+		{ 1125, 0, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4250, 3125, },
+		{ 4375, 3250, },
+		{ 4500, 3375, },
+		{ 4625, 3500, },
+		{ 4750, 3625, },
+		{ 4875, 3750, },
+		{ 5000, 3875, },
+		{ 5125, 4000, },
+		{ 5250, 4125, },
+		{ 5375, 4250, },
+		{ 5500, 4375, },
+		{ 5625, 4500, },
+		{ 5750, 4625, },
+		{ 5875, 4750, },
+		{ 6000, 4875, },
+		{ 6125, 5000, },
+		{ 6250, 5125, },
+		{ 6375, 5250, },
+		{ 6500, 5375, },
+		{ 6625, 5500, },
+		{ 6750, 5625, },
+		{ 6875, 5750, },
+		{ 7000, 5875, },
+		{ 7125, 6000, },
+		{ 7250, 6125, },
+		{ 7375, 6250, },
+		{ 7500, 6375, },
+		{ 7625, 6500, },
+		{ 7750, 6625, },
+		{ 7875, 6750, },
+		{ 8000, 6875, },
+		{ 8125, 7000, },
+		{ 8250, 7125, },
+		{ 8375, 7250, },
+		{ 8500, 7375, },
+		{ 8625, 7500, },
+		{ 8750, 7625, },
+		{ 8875, 7750, },
+		{ 9000, 7875, },
+		{ 9125, 8000, },
+		{ 9250, 8125, },
+		{ 9375, 8250, },
+		{ 9500, 8375, },
+		{ 9625, 8500, },
+		{ 9750, 8625, },
+		{ 9875, 8750, },
+		{ 10000, 8875, },
+		{ 10125, 9000, },
+		{ 10250, 9125, },
+		{ 10375, 9250, },
+		{ 10500, 9375, },
+		{ 10625, 9500, },
+		{ 10750, 9625, },
+		{ 10875, 9750, },
+		{ 11000, 9875, },
+		{ 11125, 10000, },
+		{ 11250, 10125, },
+		{ 11375, 10250, },
+		{ 11500, 10375, },
+		{ 11625, 10500, },
+		{ 11750, 10625, },
+		{ 11875, 10750, },
+		{ 12000, 10875, },
+		{ 12125, 11000, },
+		{ 12250, 11125, },
+		{ 12375, 11250, },
+		{ 12500, 11375, },
+		{ 12625, 11500, },
+		{ 12750, 11625, },
+		{ 12875, 11750, },
+		{ 13000, 11875, },
+		{ 13125, 12000, },
+		{ 13250, 12125, },
+		{ 13375, 12250, },
+		{ 13500, 12375, },
+		{ 13625, 12500, },
+		{ 13750, 12625, },
+		{ 13875, 12750, },
+		{ 14000, 12875, },
+		{ 14125, 13000, },
+		{ 14250, 13125, },
+		{ 14375, 13250, },
+		{ 14500, 13375, },
+		{ 14625, 13500, },
+		{ 14750, 13625, },
+		{ 14875, 13750, },
+		{ 15000, 13875, },
+		{ 15125, 14000, },
+		{ 15250, 14125, },
+		{ 15375, 14250, },
+		{ 15500, 14375, },
+		{ 15625, 14500, },
+		{ 15750, 14625, },
+		{ 15875, 14750, },
+		{ 16000, 14875, },
+		{ 16125, 15000, },
+	};
+	if (dev_priv->info->is_mobile)
+		return v_table[pxvid].vm;
+	else
+		return v_table[pxvid].vd;
+}
+
+void i915_update_gfx_val(struct drm_i915_private *dev_priv)
+{
+	struct timespec now, diff1;
+	u64 diff;
+	unsigned long diffms;
+	u32 count;
+
+	if (dev_priv->info->gen != 5)
+		return;
+
+	getrawmonotonic(&now);
+	diff1 = timespec_sub(now, dev_priv->last_time2);
+
+	/* Don't divide by 0 */
+	diffms = diff1.tv_sec * 1000 + diff1.tv_nsec / 1000000;
+	if (!diffms)
+		return;
+
+	count = I915_READ(GFXEC);
+
+	if (count < dev_priv->last_count2) {
+		diff = ~0UL - dev_priv->last_count2;
+		diff += count;
+	} else {
+		diff = count - dev_priv->last_count2;
+	}
+
+	dev_priv->last_count2 = count;
+	dev_priv->last_time2 = now;
+
+	/* More magic constants... */
+	diff = diff * 1181;
+	diff = div_u64(diff, diffms * 10);
+	dev_priv->gfx_power = diff;
+}
+
+unsigned long i915_gfx_val(struct drm_i915_private *dev_priv)
+{
+	unsigned long t, corr, state1, corr2, state2;
+	u32 pxvid, ext_v;
+
+	pxvid = I915_READ(PXVFREQ_BASE + (dev_priv->cur_delay * 4));
+	pxvid = (pxvid >> 24) & 0x7f;
+	ext_v = pvid_to_extvid(dev_priv, pxvid);
+
+	state1 = ext_v;
+
+	t = i915_mch_val(dev_priv);
+
+	/* Revel in the empirically derived constants */
+
+	/* Correction factor in 1/100000 units */
+	if (t > 80)
+		corr = ((t * 2349) + 135940);
+	else if (t >= 50)
+		corr = ((t * 964) + 29317);
+	else /* < 50 */
+		corr = ((t * 301) + 1004);
+
+	corr = corr * ((150142 * state1) / 10000 - 78642);
+	corr /= 100000;
+	corr2 = (corr * dev_priv->corr);
+
+	state2 = (corr2 * state1) / 10000;
+	state2 /= 100; /* convert to mW */
+
+	i915_update_gfx_val(dev_priv);
+
+	return dev_priv->gfx_power + state2;
+}
+
+/* Global for IPS driver to get at the current i915 device */
+static struct drm_i915_private *i915_mch_dev;
+/*
+ * Lock protecting IPS related data structures
+ *   - i915_mch_dev
+ *   - dev_priv->max_delay
+ *   - dev_priv->min_delay
+ *   - dev_priv->fmax
+ *   - dev_priv->gpu_busy
+ */
+static DEFINE_SPINLOCK(mchdev_lock);
+
+/**
+ * i915_read_mch_val - return value for IPS use
+ *
+ * Calculate and return a value for the IPS driver to use when deciding whether
+ * we have thermal and power headroom to increase CPU or GPU power budget.
+ */
+unsigned long i915_read_mch_val(void)
+{
+	struct drm_i915_private *dev_priv;
+	unsigned long chipset_val, graphics_val, ret = 0;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev)
+		goto out_unlock;
+	dev_priv = i915_mch_dev;
+
+	chipset_val = i915_chipset_val(dev_priv);
+	graphics_val = i915_gfx_val(dev_priv);
+
+	ret = chipset_val + graphics_val;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_read_mch_val);
+
+/**
+ * i915_gpu_raise - raise GPU frequency limit
+ *
+ * Raise the limit; IPS indicates we have thermal headroom.
+ */
+bool i915_gpu_raise(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = true;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev) {
+		ret = false;
+		goto out_unlock;
+	}
+	dev_priv = i915_mch_dev;
+
+	if (dev_priv->max_delay > dev_priv->fmax)
+		dev_priv->max_delay--;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_raise);
+
+/**
+ * i915_gpu_lower - lower GPU frequency limit
+ *
+ * IPS indicates we're close to a thermal limit, so throttle back the GPU
+ * frequency maximum.
+ */
+bool i915_gpu_lower(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = true;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev) {
+		ret = false;
+		goto out_unlock;
+	}
+	dev_priv = i915_mch_dev;
+
+	if (dev_priv->max_delay < dev_priv->min_delay)
+		dev_priv->max_delay++;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_lower);
+
+/**
+ * i915_gpu_busy - indicate GPU business to IPS
+ *
+ * Tell the IPS driver whether or not the GPU is busy.
+ */
+bool i915_gpu_busy(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = false;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev)
+		goto out_unlock;
+	dev_priv = i915_mch_dev;
+
+	ret = dev_priv->busy;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_busy);
+
+/**
+ * i915_gpu_turbo_disable - disable graphics turbo
+ *
+ * Disable graphics turbo by resetting the max frequency and setting the
+ * current frequency to the default.
+ */
+bool i915_gpu_turbo_disable(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = true;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev) {
+		ret = false;
+		goto out_unlock;
+	}
+	dev_priv = i915_mch_dev;
+
+	dev_priv->max_delay = dev_priv->fstart;
+
+	if (!ironlake_set_drps(dev_priv->dev, dev_priv->fstart))
+		ret = false;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_turbo_disable);
+
+/**
+ * Tells the intel_ips driver that the i915 driver is now loaded, if
+ * IPS got loaded first.
+ *
+ * This awkward dance is so that neither module has to depend on the
+ * other in order for IPS to do the appropriate communication of
+ * GPU turbo limits to i915.
+ */
+static void
+ips_ping_for_i915_load(void)
+{
+	void (*link)(void);
+
+	link = symbol_get(ips_link_to_i915_driver);
+	if (link) {
+		link();
+		symbol_put(ips_link_to_i915_driver);
+	}
+}
+
+void intel_gpu_ips_init(struct drm_i915_private *dev_priv)
+{
+	spin_lock(&mchdev_lock);
+	i915_mch_dev = dev_priv;
+	dev_priv->mchdev_lock = &mchdev_lock;
+	spin_unlock(&mchdev_lock);
+
+	ips_ping_for_i915_load();
+}
+
+void intel_gpu_ips_teardown(void)
+{
+	spin_lock(&mchdev_lock);
+	i915_mch_dev = NULL;
+	spin_unlock(&mchdev_lock);
+}
+
+void intel_init_emon(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 lcfuse;
+	u8 pxw[16];
+	int i;
+
+	/* Disable to program */
+	I915_WRITE(ECR, 0);
+	POSTING_READ(ECR);
+
+	/* Program energy weights for various events */
+	I915_WRITE(SDEW, 0x15040d00);
+	I915_WRITE(CSIEW0, 0x007f0000);
+	I915_WRITE(CSIEW1, 0x1e220004);
+	I915_WRITE(CSIEW2, 0x04000004);
+
+	for (i = 0; i < 5; i++)
+		I915_WRITE(PEW + (i * 4), 0);
+	for (i = 0; i < 3; i++)
+		I915_WRITE(DEW + (i * 4), 0);
+
+	/* Program P-state weights to account for frequency power adjustment */
+	for (i = 0; i < 16; i++) {
+		u32 pxvidfreq = I915_READ(PXVFREQ_BASE + (i * 4));
+		unsigned long freq = intel_pxfreq(pxvidfreq);
+		unsigned long vid = (pxvidfreq & PXVFREQ_PX_MASK) >>
+			PXVFREQ_PX_SHIFT;
+		unsigned long val;
+
+		val = vid * vid;
+		val *= (freq / 1000);
+		val *= 255;
+		val /= (127*127*900);
+		if (val > 0xff)
+			DRM_ERROR("bad pxval: %ld\n", val);
+		pxw[i] = val;
+	}
+	/* Render standby states get 0 weight */
+	pxw[14] = 0;
+	pxw[15] = 0;
+
+	for (i = 0; i < 4; i++) {
+		u32 val = (pxw[i*4] << 24) | (pxw[(i*4)+1] << 16) |
+			(pxw[(i*4)+2] << 8) | (pxw[(i*4)+3]);
+		I915_WRITE(PXW + (i * 4), val);
+	}
+
+	/* Adjust magic regs to magic values (more experimental results) */
+	I915_WRITE(OGW0, 0);
+	I915_WRITE(OGW1, 0);
+	I915_WRITE(EG0, 0x00007f00);
+	I915_WRITE(EG1, 0x0000000e);
+	I915_WRITE(EG2, 0x000e0000);
+	I915_WRITE(EG3, 0x68000300);
+	I915_WRITE(EG4, 0x42000000);
+	I915_WRITE(EG5, 0x00140031);
+	I915_WRITE(EG6, 0);
+	I915_WRITE(EG7, 0);
+
+	for (i = 0; i < 8; i++)
+		I915_WRITE(PXWL + (i * 4), 0);
+
+	/* Enable PMON + select events */
+	I915_WRITE(ECR, 0x80000019);
+
+	lcfuse = I915_READ(LCFUSE02);
+
+	dev_priv->corr = (lcfuse & LCFUSE_HIV_MASK);
+}
+
+static void ironlake_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	/* Required for FBC */
+	dspclk_gate |= DPFCUNIT_CLOCK_GATE_DISABLE |
+		DPFCRUNIT_CLOCK_GATE_DISABLE |
+		DPFDUNIT_CLOCK_GATE_DISABLE;
+	/* Required for CxSR */
+	dspclk_gate |= DPARBUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_3DCGDIS0,
+		   MARIUNIT_CLOCK_GATE_DISABLE |
+		   SVSMUNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(PCH_3DCGDIS1,
+		   VFMUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	/*
+	 * According to the spec the following bits should be set in
+	 * order to enable memory self-refresh
+	 * The bit 22/21 of 0x42004
+	 * The bit 5 of 0x42020
+	 * The bit 15 of 0x45000
+	 */
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   (I915_READ(ILK_DISPLAY_CHICKEN2) |
+		    ILK_DPARB_GATE | ILK_VSDPFD_FULL));
+	I915_WRITE(ILK_DSPCLK_GATE,
+		   (I915_READ(ILK_DSPCLK_GATE) |
+		    ILK_DPARB_CLK_GATE));
+	I915_WRITE(DISP_ARB_CTL,
+		   (I915_READ(DISP_ARB_CTL) |
+		    DISP_FBC_WM_DIS));
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	/*
+	 * Based on the document from hardware guys the following bits
+	 * should be set unconditionally in order to enable FBC.
+	 * The bit 22 of 0x42000
+	 * The bit 22 of 0x42004
+	 * The bit 7,8,9 of 0x42020.
+	 */
+	if (IS_IRONLAKE_M(dev)) {
+		I915_WRITE(ILK_DISPLAY_CHICKEN1,
+			   I915_READ(ILK_DISPLAY_CHICKEN1) |
+			   ILK_FBCQ_DIS);
+		I915_WRITE(ILK_DISPLAY_CHICKEN2,
+			   I915_READ(ILK_DISPLAY_CHICKEN2) |
+			   ILK_DPARB_GATE);
+		I915_WRITE(ILK_DSPCLK_GATE,
+			   I915_READ(ILK_DSPCLK_GATE) |
+			   ILK_DPFC_DIS1 |
+			   ILK_DPFC_DIS2 |
+			   ILK_CLK_FBC);
+	}
+
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   I915_READ(ILK_DISPLAY_CHICKEN2) |
+		   ILK_ELPIN_409_SELECT);
+	I915_WRITE(_3D_CHICKEN2,
+		   _3D_CHICKEN2_WM_READ_PIPELINED << 16 |
+		   _3D_CHICKEN2_WM_READ_PIPELINED);
+}
+
+static void gen6_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   I915_READ(ILK_DISPLAY_CHICKEN2) |
+		   ILK_ELPIN_409_SELECT);
+
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	I915_WRITE(CACHE_MODE_0,
+		   _MASKED_BIT_DISABLE(CM0_STC_EVICT_DISABLE_LRA_SNB));
+
+	I915_WRITE(GEN6_UCGCTL1,
+		   I915_READ(GEN6_UCGCTL1) |
+		   GEN6_BLBUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_CSUNIT_CLOCK_GATE_DISABLE);
+
+	/* According to the BSpec vol1g, bit 12 (RCPBUNIT) clock
+	 * gating disable must be set.  Failure to set it results in
+	 * flickering pixels due to Z write ordering failures after
+	 * some amount of runtime in the Mesa "fire" demo, and Unigine
+	 * Sanctuary and Tropics, and apparently anything else with
+	 * alpha test or pixel discard.
+	 *
+	 * According to the spec, bit 11 (RCCUNIT) must also be set,
+	 * but we didn't debug actual testcases to find it out.
+	 */
+	I915_WRITE(GEN6_UCGCTL2,
+		   GEN6_RCPBUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
+
+	/* Bspec says we need to always set all mask bits. */
+	I915_WRITE(_3D_CHICKEN, (0xFFFF << 16) |
+		   _3D_CHICKEN_SF_DISABLE_FASTCLIP_CULL);
+
+	/*
+	 * According to the spec the following bits should be
+	 * set in order to enable memory self-refresh and fbc:
+	 * The bit21 and bit22 of 0x42000
+	 * The bit21 and bit22 of 0x42004
+	 * The bit5 and bit7 of 0x42020
+	 * The bit14 of 0x70180
+	 * The bit14 of 0x71180
+	 */
+	I915_WRITE(ILK_DISPLAY_CHICKEN1,
+		   I915_READ(ILK_DISPLAY_CHICKEN1) |
+		   ILK_FBCQ_DIS | ILK_PABSTRETCH_DIS);
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   I915_READ(ILK_DISPLAY_CHICKEN2) |
+		   ILK_DPARB_GATE | ILK_VSDPFD_FULL);
+	I915_WRITE(ILK_DSPCLK_GATE,
+		   I915_READ(ILK_DSPCLK_GATE) |
+		   ILK_DPARB_CLK_GATE  |
+		   ILK_DPFD_CLK_GATE);
+
+	for_each_pipe(pipe) {
+		I915_WRITE(DSPCNTR(pipe),
+			   I915_READ(DSPCNTR(pipe)) |
+			   DISPPLANE_TRICKLE_FEED_DISABLE);
+		intel_flush_display_plane(dev_priv, pipe);
+	}
+}
+
+static void gen7_setup_fixed_func_scheduler(struct drm_i915_private *dev_priv)
+{
+	uint32_t reg = I915_READ(GEN7_FF_THREAD_MODE);
+
+	reg &= ~GEN7_FF_SCHED_MASK;
+	reg |= GEN7_FF_TS_SCHED_HW;
+	reg |= GEN7_FF_VS_SCHED_HW;
+	reg |= GEN7_FF_DS_SCHED_HW;
+
+	I915_WRITE(GEN7_FF_THREAD_MODE, reg);
+}
+
+static void ivybridge_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	/* According to the spec, bit 13 (RCZUNIT) must be set on IVB.
+	 * This implements the WaDisableRCZUnitClockGating workaround.
+	 */
+	I915_WRITE(GEN6_UCGCTL2, GEN6_RCZUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
+
+	I915_WRITE(IVB_CHICKEN3,
+		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
+		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
+
+	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
+	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
+		   GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC);
+
+	/* WaApplyL3ControlAndL3ChickenMode requires those two on Ivy Bridge */
+	I915_WRITE(GEN7_L3CNTLREG1,
+			GEN7_WA_FOR_GEN7_L3_CONTROL);
+	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER,
+			GEN7_WA_L3_CHICKEN_MODE);
+
+	/* This is required by WaCatErrorRejectionIssue */
+	I915_WRITE(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG,
+			I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
+			GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
+
+	for_each_pipe(pipe) {
+		I915_WRITE(DSPCNTR(pipe),
+			   I915_READ(DSPCNTR(pipe)) |
+			   DISPPLANE_TRICKLE_FEED_DISABLE);
+		intel_flush_display_plane(dev_priv, pipe);
+	}
+
+	gen7_setup_fixed_func_scheduler(dev_priv);
+
+	/* WaDisable4x2SubspanOptimization */
+	I915_WRITE(CACHE_MODE_1,
+		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
+}
+
+static void valleyview_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	/* According to the spec, bit 13 (RCZUNIT) must be set on IVB.
+	 * This implements the WaDisableRCZUnitClockGating workaround.
+	 */
+	I915_WRITE(GEN6_UCGCTL2, GEN6_RCZUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
+
+	I915_WRITE(IVB_CHICKEN3,
+		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
+		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
+
+	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
+	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
+		   GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC);
+
+	/* WaApplyL3ControlAndL3ChickenMode requires those two on Ivy Bridge */
+	I915_WRITE(GEN7_L3CNTLREG1, GEN7_WA_FOR_GEN7_L3_CONTROL);
+	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER, GEN7_WA_L3_CHICKEN_MODE);
+
+	/* This is required by WaCatErrorRejectionIssue */
+	I915_WRITE(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG,
+		   I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
+		   GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
+
+	for_each_pipe(pipe) {
+		I915_WRITE(DSPCNTR(pipe),
+			   I915_READ(DSPCNTR(pipe)) |
+			   DISPPLANE_TRICKLE_FEED_DISABLE);
+		intel_flush_display_plane(dev_priv, pipe);
+	}
+
+	I915_WRITE(CACHE_MODE_1,
+		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
+}
+
+static void g4x_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dspclk_gate;
+
+	I915_WRITE(RENCLK_GATE_D1, 0);
+	I915_WRITE(RENCLK_GATE_D2, VF_UNIT_CLOCK_GATE_DISABLE |
+		   GS_UNIT_CLOCK_GATE_DISABLE |
+		   CL_UNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(RAMCLK_GATE_D, 0);
+	dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE |
+		OVRUNIT_CLOCK_GATE_DISABLE |
+		OVCUNIT_CLOCK_GATE_DISABLE;
+	if (IS_GM45(dev))
+		dspclk_gate |= DSSUNIT_CLOCK_GATE_DISABLE;
+	I915_WRITE(DSPCLK_GATE_D, dspclk_gate);
+}
+
+static void crestline_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(RENCLK_GATE_D1, I965_RCC_CLOCK_GATE_DISABLE);
+	I915_WRITE(RENCLK_GATE_D2, 0);
+	I915_WRITE(DSPCLK_GATE_D, 0);
+	I915_WRITE(RAMCLK_GATE_D, 0);
+	I915_WRITE16(DEUC, 0);
+}
+
+static void broadwater_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(RENCLK_GATE_D1, I965_RCZ_CLOCK_GATE_DISABLE |
+		   I965_RCC_CLOCK_GATE_DISABLE |
+		   I965_RCPB_CLOCK_GATE_DISABLE |
+		   I965_ISC_CLOCK_GATE_DISABLE |
+		   I965_FBC_CLOCK_GATE_DISABLE);
+	I915_WRITE(RENCLK_GATE_D2, 0);
+}
+
+static void gen3_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 dstate = I915_READ(D_STATE);
+
+	dstate |= DSTATE_PLL_D3_OFF | DSTATE_GFX_CLOCK_GATING |
+		DSTATE_DOT_CLOCK_GATING;
+	I915_WRITE(D_STATE, dstate);
+
+	if (IS_PINEVIEW(dev))
+		I915_WRITE(ECOSKPD, _MASKED_BIT_ENABLE(ECO_GATING_CX_ONLY));
+}
+
+static void i85x_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(RENCLK_GATE_D1, SV_CLOCK_GATE_DISABLE);
+}
+
+static void i830_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(DSPCLK_GATE_D, OVRUNIT_CLOCK_GATE_DISABLE);
+}
+
+static void ibx_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/*
+	 * On Ibex Peak and Cougar Point, we need to disable clock
+	 * gating for the panel power sequencer or it will fail to
+	 * start up when no ports are active.
+	 */
+	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
+}
+
+static void cpt_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+
+	/*
+	 * On Ibex Peak and Cougar Point, we need to disable clock
+	 * gating for the panel power sequencer or it will fail to
+	 * start up when no ports are active.
+	 */
+	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(SOUTH_CHICKEN2, I915_READ(SOUTH_CHICKEN2) |
+		   DPLS_EDP_PPS_FIX_DIS);
+	/* Without this, mode sets may fail silently on FDI */
+	for_each_pipe(pipe)
+		I915_WRITE(TRANS_CHICKEN2(pipe), TRANS_AUTOTRAIN_GEN_STALL_DIS);
+}
+
+void intel_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	dev_priv->display.init_clock_gating(dev);
+
+	if (dev_priv->display.init_pch_clock_gating)
+		dev_priv->display.init_pch_clock_gating(dev);
+}
+
+static void gen6_sanitize_pm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 limits, delay, old;
+
+	gen6_gt_force_wake_get(dev_priv);
+
+	old = limits = I915_READ(GEN6_RP_INTERRUPT_LIMITS);
+	/* Make sure we continue to get interrupts
+	 * until we hit the minimum or maximum frequencies.
+	 */
+	limits &= ~(0x3f << 16 | 0x3f << 24);
+	delay = dev_priv->cur_delay;
+	if (delay < dev_priv->max_delay)
+		limits |= (dev_priv->max_delay & 0x3f) << 24;
+	if (delay > dev_priv->min_delay)
+		limits |= (dev_priv->min_delay & 0x3f) << 16;
+
+	if (old != limits) {
+		DRM_ERROR("Power management discrepancy: GEN6_RP_INTERRUPT_LIMITS expected %08x, was %08x\n",
+			  limits, old);
+		I915_WRITE(GEN6_RP_INTERRUPT_LIMITS, limits);
+	}
+
+	gen6_gt_force_wake_put(dev_priv);
+}
+
+void intel_sanitize_pm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.sanitize_pm)
+		dev_priv->display.sanitize_pm(dev);
+}
+
+/* Starting with Haswell, we have different power wells for
+ * different parts of the GPU. This attempts to enable them all.
+ */
+void intel_init_power_wells(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long power_wells[] = {
+		HSW_PWR_WELL_CTL1,
+		HSW_PWR_WELL_CTL2,
+		HSW_PWR_WELL_CTL4
+	};
+	int i;
+
+	if (!IS_HASWELL(dev))
+		return;
+
+	mutex_lock(&dev->struct_mutex);
+
+	for (i = 0; i < ARRAY_SIZE(power_wells); i++) {
+		int well = I915_READ(power_wells[i]);
+
+		if ((well & HSW_PWR_WELL_STATE) == 0) {
+			I915_WRITE(power_wells[i], well & HSW_PWR_WELL_ENABLE);
+			if (wait_for(I915_READ(power_wells[i] & HSW_PWR_WELL_STATE), 20))
+				DRM_ERROR("Error enabling power well %lx\n", power_wells[i]);
+		}
+	}
+
+	mutex_unlock(&dev->struct_mutex);
+}
+
+/* Set up chip specific power management-related functions */
+void intel_init_pm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (I915_HAS_FBC(dev)) {
+		if (HAS_PCH_SPLIT(dev)) {
+			dev_priv->display.fbc_enabled = ironlake_fbc_enabled;
+			dev_priv->display.enable_fbc = ironlake_enable_fbc;
+			dev_priv->display.disable_fbc = ironlake_disable_fbc;
+		} else if (IS_GM45(dev)) {
+			dev_priv->display.fbc_enabled = g4x_fbc_enabled;
+			dev_priv->display.enable_fbc = g4x_enable_fbc;
+			dev_priv->display.disable_fbc = g4x_disable_fbc;
+		} else if (IS_CRESTLINE(dev)) {
+			dev_priv->display.fbc_enabled = i8xx_fbc_enabled;
+			dev_priv->display.enable_fbc = i8xx_enable_fbc;
+			dev_priv->display.disable_fbc = i8xx_disable_fbc;
+		}
+		/* 855GM needs testing */
+	}
+
+	/* For cxsr */
+	if (IS_PINEVIEW(dev))
+		i915_pineview_get_mem_freq(dev);
+	else if (IS_GEN5(dev))
+		i915_ironlake_get_mem_freq(dev);
+
+	/* For FIFO watermark updates */
+	if (HAS_PCH_SPLIT(dev)) {
+		dev_priv->display.force_wake_get = __gen6_gt_force_wake_get;
+		dev_priv->display.force_wake_put = __gen6_gt_force_wake_put;
+
+		/* IVB configs may use multi-threaded forcewake */
+		if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
+			u32	ecobus;
+
+			/* A small trick here - if the bios hasn't configured MT forcewake,
+			 * and if the device is in RC6, then force_wake_mt_get will not wake
+			 * the device and the ECOBUS read will return zero. Which will be
+			 * (correctly) interpreted by the test below as MT forcewake being
+			 * disabled.
+			 */
+			mutex_lock(&dev->struct_mutex);
+			__gen6_gt_force_wake_mt_get(dev_priv);
+			ecobus = I915_READ_NOTRACE(ECOBUS);
+			__gen6_gt_force_wake_mt_put(dev_priv);
+			mutex_unlock(&dev->struct_mutex);
+
+			if (ecobus & FORCEWAKE_MT_ENABLE) {
+				DRM_DEBUG_KMS("Using MT version of forcewake\n");
+				dev_priv->display.force_wake_get =
+					__gen6_gt_force_wake_mt_get;
+				dev_priv->display.force_wake_put =
+					__gen6_gt_force_wake_mt_put;
+			}
+		}
+
+		if (HAS_PCH_IBX(dev))
+			dev_priv->display.init_pch_clock_gating = ibx_init_clock_gating;
+		else if (HAS_PCH_CPT(dev))
+			dev_priv->display.init_pch_clock_gating = cpt_init_clock_gating;
+
+		if (IS_GEN5(dev)) {
+			if (I915_READ(MLTR_ILK) & ILK_SRLT_MASK)
+				dev_priv->display.update_wm = ironlake_update_wm;
+			else {
+				DRM_DEBUG_KMS("Failed to get proper latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = ironlake_init_clock_gating;
+		} else if (IS_GEN6(dev)) {
+			if (SNB_READ_WM0_LATENCY()) {
+				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
+			} else {
+				DRM_DEBUG_KMS("Failed to read display plane latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = gen6_init_clock_gating;
+			dev_priv->display.sanitize_pm = gen6_sanitize_pm;
+		} else if (IS_IVYBRIDGE(dev)) {
+			/* FIXME: detect B0+ stepping and use auto training */
+			if (SNB_READ_WM0_LATENCY()) {
+				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
+			} else {
+				DRM_DEBUG_KMS("Failed to read display plane latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = ivybridge_init_clock_gating;
+			dev_priv->display.sanitize_pm = gen6_sanitize_pm;
+		} else if (IS_HASWELL(dev)) {
+			if (SNB_READ_WM0_LATENCY()) {
+				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
+				dev_priv->display.update_linetime_wm = haswell_update_linetime_wm;
+			} else {
+				DRM_DEBUG_KMS("Failed to read display plane latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = ivybridge_init_clock_gating;
+			dev_priv->display.sanitize_pm = gen6_sanitize_pm;
+		} else
+			dev_priv->display.update_wm = NULL;
+	} else if (IS_VALLEYVIEW(dev)) {
+		dev_priv->display.update_wm = valleyview_update_wm;
+		dev_priv->display.init_clock_gating =
+			valleyview_init_clock_gating;
+		dev_priv->display.force_wake_get = vlv_force_wake_get;
+		dev_priv->display.force_wake_put = vlv_force_wake_put;
+	} else if (IS_PINEVIEW(dev)) {
+		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev),
+					    dev_priv->is_ddr3,
+					    dev_priv->fsb_freq,
+					    dev_priv->mem_freq)) {
+			DRM_INFO("failed to find known CxSR latency "
+				 "(found ddr%s fsb freq %d, mem freq %d), "
+				 "disabling CxSR\n",
+				 (dev_priv->is_ddr3 == 1) ? "3" : "2",
+				 dev_priv->fsb_freq, dev_priv->mem_freq);
+			/* Disable CxSR and never update its watermark again */
+			pineview_disable_cxsr(dev);
+			dev_priv->display.update_wm = NULL;
+		} else
+			dev_priv->display.update_wm = pineview_update_wm;
+		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
+	} else if (IS_G4X(dev)) {
+		dev_priv->display.update_wm = g4x_update_wm;
+		dev_priv->display.init_clock_gating = g4x_init_clock_gating;
+	} else if (IS_GEN4(dev)) {
+		dev_priv->display.update_wm = i965_update_wm;
+		if (IS_CRESTLINE(dev))
+			dev_priv->display.init_clock_gating = crestline_init_clock_gating;
+		else if (IS_BROADWATER(dev))
+			dev_priv->display.init_clock_gating = broadwater_init_clock_gating;
+	} else if (IS_GEN3(dev)) {
+		dev_priv->display.update_wm = i9xx_update_wm;
+		dev_priv->display.get_fifo_size = i9xx_get_fifo_size;
+		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
+	} else if (IS_I865G(dev)) {
+		dev_priv->display.update_wm = i830_update_wm;
+		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
+		dev_priv->display.get_fifo_size = i830_get_fifo_size;
+	} else if (IS_I85X(dev)) {
+		dev_priv->display.update_wm = i9xx_update_wm;
+		dev_priv->display.get_fifo_size = i85x_get_fifo_size;
+		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
+	} else {
+		dev_priv->display.update_wm = i830_update_wm;
+		dev_priv->display.init_clock_gating = i830_init_clock_gating;
+		if (IS_845G(dev))
+			dev_priv->display.get_fifo_size = i845_get_fifo_size;
+		else
+			dev_priv->display.get_fifo_size = i830_get_fifo_size;
+	}
+
+	/* We attempt to init the necessary power wells early in the initialization
+	 * time, so the subsystems that expect power to be enabled can work.
+	 */
+	intel_init_power_wells(dev);
+}
+
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 62892a826ede..b59b6d5b7583 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -53,9 +53,35 @@ static inline int ring_space(struct intel_ring_buffer *ring)
 }
 
 static int
-render_ring_flush(struct intel_ring_buffer *ring,
-		  u32	invalidate_domains,
-		  u32	flush_domains)
+gen2_render_ring_flush(struct intel_ring_buffer *ring,
+		       u32	invalidate_domains,
+		       u32	flush_domains)
+{
+	u32 cmd;
+	int ret;
+
+	cmd = MI_FLUSH;
+	if (((invalidate_domains|flush_domains) & I915_GEM_DOMAIN_RENDER) == 0)
+		cmd |= MI_NO_WRITE_FLUSH;
+
+	if (invalidate_domains & I915_GEM_DOMAIN_SAMPLER)
+		cmd |= MI_READ_FLUSH;
+
+	ret = intel_ring_begin(ring, 2);
+	if (ret)
+		return ret;
+
+	intel_ring_emit(ring, cmd);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_advance(ring);
+
+	return 0;
+}
+
+static int
+gen4_render_ring_flush(struct intel_ring_buffer *ring,
+		       u32	invalidate_domains,
+		       u32	flush_domains)
 {
 	struct drm_device *dev = ring->dev;
 	u32 cmd;
@@ -90,17 +116,8 @@ render_ring_flush(struct intel_ring_buffer *ring,
 	 */
 
 	cmd = MI_FLUSH | MI_NO_WRITE_FLUSH;
-	if ((invalidate_domains|flush_domains) &
-	    I915_GEM_DOMAIN_RENDER)
+	if ((invalidate_domains|flush_domains) & I915_GEM_DOMAIN_RENDER)
 		cmd &= ~MI_NO_WRITE_FLUSH;
-	if (INTEL_INFO(dev)->gen < 4) {
-		/*
-		 * On the 965, the sampler cache always gets flushed
-		 * and this bit is reserved.
-		 */
-		if (invalidate_domains & I915_GEM_DOMAIN_SAMPLER)
-			cmd |= MI_READ_FLUSH;
-	}
 	if (invalidate_domains & I915_GEM_DOMAIN_INSTRUCTION)
 		cmd |= MI_EXE_FLUSH;
 
@@ -290,9 +307,9 @@ static int init_ring_common(struct intel_ring_buffer *ring)
 			| RING_VALID);
 
 	/* If the head is still not zero, the ring is dead */
-	if ((I915_READ_CTL(ring) & RING_VALID) == 0 ||
-	    I915_READ_START(ring) != obj->gtt_offset ||
-	    (I915_READ_HEAD(ring) & HEAD_ADDR) != 0) {
+	if (wait_for((I915_READ_CTL(ring) & RING_VALID) != 0 &&
+		     I915_READ_START(ring) == obj->gtt_offset &&
+		     (I915_READ_HEAD(ring) & HEAD_ADDR) == 0, 50)) {
 		DRM_ERROR("%s initialization failed "
 				"ctl %08x head %08x tail %08x start %08x\n",
 				ring->name,
@@ -384,12 +401,11 @@ static int init_render_ring(struct intel_ring_buffer *ring)
 	int ret = init_ring_common(ring);
 
 	if (INTEL_INFO(dev)->gen > 3) {
-		int mode = VS_TIMER_DISPATCH << 16 | VS_TIMER_DISPATCH;
-		I915_WRITE(MI_MODE, mode);
+		I915_WRITE(MI_MODE, _MASKED_BIT_ENABLE(VS_TIMER_DISPATCH));
 		if (IS_GEN7(dev))
 			I915_WRITE(GFX_MODE_GEN7,
-				   GFX_MODE_DISABLE(GFX_TLB_INVALIDATE_ALWAYS) |
-				   GFX_MODE_ENABLE(GFX_REPLAY_MODE));
+				   _MASKED_BIT_DISABLE(GFX_TLB_INVALIDATE_ALWAYS) |
+				   _MASKED_BIT_ENABLE(GFX_REPLAY_MODE));
 	}
 
 	if (INTEL_INFO(dev)->gen >= 5) {
@@ -398,7 +414,6 @@ static int init_render_ring(struct intel_ring_buffer *ring)
 			return ret;
 	}
 
-
 	if (IS_GEN6(dev)) {
 		/* From the Sandybridge PRM, volume 1 part 3, page 24:
 		 * "If this bit is set, STCunit will have LRA as replacement
@@ -406,13 +421,11 @@ static int init_render_ring(struct intel_ring_buffer *ring)
 		 *  policy is not supported."
 		 */
 		I915_WRITE(CACHE_MODE_0,
-			   CM0_STC_EVICT_DISABLE_LRA_SNB << CM0_MASK_SHIFT);
+			   _MASKED_BIT_DISABLE(CM0_STC_EVICT_DISABLE_LRA_SNB));
 	}
 
-	if (INTEL_INFO(dev)->gen >= 6) {
-		I915_WRITE(INSTPM,
-			   INSTPM_FORCE_ORDERING << 16 | INSTPM_FORCE_ORDERING);
-	}
+	if (INTEL_INFO(dev)->gen >= 6)
+		I915_WRITE(INSTPM, _MASKED_BIT_ENABLE(INSTPM_FORCE_ORDERING));
 
 	return ret;
 }
@@ -483,21 +496,30 @@ gen6_add_request(struct intel_ring_buffer *ring,
  * @seqno - seqno which the waiter will block on
  */
 static int
-intel_ring_sync(struct intel_ring_buffer *waiter,
-		struct intel_ring_buffer *signaller,
-		int ring,
-		u32 seqno)
+gen6_ring_sync(struct intel_ring_buffer *waiter,
+	       struct intel_ring_buffer *signaller,
+	       u32 seqno)
 {
 	int ret;
 	u32 dw1 = MI_SEMAPHORE_MBOX |
 		  MI_SEMAPHORE_COMPARE |
 		  MI_SEMAPHORE_REGISTER;
 
+	/* Throughout all of the GEM code, seqno passed implies our current
+	 * seqno is >= the last seqno executed. However for hardware the
+	 * comparison is strictly greater than.
+	 */
+	seqno -= 1;
+
+	WARN_ON(signaller->semaphore_register[waiter->id] ==
+		MI_SEMAPHORE_SYNC_INVALID);
+
 	ret = intel_ring_begin(waiter, 4);
 	if (ret)
 		return ret;
 
-	intel_ring_emit(waiter, dw1 | signaller->semaphore_register[ring]);
+	intel_ring_emit(waiter,
+			dw1 | signaller->semaphore_register[waiter->id]);
 	intel_ring_emit(waiter, seqno);
 	intel_ring_emit(waiter, 0);
 	intel_ring_emit(waiter, MI_NOOP);
@@ -506,47 +528,6 @@ intel_ring_sync(struct intel_ring_buffer *waiter,
 	return 0;
 }
 
-/* VCS->RCS (RVSYNC) or BCS->RCS (RBSYNC) */
-int
-render_ring_sync_to(struct intel_ring_buffer *waiter,
-		    struct intel_ring_buffer *signaller,
-		    u32 seqno)
-{
-	WARN_ON(signaller->semaphore_register[RCS] == MI_SEMAPHORE_SYNC_INVALID);
-	return intel_ring_sync(waiter,
-			       signaller,
-			       RCS,
-			       seqno);
-}
-
-/* RCS->VCS (VRSYNC) or BCS->VCS (VBSYNC) */
-int
-gen6_bsd_ring_sync_to(struct intel_ring_buffer *waiter,
-		      struct intel_ring_buffer *signaller,
-		      u32 seqno)
-{
-	WARN_ON(signaller->semaphore_register[VCS] == MI_SEMAPHORE_SYNC_INVALID);
-	return intel_ring_sync(waiter,
-			       signaller,
-			       VCS,
-			       seqno);
-}
-
-/* RCS->BCS (BRSYNC) or VCS->BCS (BVSYNC) */
-int
-gen6_blt_ring_sync_to(struct intel_ring_buffer *waiter,
-		      struct intel_ring_buffer *signaller,
-		      u32 seqno)
-{
-	WARN_ON(signaller->semaphore_register[BCS] == MI_SEMAPHORE_SYNC_INVALID);
-	return intel_ring_sync(waiter,
-			       signaller,
-			       BCS,
-			       seqno);
-}
-
-
-
 #define PIPE_CONTROL_FLUSH(ring__, addr__)					\
 do {									\
 	intel_ring_emit(ring__, GFX_OP_PIPE_CONTROL(4) | PIPE_CONTROL_QW_WRITE |		\
@@ -608,27 +589,6 @@ pc_render_add_request(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static int
-render_ring_add_request(struct intel_ring_buffer *ring,
-			u32 *result)
-{
-	u32 seqno = i915_gem_next_request_seqno(ring);
-	int ret;
-
-	ret = intel_ring_begin(ring, 4);
-	if (ret)
-		return ret;
-
-	intel_ring_emit(ring, MI_STORE_DWORD_INDEX);
-	intel_ring_emit(ring, I915_GEM_HWS_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
-	intel_ring_emit(ring, seqno);
-	intel_ring_emit(ring, MI_USER_INTERRUPT);
-	intel_ring_advance(ring);
-
-	*result = seqno;
-	return 0;
-}
-
 static u32
 gen6_ring_get_seqno(struct intel_ring_buffer *ring)
 {
@@ -655,76 +615,115 @@ pc_render_get_seqno(struct intel_ring_buffer *ring)
 	return pc->cpu_page[0];
 }
 
-static void
-ironlake_enable_irq(drm_i915_private_t *dev_priv, u32 mask)
+static bool
+gen5_ring_get_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->gt_irq_mask &= ~mask;
-	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
-	POSTING_READ(GTIMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	if (!dev->irq_enabled)
+		return false;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (ring->irq_refcount++ == 0) {
+		dev_priv->gt_irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	return true;
 }
 
 static void
-ironlake_disable_irq(drm_i915_private_t *dev_priv, u32 mask)
+gen5_ring_put_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->gt_irq_mask |= mask;
-	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
-	POSTING_READ(GTIMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (--ring->irq_refcount == 0) {
+		dev_priv->gt_irq_mask |= ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 }
 
-static void
-i915_enable_irq(drm_i915_private_t *dev_priv, u32 mask)
+static bool
+i9xx_ring_get_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->irq_mask &= ~mask;
-	I915_WRITE(IMR, dev_priv->irq_mask);
-	POSTING_READ(IMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	if (!dev->irq_enabled)
+		return false;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (ring->irq_refcount++ == 0) {
+		dev_priv->irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE(IMR, dev_priv->irq_mask);
+		POSTING_READ(IMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	return true;
 }
 
 static void
-i915_disable_irq(drm_i915_private_t *dev_priv, u32 mask)
+i9xx_ring_put_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->irq_mask |= mask;
-	I915_WRITE(IMR, dev_priv->irq_mask);
-	POSTING_READ(IMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (--ring->irq_refcount == 0) {
+		dev_priv->irq_mask |= ring->irq_enable_mask;
+		I915_WRITE(IMR, dev_priv->irq_mask);
+		POSTING_READ(IMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 }
 
 static bool
-render_ring_get_irq(struct intel_ring_buffer *ring)
+i8xx_ring_get_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
 	if (!dev->irq_enabled)
 		return false;
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (ring->irq_refcount++ == 0) {
-		if (HAS_PCH_SPLIT(dev))
-			ironlake_enable_irq(dev_priv,
-					    GT_PIPE_NOTIFY | GT_USER_INTERRUPT);
-		else
-			i915_enable_irq(dev_priv, I915_USER_INTERRUPT);
+		dev_priv->irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE16(IMR, dev_priv->irq_mask);
+		POSTING_READ16(IMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 
 	return true;
 }
 
 static void
-render_ring_put_irq(struct intel_ring_buffer *ring)
+i8xx_ring_put_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (--ring->irq_refcount == 0) {
-		if (HAS_PCH_SPLIT(dev))
-			ironlake_disable_irq(dev_priv,
-					     GT_USER_INTERRUPT |
-					     GT_PIPE_NOTIFY);
-		else
-			i915_disable_irq(dev_priv, I915_USER_INTERRUPT);
+		dev_priv->irq_mask |= ring->irq_enable_mask;
+		I915_WRITE16(IMR, dev_priv->irq_mask);
+		POSTING_READ16(IMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 }
 
 void intel_ring_setup_status_page(struct intel_ring_buffer *ring)
@@ -776,7 +775,7 @@ bsd_ring_flush(struct intel_ring_buffer *ring,
 }
 
 static int
-ring_add_request(struct intel_ring_buffer *ring,
+i9xx_add_request(struct intel_ring_buffer *ring,
 		 u32 *result)
 {
 	u32 seqno;
@@ -799,10 +798,11 @@ ring_add_request(struct intel_ring_buffer *ring,
 }
 
 static bool
-gen6_ring_get_irq(struct intel_ring_buffer *ring, u32 gflag, u32 rflag)
+gen6_ring_get_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
 	if (!dev->irq_enabled)
 	       return false;
@@ -812,120 +812,87 @@ gen6_ring_get_irq(struct intel_ring_buffer *ring, u32 gflag, u32 rflag)
 	 * blt/bsd rings on ivb. */
 	gen6_gt_force_wake_get(dev_priv);
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (ring->irq_refcount++ == 0) {
-		ring->irq_mask &= ~rflag;
-		I915_WRITE_IMR(ring, ring->irq_mask);
-		ironlake_enable_irq(dev_priv, gflag);
+		I915_WRITE_IMR(ring, ~ring->irq_enable_mask);
+		dev_priv->gt_irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 
 	return true;
 }
 
 static void
-gen6_ring_put_irq(struct intel_ring_buffer *ring, u32 gflag, u32 rflag)
+gen6_ring_put_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (--ring->irq_refcount == 0) {
-		ring->irq_mask |= rflag;
-		I915_WRITE_IMR(ring, ring->irq_mask);
-		ironlake_disable_irq(dev_priv, gflag);
+		I915_WRITE_IMR(ring, ~0);
+		dev_priv->gt_irq_mask |= ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 
 	gen6_gt_force_wake_put(dev_priv);
 }
 
-static bool
-bsd_ring_get_irq(struct intel_ring_buffer *ring)
+static int
+i965_dispatch_execbuffer(struct intel_ring_buffer *ring, u32 offset, u32 length)
 {
-	struct drm_device *dev = ring->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	if (!dev->irq_enabled)
-		return false;
+	int ret;
 
-	spin_lock(&ring->irq_lock);
-	if (ring->irq_refcount++ == 0) {
-		if (IS_G4X(dev))
-			i915_enable_irq(dev_priv, I915_BSD_USER_INTERRUPT);
-		else
-			ironlake_enable_irq(dev_priv, GT_BSD_USER_INTERRUPT);
-	}
-	spin_unlock(&ring->irq_lock);
+	ret = intel_ring_begin(ring, 2);
+	if (ret)
+		return ret;
 
-	return true;
-}
-static void
-bsd_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	struct drm_device *dev = ring->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
+	intel_ring_emit(ring,
+			MI_BATCH_BUFFER_START |
+			MI_BATCH_GTT |
+			MI_BATCH_NON_SECURE_I965);
+	intel_ring_emit(ring, offset);
+	intel_ring_advance(ring);
 
-	spin_lock(&ring->irq_lock);
-	if (--ring->irq_refcount == 0) {
-		if (IS_G4X(dev))
-			i915_disable_irq(dev_priv, I915_BSD_USER_INTERRUPT);
-		else
-			ironlake_disable_irq(dev_priv, GT_BSD_USER_INTERRUPT);
-	}
-	spin_unlock(&ring->irq_lock);
+	return 0;
 }
 
 static int
-ring_dispatch_execbuffer(struct intel_ring_buffer *ring, u32 offset, u32 length)
+i830_dispatch_execbuffer(struct intel_ring_buffer *ring,
+				u32 offset, u32 len)
 {
 	int ret;
 
-	ret = intel_ring_begin(ring, 2);
+	ret = intel_ring_begin(ring, 4);
 	if (ret)
 		return ret;
 
-	intel_ring_emit(ring,
-			MI_BATCH_BUFFER_START | (2 << 6) |
-			MI_BATCH_NON_SECURE_I965);
-	intel_ring_emit(ring, offset);
+	intel_ring_emit(ring, MI_BATCH_BUFFER);
+	intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
+	intel_ring_emit(ring, offset + len - 8);
+	intel_ring_emit(ring, 0);
 	intel_ring_advance(ring);
 
 	return 0;
 }
 
 static int
-render_ring_dispatch_execbuffer(struct intel_ring_buffer *ring,
+i915_dispatch_execbuffer(struct intel_ring_buffer *ring,
 				u32 offset, u32 len)
 {
-	struct drm_device *dev = ring->dev;
 	int ret;
 
-	if (IS_I830(dev) || IS_845G(dev)) {
-		ret = intel_ring_begin(ring, 4);
-		if (ret)
-			return ret;
-
-		intel_ring_emit(ring, MI_BATCH_BUFFER);
-		intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
-		intel_ring_emit(ring, offset + len - 8);
-		intel_ring_emit(ring, 0);
-	} else {
-		ret = intel_ring_begin(ring, 2);
-		if (ret)
-			return ret;
+	ret = intel_ring_begin(ring, 2);
+	if (ret)
+		return ret;
 
-		if (INTEL_INFO(dev)->gen >= 4) {
-			intel_ring_emit(ring,
-					MI_BATCH_BUFFER_START | (2 << 6) |
-					MI_BATCH_NON_SECURE_I965);
-			intel_ring_emit(ring, offset);
-		} else {
-			intel_ring_emit(ring,
-					MI_BATCH_BUFFER_START | (2 << 6));
-			intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
-		}
-	}
+	intel_ring_emit(ring, MI_BATCH_BUFFER_START | MI_BATCH_GTT);
+	intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
 	intel_ring_advance(ring);
 
 	return 0;
@@ -933,7 +900,6 @@ render_ring_dispatch_execbuffer(struct intel_ring_buffer *ring,
 
 static void cleanup_status_page(struct intel_ring_buffer *ring)
 {
-	drm_i915_private_t *dev_priv = ring->dev->dev_private;
 	struct drm_i915_gem_object *obj;
 
 	obj = ring->status_page.obj;
@@ -944,14 +910,11 @@ static void cleanup_status_page(struct intel_ring_buffer *ring)
 	i915_gem_object_unpin(obj);
 	drm_gem_object_unreference(&obj->base);
 	ring->status_page.obj = NULL;
-
-	memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
 }
 
 static int init_status_page(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj;
 	int ret;
 
@@ -972,7 +935,6 @@ static int init_status_page(struct intel_ring_buffer *ring)
 	ring->status_page.gfx_addr = obj->gtt_offset;
 	ring->status_page.page_addr = kmap(obj->pages[0]);
 	if (ring->status_page.page_addr == NULL) {
-		memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
 		goto err_unpin;
 	}
 	ring->status_page.obj = obj;
@@ -992,8 +954,8 @@ err:
 	return ret;
 }
 
-int intel_init_ring_buffer(struct drm_device *dev,
-			   struct intel_ring_buffer *ring)
+static int intel_init_ring_buffer(struct drm_device *dev,
+				  struct intel_ring_buffer *ring)
 {
 	struct drm_i915_gem_object *obj;
 	int ret;
@@ -1002,10 +964,9 @@ int intel_init_ring_buffer(struct drm_device *dev,
 	INIT_LIST_HEAD(&ring->active_list);
 	INIT_LIST_HEAD(&ring->request_list);
 	INIT_LIST_HEAD(&ring->gpu_write_list);
+	ring->size = 32 * PAGE_SIZE;
 
 	init_waitqueue_head(&ring->irq_queue);
-	spin_lock_init(&ring->irq_lock);
-	ring->irq_mask = ~0;
 
 	if (I915_NEED_GFX_HWS(dev)) {
 		ret = init_status_page(ring);
@@ -1026,20 +987,14 @@ int intel_init_ring_buffer(struct drm_device *dev,
 	if (ret)
 		goto err_unref;
 
-	ring->map.size = ring->size;
-	ring->map.offset = dev->agp->base + obj->gtt_offset;
-	ring->map.type = 0;
-	ring->map.flags = 0;
-	ring->map.mtrr = 0;
-
-	drm_core_ioremap_wc(&ring->map, dev);
-	if (ring->map.handle == NULL) {
+	ring->virtual_start = ioremap_wc(dev->agp->base + obj->gtt_offset,
+					 ring->size);
+	if (ring->virtual_start == NULL) {
 		DRM_ERROR("Failed to map ringbuffer.\n");
 		ret = -EINVAL;
 		goto err_unpin;
 	}
 
-	ring->virtual_start = ring->map.handle;
 	ret = ring->init(ring);
 	if (ret)
 		goto err_unmap;
@@ -1055,7 +1010,7 @@ int intel_init_ring_buffer(struct drm_device *dev,
 	return 0;
 
 err_unmap:
-	drm_core_ioremapfree(&ring->map, dev);
+	iounmap(ring->virtual_start);
 err_unpin:
 	i915_gem_object_unpin(obj);
 err_unref:
@@ -1083,7 +1038,7 @@ void intel_cleanup_ring_buffer(struct intel_ring_buffer *ring)
 
 	I915_WRITE_CTL(ring, 0);
 
-	drm_core_ioremapfree(&ring->map, ring->dev);
+	iounmap(ring->virtual_start);
 
 	i915_gem_object_unpin(ring->obj);
 	drm_gem_object_unreference(&ring->obj->base);
@@ -1097,7 +1052,7 @@ void intel_cleanup_ring_buffer(struct intel_ring_buffer *ring)
 
 static int intel_wrap_ring_buffer(struct intel_ring_buffer *ring)
 {
-	unsigned int *virt;
+	uint32_t __iomem *virt;
 	int rem = ring->size - ring->tail;
 
 	if (ring->space < rem) {
@@ -1106,12 +1061,10 @@ static int intel_wrap_ring_buffer(struct intel_ring_buffer *ring)
 			return ret;
 	}
 
-	virt = (unsigned int *)(ring->virtual_start + ring->tail);
-	rem /= 8;
-	while (rem--) {
-		*virt++ = MI_NOOP;
-		*virt++ = MI_NOOP;
-	}
+	virt = ring->virtual_start + ring->tail;
+	rem /= 4;
+	while (rem--)
+		iowrite32(MI_NOOP, virt++);
 
 	ring->tail = 0;
 	ring->space = ring_space(ring);
@@ -1132,9 +1085,11 @@ static int intel_ring_wait_seqno(struct intel_ring_buffer *ring, u32 seqno)
 	was_interruptible = dev_priv->mm.interruptible;
 	dev_priv->mm.interruptible = false;
 
-	ret = i915_wait_request(ring, seqno, true);
+	ret = i915_wait_request(ring, seqno);
 
 	dev_priv->mm.interruptible = was_interruptible;
+	if (!ret)
+		i915_gem_retire_requests_ring(ring);
 
 	return ret;
 }
@@ -1208,15 +1163,12 @@ int intel_wait_ring_buffer(struct intel_ring_buffer *ring, int n)
 		return ret;
 
 	trace_i915_ring_wait_begin(ring);
-	if (drm_core_check_feature(dev, DRIVER_GEM))
-		/* With GEM the hangcheck timer should kick us out of the loop,
-		 * leaving it early runs the risk of corrupting GEM state (due
-		 * to running on almost untested codepaths). But on resume
-		 * timers don't work yet, so prevent a complete hang in that
-		 * case by choosing an insanely large timeout. */
-		end = jiffies + 60 * HZ;
-	else
-		end = jiffies + 3 * HZ;
+	/* With GEM the hangcheck timer should kick us out of the loop,
+	 * leaving it early runs the risk of corrupting GEM state (due
+	 * to running on almost untested codepaths). But on resume
+	 * timers don't work yet, so prevent a complete hang in that
+	 * case by choosing an insanely large timeout. */
+	end = jiffies + 60 * HZ;
 
 	do {
 		ring->head = I915_READ_HEAD(ring);
@@ -1268,48 +1220,14 @@ int intel_ring_begin(struct intel_ring_buffer *ring,
 
 void intel_ring_advance(struct intel_ring_buffer *ring)
 {
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+
 	ring->tail &= ring->size - 1;
+	if (dev_priv->stop_rings & intel_ring_flag(ring))
+		return;
 	ring->write_tail(ring, ring->tail);
 }
 
-static const struct intel_ring_buffer render_ring = {
-	.name			= "render ring",
-	.id			= RCS,
-	.mmio_base		= RENDER_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_render_ring,
-	.write_tail		= ring_write_tail,
-	.flush			= render_ring_flush,
-	.add_request		= render_ring_add_request,
-	.get_seqno		= ring_get_seqno,
-	.irq_get		= render_ring_get_irq,
-	.irq_put		= render_ring_put_irq,
-	.dispatch_execbuffer	= render_ring_dispatch_execbuffer,
-	.cleanup		= render_ring_cleanup,
-	.sync_to		= render_ring_sync_to,
-	.semaphore_register	= {MI_SEMAPHORE_SYNC_INVALID,
-				   MI_SEMAPHORE_SYNC_RV,
-				   MI_SEMAPHORE_SYNC_RB},
-	.signal_mbox		= {GEN6_VRSYNC, GEN6_BRSYNC},
-};
-
-/* ring buffer for bit-stream decoder */
-
-static const struct intel_ring_buffer bsd_ring = {
-	.name                   = "bsd ring",
-	.id			= VCS,
-	.mmio_base		= BSD_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_ring_common,
-	.write_tail		= ring_write_tail,
-	.flush			= bsd_ring_flush,
-	.add_request		= ring_add_request,
-	.get_seqno		= ring_get_seqno,
-	.irq_get		= bsd_ring_get_irq,
-	.irq_put		= bsd_ring_put_irq,
-	.dispatch_execbuffer	= ring_dispatch_execbuffer,
-};
-
 
 static void gen6_bsd_ring_write_tail(struct intel_ring_buffer *ring,
 				     u32 value)
@@ -1372,77 +1290,8 @@ gen6_ring_dispatch_execbuffer(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static bool
-gen6_render_ring_get_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_get_irq(ring,
-				 GT_USER_INTERRUPT,
-				 GEN6_RENDER_USER_INTERRUPT);
-}
-
-static void
-gen6_render_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_put_irq(ring,
-				 GT_USER_INTERRUPT,
-				 GEN6_RENDER_USER_INTERRUPT);
-}
-
-static bool
-gen6_bsd_ring_get_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_get_irq(ring,
-				 GT_GEN6_BSD_USER_INTERRUPT,
-				 GEN6_BSD_USER_INTERRUPT);
-}
-
-static void
-gen6_bsd_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_put_irq(ring,
-				 GT_GEN6_BSD_USER_INTERRUPT,
-				 GEN6_BSD_USER_INTERRUPT);
-}
-
-/* ring buffer for Video Codec for Gen6+ */
-static const struct intel_ring_buffer gen6_bsd_ring = {
-	.name			= "gen6 bsd ring",
-	.id			= VCS,
-	.mmio_base		= GEN6_BSD_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_ring_common,
-	.write_tail		= gen6_bsd_ring_write_tail,
-	.flush			= gen6_ring_flush,
-	.add_request		= gen6_add_request,
-	.get_seqno		= gen6_ring_get_seqno,
-	.irq_get		= gen6_bsd_ring_get_irq,
-	.irq_put		= gen6_bsd_ring_put_irq,
-	.dispatch_execbuffer	= gen6_ring_dispatch_execbuffer,
-	.sync_to		= gen6_bsd_ring_sync_to,
-	.semaphore_register	= {MI_SEMAPHORE_SYNC_VR,
-				   MI_SEMAPHORE_SYNC_INVALID,
-				   MI_SEMAPHORE_SYNC_VB},
-	.signal_mbox		= {GEN6_RVSYNC, GEN6_BVSYNC},
-};
-
 /* Blitter support (SandyBridge+) */
 
-static bool
-blt_ring_get_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_get_irq(ring,
-				 GT_BLT_USER_INTERRUPT,
-				 GEN6_BLITTER_USER_INTERRUPT);
-}
-
-static void
-blt_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	gen6_ring_put_irq(ring,
-			  GT_BLT_USER_INTERRUPT,
-			  GEN6_BLITTER_USER_INTERRUPT);
-}
-
 static int blt_ring_flush(struct intel_ring_buffer *ring,
 			  u32 invalidate, u32 flush)
 {
@@ -1464,42 +1313,63 @@ static int blt_ring_flush(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static const struct intel_ring_buffer gen6_blt_ring = {
-	.name			= "blt ring",
-	.id			= BCS,
-	.mmio_base		= BLT_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_ring_common,
-	.write_tail		= ring_write_tail,
-	.flush			= blt_ring_flush,
-	.add_request		= gen6_add_request,
-	.get_seqno		= gen6_ring_get_seqno,
-	.irq_get		= blt_ring_get_irq,
-	.irq_put		= blt_ring_put_irq,
-	.dispatch_execbuffer	= gen6_ring_dispatch_execbuffer,
-	.sync_to		= gen6_blt_ring_sync_to,
-	.semaphore_register	= {MI_SEMAPHORE_SYNC_BR,
-				   MI_SEMAPHORE_SYNC_BV,
-				   MI_SEMAPHORE_SYNC_INVALID},
-	.signal_mbox		= {GEN6_RBSYNC, GEN6_VBSYNC},
-};
-
 int intel_init_render_ring_buffer(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 
-	*ring = render_ring;
+	ring->name = "render ring";
+	ring->id = RCS;
+	ring->mmio_base = RENDER_RING_BASE;
+
 	if (INTEL_INFO(dev)->gen >= 6) {
 		ring->add_request = gen6_add_request;
 		ring->flush = gen6_render_ring_flush;
-		ring->irq_get = gen6_render_ring_get_irq;
-		ring->irq_put = gen6_render_ring_put_irq;
+		ring->irq_get = gen6_ring_get_irq;
+		ring->irq_put = gen6_ring_put_irq;
+		ring->irq_enable_mask = GT_USER_INTERRUPT;
 		ring->get_seqno = gen6_ring_get_seqno;
+		ring->sync_to = gen6_ring_sync;
+		ring->semaphore_register[0] = MI_SEMAPHORE_SYNC_INVALID;
+		ring->semaphore_register[1] = MI_SEMAPHORE_SYNC_RV;
+		ring->semaphore_register[2] = MI_SEMAPHORE_SYNC_RB;
+		ring->signal_mbox[0] = GEN6_VRSYNC;
+		ring->signal_mbox[1] = GEN6_BRSYNC;
 	} else if (IS_GEN5(dev)) {
 		ring->add_request = pc_render_add_request;
+		ring->flush = gen4_render_ring_flush;
 		ring->get_seqno = pc_render_get_seqno;
+		ring->irq_get = gen5_ring_get_irq;
+		ring->irq_put = gen5_ring_put_irq;
+		ring->irq_enable_mask = GT_USER_INTERRUPT | GT_PIPE_NOTIFY;
+	} else {
+		ring->add_request = i9xx_add_request;
+		if (INTEL_INFO(dev)->gen < 4)
+			ring->flush = gen2_render_ring_flush;
+		else
+			ring->flush = gen4_render_ring_flush;
+		ring->get_seqno = ring_get_seqno;
+		if (IS_GEN2(dev)) {
+			ring->irq_get = i8xx_ring_get_irq;
+			ring->irq_put = i8xx_ring_put_irq;
+		} else {
+			ring->irq_get = i9xx_ring_get_irq;
+			ring->irq_put = i9xx_ring_put_irq;
+		}
+		ring->irq_enable_mask = I915_USER_INTERRUPT;
 	}
+	ring->write_tail = ring_write_tail;
+	if (INTEL_INFO(dev)->gen >= 6)
+		ring->dispatch_execbuffer = gen6_ring_dispatch_execbuffer;
+	else if (INTEL_INFO(dev)->gen >= 4)
+		ring->dispatch_execbuffer = i965_dispatch_execbuffer;
+	else if (IS_I830(dev) || IS_845G(dev))
+		ring->dispatch_execbuffer = i830_dispatch_execbuffer;
+	else
+		ring->dispatch_execbuffer = i915_dispatch_execbuffer;
+	ring->init = init_render_ring;
+	ring->cleanup = render_ring_cleanup;
+
 
 	if (!I915_NEED_GFX_HWS(dev)) {
 		ring->status_page.page_addr = dev_priv->status_page_dmah->vaddr;
@@ -1514,15 +1384,41 @@ int intel_render_ring_init_dri(struct drm_device *dev, u64 start, u32 size)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 
-	*ring = render_ring;
+	ring->name = "render ring";
+	ring->id = RCS;
+	ring->mmio_base = RENDER_RING_BASE;
+
 	if (INTEL_INFO(dev)->gen >= 6) {
-		ring->add_request = gen6_add_request;
-		ring->irq_get = gen6_render_ring_get_irq;
-		ring->irq_put = gen6_render_ring_put_irq;
-	} else if (IS_GEN5(dev)) {
-		ring->add_request = pc_render_add_request;
-		ring->get_seqno = pc_render_get_seqno;
+		/* non-kms not supported on gen6+ */
+		return -ENODEV;
+	}
+
+	/* Note: gem is not supported on gen5/ilk without kms (the corresponding
+	 * gem_init ioctl returns with -ENODEV). Hence we do not need to set up
+	 * the special gen5 functions. */
+	ring->add_request = i9xx_add_request;
+	if (INTEL_INFO(dev)->gen < 4)
+		ring->flush = gen2_render_ring_flush;
+	else
+		ring->flush = gen4_render_ring_flush;
+	ring->get_seqno = ring_get_seqno;
+	if (IS_GEN2(dev)) {
+		ring->irq_get = i8xx_ring_get_irq;
+		ring->irq_put = i8xx_ring_put_irq;
+	} else {
+		ring->irq_get = i9xx_ring_get_irq;
+		ring->irq_put = i9xx_ring_put_irq;
 	}
+	ring->irq_enable_mask = I915_USER_INTERRUPT;
+	ring->write_tail = ring_write_tail;
+	if (INTEL_INFO(dev)->gen >= 4)
+		ring->dispatch_execbuffer = i965_dispatch_execbuffer;
+	else if (IS_I830(dev) || IS_845G(dev))
+		ring->dispatch_execbuffer = i830_dispatch_execbuffer;
+	else
+		ring->dispatch_execbuffer = i915_dispatch_execbuffer;
+	ring->init = init_render_ring;
+	ring->cleanup = render_ring_cleanup;
 
 	if (!I915_NEED_GFX_HWS(dev))
 		ring->status_page.page_addr = dev_priv->status_page_dmah->vaddr;
@@ -1537,20 +1433,13 @@ int intel_render_ring_init_dri(struct drm_device *dev, u64 start, u32 size)
 	if (IS_I830(ring->dev))
 		ring->effective_size -= 128;
 
-	ring->map.offset = start;
-	ring->map.size = size;
-	ring->map.type = 0;
-	ring->map.flags = 0;
-	ring->map.mtrr = 0;
-
-	drm_core_ioremap_wc(&ring->map, dev);
-	if (ring->map.handle == NULL) {
+	ring->virtual_start = ioremap_wc(start, size);
+	if (ring->virtual_start == NULL) {
 		DRM_ERROR("can not ioremap virtual address for"
 			  " ring buffer\n");
 		return -ENOMEM;
 	}
 
-	ring->virtual_start = (void __force __iomem *)ring->map.handle;
 	return 0;
 }
 
@@ -1559,10 +1448,46 @@ int intel_init_bsd_ring_buffer(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[VCS];
 
-	if (IS_GEN6(dev) || IS_GEN7(dev))
-		*ring = gen6_bsd_ring;
-	else
-		*ring = bsd_ring;
+	ring->name = "bsd ring";
+	ring->id = VCS;
+
+	ring->write_tail = ring_write_tail;
+	if (IS_GEN6(dev) || IS_GEN7(dev)) {
+		ring->mmio_base = GEN6_BSD_RING_BASE;
+		/* gen6 bsd needs a special wa for tail updates */
+		if (IS_GEN6(dev))
+			ring->write_tail = gen6_bsd_ring_write_tail;
+		ring->flush = gen6_ring_flush;
+		ring->add_request = gen6_add_request;
+		ring->get_seqno = gen6_ring_get_seqno;
+		ring->irq_enable_mask = GEN6_BSD_USER_INTERRUPT;
+		ring->irq_get = gen6_ring_get_irq;
+		ring->irq_put = gen6_ring_put_irq;
+		ring->dispatch_execbuffer = gen6_ring_dispatch_execbuffer;
+		ring->sync_to = gen6_ring_sync;
+		ring->semaphore_register[0] = MI_SEMAPHORE_SYNC_VR;
+		ring->semaphore_register[1] = MI_SEMAPHORE_SYNC_INVALID;
+		ring->semaphore_register[2] = MI_SEMAPHORE_SYNC_VB;
+		ring->signal_mbox[0] = GEN6_RVSYNC;
+		ring->signal_mbox[1] = GEN6_BVSYNC;
+	} else {
+		ring->mmio_base = BSD_RING_BASE;
+		ring->flush = bsd_ring_flush;
+		ring->add_request = i9xx_add_request;
+		ring->get_seqno = ring_get_seqno;
+		if (IS_GEN5(dev)) {
+			ring->irq_enable_mask = GT_BSD_USER_INTERRUPT;
+			ring->irq_get = gen5_ring_get_irq;
+			ring->irq_put = gen5_ring_put_irq;
+		} else {
+			ring->irq_enable_mask = I915_BSD_USER_INTERRUPT;
+			ring->irq_get = i9xx_ring_get_irq;
+			ring->irq_put = i9xx_ring_put_irq;
+		}
+		ring->dispatch_execbuffer = i965_dispatch_execbuffer;
+	}
+	ring->init = init_ring_common;
+
 
 	return intel_init_ring_buffer(dev, ring);
 }
@@ -1572,7 +1497,25 @@ int intel_init_blt_ring_buffer(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[BCS];
 
-	*ring = gen6_blt_ring;
+	ring->name = "blitter ring";
+	ring->id = BCS;
+
+	ring->mmio_base = BLT_RING_BASE;
+	ring->write_tail = ring_write_tail;
+	ring->flush = blt_ring_flush;
+	ring->add_request = gen6_add_request;
+	ring->get_seqno = gen6_ring_get_seqno;
+	ring->irq_enable_mask = GEN6_BLITTER_USER_INTERRUPT;
+	ring->irq_get = gen6_ring_get_irq;
+	ring->irq_put = gen6_ring_put_irq;
+	ring->dispatch_execbuffer = gen6_ring_dispatch_execbuffer;
+	ring->sync_to = gen6_ring_sync;
+	ring->semaphore_register[0] = MI_SEMAPHORE_SYNC_BR;
+	ring->semaphore_register[1] = MI_SEMAPHORE_SYNC_BV;
+	ring->semaphore_register[2] = MI_SEMAPHORE_SYNC_INVALID;
+	ring->signal_mbox[0] = GEN6_RBSYNC;
+	ring->signal_mbox[1] = GEN6_VBSYNC;
+	ring->init = init_ring_common;
 
 	return intel_init_ring_buffer(dev, ring);
 }
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index bc0365b8fa4d..55d3da26bae7 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -2,7 +2,7 @@
 #define _INTEL_RINGBUFFER_H_
 
 struct  intel_hw_status_page {
-	u32	__iomem	*page_addr;
+	u32		*page_addr;
 	unsigned int	gfx_addr;
 	struct		drm_i915_gem_object *obj;
 };
@@ -56,12 +56,9 @@ struct  intel_ring_buffer {
 	 */
 	u32		last_retired_head;
 
-	spinlock_t	irq_lock;
-	u32		irq_refcount;
-	u32		irq_mask;
-	u32		irq_seqno;		/* last seq seem at irq time */
+	u32		irq_refcount;		/* protected by dev_priv->irq_lock */
+	u32		irq_enable_mask;	/* bitmask to enable ring interrupt */
 	u32		trace_irq_seqno;
-	u32		waiting_seqno;
 	u32		sync_seqno[I915_NUM_RINGS-1];
 	bool __must_check (*irq_get)(struct intel_ring_buffer *ring);
 	void		(*irq_put)(struct intel_ring_buffer *ring);
@@ -118,11 +115,16 @@ struct  intel_ring_buffer {
 	u32 outstanding_lazy_request;
 
 	wait_queue_head_t irq_queue;
-	drm_local_map_t map;
 
 	void *private;
 };
 
+static inline bool
+intel_ring_initialized(struct intel_ring_buffer *ring)
+{
+	return ring->obj != NULL;
+}
+
 static inline unsigned
 intel_ring_flag(struct intel_ring_buffer *ring)
 {
@@ -152,7 +154,9 @@ static inline u32
 intel_read_status_page(struct intel_ring_buffer *ring,
 		       int reg)
 {
-	return ioread32(ring->status_page.page_addr + reg);
+	/* Ensure that the compiler doesn't optimize away the load. */
+	barrier();
+	return ring->status_page.page_addr[reg];
 }
 
 /**
@@ -170,10 +174,7 @@ intel_read_status_page(struct intel_ring_buffer *ring,
  *
  * The area from dword 0x20 to 0x3ff is available for driver usage.
  */
-#define READ_HWSP(dev_priv, reg) intel_read_status_page(LP_RING(dev_priv), reg)
-#define READ_BREADCRUMB(dev_priv) READ_HWSP(dev_priv, I915_BREADCRUMB_INDEX)
 #define I915_GEM_HWS_INDEX		0x20
-#define I915_BREADCRUMB_INDEX		0x21
 
 void intel_cleanup_ring_buffer(struct intel_ring_buffer *ring);
 
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index ae5e748f39bb..a949b73880c8 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -41,7 +41,7 @@
 #define SDVO_TMDS_MASK (SDVO_OUTPUT_TMDS0 | SDVO_OUTPUT_TMDS1)
 #define SDVO_RGB_MASK  (SDVO_OUTPUT_RGB0 | SDVO_OUTPUT_RGB1)
 #define SDVO_LVDS_MASK (SDVO_OUTPUT_LVDS0 | SDVO_OUTPUT_LVDS1)
-#define SDVO_TV_MASK   (SDVO_OUTPUT_CVBS0 | SDVO_OUTPUT_SVID0)
+#define SDVO_TV_MASK   (SDVO_OUTPUT_CVBS0 | SDVO_OUTPUT_SVID0 | SDVO_OUTPUT_YPRPB0)
 
 #define SDVO_OUTPUT_MASK (SDVO_TMDS_MASK | SDVO_RGB_MASK | SDVO_LVDS_MASK |\
 			SDVO_TV_MASK)
@@ -74,7 +74,7 @@ struct intel_sdvo {
 	struct i2c_adapter ddc;
 
 	/* Register for the SDVO device: SDVOB or SDVOC */
-	int sdvo_reg;
+	uint32_t sdvo_reg;
 
 	/* Active outputs controlled by this SDVO output */
 	uint16_t controlled_output;
@@ -114,6 +114,9 @@ struct intel_sdvo {
 	 */
 	bool is_tv;
 
+	/* On different gens SDVOB is at different places. */
+	bool is_sdvob;
+
 	/* This is for current tv format name */
 	int tv_format_index;
 
@@ -403,8 +406,7 @@ static const struct _sdvo_cmd_name {
 	SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_DATA),
 };
 
-#define IS_SDVOB(reg)	(reg == SDVOB || reg == PCH_SDVOB)
-#define SDVO_NAME(svdo) (IS_SDVOB((svdo)->sdvo_reg) ? "SDVOB" : "SDVOC")
+#define SDVO_NAME(svdo) ((svdo)->is_sdvob ? "SDVOB" : "SDVOC")
 
 static void intel_sdvo_debug_write(struct intel_sdvo *intel_sdvo, u8 cmd,
 				   const void *args, int args_len)
@@ -441,9 +443,17 @@ static const char *cmd_status_names[] = {
 static bool intel_sdvo_write_cmd(struct intel_sdvo *intel_sdvo, u8 cmd,
 				 const void *args, int args_len)
 {
-	u8 buf[args_len*2 + 2], status;
-	struct i2c_msg msgs[args_len + 3];
-	int i, ret;
+	u8 *buf, status;
+	struct i2c_msg *msgs;
+	int i, ret = true;
+
+	buf = (u8 *)kzalloc(args_len * 2 + 2, GFP_KERNEL);
+	if (!buf)
+		return false;
+
+	msgs = kcalloc(args_len + 3, sizeof(*msgs), GFP_KERNEL);
+	if (!msgs)
+		return false;
 
 	intel_sdvo_debug_write(intel_sdvo, cmd, args, args_len);
 
@@ -477,15 +487,19 @@ static bool intel_sdvo_write_cmd(struct intel_sdvo *intel_sdvo, u8 cmd,
 	ret = i2c_transfer(intel_sdvo->i2c, msgs, i+3);
 	if (ret < 0) {
 		DRM_DEBUG_KMS("I2c transfer returned %d\n", ret);
-		return false;
+		ret = false;
+		goto out;
 	}
 	if (ret != i+3) {
 		/* failure in I2C transfer */
 		DRM_DEBUG_KMS("I2c transfer returned %d/%d\n", ret, i+3);
-		return false;
+		ret = false;
 	}
 
-	return true;
+out:
+	kfree(msgs);
+	kfree(buf);
+	return ret;
 }
 
 static bool intel_sdvo_read_response(struct intel_sdvo *intel_sdvo,
@@ -733,18 +747,18 @@ static void intel_sdvo_get_dtd_from_mode(struct intel_sdvo_dtd *dtd,
 	uint16_t h_sync_offset, v_sync_offset;
 	int mode_clock;
 
-	width = mode->crtc_hdisplay;
-	height = mode->crtc_vdisplay;
+	width = mode->hdisplay;
+	height = mode->vdisplay;
 
 	/* do some mode translations */
-	h_blank_len = mode->crtc_hblank_end - mode->crtc_hblank_start;
-	h_sync_len = mode->crtc_hsync_end - mode->crtc_hsync_start;
+	h_blank_len = mode->htotal - mode->hdisplay;
+	h_sync_len = mode->hsync_end - mode->hsync_start;
 
-	v_blank_len = mode->crtc_vblank_end - mode->crtc_vblank_start;
-	v_sync_len = mode->crtc_vsync_end - mode->crtc_vsync_start;
+	v_blank_len = mode->vtotal - mode->vdisplay;
+	v_sync_len = mode->vsync_end - mode->vsync_start;
 
-	h_sync_offset = mode->crtc_hsync_start - mode->crtc_hblank_start;
-	v_sync_offset = mode->crtc_vsync_start - mode->crtc_vblank_start;
+	h_sync_offset = mode->hsync_start - mode->hdisplay;
+	v_sync_offset = mode->vsync_start - mode->vdisplay;
 
 	mode_clock = mode->clock;
 	mode_clock /= intel_mode_get_pixel_multiplier(mode) ?: 1;
@@ -873,17 +887,24 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
 	};
 	uint8_t tx_rate = SDVO_HBUF_TX_VSYNC;
 	uint8_t set_buf_index[2] = { 1, 0 };
-	uint64_t *data = (uint64_t *)&avi_if;
+	uint8_t sdvo_data[4 + sizeof(avi_if.body.avi)];
+	uint64_t *data = (uint64_t *)sdvo_data;
 	unsigned i;
 
 	intel_dip_infoframe_csum(&avi_if);
 
+	/* sdvo spec says that the ecc is handled by the hw, and it looks like
+	 * we must not send the ecc field, either. */
+	memcpy(sdvo_data, &avi_if, 3);
+	sdvo_data[3] = avi_if.checksum;
+	memcpy(&sdvo_data[4], &avi_if.body, sizeof(avi_if.body.avi));
+
 	if (!intel_sdvo_set_value(intel_sdvo,
 				  SDVO_CMD_SET_HBUF_INDEX,
 				  set_buf_index, 2))
 		return false;
 
-	for (i = 0; i < sizeof(avi_if); i += 8) {
+	for (i = 0; i < sizeof(sdvo_data); i += 8) {
 		if (!intel_sdvo_set_value(intel_sdvo,
 					  SDVO_CMD_SET_HBUF_DATA,
 					  data, 8))
@@ -1260,10 +1281,11 @@ intel_sdvo_get_analog_edid(struct drm_connector *connector)
 	struct drm_i915_private *dev_priv = connector->dev->dev_private;
 
 	return drm_get_edid(connector,
-			    &dev_priv->gmbus[dev_priv->crt_ddc_pin].adapter);
+			    intel_gmbus_get_adapter(dev_priv,
+						    dev_priv->crt_ddc_pin));
 }
 
-enum drm_connector_status
+static enum drm_connector_status
 intel_sdvo_tmds_sink_detect(struct drm_connector *connector)
 {
 	struct intel_sdvo *intel_sdvo = intel_attached_sdvo(connector);
@@ -1349,8 +1371,7 @@ intel_sdvo_detect(struct drm_connector *connector, bool force)
 		return connector_status_unknown;
 
 	/* add 30ms delay when the output type might be TV */
-	if (intel_sdvo->caps.output_flags &
-	    (SDVO_OUTPUT_SVID0 | SDVO_OUTPUT_CVBS0))
+	if (intel_sdvo->caps.output_flags & SDVO_TV_MASK)
 		mdelay(30);
 
 	if (!intel_sdvo_read_response(intel_sdvo, &response, 2))
@@ -1570,9 +1591,6 @@ end:
 			intel_sdvo->sdvo_lvds_fixed_mode =
 				drm_mode_duplicate(connector->dev, newmode);
 
-			drm_mode_set_crtcinfo(intel_sdvo->sdvo_lvds_fixed_mode,
-					      0);
-
 			intel_sdvo->is_lvds = true;
 			break;
 		}
@@ -1901,7 +1919,7 @@ intel_sdvo_select_ddc_bus(struct drm_i915_private *dev_priv,
 {
 	struct sdvo_device_mapping *mapping;
 
-	if (IS_SDVOB(reg))
+	if (sdvo->is_sdvob)
 		mapping = &(dev_priv->sdvo_mappings[0]);
 	else
 		mapping = &(dev_priv->sdvo_mappings[1]);
@@ -1919,7 +1937,7 @@ intel_sdvo_select_i2c_bus(struct drm_i915_private *dev_priv,
 	struct sdvo_device_mapping *mapping;
 	u8 pin;
 
-	if (IS_SDVOB(reg))
+	if (sdvo->is_sdvob)
 		mapping = &dev_priv->sdvo_mappings[0];
 	else
 		mapping = &dev_priv->sdvo_mappings[1];
@@ -1928,12 +1946,12 @@ intel_sdvo_select_i2c_bus(struct drm_i915_private *dev_priv,
 	if (mapping->initialized)
 		pin = mapping->i2c_pin;
 
-	if (pin < GMBUS_NUM_PORTS) {
-		sdvo->i2c = &dev_priv->gmbus[pin].adapter;
+	if (intel_gmbus_is_port_valid(pin)) {
+		sdvo->i2c = intel_gmbus_get_adapter(dev_priv, pin);
 		intel_gmbus_set_speed(sdvo->i2c, GMBUS_RATE_1MHZ);
 		intel_gmbus_force_bit(sdvo->i2c, true);
 	} else {
-		sdvo->i2c = &dev_priv->gmbus[GMBUS_PORT_DPB].adapter;
+		sdvo->i2c = intel_gmbus_get_adapter(dev_priv, GMBUS_PORT_DPB);
 	}
 }
 
@@ -1944,12 +1962,12 @@ intel_sdvo_is_hdmi_connector(struct intel_sdvo *intel_sdvo, int device)
 }
 
 static u8
-intel_sdvo_get_slave_addr(struct drm_device *dev, int sdvo_reg)
+intel_sdvo_get_slave_addr(struct drm_device *dev, struct intel_sdvo *sdvo)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct sdvo_device_mapping *my_mapping, *other_mapping;
 
-	if (IS_SDVOB(sdvo_reg)) {
+	if (sdvo->is_sdvob) {
 		my_mapping = &dev_priv->sdvo_mappings[0];
 		other_mapping = &dev_priv->sdvo_mappings[1];
 	} else {
@@ -1974,7 +1992,7 @@ intel_sdvo_get_slave_addr(struct drm_device *dev, int sdvo_reg)
 	/* No SDVO device info is found for another DVO port,
 	 * so use mapping assumption we had before BIOS parsing.
 	 */
-	if (IS_SDVOB(sdvo_reg))
+	if (sdvo->is_sdvob)
 		return 0x70;
 	else
 		return 0x72;
@@ -2199,6 +2217,10 @@ intel_sdvo_output_setup(struct intel_sdvo *intel_sdvo, uint16_t flags)
 		if (!intel_sdvo_tv_init(intel_sdvo, SDVO_OUTPUT_CVBS0))
 			return false;
 
+	if (flags & SDVO_OUTPUT_YPRPB0)
+		if (!intel_sdvo_tv_init(intel_sdvo, SDVO_OUTPUT_YPRPB0))
+			return false;
+
 	if (flags & SDVO_OUTPUT_RGB0)
 		if (!intel_sdvo_analog_init(intel_sdvo, 0))
 			return false;
@@ -2490,7 +2512,7 @@ intel_sdvo_init_ddc_proxy(struct intel_sdvo *sdvo,
 	return i2c_add_adapter(&sdvo->ddc) == 0;
 }
 
-bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
+bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *intel_encoder;
@@ -2502,7 +2524,8 @@ bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
 		return false;
 
 	intel_sdvo->sdvo_reg = sdvo_reg;
-	intel_sdvo->slave_addr = intel_sdvo_get_slave_addr(dev, sdvo_reg) >> 1;
+	intel_sdvo->is_sdvob = is_sdvob;
+	intel_sdvo->slave_addr = intel_sdvo_get_slave_addr(dev, intel_sdvo) >> 1;
 	intel_sdvo_select_i2c_bus(dev_priv, intel_sdvo, sdvo_reg);
 	if (!intel_sdvo_init_ddc_proxy(intel_sdvo, dev)) {
 		kfree(intel_sdvo);
@@ -2519,13 +2542,13 @@ bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
 		u8 byte;
 
 		if (!intel_sdvo_read_byte(intel_sdvo, i, &byte)) {
-			DRM_DEBUG_KMS("No SDVO device found on SDVO%c\n",
-				      IS_SDVOB(sdvo_reg) ? 'B' : 'C');
+			DRM_DEBUG_KMS("No SDVO device found on %s\n",
+				      SDVO_NAME(intel_sdvo));
 			goto err;
 		}
 	}
 
-	if (IS_SDVOB(sdvo_reg))
+	if (intel_sdvo->is_sdvob)
 		dev_priv->hotplug_supported_mask |= SDVOB_HOTPLUG_INT_STATUS;
 	else
 		dev_priv->hotplug_supported_mask |= SDVOC_HOTPLUG_INT_STATUS;
@@ -2546,8 +2569,8 @@ bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
 
 	if (intel_sdvo_output_setup(intel_sdvo,
 				    intel_sdvo->caps.output_flags) != true) {
-		DRM_DEBUG_KMS("SDVO output failed to setup on SDVO%c\n",
-			      IS_SDVOB(sdvo_reg) ? 'B' : 'C');
+		DRM_DEBUG_KMS("SDVO output failed to setup on %s\n",
+			      SDVO_NAME(intel_sdvo));
 		goto err;
 	}
 
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index e90dfb625c42..2a20fb0781d7 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -110,14 +110,18 @@ ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	 * when scaling is disabled.
 	 */
 	if (crtc_w != src_w || crtc_h != src_h) {
-		dev_priv->sprite_scaling_enabled = true;
-		sandybridge_update_wm(dev);
-		intel_wait_for_vblank(dev, pipe);
+		if (!dev_priv->sprite_scaling_enabled) {
+			dev_priv->sprite_scaling_enabled = true;
+			intel_update_watermarks(dev);
+			intel_wait_for_vblank(dev, pipe);
+		}
 		sprscale = SPRITE_SCALE_ENABLE | (src_w << 16) | src_h;
 	} else {
-		dev_priv->sprite_scaling_enabled = false;
-		/* potentially re-enable LP watermarks */
-		sandybridge_update_wm(dev);
+		if (dev_priv->sprite_scaling_enabled) {
+			dev_priv->sprite_scaling_enabled = false;
+			/* potentially re-enable LP watermarks */
+			intel_update_watermarks(dev);
+		}
 	}
 
 	I915_WRITE(SPRSTRIDE(pipe), fb->pitches[0]);
@@ -133,7 +137,7 @@ ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	I915_WRITE(SPRSIZE(pipe), (crtc_h << 16) | crtc_w);
 	I915_WRITE(SPRSCALE(pipe), sprscale);
 	I915_WRITE(SPRCTL(pipe), sprctl);
-	I915_WRITE(SPRSURF(pipe), obj->gtt_offset);
+	I915_MODIFY_DISPBASE(SPRSURF(pipe), obj->gtt_offset);
 	POSTING_READ(SPRSURF(pipe));
 }
 
@@ -149,8 +153,11 @@ ivb_disable_plane(struct drm_plane *plane)
 	/* Can't leave the scaler enabled... */
 	I915_WRITE(SPRSCALE(pipe), 0);
 	/* Activate double buffered register update */
-	I915_WRITE(SPRSURF(pipe), 0);
+	I915_MODIFY_DISPBASE(SPRSURF(pipe), 0);
 	POSTING_READ(SPRSURF(pipe));
+
+	dev_priv->sprite_scaling_enabled = false;
+	intel_update_watermarks(dev);
 }
 
 static int
@@ -208,7 +215,7 @@ ivb_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
 }
 
 static void
-snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
+ilk_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 		 struct drm_i915_gem_object *obj, int crtc_x, int crtc_y,
 		 unsigned int crtc_w, unsigned int crtc_h,
 		 uint32_t x, uint32_t y,
@@ -218,7 +225,7 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_plane *intel_plane = to_intel_plane(plane);
 	int pipe = intel_plane->pipe, pixel_size;
-	u32 dvscntr, dvsscale = 0;
+	u32 dvscntr, dvsscale;
 
 	dvscntr = I915_READ(DVSCNTR(pipe));
 
@@ -262,8 +269,8 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	if (obj->tiling_mode != I915_TILING_NONE)
 		dvscntr |= DVS_TILED;
 
-	/* must disable */
-	dvscntr |= DVS_TRICKLE_FEED_DISABLE;
+	if (IS_GEN6(dev))
+		dvscntr |= DVS_TRICKLE_FEED_DISABLE; /* must disable */
 	dvscntr |= DVS_ENABLE;
 
 	/* Sizes are 0 based */
@@ -274,7 +281,8 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 
 	intel_update_sprite_watermarks(dev, pipe, crtc_w, pixel_size);
 
-	if (crtc_w != src_w || crtc_h != src_h)
+	dvsscale = 0;
+	if (IS_GEN5(dev) || crtc_w != src_w || crtc_h != src_h)
 		dvsscale = DVS_SCALE_ENABLE | (src_w << 16) | src_h;
 
 	I915_WRITE(DVSSTRIDE(pipe), fb->pitches[0]);
@@ -290,12 +298,12 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	I915_WRITE(DVSSIZE(pipe), (crtc_h << 16) | crtc_w);
 	I915_WRITE(DVSSCALE(pipe), dvsscale);
 	I915_WRITE(DVSCNTR(pipe), dvscntr);
-	I915_WRITE(DVSSURF(pipe), obj->gtt_offset);
+	I915_MODIFY_DISPBASE(DVSSURF(pipe), obj->gtt_offset);
 	POSTING_READ(DVSSURF(pipe));
 }
 
 static void
-snb_disable_plane(struct drm_plane *plane)
+ilk_disable_plane(struct drm_plane *plane)
 {
 	struct drm_device *dev = plane->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -306,7 +314,7 @@ snb_disable_plane(struct drm_plane *plane)
 	/* Disable the scaler */
 	I915_WRITE(DVSSCALE(pipe), 0);
 	/* Flush double buffered register updates */
-	I915_WRITE(DVSSURF(pipe), 0);
+	I915_MODIFY_DISPBASE(DVSSURF(pipe), 0);
 	POSTING_READ(DVSSURF(pipe));
 }
 
@@ -333,7 +341,7 @@ intel_disable_primary(struct drm_crtc *crtc)
 }
 
 static int
-snb_update_colorkey(struct drm_plane *plane,
+ilk_update_colorkey(struct drm_plane *plane,
 		    struct drm_intel_sprite_colorkey *key)
 {
 	struct drm_device *dev = plane->dev;
@@ -362,7 +370,7 @@ snb_update_colorkey(struct drm_plane *plane,
 }
 
 static void
-snb_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
+ilk_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
 {
 	struct drm_device *dev = plane->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -550,14 +558,13 @@ int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv)
 {
 	struct drm_intel_sprite_colorkey *set = data;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_mode_object *obj;
 	struct drm_plane *plane;
 	struct intel_plane *intel_plane;
 	int ret = 0;
 
-	if (!dev_priv)
-		return -EINVAL;
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
 
 	/* Make sure we don't try to enable both src & dest simultaneously */
 	if ((set->flags & (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE)) == (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE))
@@ -584,14 +591,13 @@ int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv)
 {
 	struct drm_intel_sprite_colorkey *get = data;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_mode_object *obj;
 	struct drm_plane *plane;
 	struct intel_plane *intel_plane;
 	int ret = 0;
 
-	if (!dev_priv)
-		return -EINVAL;
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
 
 	mutex_lock(&dev->mode_config.mutex);
 
@@ -616,6 +622,14 @@ static const struct drm_plane_funcs intel_plane_funcs = {
 	.destroy = intel_destroy_plane,
 };
 
+static uint32_t ilk_plane_formats[] = {
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_YUYV,
+	DRM_FORMAT_YVYU,
+	DRM_FORMAT_UYVY,
+	DRM_FORMAT_VYUY,
+};
+
 static uint32_t snb_plane_formats[] = {
 	DRM_FORMAT_XBGR8888,
 	DRM_FORMAT_XRGB8888,
@@ -630,34 +644,56 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe)
 {
 	struct intel_plane *intel_plane;
 	unsigned long possible_crtcs;
+	const uint32_t *plane_formats;
+	int num_plane_formats;
 	int ret;
 
-	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
+	if (INTEL_INFO(dev)->gen < 5)
 		return -ENODEV;
 
 	intel_plane = kzalloc(sizeof(struct intel_plane), GFP_KERNEL);
 	if (!intel_plane)
 		return -ENOMEM;
 
-	if (IS_GEN6(dev)) {
+	switch (INTEL_INFO(dev)->gen) {
+	case 5:
+	case 6:
 		intel_plane->max_downscale = 16;
-		intel_plane->update_plane = snb_update_plane;
-		intel_plane->disable_plane = snb_disable_plane;
-		intel_plane->update_colorkey = snb_update_colorkey;
-		intel_plane->get_colorkey = snb_get_colorkey;
-	} else if (IS_GEN7(dev)) {
+		intel_plane->update_plane = ilk_update_plane;
+		intel_plane->disable_plane = ilk_disable_plane;
+		intel_plane->update_colorkey = ilk_update_colorkey;
+		intel_plane->get_colorkey = ilk_get_colorkey;
+
+		if (IS_GEN6(dev)) {
+			plane_formats = snb_plane_formats;
+			num_plane_formats = ARRAY_SIZE(snb_plane_formats);
+		} else {
+			plane_formats = ilk_plane_formats;
+			num_plane_formats = ARRAY_SIZE(ilk_plane_formats);
+		}
+		break;
+
+	case 7:
 		intel_plane->max_downscale = 2;
 		intel_plane->update_plane = ivb_update_plane;
 		intel_plane->disable_plane = ivb_disable_plane;
 		intel_plane->update_colorkey = ivb_update_colorkey;
 		intel_plane->get_colorkey = ivb_get_colorkey;
+
+		plane_formats = snb_plane_formats;
+		num_plane_formats = ARRAY_SIZE(snb_plane_formats);
+		break;
+
+	default:
+		return -ENODEV;
 	}
 
 	intel_plane->pipe = pipe;
 	possible_crtcs = (1 << pipe);
 	ret = drm_plane_init(dev, &intel_plane->base, possible_crtcs,
-			     &intel_plane_funcs, snb_plane_formats,
-			     ARRAY_SIZE(snb_plane_formats), false);
+			     &intel_plane_funcs,
+			     plane_formats, num_plane_formats,
+			     false);
 	if (ret)
 		kfree(intel_plane);
 
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index 05f765ef5464..3346612d2953 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -811,7 +811,7 @@ intel_tv_mode_lookup(const char *tv_format)
 {
 	int i;
 
-	for (i = 0; i < sizeof(tv_modes) / sizeof(tv_modes[0]); i++) {
+	for (i = 0; i < ARRAY_SIZE(tv_modes); i++) {
 		const struct tv_mode *tv_mode = &tv_modes[i];
 
 		if (!strcmp(tv_format, tv_mode->name))
@@ -1153,6 +1153,15 @@ intel_tv_detect_type(struct intel_tv *intel_tv,
 		   DAC_B_0_7_V |
 		   DAC_C_0_7_V);
 
+
+	/*
+	 * The TV sense state should be cleared to zero on cantiga platform. Otherwise
+	 * the TV is misdetected. This is hardware requirement.
+	 */
+	if (IS_GM45(dev))
+		tv_dac &= ~(TVDAC_STATE_CHG_EN | TVDAC_A_SENSE_CTL |
+			    TVDAC_B_SENSE_CTL | TVDAC_C_SENSE_CTL);
+
 	I915_WRITE(TV_CTL, tv_ctl);
 	I915_WRITE(TV_DAC, tv_dac);
 	POSTING_READ(TV_DAC);
@@ -1240,11 +1249,8 @@ intel_tv_detect(struct drm_connector *connector, bool force)
 	int type;
 
 	mode = reported_modes[0];
-	drm_mode_set_crtcinfo(&mode, 0);
 
-	if (intel_tv->base.base.crtc && intel_tv->base.base.crtc->enabled) {
-		type = intel_tv_detect_type(intel_tv, connector);
-	} else if (force) {
+	if (force) {
 		struct intel_load_detect_pipe tmp;
 
 		if (intel_get_load_detect_pipe(&intel_tv->base, connector,
diff --git a/drivers/gpu/drm/mgag200/Kconfig b/drivers/gpu/drm/mgag200/Kconfig
new file mode 100644
index 000000000000..d63013497f66
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/Kconfig
@@ -0,0 +1,15 @@
+config DRM_MGAG200
+	tristate "Kernel modesetting driver for MGA G200 server engines"
+	depends on DRM && PCI && EXPERIMENTAL
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select DRM_KMS_HELPER
+	select DRM_TTM
+	help
+	 This is a KMS driver for the MGA G200 server chips, it
+         does not support the original MGA G200 or any of the desktop
+         chips. It requires 0.3.0 of the modesetting userspace driver,
+         and a version of mga driver that will fail on KMS enabled
+         devices.
+
diff --git a/drivers/gpu/drm/mgag200/Makefile b/drivers/gpu/drm/mgag200/Makefile
new file mode 100644
index 000000000000..7db592eedbf1
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/Makefile
@@ -0,0 +1,5 @@
+ccflags-y := -Iinclude/drm
+mgag200-y   := mgag200_main.o mgag200_mode.o \
+	mgag200_drv.o mgag200_fb.o mgag200_i2c.o mgag200_ttm.o
+
+obj-$(CONFIG_DRM_MGAG200) += mgag200.o
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.c b/drivers/gpu/drm/mgag200/mgag200_drv.c
new file mode 100644
index 000000000000..3c8e04f54713
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include <linux/console.h>
+#include "drmP.h"
+#include "drm.h"
+
+#include "mgag200_drv.h"
+
+#include "drm_pciids.h"
+
+/*
+ * This is the generic driver code. This binds the driver to the drm core,
+ * which then performs further device association and calls our graphics init
+ * functions
+ */
+int mgag200_modeset = -1;
+
+MODULE_PARM_DESC(modeset, "Disable/Enable modesetting");
+module_param_named(modeset, mgag200_modeset, int, 0400);
+
+static struct drm_driver driver;
+
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	{ PCI_VENDOR_ID_MATROX, 0x522, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_SE_A },
+	{ PCI_VENDOR_ID_MATROX, 0x524, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_SE_B },
+	{ PCI_VENDOR_ID_MATROX, 0x530, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_EV },
+	{ PCI_VENDOR_ID_MATROX, 0x532, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_WB },
+	{ PCI_VENDOR_ID_MATROX, 0x533, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_EH },
+	{ PCI_VENDOR_ID_MATROX, 0x534, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_ER },
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, pciidlist);
+
+static int __devinit
+mga_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static void mga_pci_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	drm_put_dev(dev);
+}
+
+static const struct file_operations mgag200_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = mgag200_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+};
+
+static struct drm_driver driver = {
+	.driver_features = DRIVER_GEM | DRIVER_MODESET | DRIVER_USE_MTRR,
+	.load = mgag200_driver_load,
+	.unload = mgag200_driver_unload,
+	.fops = &mgag200_driver_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+
+	.gem_init_object = mgag200_gem_init_object,
+	.gem_free_object = mgag200_gem_free_object,
+	.dumb_create = mgag200_dumb_create,
+	.dumb_map_offset = mgag200_dumb_mmap_offset,
+	.dumb_destroy = mgag200_dumb_destroy,
+};
+
+static struct pci_driver mgag200_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = mga_pci_probe,
+	.remove = mga_pci_remove,
+};
+
+static int __init mgag200_init(void)
+{
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force() && mgag200_modeset == -1)
+		return -EINVAL;
+#endif
+
+	if (mgag200_modeset == 0)
+		return -EINVAL;
+	return drm_pci_init(&driver, &mgag200_pci_driver);
+}
+
+static void __exit mgag200_exit(void)
+{
+	drm_pci_exit(&driver, &mgag200_pci_driver);
+}
+
+module_init(mgag200_init);
+module_exit(mgag200_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.h b/drivers/gpu/drm/mgag200/mgag200_drv.h
new file mode 100644
index 000000000000..6f13b3563234
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.h
@@ -0,0 +1,276 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat 
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ * 	    Matt Turner
+ *	    Dave Airlie
+ */
+#ifndef __MGAG200_DRV_H__
+#define __MGAG200_DRV_H__
+
+#include <video/vga.h>
+
+#include "drm/drm_fb_helper.h"
+#include "ttm/ttm_bo_api.h"
+#include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_placement.h"
+#include "ttm/ttm_memory.h"
+#include "ttm/ttm_module.h"
+
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
+#include "mgag200_reg.h"
+
+#define DRIVER_AUTHOR		"Matthew Garrett"
+
+#define DRIVER_NAME		"mgag200"
+#define DRIVER_DESC		"MGA G200 SE"
+#define DRIVER_DATE		"20110418"
+
+#define DRIVER_MAJOR		1
+#define DRIVER_MINOR		0
+#define DRIVER_PATCHLEVEL	0
+
+#define MGAG200FB_CONN_LIMIT 1
+
+#define RREG8(reg) ioread8(((void __iomem *)mdev->rmmio) + (reg))
+#define WREG8(reg, v) iowrite8(v, ((void __iomem *)mdev->rmmio) + (reg))
+#define RREG32(reg) ioread32(((void __iomem *)mdev->rmmio) + (reg))
+#define WREG32(reg, v) iowrite32(v, ((void __iomem *)mdev->rmmio) + (reg))
+
+#define ATTR_INDEX 0x1fc0
+#define ATTR_DATA 0x1fc1
+
+#define WREG_ATTR(reg, v)					\
+	do {							\
+		RREG8(0x1fda);					\
+		WREG8(ATTR_INDEX, reg);				\
+		WREG8(ATTR_DATA, v);				\
+	} while (0)						\
+
+#define WREG_SEQ(reg, v)					\
+	do {							\
+		WREG8(MGAREG_SEQ_INDEX, reg);			\
+		WREG8(MGAREG_SEQ_DATA, v);			\
+	} while (0)						\
+
+#define WREG_CRT(reg, v)					\
+	do {							\
+		WREG8(MGAREG_CRTC_INDEX, reg);			\
+		WREG8(MGAREG_CRTC_DATA, v);			\
+	} while (0)						\
+
+
+#define WREG_ECRT(reg, v)					\
+	do {							\
+		WREG8(MGAREG_CRTCEXT_INDEX, reg);				\
+		WREG8(MGAREG_CRTCEXT_DATA, v);				\
+	} while (0)						\
+
+#define GFX_INDEX 0x1fce
+#define GFX_DATA 0x1fcf
+
+#define WREG_GFX(reg, v)					\
+	do {							\
+		WREG8(GFX_INDEX, reg);				\
+		WREG8(GFX_DATA, v);				\
+	} while (0)						\
+
+#define DAC_INDEX 0x3c00
+#define DAC_DATA 0x3c0a
+
+#define WREG_DAC(reg, v)					\
+	do {							\
+		WREG8(DAC_INDEX, reg);				\
+		WREG8(DAC_DATA, v);				\
+	} while (0)						\
+
+#define MGA_MISC_OUT 0x1fc2
+#define MGA_MISC_IN 0x1fcc
+
+#define MGAG200_MAX_FB_HEIGHT 4096
+#define MGAG200_MAX_FB_WIDTH 4096
+
+#define MATROX_DPMS_CLEARED (-1)
+
+#define to_mga_crtc(x) container_of(x, struct mga_crtc, base)
+#define to_mga_encoder(x) container_of(x, struct mga_encoder, base)
+#define to_mga_connector(x) container_of(x, struct mga_connector, base)
+#define to_mga_framebuffer(x) container_of(x, struct mga_framebuffer, base)
+
+struct mga_framebuffer {
+	struct drm_framebuffer base;
+	struct drm_gem_object *obj;
+};
+
+struct mga_fbdev {
+	struct drm_fb_helper helper;
+	struct mga_framebuffer mfb;
+	struct list_head fbdev_list;
+	void *sysram;
+	int size;
+	struct ttm_bo_kmap_obj mapping;
+};
+
+struct mga_crtc {
+	struct drm_crtc base;
+	u8 lut_r[256], lut_g[256], lut_b[256];
+	int last_dpms;
+	bool enabled;
+};
+
+struct mga_mode_info {
+	bool mode_config_initialized;
+	struct mga_crtc *crtc;
+};
+
+struct mga_encoder {
+	struct drm_encoder base;
+	int last_dpms;
+};
+
+
+struct mga_i2c_chan {
+	struct i2c_adapter adapter;
+	struct drm_device *dev;
+	struct i2c_algo_bit_data bit;
+	int data, clock;
+};
+
+struct mga_connector {
+	struct drm_connector base;
+	struct mga_i2c_chan *i2c;
+};
+
+
+struct mga_mc {
+	resource_size_t			vram_size;
+	resource_size_t			vram_base;
+	resource_size_t			vram_window;
+};
+
+enum mga_type {
+	G200_SE_A,
+	G200_SE_B,
+	G200_WB,
+	G200_EV,
+	G200_EH,
+	G200_ER,
+};
+
+#define IS_G200_SE(mdev) (mdev->type == G200_SE_A || mdev->type == G200_SE_B)
+
+struct mga_device {
+	struct drm_device		*dev;
+	unsigned long			flags;
+
+	resource_size_t			rmmio_base;
+	resource_size_t			rmmio_size;
+	void __iomem			*rmmio;
+
+	drm_local_map_t			*framebuffer;
+
+	struct mga_mc			mc;
+	struct mga_mode_info		mode_info;
+
+	struct mga_fbdev *mfbdev;
+
+	bool				suspended;
+	int				num_crtc;
+	enum mga_type			type;
+	int				has_sdram;
+	struct drm_display_mode		mode;
+
+	int bpp_shifts[4];
+
+	int fb_mtrr;
+
+	struct {
+		struct drm_global_reference mem_global_ref;
+		struct ttm_bo_global_ref bo_global_ref;
+		struct ttm_bo_device bdev;
+		atomic_t validate_sequence;
+	} ttm;
+
+	u32 reg_1e24; /* SE model number */
+};
+
+
+struct mgag200_bo {
+	struct ttm_buffer_object bo;
+	struct ttm_placement placement;
+	struct ttm_bo_kmap_obj kmap;
+	struct drm_gem_object gem;
+	u32 placements[3];
+	int pin_count;
+};
+#define gem_to_mga_bo(gobj) container_of((gobj), struct mgag200_bo, gem)
+
+static inline struct mgag200_bo *
+mgag200_bo(struct ttm_buffer_object *bo)
+{
+	return container_of(bo, struct mgag200_bo, bo);
+}
+				/* mga_crtc.c */
+void mga_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			     u16 blue, int regno);
+void mga_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			     u16 *blue, int regno);
+
+				/* mgag200_mode.c */
+int mgag200_modeset_init(struct mga_device *mdev);
+void mgag200_modeset_fini(struct mga_device *mdev);
+
+				/* mga_fbdev.c */
+int mgag200_fbdev_init(struct mga_device *mdev);
+void mgag200_fbdev_fini(struct mga_device *mdev);
+
+				/* mgag200_main.c */
+int mgag200_framebuffer_init(struct drm_device *dev,
+			     struct mga_framebuffer *mfb,
+			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     struct drm_gem_object *obj);
+
+
+int mgag200_driver_load(struct drm_device *dev, unsigned long flags);
+int mgag200_driver_unload(struct drm_device *dev);
+int mgag200_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		       struct drm_gem_object **obj);
+int mgag200_gem_init_object(struct drm_gem_object *obj);
+int mgag200_dumb_create(struct drm_file *file,
+			struct drm_device *dev,
+			struct drm_mode_create_dumb *args);
+int mgag200_dumb_destroy(struct drm_file *file,
+			 struct drm_device *dev,
+			 uint32_t handle);
+void mgag200_gem_free_object(struct drm_gem_object *obj);
+int
+mgag200_dumb_mmap_offset(struct drm_file *file,
+			 struct drm_device *dev,
+			 uint32_t handle,
+			 uint64_t *offset);
+				/* mga_i2c.c */
+struct mga_i2c_chan *mgag200_i2c_create(struct drm_device *dev);
+void mgag200_i2c_destroy(struct mga_i2c_chan *i2c);
+
+#define DRM_FILE_PAGE_OFFSET (0x100000000ULL >> PAGE_SHIFT)
+void mgag200_ttm_placement(struct mgag200_bo *bo, int domain);
+
+int mgag200_bo_reserve(struct mgag200_bo *bo, bool no_wait);
+void mgag200_bo_unreserve(struct mgag200_bo *bo);
+int mgag200_bo_create(struct drm_device *dev, int size, int align,
+		      uint32_t flags, struct mgag200_bo **pastbo);
+int mgag200_mm_init(struct mga_device *mdev);
+void mgag200_mm_fini(struct mga_device *mdev);
+int mgag200_mmap(struct file *filp, struct vm_area_struct *vma);
+int mgag200_bo_pin(struct mgag200_bo *bo, u32 pl_flag, u64 *gpu_addr);
+int mgag200_bo_unpin(struct mgag200_bo *bo);
+int mgag200_bo_push_sysram(struct mgag200_bo *bo);
+#endif				/* __MGAG200_DRV_H__ */
diff --git a/drivers/gpu/drm/mgag200/mgag200_fb.c b/drivers/gpu/drm/mgag200/mgag200_fb.c
new file mode 100644
index 000000000000..880d3369760e
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_fb.c
@@ -0,0 +1,294 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Matt Turner
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include "drmP.h"
+#include "drm.h"
+#include "drm_fb_helper.h"
+
+#include <linux/fb.h>
+
+#include "mgag200_drv.h"
+
+static void mga_dirty_update(struct mga_fbdev *mfbdev,
+			     int x, int y, int width, int height)
+{
+	int i;
+	struct drm_gem_object *obj;
+	struct mgag200_bo *bo;
+	int src_offset, dst_offset;
+	int bpp = (mfbdev->mfb.base.bits_per_pixel + 7)/8;
+	int ret;
+	bool unmap = false;
+
+	obj = mfbdev->mfb.obj;
+	bo = gem_to_mga_bo(obj);
+
+	ret = mgag200_bo_reserve(bo, true);
+	if (ret) {
+		DRM_ERROR("failed to reserve fb bo\n");
+		return;
+	}
+
+	if (!bo->kmap.virtual) {
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret) {
+			DRM_ERROR("failed to kmap fb updates\n");
+			mgag200_bo_unreserve(bo);
+			return;
+		}
+		unmap = true;
+	}
+	for (i = y; i < y + height; i++) {
+		/* assume equal stride for now */
+		src_offset = dst_offset = i * mfbdev->mfb.base.pitches[0] + (x * bpp);
+		memcpy_toio(bo->kmap.virtual + src_offset, mfbdev->sysram + src_offset, width * bpp);
+
+	}
+	if (unmap)
+		ttm_bo_kunmap(&bo->kmap);
+
+	mgag200_bo_unreserve(bo);
+}
+
+static void mga_fillrect(struct fb_info *info,
+			 const struct fb_fillrect *rect)
+{
+	struct mga_fbdev *mfbdev = info->par;
+	sys_fillrect(info, rect);
+	mga_dirty_update(mfbdev, rect->dx, rect->dy, rect->width,
+			 rect->height);
+}
+
+static void mga_copyarea(struct fb_info *info,
+			 const struct fb_copyarea *area)
+{
+	struct mga_fbdev *mfbdev = info->par;
+	sys_copyarea(info, area);
+	mga_dirty_update(mfbdev, area->dx, area->dy, area->width,
+			 area->height);
+}
+
+static void mga_imageblit(struct fb_info *info,
+			  const struct fb_image *image)
+{
+	struct mga_fbdev *mfbdev = info->par;
+	sys_imageblit(info, image);
+	mga_dirty_update(mfbdev, image->dx, image->dy, image->width,
+			 image->height);
+}
+
+
+static struct fb_ops mgag200fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_fillrect = mga_fillrect,
+	.fb_copyarea = mga_copyarea,
+	.fb_imageblit = mga_imageblit,
+	.fb_pan_display = drm_fb_helper_pan_display,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcmap = drm_fb_helper_setcmap,
+};
+
+static int mgag200fb_create_object(struct mga_fbdev *afbdev,
+				   struct drm_mode_fb_cmd2 *mode_cmd,
+				   struct drm_gem_object **gobj_p)
+{
+	struct drm_device *dev = afbdev->helper.dev;
+	u32 bpp, depth;
+	u32 size;
+	struct drm_gem_object *gobj;
+
+	int ret = 0;
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
+
+	size = mode_cmd->pitches[0] * mode_cmd->height;
+	ret = mgag200_gem_create(dev, size, true, &gobj);
+	if (ret)
+		return ret;
+
+	*gobj_p = gobj;
+	return ret;
+}
+
+static int mgag200fb_create(struct mga_fbdev *mfbdev,
+			   struct drm_fb_helper_surface_size *sizes)
+{
+	struct drm_device *dev = mfbdev->helper.dev;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct mga_device *mdev = dev->dev_private;
+	struct fb_info *info;
+	struct drm_framebuffer *fb;
+	struct drm_gem_object *gobj = NULL;
+	struct device *device = &dev->pdev->dev;
+	struct mgag200_bo *bo;
+	int ret;
+	void *sysram;
+	int size;
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	mode_cmd.pitches[0] = mode_cmd.width * ((sizes->surface_bpp + 7) / 8);
+
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	ret = mgag200fb_create_object(mfbdev, &mode_cmd, &gobj);
+	if (ret) {
+		DRM_ERROR("failed to create fbcon backing object %d\n", ret);
+		return ret;
+	}
+	bo = gem_to_mga_bo(gobj);
+
+	sysram = vmalloc(size);
+	if (!sysram)
+		return -ENOMEM;
+
+	info = framebuffer_alloc(0, device);
+	if (info == NULL)
+		return -ENOMEM;
+
+	info->par = mfbdev;
+
+	ret = mgag200_framebuffer_init(dev, &mfbdev->mfb, &mode_cmd, gobj);
+	if (ret)
+		return ret;
+
+	mfbdev->sysram = sysram;
+	mfbdev->size = size;
+
+	fb = &mfbdev->mfb.base;
+
+	/* setup helper */
+	mfbdev->helper.fb = fb;
+	mfbdev->helper.fbdev = info;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		DRM_ERROR("%s: can't allocate color map\n", info->fix.id);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	strcpy(info->fix.id, "mgadrmfb");
+
+	info->flags = FBINFO_DEFAULT | FBINFO_CAN_FORCE_OUTPUT;
+	info->fbops = &mgag200fb_ops;
+
+	/* setup aperture base/size for vesafb takeover */
+	info->apertures = alloc_apertures(1);
+	if (!info->apertures) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	info->apertures->ranges[0].base = mdev->dev->mode_config.fb_base;
+	info->apertures->ranges[0].size = mdev->mc.vram_size;
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, &mfbdev->helper, sizes->fb_width,
+			       sizes->fb_height);
+
+	info->screen_base = sysram;
+	info->screen_size = size;
+	info->pixmap.flags = FB_PIXMAP_SYSTEM;
+
+	DRM_DEBUG_KMS("allocated %dx%d\n",
+		      fb->width, fb->height);
+	return 0;
+out:
+	return ret;
+}
+
+static int mga_fb_find_or_create_single(struct drm_fb_helper *helper,
+					   struct drm_fb_helper_surface_size
+					   *sizes)
+{
+	struct mga_fbdev *mfbdev = (struct mga_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = mgag200fb_create(mfbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static int mga_fbdev_destroy(struct drm_device *dev,
+				struct mga_fbdev *mfbdev)
+{
+	struct fb_info *info;
+	struct mga_framebuffer *mfb = &mfbdev->mfb;
+
+	if (mfbdev->helper.fbdev) {
+		info = mfbdev->helper.fbdev;
+
+		unregister_framebuffer(info);
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+
+	if (mfb->obj) {
+		drm_gem_object_unreference_unlocked(mfb->obj);
+		mfb->obj = NULL;
+	}
+	drm_fb_helper_fini(&mfbdev->helper);
+	vfree(mfbdev->sysram);
+	drm_framebuffer_cleanup(&mfb->base);
+
+	return 0;
+}
+
+static struct drm_fb_helper_funcs mga_fb_helper_funcs = {
+	.gamma_set = mga_crtc_fb_gamma_set,
+	.gamma_get = mga_crtc_fb_gamma_get,
+	.fb_probe = mga_fb_find_or_create_single,
+};
+
+int mgag200_fbdev_init(struct mga_device *mdev)
+{
+	struct mga_fbdev *mfbdev;
+	int ret;
+
+	mfbdev = kzalloc(sizeof(struct mga_fbdev), GFP_KERNEL);
+	if (!mfbdev)
+		return -ENOMEM;
+
+	mdev->mfbdev = mfbdev;
+	mfbdev->helper.funcs = &mga_fb_helper_funcs;
+
+	ret = drm_fb_helper_init(mdev->dev, &mfbdev->helper,
+				 mdev->num_crtc, MGAG200FB_CONN_LIMIT);
+	if (ret) {
+		kfree(mfbdev);
+		return ret;
+	}
+	drm_fb_helper_single_add_all_connectors(&mfbdev->helper);
+	drm_fb_helper_initial_config(&mfbdev->helper, 32);
+
+	return 0;
+}
+
+void mgag200_fbdev_fini(struct mga_device *mdev)
+{
+	if (!mdev->mfbdev)
+		return;
+
+	mga_fbdev_destroy(mdev->dev, mdev->mfbdev);
+	kfree(mdev->mfbdev);
+	mdev->mfbdev = NULL;
+}
diff --git a/drivers/gpu/drm/mgag200/mgag200_i2c.c b/drivers/gpu/drm/mgag200/mgag200_i2c.c
new file mode 100644
index 000000000000..dd3568a1b6b0
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_i2c.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/export.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#include "drmP.h"
+#include "drm.h"
+
+#include "mgag200_drv.h"
+
+static int mga_i2c_read_gpio(struct mga_device *mdev)
+{
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
+	return RREG8(DAC_DATA);
+}
+
+static void mga_i2c_set_gpio(struct mga_device *mdev, int mask, int val)
+{
+	int tmp;
+
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_CTL);
+	tmp = (RREG8(DAC_DATA) & mask) | val;
+	WREG_DAC(MGA1064_GEN_IO_CTL, tmp);
+	WREG_DAC(MGA1064_GEN_IO_DATA, 0);
+}
+
+static inline void mga_i2c_set(struct mga_device *mdev, int mask, int state)
+{
+	if (state)
+		state = 0;
+	else
+		state = mask;
+	mga_i2c_set_gpio(mdev, ~mask, state);
+}
+
+static void mga_gpio_setsda(void *data, int state)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	mga_i2c_set(mdev, i2c->data, state);
+}
+
+static void mga_gpio_setscl(void *data, int state)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	mga_i2c_set(mdev, i2c->clock, state);
+}
+
+static int mga_gpio_getsda(void *data)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	return (mga_i2c_read_gpio(mdev) & i2c->data) ? 1 : 0;
+}
+
+static int mga_gpio_getscl(void *data)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	return (mga_i2c_read_gpio(mdev) & i2c->clock) ? 1 : 0;
+}
+
+struct mga_i2c_chan *mgag200_i2c_create(struct drm_device *dev)
+{
+	struct mga_device *mdev = dev->dev_private;
+	struct mga_i2c_chan *i2c;
+	int ret;
+	int data, clock;
+
+	WREG_DAC(MGA1064_GEN_IO_DATA, 0xff);
+	WREG_DAC(MGA1064_GEN_IO_CTL, 0);
+
+	switch (mdev->type) {
+	case G200_SE_A:
+	case G200_SE_B:
+	case G200_EV:
+	case G200_WB:
+		data = 1;
+		clock = 2;
+		break;
+	case G200_EH:
+	case G200_ER:
+		data = 2;
+		clock = 1;
+		break;
+	default:
+		data = 2;
+		clock = 8;
+		break;
+	}
+
+	i2c = kzalloc(sizeof(struct mga_i2c_chan), GFP_KERNEL);
+	if (!i2c)
+		return NULL;
+
+	i2c->data = data;
+	i2c->clock = clock;
+	i2c->adapter.owner = THIS_MODULE;
+	i2c->adapter.class = I2C_CLASS_DDC;
+	i2c->adapter.dev.parent = &dev->pdev->dev;
+	i2c->dev = dev;
+	i2c_set_adapdata(&i2c->adapter, i2c);
+	snprintf(i2c->adapter.name, sizeof(i2c->adapter.name), "mga i2c");
+
+	i2c->adapter.algo_data = &i2c->bit;
+
+	i2c->bit.udelay = 10;
+	i2c->bit.timeout = 2;
+	i2c->bit.data = i2c;
+	i2c->bit.setsda		= mga_gpio_setsda;
+	i2c->bit.setscl		= mga_gpio_setscl;
+	i2c->bit.getsda		= mga_gpio_getsda;
+	i2c->bit.getscl		= mga_gpio_getscl;
+
+	ret = i2c_bit_add_bus(&i2c->adapter);
+	if (ret) {
+		kfree(i2c);
+		i2c = NULL;
+	}
+	return i2c;
+}
+
+void mgag200_i2c_destroy(struct mga_i2c_chan *i2c)
+{
+	if (!i2c)
+		return;
+	i2c_del_adapter(&i2c->adapter);
+	kfree(i2c);
+}
+
diff --git a/drivers/gpu/drm/mgag200/mgag200_main.c b/drivers/gpu/drm/mgag200/mgag200_main.c
new file mode 100644
index 000000000000..636a81cd2f37
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_main.c
@@ -0,0 +1,388 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Matt Turner
+ *          Dave Airlie
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+#include "mgag200_drv.h"
+
+static void mga_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct mga_framebuffer *mga_fb = to_mga_framebuffer(fb);
+	if (mga_fb->obj)
+		drm_gem_object_unreference_unlocked(mga_fb->obj);
+	drm_framebuffer_cleanup(fb);
+	kfree(fb);
+}
+
+static int mga_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+						 struct drm_file *file_priv,
+						 unsigned int *handle)
+{
+	return 0;
+}
+
+static const struct drm_framebuffer_funcs mga_fb_funcs = {
+	.destroy = mga_user_framebuffer_destroy,
+	.create_handle = mga_user_framebuffer_create_handle,
+};
+
+int mgag200_framebuffer_init(struct drm_device *dev,
+			     struct mga_framebuffer *gfb,
+			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     struct drm_gem_object *obj)
+{
+	int ret = drm_framebuffer_init(dev, &gfb->base, &mga_fb_funcs);
+	if (ret) {
+		DRM_ERROR("drm_framebuffer_init failed: %d\n", ret);
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&gfb->base, mode_cmd);
+	gfb->obj = obj;
+	return 0;
+}
+
+static struct drm_framebuffer *
+mgag200_user_framebuffer_create(struct drm_device *dev,
+				struct drm_file *filp,
+				struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct mga_framebuffer *mga_fb;
+	int ret;
+
+	obj = drm_gem_object_lookup(dev, filp, mode_cmd->handles[0]);
+	if (obj == NULL)
+		return ERR_PTR(-ENOENT);
+
+	mga_fb = kzalloc(sizeof(*mga_fb), GFP_KERNEL);
+	if (!mga_fb) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	ret = mgag200_framebuffer_init(dev, mga_fb, mode_cmd, obj);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(obj);
+		kfree(mga_fb);
+		return ERR_PTR(ret);
+	}
+	return &mga_fb->base;
+}
+
+static const struct drm_mode_config_funcs mga_mode_funcs = {
+	.fb_create = mgag200_user_framebuffer_create,
+};
+
+/* Unmap the framebuffer from the core and release the memory */
+static void mga_vram_fini(struct mga_device *mdev)
+{
+	pci_iounmap(mdev->dev->pdev, mdev->rmmio);
+	mdev->rmmio = NULL;
+	if (mdev->mc.vram_base)
+		release_mem_region(mdev->mc.vram_base, mdev->mc.vram_window);
+}
+
+static int mga_probe_vram(struct mga_device *mdev, void __iomem *mem)
+{
+	int offset;
+	int orig;
+	int test1, test2;
+	int orig1, orig2;
+
+	/* Probe */
+	orig = ioread16(mem);
+	iowrite16(0, mem);
+
+	for (offset = 0x100000; offset < mdev->mc.vram_window; offset += 0x4000) {
+		orig1 = ioread8(mem + offset);
+		orig2 = ioread8(mem + offset + 0x100);
+
+		iowrite16(0xaa55, mem + offset);
+		iowrite16(0xaa55, mem + offset + 0x100);
+
+		test1 = ioread16(mem + offset);
+		test2 = ioread16(mem);
+
+		iowrite16(orig1, mem + offset);
+		iowrite16(orig2, mem + offset + 0x100);
+
+		if (test1 != 0xaa55) {
+			break;
+		}
+
+		if (test2) {
+			break;
+		}
+	}
+
+	iowrite16(orig, mem);
+	return offset - 65536;
+}
+
+/* Map the framebuffer from the card and configure the core */
+static int mga_vram_init(struct mga_device *mdev)
+{
+	void __iomem *mem;
+	struct apertures_struct *aper = alloc_apertures(1);
+
+	/* BAR 0 is VRAM */
+	mdev->mc.vram_base = pci_resource_start(mdev->dev->pdev, 0);
+	mdev->mc.vram_window = pci_resource_len(mdev->dev->pdev, 0);
+
+	aper->ranges[0].base = mdev->mc.vram_base;
+	aper->ranges[0].size = mdev->mc.vram_window;
+	aper->count = 1;
+
+	remove_conflicting_framebuffers(aper, "mgafb", true);
+
+	if (!request_mem_region(mdev->mc.vram_base, mdev->mc.vram_window,
+				"mgadrmfb_vram")) {
+		DRM_ERROR("can't reserve VRAM\n");
+		return -ENXIO;
+	}
+
+	mem = pci_iomap(mdev->dev->pdev, 0, 0);
+
+	mdev->mc.vram_size = mga_probe_vram(mdev, mem);
+
+	pci_iounmap(mdev->dev->pdev, mem);
+
+	return 0;
+}
+
+static int mgag200_device_init(struct drm_device *dev,
+			       uint32_t flags)
+{
+	struct mga_device *mdev = dev->dev_private;
+	int ret, option;
+
+	mdev->type = flags;
+
+	/* Hardcode the number of CRTCs to 1 */
+	mdev->num_crtc = 1;
+
+	pci_read_config_dword(dev->pdev, PCI_MGA_OPTION, &option);
+	mdev->has_sdram = !(option & (1 << 14));
+
+	/* BAR 0 is the framebuffer, BAR 1 contains registers */
+	mdev->rmmio_base = pci_resource_start(mdev->dev->pdev, 1);
+	mdev->rmmio_size = pci_resource_len(mdev->dev->pdev, 1);
+
+	if (!request_mem_region(mdev->rmmio_base, mdev->rmmio_size,
+				"mgadrmfb_mmio")) {
+		DRM_ERROR("can't reserve mmio registers\n");
+		return -ENOMEM;
+	}
+
+	mdev->rmmio = pci_iomap(dev->pdev, 1, 0);
+	if (mdev->rmmio == NULL)
+		return -ENOMEM;
+
+	/* stash G200 SE model number for later use */
+	if (IS_G200_SE(mdev))
+		mdev->reg_1e24 = RREG32(0x1e24);
+
+	ret = mga_vram_init(mdev);
+	if (ret) {
+		release_mem_region(mdev->rmmio_base, mdev->rmmio_size);
+		return ret;
+	}
+
+	mdev->bpp_shifts[0] = 0;
+	mdev->bpp_shifts[1] = 1;
+	mdev->bpp_shifts[2] = 0;
+	mdev->bpp_shifts[3] = 2;
+	return 0;
+}
+
+void mgag200_device_fini(struct mga_device *mdev)
+{
+	release_mem_region(mdev->rmmio_base, mdev->rmmio_size);
+	mga_vram_fini(mdev);
+}
+
+/*
+ * Functions here will be called by the core once it's bound the driver to
+ * a PCI device
+ */
+
+
+int mgag200_driver_load(struct drm_device *dev, unsigned long flags)
+{
+	struct mga_device *mdev;
+	int r;
+
+	mdev = kzalloc(sizeof(struct mga_device), GFP_KERNEL);
+	if (mdev == NULL)
+		return -ENOMEM;
+	dev->dev_private = (void *)mdev;
+	mdev->dev = dev;
+
+	r = mgag200_device_init(dev, flags);
+	if (r) {
+		dev_err(&dev->pdev->dev, "Fatal error during GPU init: %d\n", r);
+		goto out;
+	}
+	r = mgag200_mm_init(mdev);
+	if (r)
+		goto out;
+
+	drm_mode_config_init(dev);
+	dev->mode_config.funcs = (void *)&mga_mode_funcs;
+	dev->mode_config.min_width = 0;
+	dev->mode_config.min_height = 0;
+	dev->mode_config.preferred_depth = 24;
+	dev->mode_config.prefer_shadow = 1;
+
+	r = mgag200_modeset_init(mdev);
+	if (r)
+		dev_err(&dev->pdev->dev, "Fatal error during modeset init: %d\n", r);
+out:
+	if (r)
+		mgag200_driver_unload(dev);
+	return r;
+}
+
+int mgag200_driver_unload(struct drm_device *dev)
+{
+	struct mga_device *mdev = dev->dev_private;
+
+	if (mdev == NULL)
+		return 0;
+	mgag200_modeset_fini(mdev);
+	mgag200_fbdev_fini(mdev);
+	drm_mode_config_cleanup(dev);
+	mgag200_mm_fini(mdev);
+	mgag200_device_fini(mdev);
+	kfree(mdev);
+	dev->dev_private = NULL;
+	return 0;
+}
+
+int mgag200_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj)
+{
+	struct mgag200_bo *astbo;
+	int ret;
+
+	*obj = NULL;
+
+	size = roundup(size, PAGE_SIZE);
+	if (size == 0)
+		return -EINVAL;
+
+	ret = mgag200_bo_create(dev, size, 0, 0, &astbo);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("failed to allocate GEM object\n");
+		return ret;
+	}
+	*obj = &astbo->gem;
+	return 0;
+}
+
+int mgag200_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		    struct drm_mode_create_dumb *args)
+{
+	int ret;
+	struct drm_gem_object *gobj;
+	u32 handle;
+
+	args->pitch = args->width * ((args->bpp + 7) / 8);
+	args->size = args->pitch * args->height;
+
+	ret = mgag200_gem_create(dev, args->size, false,
+			     &gobj);
+	if (ret)
+		return ret;
+
+	ret = drm_gem_handle_create(file, gobj, &handle);
+	drm_gem_object_unreference_unlocked(gobj);
+	if (ret)
+		return ret;
+
+	args->handle = handle;
+	return 0;
+}
+
+int mgag200_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
+
+int mgag200_gem_init_object(struct drm_gem_object *obj)
+{
+	BUG();
+	return 0;
+}
+
+void mgag200_bo_unref(struct mgag200_bo **bo)
+{
+	struct ttm_buffer_object *tbo;
+
+	if ((*bo) == NULL)
+		return;
+
+	tbo = &((*bo)->bo);
+	ttm_bo_unref(&tbo);
+	if (tbo == NULL)
+		*bo = NULL;
+
+}
+
+void mgag200_gem_free_object(struct drm_gem_object *obj)
+{
+	struct mgag200_bo *mgag200_bo = gem_to_mga_bo(obj);
+
+	if (!mgag200_bo)
+		return;
+	mgag200_bo_unref(&mgag200_bo);
+}
+
+
+static inline u64 mgag200_bo_mmap_offset(struct mgag200_bo *bo)
+{
+	return bo->bo.addr_space_offset;
+}
+
+int
+mgag200_dumb_mmap_offset(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle,
+		     uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret;
+	struct mgag200_bo *bo;
+
+	mutex_lock(&dev->struct_mutex);
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto out_unlock;
+	}
+
+	bo = gem_to_mga_bo(obj);
+	*offset = mgag200_bo_mmap_offset(bo);
+
+	drm_gem_object_unreference(obj);
+	ret = 0;
+out_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+
+}
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
new file mode 100644
index 000000000000..d303061b251e
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -0,0 +1,1533 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *	    Matt Turner
+ *	    Dave Airlie
+ */
+
+#include <linux/delay.h>
+
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include "mgag200_drv.h"
+
+#define MGAG200_LUT_SIZE 256
+
+/*
+ * This file contains setup code for the CRTC.
+ */
+
+static void mga_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	int i;
+
+	if (!crtc->enabled)
+		return;
+
+	WREG8(DAC_INDEX + MGA1064_INDEX, 0);
+
+	for (i = 0; i < MGAG200_LUT_SIZE; i++) {
+		/* VGA registers */
+		WREG8(DAC_INDEX + MGA1064_COL_PAL, mga_crtc->lut_r[i]);
+		WREG8(DAC_INDEX + MGA1064_COL_PAL, mga_crtc->lut_g[i]);
+		WREG8(DAC_INDEX + MGA1064_COL_PAL, mga_crtc->lut_b[i]);
+	}
+}
+
+static inline void mga_wait_vsync(struct mga_device *mdev)
+{
+	unsigned int count = 0;
+	unsigned int status = 0;
+
+	do {
+		status = RREG32(MGAREG_Status);
+		count++;
+	} while ((status & 0x08) && (count < 250000));
+	count = 0;
+	status = 0;
+	do {
+		status = RREG32(MGAREG_Status);
+		count++;
+	} while (!(status & 0x08) && (count < 250000));
+}
+
+static inline void mga_wait_busy(struct mga_device *mdev)
+{
+	unsigned int count = 0;
+	unsigned int status = 0;
+	do {
+		status = RREG8(MGAREG_Status + 2);
+		count++;
+	} while ((status & 0x01) && (count < 500000));
+}
+
+/*
+ * The core passes the desired mode to the CRTC code to see whether any
+ * CRTC-specific modifications need to be made to it. We're in a position
+ * to just pass that straight through, so this does nothing
+ */
+static bool mga_crtc_mode_fixup(struct drm_crtc *crtc,
+				   struct drm_display_mode *mode,
+				   struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static int mga_g200se_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+
+	m = n = p = 0;
+	vcomax = 320000;
+	vcomin = 160000;
+	pllreffreq = 25000;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 8; testp > 0; testp /= 2) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testn = 17; testn < 256; testn++) {
+			for (testm = 1; testm < 32; testm++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					m = testm - 1;
+					n = testn - 1;
+					p = testp - 1;
+				}
+			}
+		}
+	}
+
+	if (delta > permitteddelta) {
+		printk(KERN_WARNING "PLL delta too large\n");
+		return 1;
+	}
+
+	WREG_DAC(MGA1064_PIX_PLLC_M, m);
+	WREG_DAC(MGA1064_PIX_PLLC_N, n);
+	WREG_DAC(MGA1064_PIX_PLLC_P, p);
+	return 0;
+}
+
+static int mga_g200wb_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+	int i, j, tmpcount, vcount;
+	bool pll_locked = false;
+	u8 tmp;
+
+	m = n = p = 0;
+	vcomax = 550000;
+	vcomin = 150000;
+	pllreffreq = 48000;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 1; testp < 9; testp++) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testm = 1; testm < 17; testm++) {
+			for (testn = 1; testn < 151; testn++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					n = testn - 1;
+					m = (testm - 1) | ((n >> 1) & 0x80);
+					p = testp - 1;
+				}
+			}
+		}
+	}
+
+	for (i = 0; i <= 32 && pll_locked == false; i++) {
+		if (i > 0) {
+			WREG8(MGAREG_CRTC_INDEX, 0x1e);
+			tmp = RREG8(MGAREG_CRTC_DATA);
+			if (tmp < 0xff)
+				WREG8(MGAREG_CRTC_DATA, tmp+1);
+		}
+
+		/* set pixclkdis to 1 */
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+		WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_REMHEADCTL_CLKDIS;
+		WREG_DAC(MGA1064_REMHEADCTL, tmp);
+
+		/* select PLL Set C */
+		tmp = RREG8(MGAREG_MEM_MISC_READ);
+		tmp |= 0x3 << 2;
+		WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN | 0x80;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		udelay(500);
+
+		/* reset the PLL */
+		WREG8(DAC_INDEX, MGA1064_VREF_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~0x04;
+		WREG_DAC(MGA1064_VREF_CTL, tmp);
+
+		udelay(50);
+
+		/* program pixel pll register */
+		WREG_DAC(MGA1064_WB_PIX_PLLC_N, n);
+		WREG_DAC(MGA1064_WB_PIX_PLLC_M, m);
+		WREG_DAC(MGA1064_WB_PIX_PLLC_P, p);
+
+		udelay(50);
+
+		/* turn pll on */
+		WREG8(DAC_INDEX, MGA1064_VREF_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= 0x04;
+		WREG_DAC(MGA1064_VREF_CTL, tmp);
+
+		udelay(500);
+
+		/* select the pixel pll */
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
+		tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_REMHEADCTL_CLKSL_MSK;
+		tmp |= MGA1064_REMHEADCTL_CLKSL_PLL;
+		WREG_DAC(MGA1064_REMHEADCTL, tmp);
+
+		/* reset dotclock rate bit */
+		WREG8(MGAREG_SEQ_INDEX, 1);
+		tmp = RREG8(MGAREG_SEQ_DATA);
+		tmp &= ~0x8;
+		WREG8(MGAREG_SEQ_DATA, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		vcount = RREG8(MGAREG_VCOUNT);
+
+		for (j = 0; j < 30 && pll_locked == false; j++) {
+			tmpcount = RREG8(MGAREG_VCOUNT);
+			if (tmpcount < vcount)
+				vcount = 0;
+			if ((tmpcount - vcount) > 2)
+				pll_locked = true;
+			else
+				udelay(5);
+		}
+	}
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_REMHEADCTL_CLKDIS;
+	WREG_DAC(MGA1064_REMHEADCTL, tmp);
+	return 0;
+}
+
+static int mga_g200ev_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+	u8 tmp;
+
+	m = n = p = 0;
+	vcomax = 550000;
+	vcomin = 150000;
+	pllreffreq = 50000;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 16; testp > 0; testp--) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testn = 1; testn < 257; testn++) {
+			for (testm = 1; testm < 17; testm++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					n = testn - 1;
+					m = testm - 1;
+					p = testp - 1;
+				}
+			}
+		}
+	}
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+	WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+	tmp = RREG8(MGAREG_MEM_MISC_READ);
+	tmp |= 0x3 << 2;
+	WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_PLL_STAT);
+	tmp = RREG8(DAC_DATA);
+	WREG_DAC(MGA1064_PIX_PLL_STAT, tmp & ~0x40);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	WREG_DAC(MGA1064_EV_PIX_PLLC_M, m);
+	WREG_DAC(MGA1064_EV_PIX_PLLC_N, n);
+	WREG_DAC(MGA1064_EV_PIX_PLLC_P, p);
+
+	udelay(50);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	udelay(500);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
+	tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_PLL_STAT);
+	tmp = RREG8(DAC_DATA);
+	WREG_DAC(MGA1064_PIX_PLL_STAT, tmp | 0x40);
+
+	tmp = RREG8(MGAREG_MEM_MISC_READ);
+	tmp |= (0x3 << 2);
+	WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	return 0;
+}
+
+static int mga_g200eh_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+	int i, j, tmpcount, vcount;
+	u8 tmp;
+	bool pll_locked = false;
+
+	m = n = p = 0;
+	vcomax = 800000;
+	vcomin = 400000;
+	pllreffreq = 3333;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 16; testp > 0; testp--) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testm = 1; testm < 33; testm++) {
+			for (testn = 1; testn < 257; testn++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					n = testn - 1;
+					m = (testm - 1) | ((n >> 1) & 0x80);
+					p = testp - 1;
+				}
+				if ((clock * testp) >= 600000)
+					p |= 80;
+			}
+		}
+	}
+	for (i = 0; i <= 32 && pll_locked == false; i++) {
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+		WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+		tmp = RREG8(MGAREG_MEM_MISC_READ);
+		tmp |= 0x3 << 2;
+		WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		udelay(500);
+
+		WREG_DAC(MGA1064_EH_PIX_PLLC_M, m);
+		WREG_DAC(MGA1064_EH_PIX_PLLC_N, n);
+		WREG_DAC(MGA1064_EH_PIX_PLLC_P, p);
+
+		udelay(500);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
+		tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+		tmp &= ~MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		vcount = RREG8(MGAREG_VCOUNT);
+
+		for (j = 0; j < 30 && pll_locked == false; j++) {
+			tmpcount = RREG8(MGAREG_VCOUNT);
+			if (tmpcount < vcount)
+				vcount = 0;
+			if ((tmpcount - vcount) > 2)
+				pll_locked = true;
+			else
+				udelay(5);
+		}
+	}
+
+	return 0;
+}
+
+static int mga_g200er_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta;
+	unsigned int testr, testn, testm, testo;
+	unsigned int p, m, n;
+	unsigned int computed;
+	int tmp;
+
+	m = n = p = 0;
+	vcomax = 1488000;
+	vcomin = 1056000;
+	pllreffreq = 48000;
+
+	delta = 0xffffffff;
+
+	for (testr = 0; testr < 4; testr++) {
+		if (delta == 0)
+			break;
+		for (testn = 5; testn < 129; testn++) {
+			if (delta == 0)
+				break;
+			for (testm = 3; testm >= 0; testm--) {
+				if (delta == 0)
+					break;
+				for (testo = 5; testo < 33; testo++) {
+					computed = pllreffreq * (testn + 1) /
+						(testr + 1);
+					if (computed < vcomin)
+						continue;
+					if (computed > vcomax)
+						continue;
+					if (computed > clock)
+						tmpdelta = computed - clock;
+					else
+						tmpdelta = clock - computed;
+					if (tmpdelta < delta) {
+						delta = tmpdelta;
+						m = testm | (testo << 3);
+						n = testn;
+						p = testr | (testr << 3);
+					}
+				}
+			}
+		}
+	}
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+	WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_REMHEADCTL_CLKDIS;
+	WREG_DAC(MGA1064_REMHEADCTL, tmp);
+
+	tmp = RREG8(MGAREG_MEM_MISC_READ);
+	tmp |= (0x3<<2) | 0xc0;
+	WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	udelay(500);
+
+	WREG_DAC(MGA1064_ER_PIX_PLLC_N, n);
+	WREG_DAC(MGA1064_ER_PIX_PLLC_M, m);
+	WREG_DAC(MGA1064_ER_PIX_PLLC_P, p);
+
+	udelay(50);
+
+	return 0;
+}
+
+static int mga_crtc_set_plls(struct mga_device *mdev, long clock)
+{
+	switch(mdev->type) {
+	case G200_SE_A:
+	case G200_SE_B:
+		return mga_g200se_set_plls(mdev, clock);
+		break;
+	case G200_WB:
+		return mga_g200wb_set_plls(mdev, clock);
+		break;
+	case G200_EV:
+		return mga_g200ev_set_plls(mdev, clock);
+		break;
+	case G200_EH:
+		return mga_g200eh_set_plls(mdev, clock);
+		break;
+	case G200_ER:
+		return mga_g200er_set_plls(mdev, clock);
+		break;
+	}
+	return 0;
+}
+
+static void mga_g200wb_prepare(struct drm_crtc *crtc)
+{
+	struct mga_device *mdev = crtc->dev->dev_private;
+	u8 tmp;
+	int iter_max;
+
+	/* 1- The first step is to warn the BMC of an upcoming mode change.
+	 * We are putting the misc<0> to output.*/
+
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x10;
+	WREG_DAC(MGA1064_GEN_IO_CTL, tmp);
+
+	/* we are putting a 1 on the misc<0> line */
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x10;
+	WREG_DAC(MGA1064_GEN_IO_DATA, tmp);
+
+	/* 2- Second step to mask and further scan request
+	 * This will be done by asserting the remfreqmsk bit (XSPAREREG<7>)
+	 */
+	WREG8(DAC_INDEX, MGA1064_SPAREREG);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x80;
+	WREG_DAC(MGA1064_SPAREREG, tmp);
+
+	/* 3a- the third step is to verifu if there is an active scan
+	 * We are searching for a 0 on remhsyncsts <XSPAREREG<0>)
+	 */
+	iter_max = 300;
+	while (!(tmp & 0x1) && iter_max) {
+		WREG8(DAC_INDEX, MGA1064_SPAREREG);
+		tmp = RREG8(DAC_DATA);
+		udelay(1000);
+		iter_max--;
+	}
+
+	/* 3b- this step occurs only if the remove is actually scanning
+	 * we are waiting for the end of the frame which is a 1 on
+	 * remvsyncsts (XSPAREREG<1>)
+	 */
+	if (iter_max) {
+		iter_max = 300;
+		while ((tmp & 0x2) && iter_max) {
+			WREG8(DAC_INDEX, MGA1064_SPAREREG);
+			tmp = RREG8(DAC_DATA);
+			udelay(1000);
+			iter_max--;
+		}
+	}
+}
+
+static void mga_g200wb_commit(struct drm_crtc *crtc)
+{
+	u8 tmp;
+	struct mga_device *mdev = crtc->dev->dev_private;
+
+	/* 1- The first step is to ensure that the vrsten and hrsten are set */
+	WREG8(MGAREG_CRTCEXT_INDEX, 1);
+	tmp = RREG8(MGAREG_CRTCEXT_DATA);
+	WREG8(MGAREG_CRTCEXT_DATA, tmp | 0x88);
+
+	/* 2- second step is to assert the rstlvl2 */
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL2);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x8;
+	WREG8(DAC_DATA, tmp);
+
+	/* wait 10 us */
+	udelay(10);
+
+	/* 3- deassert rstlvl2 */
+	tmp &= ~0x08;
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL2);
+	WREG8(DAC_DATA, tmp);
+
+	/* 4- remove mask of scan request */
+	WREG8(DAC_INDEX, MGA1064_SPAREREG);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~0x80;
+	WREG8(DAC_DATA, tmp);
+
+	/* 5- put back a 0 on the misc<0> line */
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~0x10;
+	WREG_DAC(MGA1064_GEN_IO_DATA, tmp);
+}
+
+
+void mga_set_start_address(struct drm_crtc *crtc, unsigned offset)
+{
+	struct mga_device *mdev = crtc->dev->dev_private;
+	u32 addr;
+	int count;
+
+	while (RREG8(0x1fda) & 0x08);
+	while (!(RREG8(0x1fda) & 0x08));
+
+	count = RREG8(MGAREG_VCOUNT) + 2;
+	while (RREG8(MGAREG_VCOUNT) < count);
+
+	addr = offset >> 2;
+	WREG_CRT(0x0d, (u8)(addr & 0xff));
+	WREG_CRT(0x0c, (u8)(addr >> 8) & 0xff);
+	WREG_CRT(0xaf, (u8)(addr >> 16) & 0xf);
+}
+
+
+/* ast is different - we will force move buffers out of VRAM */
+static int mga_crtc_do_set_base(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				int x, int y, int atomic)
+{
+	struct mga_device *mdev = crtc->dev->dev_private;
+	struct drm_gem_object *obj;
+	struct mga_framebuffer *mga_fb;
+	struct mgag200_bo *bo;
+	int ret;
+	u64 gpu_addr;
+
+	/* push the previous fb to system ram */
+	if (!atomic && fb) {
+		mga_fb = to_mga_framebuffer(fb);
+		obj = mga_fb->obj;
+		bo = gem_to_mga_bo(obj);
+		ret = mgag200_bo_reserve(bo, false);
+		if (ret)
+			return ret;
+		mgag200_bo_push_sysram(bo);
+		mgag200_bo_unreserve(bo);
+	}
+
+	mga_fb = to_mga_framebuffer(crtc->fb);
+	obj = mga_fb->obj;
+	bo = gem_to_mga_bo(obj);
+
+	ret = mgag200_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	ret = mgag200_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	if (ret) {
+		mgag200_bo_unreserve(bo);
+		return ret;
+	}
+
+	if (&mdev->mfbdev->mfb == mga_fb) {
+		/* if pushing console in kmap it */
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret)
+			DRM_ERROR("failed to kmap fbcon\n");
+
+	}
+	mgag200_bo_unreserve(bo);
+
+	DRM_INFO("mga base %llx\n", gpu_addr);
+
+	mga_set_start_address(crtc, (u32)gpu_addr);
+
+	return 0;
+}
+
+static int mga_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+				  struct drm_framebuffer *old_fb)
+{
+	return mga_crtc_do_set_base(crtc, old_fb, x, y, 0);
+}
+
+static int mga_crtc_mode_set(struct drm_crtc *crtc,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode,
+				int x, int y, struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	int hdisplay, hsyncstart, hsyncend, htotal;
+	int vdisplay, vsyncstart, vsyncend, vtotal;
+	int pitch;
+	int option = 0, option2 = 0;
+	int i;
+	unsigned char misc = 0;
+	unsigned char ext_vga[6];
+	unsigned char ext_vga_index24;
+	unsigned char dac_index90 = 0;
+	u8 bppshift;
+
+	static unsigned char dacvalue[] = {
+		/* 0x00: */        0,    0,    0,    0,    0,    0, 0x00,    0,
+		/* 0x08: */        0,    0,    0,    0,    0,    0,    0,    0,
+		/* 0x10: */        0,    0,    0,    0,    0,    0,    0,    0,
+		/* 0x18: */     0x00,    0, 0xC9, 0xFF, 0xBF, 0x20, 0x1F, 0x20,
+		/* 0x20: */     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 0x28: */     0x00, 0x00, 0x00, 0x00,    0,    0,    0, 0x40,
+		/* 0x30: */     0x00, 0xB0, 0x00, 0xC2, 0x34, 0x14, 0x02, 0x83,
+		/* 0x38: */     0x00, 0x93, 0x00, 0x77, 0x00, 0x00, 0x00, 0x3A,
+		/* 0x40: */        0,    0,    0,    0,    0,    0,    0,    0,
+		/* 0x48: */        0,    0,    0,    0,    0,    0,    0,    0
+	};
+
+	bppshift = mdev->bpp_shifts[(crtc->fb->bits_per_pixel >> 3) - 1];
+
+	switch (mdev->type) {
+	case G200_SE_A:
+	case G200_SE_B:
+		dacvalue[MGA1064_VREF_CTL] = 0x03;
+		dacvalue[MGA1064_PIX_CLK_CTL] = MGA1064_PIX_CLK_CTL_SEL_PLL;
+		dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_DAC_EN |
+					     MGA1064_MISC_CTL_VGA8 |
+					     MGA1064_MISC_CTL_DAC_RAM_CS;
+		if (mdev->has_sdram)
+			option = 0x40049120;
+		else
+			option = 0x4004d120;
+		option2 = 0x00008000;
+		break;
+	case G200_WB:
+		dacvalue[MGA1064_VREF_CTL] = 0x07;
+		option = 0x41049120;
+		option2 = 0x0000b000;
+		break;
+	case G200_EV:
+		dacvalue[MGA1064_PIX_CLK_CTL] = MGA1064_PIX_CLK_CTL_SEL_PLL;
+		dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_VGA8 |
+					     MGA1064_MISC_CTL_DAC_RAM_CS;
+		option = 0x00000120;
+		option2 = 0x0000b000;
+		break;
+	case G200_EH:
+		dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_VGA8 |
+					     MGA1064_MISC_CTL_DAC_RAM_CS;
+		option = 0x00000120;
+		option2 = 0x0000b000;
+		break;
+	case G200_ER:
+		dac_index90 = 0;
+		break;
+	}
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_8bits;
+		break;
+	case 16:
+		if (crtc->fb->depth == 15)
+			dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_15bits;
+		else
+			dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_16bits;
+		break;
+	case 24:
+		dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_24bits;
+		break;
+	case 32:
+		dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_32_24bits;
+		break;
+	}
+
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		misc |= 0x40;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		misc |= 0x80;
+
+
+	for (i = 0; i < sizeof(dacvalue); i++) {
+		if ((i <= 0x03) ||
+		    (i == 0x07) ||
+		    (i == 0x0b) ||
+		    (i == 0x0f) ||
+		    ((i >= 0x13) && (i <= 0x17)) ||
+		    (i == 0x1b) ||
+		    (i == 0x1c) ||
+		    ((i >= 0x1f) && (i <= 0x29)) ||
+		    ((i >= 0x30) && (i <= 0x37)))
+			continue;
+		if (IS_G200_SE(mdev) &&
+		    ((i == 0x2c) || (i == 0x2d) || (i == 0x2e)))
+			continue;
+		if ((mdev->type == G200_EV || mdev->type == G200_WB || mdev->type == G200_EH) &&
+		    (i >= 0x44) && (i <= 0x4e))
+			continue;
+
+		WREG_DAC(i, dacvalue[i]);
+	}
+
+	if (mdev->type == G200_ER) {
+		WREG_DAC(0x90, dac_index90);
+	}
+
+
+	if (option)
+		pci_write_config_dword(dev->pdev, PCI_MGA_OPTION, option);
+	if (option2)
+		pci_write_config_dword(dev->pdev, PCI_MGA_OPTION2, option2);
+
+	WREG_SEQ(2, 0xf);
+	WREG_SEQ(3, 0);
+	WREG_SEQ(4, 0xe);
+
+	pitch = crtc->fb->pitches[0] / (crtc->fb->bits_per_pixel / 8);
+	if (crtc->fb->bits_per_pixel == 24)
+		pitch = pitch >> (4 - bppshift);
+	else
+		pitch = pitch >> (4 - bppshift);
+
+	hdisplay = mode->hdisplay / 8 - 1;
+	hsyncstart = mode->hsync_start / 8 - 1;
+	hsyncend = mode->hsync_end / 8 - 1;
+	htotal = mode->htotal / 8 - 1;
+
+	/* Work around hardware quirk */
+	if ((htotal & 0x07) == 0x06 || (htotal & 0x07) == 0x04)
+		htotal++;
+
+	vdisplay = mode->vdisplay - 1;
+	vsyncstart = mode->vsync_start - 1;
+	vsyncend = mode->vsync_end - 1;
+	vtotal = mode->vtotal - 2;
+
+	WREG_GFX(0, 0);
+	WREG_GFX(1, 0);
+	WREG_GFX(2, 0);
+	WREG_GFX(3, 0);
+	WREG_GFX(4, 0);
+	WREG_GFX(5, 0x40);
+	WREG_GFX(6, 0x5);
+	WREG_GFX(7, 0xf);
+	WREG_GFX(8, 0xf);
+
+	WREG_CRT(0, htotal - 4);
+	WREG_CRT(1, hdisplay);
+	WREG_CRT(2, hdisplay);
+	WREG_CRT(3, (htotal & 0x1F) | 0x80);
+	WREG_CRT(4, hsyncstart);
+	WREG_CRT(5, ((htotal & 0x20) << 2) | (hsyncend & 0x1F));
+	WREG_CRT(6, vtotal & 0xFF);
+	WREG_CRT(7, ((vtotal & 0x100) >> 8) |
+		 ((vdisplay & 0x100) >> 7) |
+		 ((vsyncstart & 0x100) >> 6) |
+		 ((vdisplay & 0x100) >> 5) |
+		 ((vdisplay & 0x100) >> 4) | /* linecomp */
+		 ((vtotal & 0x200) >> 4)|
+		 ((vdisplay & 0x200) >> 3) |
+		 ((vsyncstart & 0x200) >> 2));
+	WREG_CRT(9, ((vdisplay & 0x200) >> 4) |
+		 ((vdisplay & 0x200) >> 3));
+	WREG_CRT(10, 0);
+	WREG_CRT(11, 0);
+	WREG_CRT(12, 0);
+	WREG_CRT(13, 0);
+	WREG_CRT(14, 0);
+	WREG_CRT(15, 0);
+	WREG_CRT(16, vsyncstart & 0xFF);
+	WREG_CRT(17, (vsyncend & 0x0F) | 0x20);
+	WREG_CRT(18, vdisplay & 0xFF);
+	WREG_CRT(19, pitch & 0xFF);
+	WREG_CRT(20, 0);
+	WREG_CRT(21, vdisplay & 0xFF);
+	WREG_CRT(22, (vtotal + 1) & 0xFF);
+	WREG_CRT(23, 0xc3);
+	WREG_CRT(24, vdisplay & 0xFF);
+
+	ext_vga[0] = 0;
+	ext_vga[5] = 0;
+
+	/* TODO interlace */
+
+	ext_vga[0] |= (pitch & 0x300) >> 4;
+	ext_vga[1] = (((htotal - 4) & 0x100) >> 8) |
+		((hdisplay & 0x100) >> 7) |
+		((hsyncstart & 0x100) >> 6) |
+		(htotal & 0x40);
+	ext_vga[2] = ((vtotal & 0xc00) >> 10) |
+		((vdisplay & 0x400) >> 8) |
+		((vdisplay & 0xc00) >> 7) |
+		((vsyncstart & 0xc00) >> 5) |
+		((vdisplay & 0x400) >> 3);
+	if (crtc->fb->bits_per_pixel == 24)
+		ext_vga[3] = (((1 << bppshift) * 3) - 1) | 0x80;
+	else
+		ext_vga[3] = ((1 << bppshift) - 1) | 0x80;
+	ext_vga[4] = 0;
+	if (mdev->type == G200_WB)
+		ext_vga[1] |= 0x88;
+
+	ext_vga_index24 = 0x05;
+
+	/* Set pixel clocks */
+	misc = 0x2d;
+	WREG8(MGA_MISC_OUT, misc);
+
+	mga_crtc_set_plls(mdev, mode->clock);
+
+	for (i = 0; i < 6; i++) {
+		WREG_ECRT(i, ext_vga[i]);
+	}
+
+	if (mdev->type == G200_ER)
+		WREG_ECRT(24, ext_vga_index24);
+
+	if (mdev->type == G200_EV) {
+		WREG_ECRT(6, 0);
+	}
+
+	WREG_ECRT(0, ext_vga[0]);
+	/* Enable mga pixel clock */
+	misc = 0x2d;
+
+	WREG8(MGA_MISC_OUT, misc);
+
+	if (adjusted_mode)
+		memcpy(&mdev->mode, mode, sizeof(struct drm_display_mode));
+
+	mga_crtc_do_set_base(crtc, old_fb, x, y, 0);
+
+	/* reset tagfifo */
+	if (mdev->type == G200_ER) {
+		u32 mem_ctl = RREG32(MGAREG_MEMCTL);
+		u8 seq1;
+
+		/* screen off */
+		WREG8(MGAREG_SEQ_INDEX, 0x01);
+		seq1 = RREG8(MGAREG_SEQ_DATA) | 0x20;
+		WREG8(MGAREG_SEQ_DATA, seq1);
+
+		WREG32(MGAREG_MEMCTL, mem_ctl | 0x00200000);
+		udelay(1000);
+		WREG32(MGAREG_MEMCTL, mem_ctl & ~0x00200000);
+
+		WREG8(MGAREG_SEQ_DATA, seq1 & ~0x20);
+	}
+
+
+	if (IS_G200_SE(mdev)) {
+		if (mdev->reg_1e24 >= 0x02) {
+			u8 hi_pri_lvl;
+			u32 bpp;
+			u32 mb;
+
+			if (crtc->fb->bits_per_pixel > 16)
+				bpp = 32;
+			else if (crtc->fb->bits_per_pixel > 8)
+				bpp = 16;
+			else
+				bpp = 8;
+
+			mb = (mode->clock * bpp) / 1000;
+			if (mb > 3100)
+				hi_pri_lvl = 0;
+			else if (mb > 2600)
+				hi_pri_lvl = 1;
+			else if (mb > 1900)
+				hi_pri_lvl = 2;
+			else if (mb > 1160)
+				hi_pri_lvl = 3;
+			else if (mb > 440)
+				hi_pri_lvl = 4;
+			else
+				hi_pri_lvl = 5;
+
+			WREG8(0x1fde, 0x06);
+			WREG8(0x1fdf, hi_pri_lvl);
+		} else {
+			if (mdev->reg_1e24 >= 0x01)
+				WREG8(0x1fdf, 0x03);
+			else
+				WREG8(0x1fdf, 0x04);
+		}
+	}
+	return 0;
+}
+
+#if 0 /* code from mjg to attempt D3 on crtc dpms off - revisit later */
+static int mga_suspend(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	struct pci_dev *pdev = dev->pdev;
+	int option;
+
+	if (mdev->suspended)
+		return 0;
+
+	WREG_SEQ(1, 0x20);
+	WREG_ECRT(1, 0x30);
+	/* Disable the pixel clock */
+	WREG_DAC(0x1a, 0x05);
+	/* Power down the DAC */
+	WREG_DAC(0x1e, 0x18);
+	/* Power down the pixel PLL */
+	WREG_DAC(0x1a, 0x0d);
+
+	/* Disable PLLs and clocks */
+	pci_read_config_dword(pdev, PCI_MGA_OPTION, &option);
+	option &= ~(0x1F8024);
+	pci_write_config_dword(pdev, PCI_MGA_OPTION, option);
+	pci_set_power_state(pdev, PCI_D3hot);
+	pci_disable_device(pdev);
+
+	mdev->suspended = true;
+
+	return 0;
+}
+
+static int mga_resume(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	struct pci_dev *pdev = dev->pdev;
+	int option;
+
+	if (!mdev->suspended)
+		return 0;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_enable_device(pdev);
+
+	/* Disable sysclk */
+	pci_read_config_dword(pdev, PCI_MGA_OPTION, &option);
+	option &= ~(0x4);
+	pci_write_config_dword(pdev, PCI_MGA_OPTION, option);
+
+	mdev->suspended = false;
+
+	return 0;
+}
+
+#endif
+
+static void mga_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	u8 seq1 = 0, crtcext1 = 0;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		seq1 = 0;
+		crtcext1 = 0;
+		mga_crtc_load_lut(crtc);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		seq1 = 0x20;
+		crtcext1 = 0x10;
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		seq1 = 0x20;
+		crtcext1 = 0x20;
+		break;
+	case DRM_MODE_DPMS_OFF:
+		seq1 = 0x20;
+		crtcext1 = 0x30;
+		break;
+	}
+
+#if 0
+	if (mode == DRM_MODE_DPMS_OFF) {
+		mga_suspend(crtc);
+	}
+#endif
+	WREG8(MGAREG_SEQ_INDEX, 0x01);
+	seq1 |= RREG8(MGAREG_SEQ_DATA) & ~0x20;
+	mga_wait_vsync(mdev);
+	mga_wait_busy(mdev);
+	WREG8(MGAREG_SEQ_DATA, seq1);
+	msleep(20);
+	WREG8(MGAREG_CRTCEXT_INDEX, 0x01);
+	crtcext1 |= RREG8(MGAREG_CRTCEXT_DATA) & ~0x30;
+	WREG8(MGAREG_CRTCEXT_DATA, crtcext1);
+
+#if 0
+	if (mode == DRM_MODE_DPMS_ON && mdev->suspended == true) {
+		mga_resume(crtc);
+		drm_helper_resume_force_mode(dev);
+	}
+#endif
+}
+
+/*
+ * This is called before a mode is programmed. A typical use might be to
+ * enable DPMS during the programming to avoid seeing intermediate stages,
+ * but that's not relevant to us
+ */
+static void mga_crtc_prepare(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	u8 tmp;
+
+	/*	mga_resume(crtc);*/
+
+	WREG8(MGAREG_CRTC_INDEX, 0x11);
+	tmp = RREG8(MGAREG_CRTC_DATA);
+	WREG_CRT(0x11, tmp | 0x80);
+
+	if (mdev->type == G200_SE_A || mdev->type == G200_SE_B) {
+		WREG_SEQ(0, 1);
+		msleep(50);
+		WREG_SEQ(1, 0x20);
+		msleep(20);
+	} else {
+		WREG8(MGAREG_SEQ_INDEX, 0x1);
+		tmp = RREG8(MGAREG_SEQ_DATA);
+
+		/* start sync reset */
+		WREG_SEQ(0, 1);
+		WREG_SEQ(1, tmp | 0x20);
+	}
+
+	if (mdev->type == G200_WB)
+		mga_g200wb_prepare(crtc);
+
+	WREG_CRT(17, 0);
+}
+
+/*
+ * This is called after a mode is programmed. It should reverse anything done
+ * by the prepare function
+ */
+static void mga_crtc_commit(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	u8 tmp;
+
+	if (mdev->type == G200_WB)
+		mga_g200wb_commit(crtc);
+
+	if (mdev->type == G200_SE_A || mdev->type == G200_SE_B) {
+		msleep(50);
+		WREG_SEQ(1, 0x0);
+		msleep(20);
+		WREG_SEQ(0, 0x3);
+	} else {
+		WREG8(MGAREG_SEQ_INDEX, 0x1);
+		tmp = RREG8(MGAREG_SEQ_DATA);
+
+		tmp &= ~0x20;
+		WREG_SEQ(0x1, tmp);
+		WREG_SEQ(0, 3);
+	}
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+/*
+ * The core can pass us a set of gamma values to program. We actually only
+ * use this for 8-bit mode so can't perform smooth fades on deeper modes,
+ * but it's a requirement that we provide the function
+ */
+static void mga_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
+				  u16 *blue, uint32_t start, uint32_t size)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	int end = (start + size > MGAG200_LUT_SIZE) ? MGAG200_LUT_SIZE : start + size;
+	int i;
+
+	for (i = start; i < end; i++) {
+		mga_crtc->lut_r[i] = red[i] >> 8;
+		mga_crtc->lut_g[i] = green[i] >> 8;
+		mga_crtc->lut_b[i] = blue[i] >> 8;
+	}
+	mga_crtc_load_lut(crtc);
+}
+
+/* Simple cleanup function */
+static void mga_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+
+	drm_crtc_cleanup(crtc);
+	kfree(mga_crtc);
+}
+
+/* These provide the minimum set of functions required to handle a CRTC */
+static const struct drm_crtc_funcs mga_crtc_funcs = {
+	.gamma_set = mga_crtc_gamma_set,
+	.set_config = drm_crtc_helper_set_config,
+	.destroy = mga_crtc_destroy,
+};
+
+static const struct drm_crtc_helper_funcs mga_helper_funcs = {
+	.dpms = mga_crtc_dpms,
+	.mode_fixup = mga_crtc_mode_fixup,
+	.mode_set = mga_crtc_mode_set,
+	.mode_set_base = mga_crtc_mode_set_base,
+	.prepare = mga_crtc_prepare,
+	.commit = mga_crtc_commit,
+	.load_lut = mga_crtc_load_lut,
+};
+
+/* CRTC setup */
+static void mga_crtc_init(struct drm_device *dev)
+{
+	struct mga_device *mdev = dev->dev_private;
+	struct mga_crtc *mga_crtc;
+	int i;
+
+	mga_crtc = kzalloc(sizeof(struct mga_crtc) +
+			      (MGAG200FB_CONN_LIMIT * sizeof(struct drm_connector *)),
+			      GFP_KERNEL);
+
+	if (mga_crtc == NULL)
+		return;
+
+	drm_crtc_init(dev, &mga_crtc->base, &mga_crtc_funcs);
+
+	drm_mode_crtc_set_gamma_size(&mga_crtc->base, MGAG200_LUT_SIZE);
+	mdev->mode_info.crtc = mga_crtc;
+
+	for (i = 0; i < MGAG200_LUT_SIZE; i++) {
+		mga_crtc->lut_r[i] = i;
+		mga_crtc->lut_g[i] = i;
+		mga_crtc->lut_b[i] = i;
+	}
+
+	drm_crtc_helper_add(&mga_crtc->base, &mga_helper_funcs);
+}
+
+/** Sets the color ramps on behalf of fbcon */
+void mga_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			      u16 blue, int regno)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+
+	mga_crtc->lut_r[regno] = red >> 8;
+	mga_crtc->lut_g[regno] = green >> 8;
+	mga_crtc->lut_b[regno] = blue >> 8;
+}
+
+/** Gets the color ramps on behalf of fbcon */
+void mga_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			      u16 *blue, int regno)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+
+	*red = (u16)mga_crtc->lut_r[regno] << 8;
+	*green = (u16)mga_crtc->lut_g[regno] << 8;
+	*blue = (u16)mga_crtc->lut_b[regno] << 8;
+}
+
+/*
+ * The encoder comes after the CRTC in the output pipeline, but before
+ * the connector. It's responsible for ensuring that the digital
+ * stream is appropriately converted into the output format. Setup is
+ * very simple in this case - all we have to do is inform qemu of the
+ * colour depth in order to ensure that it displays appropriately
+ */
+
+/*
+ * These functions are analagous to those in the CRTC code, but are intended
+ * to handle any encoder-specific limitations
+ */
+static bool mga_encoder_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void mga_encoder_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+
+}
+
+static void mga_encoder_dpms(struct drm_encoder *encoder, int state)
+{
+	return;
+}
+
+static void mga_encoder_prepare(struct drm_encoder *encoder)
+{
+}
+
+static void mga_encoder_commit(struct drm_encoder *encoder)
+{
+}
+
+void mga_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct mga_encoder *mga_encoder = to_mga_encoder(encoder);
+	drm_encoder_cleanup(encoder);
+	kfree(mga_encoder);
+}
+
+static const struct drm_encoder_helper_funcs mga_encoder_helper_funcs = {
+	.dpms = mga_encoder_dpms,
+	.mode_fixup = mga_encoder_mode_fixup,
+	.mode_set = mga_encoder_mode_set,
+	.prepare = mga_encoder_prepare,
+	.commit = mga_encoder_commit,
+};
+
+static const struct drm_encoder_funcs mga_encoder_encoder_funcs = {
+	.destroy = mga_encoder_destroy,
+};
+
+static struct drm_encoder *mga_encoder_init(struct drm_device *dev)
+{
+	struct drm_encoder *encoder;
+	struct mga_encoder *mga_encoder;
+
+	mga_encoder = kzalloc(sizeof(struct mga_encoder), GFP_KERNEL);
+	if (!mga_encoder)
+		return NULL;
+
+	encoder = &mga_encoder->base;
+	encoder->possible_crtcs = 0x1;
+
+	drm_encoder_init(dev, encoder, &mga_encoder_encoder_funcs,
+			 DRM_MODE_ENCODER_DAC);
+	drm_encoder_helper_add(encoder, &mga_encoder_helper_funcs);
+
+	return encoder;
+}
+
+
+static int mga_vga_get_modes(struct drm_connector *connector)
+{
+	struct mga_connector *mga_connector = to_mga_connector(connector);
+	struct edid *edid;
+	int ret = 0;
+
+	edid = drm_get_edid(connector, &mga_connector->i2c->adapter);
+	if (edid) {
+		drm_mode_connector_update_edid_property(connector, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		connector->display_info.raw_edid = NULL;
+		kfree(edid);
+	}
+	return ret;
+}
+
+static int mga_vga_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	/* FIXME: Add bandwidth and g200se limitations */
+
+	if (mode->crtc_hdisplay > 2048 || mode->crtc_hsync_start > 4096 ||
+	    mode->crtc_hsync_end > 4096 || mode->crtc_htotal > 4096 ||
+	    mode->crtc_vdisplay > 2048 || mode->crtc_vsync_start > 4096 ||
+	    mode->crtc_vsync_end > 4096 || mode->crtc_vtotal > 4096) {
+		return MODE_BAD;
+	}
+
+	return MODE_OK;
+}
+
+struct drm_encoder *mga_connector_best_encoder(struct drm_connector
+						  *connector)
+{
+	int enc_id = connector->encoder_ids[0];
+	struct drm_mode_object *obj;
+	struct drm_encoder *encoder;
+
+	/* pick the encoder ids */
+	if (enc_id) {
+		obj =
+		    drm_mode_object_find(connector->dev, enc_id,
+					 DRM_MODE_OBJECT_ENCODER);
+		if (!obj)
+			return NULL;
+		encoder = obj_to_encoder(obj);
+		return encoder;
+	}
+	return NULL;
+}
+
+static enum drm_connector_status mga_vga_detect(struct drm_connector
+						   *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static void mga_connector_destroy(struct drm_connector *connector)
+{
+	struct mga_connector *mga_connector = to_mga_connector(connector);
+	mgag200_i2c_destroy(mga_connector->i2c);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+struct drm_connector_helper_funcs mga_vga_connector_helper_funcs = {
+	.get_modes = mga_vga_get_modes,
+	.mode_valid = mga_vga_mode_valid,
+	.best_encoder = mga_connector_best_encoder,
+};
+
+struct drm_connector_funcs mga_vga_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = mga_vga_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = mga_connector_destroy,
+};
+
+static struct drm_connector *mga_vga_init(struct drm_device *dev)
+{
+	struct drm_connector *connector;
+	struct mga_connector *mga_connector;
+
+	mga_connector = kzalloc(sizeof(struct mga_connector), GFP_KERNEL);
+	if (!mga_connector)
+		return NULL;
+
+	connector = &mga_connector->base;
+
+	drm_connector_init(dev, connector,
+			   &mga_vga_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+	drm_connector_helper_add(connector, &mga_vga_connector_helper_funcs);
+
+	mga_connector->i2c = mgag200_i2c_create(dev);
+	if (!mga_connector->i2c)
+		DRM_ERROR("failed to add ddc bus\n");
+
+	return connector;
+}
+
+
+int mgag200_modeset_init(struct mga_device *mdev)
+{
+	struct drm_encoder *encoder;
+	struct drm_connector *connector;
+	int ret;
+
+	mdev->mode_info.mode_config_initialized = true;
+
+	mdev->dev->mode_config.max_width = MGAG200_MAX_FB_WIDTH;
+	mdev->dev->mode_config.max_height = MGAG200_MAX_FB_HEIGHT;
+
+	mdev->dev->mode_config.fb_base = mdev->mc.vram_base;
+
+	mga_crtc_init(mdev->dev);
+
+	encoder = mga_encoder_init(mdev->dev);
+	if (!encoder) {
+		DRM_ERROR("mga_encoder_init failed\n");
+		return -1;
+	}
+
+	connector = mga_vga_init(mdev->dev);
+	if (!connector) {
+		DRM_ERROR("mga_vga_init failed\n");
+		return -1;
+	}
+
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	ret = mgag200_fbdev_init(mdev);
+	if (ret) {
+		DRM_ERROR("mga_fbdev_init failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+void mgag200_modeset_fini(struct mga_device *mdev)
+{
+
+}
diff --git a/drivers/gpu/drm/mgag200/mgag200_reg.h b/drivers/gpu/drm/mgag200/mgag200_reg.h
new file mode 100644
index 000000000000..fb24d8655feb
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_reg.h
@@ -0,0 +1,661 @@
+/*
+ * MGA Millennium (MGA2064W) functions
+ * MGA Mystique (MGA1064SG) functions
+ *
+ * Copyright 1996 The XFree86 Project, Inc.
+ *
+ * Authors
+ *		Dirk Hohndel
+ *			hohndel@XFree86.Org
+ *		David Dawes
+ *			dawes@XFree86.Org
+ * Contributors:
+ *		Guy DESBIEF, Aix-en-provence, France
+ *			g.desbief@aix.pacwan.net
+ *		MGA1064SG Mystique register file
+ */
+
+
+#ifndef _MGA_REG_H_
+#define _MGA_REG_H_
+
+#define	MGAREG_DWGCTL		0x1c00
+#define	MGAREG_MACCESS		0x1c04
+/* the following is a mystique only register */
+#define MGAREG_MCTLWTST		0x1c08
+#define	MGAREG_ZORG		0x1c0c
+
+#define	MGAREG_PAT0		0x1c10
+#define	MGAREG_PAT1		0x1c14
+#define	MGAREG_PLNWT		0x1c1c
+
+#define	MGAREG_BCOL		0x1c20
+#define	MGAREG_FCOL		0x1c24
+
+#define	MGAREG_SRC0		0x1c30
+#define	MGAREG_SRC1		0x1c34
+#define	MGAREG_SRC2		0x1c38
+#define	MGAREG_SRC3		0x1c3c
+
+#define	MGAREG_XYSTRT		0x1c40
+#define	MGAREG_XYEND		0x1c44
+
+#define	MGAREG_SHIFT		0x1c50
+/* the following is a mystique only register */
+#define MGAREG_DMAPAD		0x1c54
+#define	MGAREG_SGN		0x1c58
+#define	MGAREG_LEN		0x1c5c
+
+#define	MGAREG_AR0		0x1c60
+#define	MGAREG_AR1		0x1c64
+#define	MGAREG_AR2		0x1c68
+#define	MGAREG_AR3		0x1c6c
+#define	MGAREG_AR4		0x1c70
+#define	MGAREG_AR5		0x1c74
+#define	MGAREG_AR6		0x1c78
+
+#define	MGAREG_CXBNDRY		0x1c80
+#define	MGAREG_FXBNDRY		0x1c84
+#define	MGAREG_YDSTLEN		0x1c88
+#define	MGAREG_PITCH		0x1c8c
+
+#define	MGAREG_YDST		0x1c90
+#define	MGAREG_YDSTORG		0x1c94
+#define	MGAREG_YTOP		0x1c98
+#define	MGAREG_YBOT		0x1c9c
+
+#define	MGAREG_CXLEFT		0x1ca0
+#define	MGAREG_CXRIGHT		0x1ca4
+#define	MGAREG_FXLEFT		0x1ca8
+#define	MGAREG_FXRIGHT		0x1cac
+
+#define	MGAREG_XDST		0x1cb0
+
+#define	MGAREG_DR0		0x1cc0
+#define	MGAREG_DR1		0x1cc4
+#define	MGAREG_DR2		0x1cc8
+#define	MGAREG_DR3		0x1ccc
+
+#define	MGAREG_DR4		0x1cd0
+#define	MGAREG_DR5		0x1cd4
+#define	MGAREG_DR6		0x1cd8
+#define	MGAREG_DR7		0x1cdc
+
+#define	MGAREG_DR8		0x1ce0
+#define	MGAREG_DR9		0x1ce4
+#define	MGAREG_DR10		0x1ce8
+#define	MGAREG_DR11		0x1cec
+
+#define	MGAREG_DR12		0x1cf0
+#define	MGAREG_DR13		0x1cf4
+#define	MGAREG_DR14		0x1cf8
+#define	MGAREG_DR15		0x1cfc
+
+#define MGAREG_SRCORG		0x2cb4
+#define MGAREG_DSTORG		0x2cb8
+
+/* add or or this to one of the previous "power registers" to start
+   the drawing engine */
+
+#define MGAREG_EXEC		0x0100
+
+#define	MGAREG_FIFOSTATUS	0x1e10
+#define	MGAREG_Status		0x1e14
+#define MGAREG_CACHEFLUSH       0x1fff
+#define	MGAREG_ICLEAR		0x1e18
+#define	MGAREG_IEN		0x1e1c
+
+#define	MGAREG_VCOUNT		0x1e20
+
+#define	MGAREG_Reset		0x1e40
+
+#define	MGAREG_OPMODE		0x1e54
+
+/* Warp Registers */
+#define MGAREG_WIADDR           0x1dc0
+#define MGAREG_WIADDR2          0x1dd8
+#define MGAREG_WGETMSB          0x1dc8
+#define MGAREG_WVRTXSZ          0x1dcc
+#define MGAREG_WACCEPTSEQ       0x1dd4
+#define MGAREG_WMISC            0x1e70
+
+#define MGAREG_MEMCTL           0x2e08
+
+/* OPMODE register additives */
+
+#define MGAOPM_DMA_GENERAL	(0x00 << 2)
+#define MGAOPM_DMA_BLIT		(0x01 << 2)
+#define MGAOPM_DMA_VECTOR	(0x10 << 2)
+
+/* MACCESS register additives */
+#define MGAMAC_PW8               0x00
+#define MGAMAC_PW16              0x01
+#define MGAMAC_PW24              0x03 /* not a typo */
+#define MGAMAC_PW32              0x02 /* not a typo */
+#define MGAMAC_BYPASS332         0x10000000
+#define MGAMAC_NODITHER          0x40000000
+#define MGAMAC_DIT555            0x80000000
+
+/* DWGCTL register additives */
+
+/* Lines */
+
+#define MGADWG_LINE_OPEN	0x00
+#define MGADWG_AUTOLINE_OPEN	0x01
+#define MGADWG_LINE_CLOSE	0x02
+#define MGADWG_AUTOLINE_CLOSE	0x03
+
+/* Trapezoids */
+#define MGADWG_TRAP		0x04
+#define MGADWG_TEXTURE_TRAP	0x06
+
+/* BitBlts */
+
+#define MGADWG_BITBLT		0x08
+#define MGADWG_FBITBLT		0x0c
+#define MGADWG_ILOAD		0x09
+#define MGADWG_ILOAD_SCALE	0x0d
+#define MGADWG_ILOAD_FILTER	0x0f
+#define MGADWG_ILOAD_HIQH	0x07
+#define MGADWG_ILOAD_HIQHV	0x0e
+#define MGADWG_IDUMP		0x0a
+
+/* atype access to WRAM */
+
+#define MGADWG_RPL		( 0x00 << 4 )
+#define MGADWG_RSTR		( 0x01 << 4 )
+#define MGADWG_ZI		( 0x03 << 4 )
+#define MGADWG_BLK 		( 0x04 << 4 )
+#define MGADWG_I		( 0x07 << 4 )
+
+/* specifies whether bit blits are linear or xy */
+#define MGADWG_LINEAR		( 0x01 << 7 )
+
+/* z drawing mode. use MGADWG_NOZCMP for always */
+
+#define MGADWG_NOZCMP		( 0x00 << 8 )
+#define MGADWG_ZE		( 0x02 << 8 )
+#define MGADWG_ZNE		( 0x03 << 8 )
+#define MGADWG_ZLT		( 0x04 << 8 )
+#define MGADWG_ZLTE		( 0x05 << 8 )
+#define MGADWG_GT		( 0x06 << 8 )
+#define MGADWG_GTE		( 0x07 << 8 )
+
+/* use this to force colour expansion circuitry to do its stuff */
+
+#define MGADWG_SOLID		( 0x01 << 11 )
+
+/* ar register at zero */
+
+#define MGADWG_ARZERO		( 0x01 << 12 )
+
+#define MGADWG_SGNZERO		( 0x01 << 13 )
+
+#define MGADWG_SHIFTZERO	( 0x01 << 14 )
+
+/* See table on 4-43 for bop ALU operations */
+
+/* See table on 4-44 for translucidity masks */
+
+#define MGADWG_BMONOLEF		( 0x00 << 25 )
+#define MGADWG_BMONOWF		( 0x04 << 25 )
+#define MGADWG_BPLAN		( 0x01 << 25 )
+
+/* note that if bfcol is specified and you're doing a bitblt, it causes
+   a fbitblt to be performed, so check that you obey the fbitblt rules */
+
+#define MGADWG_BFCOL   		( 0x02 << 25 )
+#define MGADWG_BUYUV		( 0x0e << 25 )
+#define MGADWG_BU32BGR		( 0x03 << 25 )
+#define MGADWG_BU32RGB		( 0x07 << 25 )
+#define MGADWG_BU24BGR		( 0x0b << 25 )
+#define MGADWG_BU24RGB		( 0x0f << 25 )
+
+#define MGADWG_PATTERN		( 0x01 << 29 )
+#define MGADWG_TRANSC		( 0x01 << 30 )
+#define MGAREG_MISC_WRITE	0x3c2
+#define MGAREG_MISC_READ	0x3cc
+#define MGAREG_MEM_MISC_WRITE       0x1fc2
+#define MGAREG_MEM_MISC_READ        0x1fcc
+
+#define MGAREG_MISC_IOADSEL	(0x1 << 0)
+#define MGAREG_MISC_RAMMAPEN	(0x1 << 1)
+#define MGAREG_MISC_CLK_SEL_VGA25	(0x0 << 2)
+#define MGAREG_MISC_CLK_SEL_VGA28	(0x1 << 2)
+#define MGAREG_MISC_CLK_SEL_MGA_PIX	(0x2 << 2)
+#define MGAREG_MISC_CLK_SEL_MGA_MSK	(0x3 << 2)
+#define MGAREG_MISC_VIDEO_DIS	(0x1 << 4)
+#define MGAREG_MISC_HIGH_PG_SEL	(0x1 << 5)
+
+/* MMIO VGA registers */
+#define MGAREG_SEQ_INDEX	0x1fc4
+#define MGAREG_SEQ_DATA		0x1fc5
+#define MGAREG_CRTC_INDEX	0x1fd4
+#define MGAREG_CRTC_DATA	0x1fd5
+#define MGAREG_CRTCEXT_INDEX	0x1fde
+#define MGAREG_CRTCEXT_DATA	0x1fdf
+
+
+
+/* MGA bits for registers PCI_OPTION_REG */
+#define MGA1064_OPT_SYS_CLK_PCI   		( 0x00 << 0 )
+#define MGA1064_OPT_SYS_CLK_PLL   		( 0x01 << 0 )
+#define MGA1064_OPT_SYS_CLK_EXT   		( 0x02 << 0 )
+#define MGA1064_OPT_SYS_CLK_MSK   		( 0x03 << 0 )
+
+#define MGA1064_OPT_SYS_CLK_DIS   		( 0x01 << 2 )
+#define MGA1064_OPT_G_CLK_DIV_1   		( 0x01 << 3 )
+#define MGA1064_OPT_M_CLK_DIV_1   		( 0x01 << 4 )
+
+#define MGA1064_OPT_SYS_PLL_PDN   		( 0x01 << 5 )
+#define MGA1064_OPT_VGA_ION   		( 0x01 << 8 )
+
+/* MGA registers in PCI config space */
+#define PCI_MGA_INDEX		0x44
+#define PCI_MGA_DATA		0x48
+#define PCI_MGA_OPTION		0x40
+#define PCI_MGA_OPTION2		0x50
+#define PCI_MGA_OPTION3		0x54
+
+#define RAMDAC_OFFSET		0x3c00
+
+/* TVP3026 direct registers */
+
+#define TVP3026_INDEX		0x00
+#define TVP3026_WADR_PAL	0x00
+#define TVP3026_COL_PAL		0x01
+#define TVP3026_PIX_RD_MSK	0x02
+#define TVP3026_RADR_PAL	0x03
+#define TVP3026_CUR_COL_ADDR	0x04
+#define TVP3026_CUR_COL_DATA	0x05
+#define TVP3026_DATA		0x0a
+#define TVP3026_CUR_RAM		0x0b
+#define TVP3026_CUR_XLOW	0x0c
+#define TVP3026_CUR_XHI		0x0d
+#define TVP3026_CUR_YLOW	0x0e
+#define TVP3026_CUR_YHI		0x0f
+
+/* TVP3026 indirect registers */
+
+#define TVP3026_SILICON_REV	0x01
+#define TVP3026_CURSOR_CTL	0x06
+#define TVP3026_LATCH_CTL	0x0f
+#define TVP3026_TRUE_COLOR_CTL	0x18
+#define TVP3026_MUX_CTL		0x19
+#define TVP3026_CLK_SEL		0x1a
+#define TVP3026_PAL_PAGE	0x1c
+#define TVP3026_GEN_CTL		0x1d
+#define TVP3026_MISC_CTL	0x1e
+#define TVP3026_GEN_IO_CTL	0x2a
+#define TVP3026_GEN_IO_DATA	0x2b
+#define TVP3026_PLL_ADDR	0x2c
+#define TVP3026_PIX_CLK_DATA	0x2d
+#define TVP3026_MEM_CLK_DATA	0x2e
+#define TVP3026_LOAD_CLK_DATA	0x2f
+#define TVP3026_KEY_RED_LOW	0x32
+#define TVP3026_KEY_RED_HI	0x33
+#define TVP3026_KEY_GREEN_LOW	0x34
+#define TVP3026_KEY_GREEN_HI	0x35
+#define TVP3026_KEY_BLUE_LOW	0x36
+#define TVP3026_KEY_BLUE_HI	0x37
+#define TVP3026_KEY_CTL		0x38
+#define TVP3026_MCLK_CTL	0x39
+#define TVP3026_SENSE_TEST	0x3a
+#define TVP3026_TEST_DATA	0x3b
+#define TVP3026_CRC_LSB		0x3c
+#define TVP3026_CRC_MSB		0x3d
+#define TVP3026_CRC_CTL		0x3e
+#define TVP3026_ID		0x3f
+#define TVP3026_RESET		0xff
+
+
+/* MGA1064 DAC Register file */
+/* MGA1064 direct registers */
+
+#define MGA1064_INDEX		0x00
+#define MGA1064_WADR_PAL	0x00
+#define MGA1064_SPAREREG        0x00
+#define MGA1064_COL_PAL		0x01
+#define MGA1064_PIX_RD_MSK	0x02
+#define MGA1064_RADR_PAL	0x03
+#define MGA1064_DATA		0x0a
+
+#define MGA1064_CUR_XLOW	0x0c
+#define MGA1064_CUR_XHI		0x0d
+#define MGA1064_CUR_YLOW	0x0e
+#define MGA1064_CUR_YHI		0x0f
+
+/* MGA1064 indirect registers */
+#define MGA1064_DVI_PIPE_CTL    0x03
+#define MGA1064_CURSOR_BASE_ADR_LOW	0x04
+#define MGA1064_CURSOR_BASE_ADR_HI	0x05
+#define MGA1064_CURSOR_CTL	0x06
+#define MGA1064_CURSOR_COL0_RED	0x08
+#define MGA1064_CURSOR_COL0_GREEN	0x09
+#define MGA1064_CURSOR_COL0_BLUE	0x0a
+
+#define MGA1064_CURSOR_COL1_RED	0x0c
+#define MGA1064_CURSOR_COL1_GREEN	0x0d
+#define MGA1064_CURSOR_COL1_BLUE	0x0e
+
+#define MGA1064_CURSOR_COL2_RED	0x010
+#define MGA1064_CURSOR_COL2_GREEN	0x011
+#define MGA1064_CURSOR_COL2_BLUE	0x012
+
+#define MGA1064_VREF_CTL	0x018
+
+#define MGA1064_MUL_CTL		0x19
+#define MGA1064_MUL_CTL_8bits		0x0
+#define MGA1064_MUL_CTL_15bits		0x01
+#define MGA1064_MUL_CTL_16bits		0x02
+#define MGA1064_MUL_CTL_24bits		0x03
+#define MGA1064_MUL_CTL_32bits		0x04
+#define MGA1064_MUL_CTL_2G8V16bits		0x05
+#define MGA1064_MUL_CTL_G16V16bits		0x06
+#define MGA1064_MUL_CTL_32_24bits		0x07
+
+#define MGA1064_PIX_CLK_CTL		0x1a
+#define MGA1064_PIX_CLK_CTL_CLK_DIS		( 0x01 << 2 )
+#define MGA1064_PIX_CLK_CTL_CLK_POW_DOWN	( 0x01 << 3 )
+#define MGA1064_PIX_CLK_CTL_SEL_PCI		( 0x00 << 0 )
+#define MGA1064_PIX_CLK_CTL_SEL_PLL		( 0x01 << 0 )
+#define MGA1064_PIX_CLK_CTL_SEL_EXT		( 0x02 << 0 )
+#define MGA1064_PIX_CLK_CTL_SEL_MSK		( 0x03 << 0 )
+
+#define MGA1064_GEN_CTL		0x1d
+#define MGA1064_GEN_CTL_SYNC_ON_GREEN_DIS      (0x01 << 5)
+#define MGA1064_MISC_CTL	0x1e
+#define MGA1064_MISC_CTL_DAC_EN                ( 0x01 << 0 )
+#define MGA1064_MISC_CTL_VGA   		( 0x01 << 1 )
+#define MGA1064_MISC_CTL_DIS_CON   		( 0x03 << 1 )
+#define MGA1064_MISC_CTL_MAFC   		( 0x02 << 1 )
+#define MGA1064_MISC_CTL_VGA8   		( 0x01 << 3 )
+#define MGA1064_MISC_CTL_DAC_RAM_CS   		( 0x01 << 4 )
+
+#define MGA1064_GEN_IO_CTL2	0x29
+#define MGA1064_GEN_IO_CTL	0x2a
+#define MGA1064_GEN_IO_DATA	0x2b
+#define MGA1064_SYS_PLL_M	0x2c
+#define MGA1064_SYS_PLL_N	0x2d
+#define MGA1064_SYS_PLL_P	0x2e
+#define MGA1064_SYS_PLL_STAT	0x2f
+
+#define MGA1064_REMHEADCTL     0x30
+#define MGA1064_REMHEADCTL_CLKDIS ( 0x01 << 0 )
+#define MGA1064_REMHEADCTL_CLKSL_OFF ( 0x00 << 1 )
+#define MGA1064_REMHEADCTL_CLKSL_PLL ( 0x01 << 1 )
+#define MGA1064_REMHEADCTL_CLKSL_PCI ( 0x02 << 1 )
+#define MGA1064_REMHEADCTL_CLKSL_MSK ( 0x03 << 1 )
+
+#define MGA1064_REMHEADCTL2     0x31
+
+#define MGA1064_ZOOM_CTL	0x38
+#define MGA1064_SENSE_TST	0x3a
+
+#define MGA1064_CRC_LSB		0x3c
+#define MGA1064_CRC_MSB		0x3d
+#define MGA1064_CRC_CTL		0x3e
+#define MGA1064_COL_KEY_MSK_LSB		0x40
+#define MGA1064_COL_KEY_MSK_MSB		0x41
+#define MGA1064_COL_KEY_LSB		0x42
+#define MGA1064_COL_KEY_MSB		0x43
+#define MGA1064_PIX_PLLA_M	0x44
+#define MGA1064_PIX_PLLA_N	0x45
+#define MGA1064_PIX_PLLA_P	0x46
+#define MGA1064_PIX_PLLB_M	0x48
+#define MGA1064_PIX_PLLB_N	0x49
+#define MGA1064_PIX_PLLB_P	0x4a
+#define MGA1064_PIX_PLLC_M	0x4c
+#define MGA1064_PIX_PLLC_N	0x4d
+#define MGA1064_PIX_PLLC_P	0x4e
+
+#define MGA1064_PIX_PLL_STAT	0x4f
+
+/*Added for G450 dual head*/
+
+#define MGA1064_VID_PLL_STAT    0x8c
+#define MGA1064_VID_PLL_P       0x8D
+#define MGA1064_VID_PLL_M       0x8E
+#define MGA1064_VID_PLL_N       0x8F
+
+/* Modified PLL for G200 Winbond (G200WB) */
+#define MGA1064_WB_PIX_PLLC_M	0xb7
+#define MGA1064_WB_PIX_PLLC_N	0xb6
+#define MGA1064_WB_PIX_PLLC_P	0xb8
+
+/* Modified PLL for G200 Maxim (G200EV) */
+#define MGA1064_EV_PIX_PLLC_M	0xb6
+#define MGA1064_EV_PIX_PLLC_N	0xb7
+#define MGA1064_EV_PIX_PLLC_P	0xb8
+
+/* Modified PLL for G200 EH */
+#define MGA1064_EH_PIX_PLLC_M   0xb6
+#define MGA1064_EH_PIX_PLLC_N   0xb7
+#define MGA1064_EH_PIX_PLLC_P   0xb8
+
+/* Modified PLL for G200 Maxim (G200ER) */
+#define MGA1064_ER_PIX_PLLC_M	0xb7
+#define MGA1064_ER_PIX_PLLC_N	0xb6
+#define MGA1064_ER_PIX_PLLC_P	0xb8
+
+#define MGA1064_DISP_CTL        0x8a
+#define MGA1064_DISP_CTL_DAC1OUTSEL_MASK       0x01
+#define MGA1064_DISP_CTL_DAC1OUTSEL_DIS        0x00
+#define MGA1064_DISP_CTL_DAC1OUTSEL_EN         0x01
+#define MGA1064_DISP_CTL_DAC2OUTSEL_MASK       (0x03 << 2)
+#define MGA1064_DISP_CTL_DAC2OUTSEL_DIS        0x00
+#define MGA1064_DISP_CTL_DAC2OUTSEL_CRTC1      (0x01 << 2)
+#define MGA1064_DISP_CTL_DAC2OUTSEL_CRTC2      (0x02 << 2)
+#define MGA1064_DISP_CTL_DAC2OUTSEL_TVE        (0x03 << 2)
+#define MGA1064_DISP_CTL_PANOUTSEL_MASK        (0x03 << 5)
+#define MGA1064_DISP_CTL_PANOUTSEL_DIS         0x00
+#define MGA1064_DISP_CTL_PANOUTSEL_CRTC1       (0x01 << 5)
+#define MGA1064_DISP_CTL_PANOUTSEL_CRTC2RGB    (0x02 << 5)
+#define MGA1064_DISP_CTL_PANOUTSEL_CRTC2656    (0x03 << 5)
+
+#define MGA1064_SYNC_CTL        0x8b
+
+#define MGA1064_PWR_CTL         0xa0
+#define MGA1064_PWR_CTL_DAC2_EN                (0x01 << 0)
+#define MGA1064_PWR_CTL_VID_PLL_EN             (0x01 << 1)
+#define MGA1064_PWR_CTL_PANEL_EN               (0x01 << 2)
+#define MGA1064_PWR_CTL_RFIFO_EN               (0x01 << 3)
+#define MGA1064_PWR_CTL_CFIFO_EN               (0x01 << 4)
+
+#define MGA1064_PAN_CTL         0xa2
+
+/* Using crtc2 */
+#define MGAREG2_C2CTL            0x10
+#define MGAREG2_C2HPARAM         0x14
+#define MGAREG2_C2HSYNC          0x18
+#define MGAREG2_C2VPARAM         0x1c
+#define MGAREG2_C2VSYNC          0x20
+#define MGAREG2_C2STARTADD0      0x28
+
+#define MGAREG2_C2OFFSET         0x40
+#define MGAREG2_C2DATACTL        0x4c
+
+#define MGAREG_C2CTL            0x3c10
+#define MGAREG_C2CTL_C2_EN                     0x01
+
+#define MGAREG_C2_HIPRILVL_M                   (0x07 << 4)
+#define MGAREG_C2_MAXHIPRI_M                   (0x07 << 8)
+
+#define MGAREG_C2CTL_PIXCLKSEL_MASK            (0x03 << 1)
+#define MGAREG_C2CTL_PIXCLKSELH_MASK           (0x01 << 14)
+#define MGAREG_C2CTL_PIXCLKSEL_PCICLK          0x00
+#define MGAREG_C2CTL_PIXCLKSEL_VDOCLK          (0x01 << 1)
+#define MGAREG_C2CTL_PIXCLKSEL_PIXELPLL        (0x02 << 1)
+#define MGAREG_C2CTL_PIXCLKSEL_VIDEOPLL        (0x03 << 1)
+#define MGAREG_C2CTL_PIXCLKSEL_VDCLK           (0x01 << 14)
+
+#define MGAREG_C2CTL_PIXCLKSEL_CRISTAL         (0x01 << 1) | (0x01 << 14)
+#define MGAREG_C2CTL_PIXCLKSEL_SYSTEMPLL       (0x02 << 1) | (0x01 << 14)
+
+#define MGAREG_C2CTL_PIXCLKDIS_MASK            (0x01 << 3)
+#define MGAREG_C2CTL_PIXCLKDIS_DISABLE         (0x01 << 3)
+
+#define MGAREG_C2CTL_CRTCDACSEL_MASK           (0x01 << 20)
+#define MGAREG_C2CTL_CRTCDACSEL_CRTC1          0x00
+#define MGAREG_C2CTL_CRTCDACSEL_CRTC2          (0x01 << 20)
+
+#define MGAREG_C2HPARAM         0x3c14
+#define MGAREG_C2HSYNC          0x3c18
+#define MGAREG_C2VPARAM         0x3c1c
+#define MGAREG_C2VSYNC          0x3c20
+#define MGAREG_C2STARTADD0      0x3c28
+
+#define MGAREG_C2OFFSET         0x3c40
+#define MGAREG_C2DATACTL        0x3c4c
+
+/* video register */
+
+#define MGAREG_BESA1C3ORG	0x3d60
+#define MGAREG_BESA1CORG	0x3d10
+#define MGAREG_BESA1ORG		0x3d00
+#define MGAREG_BESCTL		0x3d20
+#define MGAREG_BESGLOBCTL	0x3dc0
+#define MGAREG_BESHCOORD	0x3d28
+#define MGAREG_BESHISCAL	0x3d30
+#define MGAREG_BESHSRCEND	0x3d3c
+#define MGAREG_BESHSRCLST	0x3d50
+#define MGAREG_BESHSRCST	0x3d38
+#define MGAREG_BESLUMACTL	0x3d40
+#define MGAREG_BESPITCH		0x3d24
+#define MGAREG_BESV1SRCLST	0x3d54
+#define MGAREG_BESV1WGHT	0x3d48
+#define MGAREG_BESVCOORD	0x3d2c
+#define MGAREG_BESVISCAL	0x3d34
+
+/* texture engine registers */
+
+#define MGAREG_TMR0		0x2c00
+#define MGAREG_TMR1		0x2c04
+#define MGAREG_TMR2		0x2c08
+#define MGAREG_TMR3		0x2c0c
+#define MGAREG_TMR4		0x2c10
+#define MGAREG_TMR5		0x2c14
+#define MGAREG_TMR6		0x2c18
+#define MGAREG_TMR7		0x2c1c
+#define MGAREG_TMR8		0x2c20
+#define MGAREG_TEXORG		0x2c24
+#define MGAREG_TEXWIDTH		0x2c28
+#define MGAREG_TEXHEIGHT	0x2c2c
+#define MGAREG_TEXCTL		0x2c30
+#    define MGA_TW4                             (0x00000000)
+#    define MGA_TW8                             (0x00000001)
+#    define MGA_TW15                            (0x00000002)
+#    define MGA_TW16                            (0x00000003)
+#    define MGA_TW12                            (0x00000004)
+#    define MGA_TW32                            (0x00000006)
+#    define MGA_TW8A                            (0x00000007)
+#    define MGA_TW8AL                           (0x00000008)
+#    define MGA_TW422                           (0x0000000A)
+#    define MGA_TW422UYVY                       (0x0000000B)
+#    define MGA_PITCHLIN                        (0x00000100)
+#    define MGA_NOPERSPECTIVE                   (0x00200000)
+#    define MGA_TAKEY                           (0x02000000)
+#    define MGA_TAMASK                          (0x04000000)
+#    define MGA_CLAMPUV                         (0x18000000)
+#    define MGA_TEXMODULATE                     (0x20000000)
+#define MGAREG_TEXCTL2		0x2c3c
+#    define MGA_G400_TC2_MAGIC                  (0x00008000)
+#    define MGA_TC2_DECALBLEND                  (0x00000001)
+#    define MGA_TC2_IDECAL                      (0x00000002)
+#    define MGA_TC2_DECALDIS                    (0x00000004)
+#    define MGA_TC2_CKSTRANSDIS                 (0x00000010)
+#    define MGA_TC2_BORDEREN                    (0x00000020)
+#    define MGA_TC2_SPECEN                      (0x00000040)
+#    define MGA_TC2_DUALTEX                     (0x00000080)
+#    define MGA_TC2_TABLEFOG                    (0x00000100)
+#    define MGA_TC2_BUMPMAP                     (0x00000200)
+#    define MGA_TC2_SELECT_TMU1                 (0x80000000)
+#define MGAREG_TEXTRANS		0x2c34
+#define MGAREG_TEXTRANSHIGH	0x2c38
+#define MGAREG_TEXFILTER	0x2c58
+#    define MGA_MIN_NRST                        (0x00000000)
+#    define MGA_MIN_BILIN                       (0x00000002)
+#    define MGA_MIN_ANISO                       (0x0000000D)
+#    define MGA_MAG_NRST                        (0x00000000)
+#    define MGA_MAG_BILIN                       (0x00000020)
+#    define MGA_FILTERALPHA                     (0x00100000)
+#define MGAREG_ALPHASTART	0x2c70
+#define MGAREG_ALPHAXINC	0x2c74
+#define MGAREG_ALPHAYINC	0x2c78
+#define MGAREG_ALPHACTRL	0x2c7c
+#    define MGA_SRC_ZERO                        (0x00000000)
+#    define MGA_SRC_ONE                         (0x00000001)
+#    define MGA_SRC_DST_COLOR                   (0x00000002)
+#    define MGA_SRC_ONE_MINUS_DST_COLOR         (0x00000003)
+#    define MGA_SRC_ALPHA                       (0x00000004)
+#    define MGA_SRC_ONE_MINUS_SRC_ALPHA         (0x00000005)
+#    define MGA_SRC_DST_ALPHA                   (0x00000006)
+#    define MGA_SRC_ONE_MINUS_DST_ALPHA         (0x00000007)
+#    define MGA_SRC_SRC_ALPHA_SATURATE          (0x00000008)
+#    define MGA_SRC_BLEND_MASK                  (0x0000000f)
+#    define MGA_DST_ZERO                        (0x00000000)
+#    define MGA_DST_ONE                         (0x00000010)
+#    define MGA_DST_SRC_COLOR                   (0x00000020)
+#    define MGA_DST_ONE_MINUS_SRC_COLOR         (0x00000030)
+#    define MGA_DST_SRC_ALPHA                   (0x00000040)
+#    define MGA_DST_ONE_MINUS_SRC_ALPHA         (0x00000050)
+#    define MGA_DST_DST_ALPHA                   (0x00000060)
+#    define MGA_DST_ONE_MINUS_DST_ALPHA         (0x00000070)
+#    define MGA_DST_BLEND_MASK                  (0x00000070)
+#    define MGA_ALPHACHANNEL                    (0x00000100)
+#    define MGA_VIDEOALPHA                      (0x00000200)
+#    define MGA_DIFFUSEDALPHA                   (0x01000000)
+#    define MGA_MODULATEDALPHA                  (0x02000000)
+#define MGAREG_TDUALSTAGE0                      (0x2CF8)
+#define MGAREG_TDUALSTAGE1                      (0x2CFC)
+#    define MGA_TDS_COLOR_ARG2_DIFFUSE          (0x00000000)
+#    define MGA_TDS_COLOR_ARG2_SPECULAR         (0x00000001)
+#    define MGA_TDS_COLOR_ARG2_FCOL             (0x00000002)
+#    define MGA_TDS_COLOR_ARG2_PREVSTAGE        (0x00000003)
+#    define MGA_TDS_COLOR_ALPHA_DIFFUSE         (0x00000000)
+#    define MGA_TDS_COLOR_ALPHA_FCOL            (0x00000004)
+#    define MGA_TDS_COLOR_ALPHA_CURRTEX         (0x00000008)
+#    define MGA_TDS_COLOR_ALPHA_PREVTEX         (0x0000000c)
+#    define MGA_TDS_COLOR_ALPHA_PREVSTAGE       (0x00000010)
+#    define MGA_TDS_COLOR_ARG1_REPLICATEALPHA   (0x00000020)
+#    define MGA_TDS_COLOR_ARG1_INV              (0x00000040)
+#    define MGA_TDS_COLOR_ARG2_REPLICATEALPHA   (0x00000080)
+#    define MGA_TDS_COLOR_ARG2_INV              (0x00000100)
+#    define MGA_TDS_COLOR_ALPHA1INV             (0x00000200)
+#    define MGA_TDS_COLOR_ALPHA2INV             (0x00000400)
+#    define MGA_TDS_COLOR_ARG1MUL_ALPHA1        (0x00000800)
+#    define MGA_TDS_COLOR_ARG2MUL_ALPHA2        (0x00001000)
+#    define MGA_TDS_COLOR_ARG1ADD_MULOUT        (0x00002000)
+#    define MGA_TDS_COLOR_ARG2ADD_MULOUT        (0x00004000)
+#    define MGA_TDS_COLOR_MODBRIGHT_2X          (0x00008000)
+#    define MGA_TDS_COLOR_MODBRIGHT_4X          (0x00010000)
+#    define MGA_TDS_COLOR_ADD_SUB               (0x00000000)
+#    define MGA_TDS_COLOR_ADD_ADD               (0x00020000)
+#    define MGA_TDS_COLOR_ADD2X                 (0x00040000)
+#    define MGA_TDS_COLOR_ADDBIAS               (0x00080000)
+#    define MGA_TDS_COLOR_BLEND                 (0x00100000)
+#    define MGA_TDS_COLOR_SEL_ARG1              (0x00000000)
+#    define MGA_TDS_COLOR_SEL_ARG2              (0x00200000)
+#    define MGA_TDS_COLOR_SEL_ADD               (0x00400000)
+#    define MGA_TDS_COLOR_SEL_MUL               (0x00600000)
+#    define MGA_TDS_ALPHA_ARG1_INV              (0x00800000)
+#    define MGA_TDS_ALPHA_ARG2_DIFFUSE          (0x00000000)
+#    define MGA_TDS_ALPHA_ARG2_FCOL             (0x01000000)
+#    define MGA_TDS_ALPHA_ARG2_PREVTEX          (0x02000000)
+#    define MGA_TDS_ALPHA_ARG2_PREVSTAGE        (0x03000000)
+#    define MGA_TDS_ALPHA_ARG2_INV              (0x04000000)
+#    define MGA_TDS_ALPHA_ADD                   (0x08000000)
+#    define MGA_TDS_ALPHA_ADDBIAS               (0x10000000)
+#    define MGA_TDS_ALPHA_ADD2X                 (0x20000000)
+#    define MGA_TDS_ALPHA_SEL_ARG1              (0x00000000)
+#    define MGA_TDS_ALPHA_SEL_ARG2              (0x40000000)
+#    define MGA_TDS_ALPHA_SEL_ADD               (0x80000000)
+#    define MGA_TDS_ALPHA_SEL_MUL               (0xc0000000)
+
+#define MGAREG_DWGSYNC		0x2c4c
+
+#define MGAREG_AGP_PLL		0x1e4c
+#define MGA_AGP2XPLL_ENABLE		0x1
+#define MGA_AGP2XPLL_DISABLE		0x0
+
+#endif
diff --git a/drivers/gpu/drm/mgag200/mgag200_ttm.c b/drivers/gpu/drm/mgag200/mgag200_ttm.c
new file mode 100644
index 000000000000..b223dcb7a710
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_ttm.c
@@ -0,0 +1,452 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "mgag200_drv.h"
+#include <ttm/ttm_page_alloc.h>
+
+static inline struct mga_device *
+mgag200_bdev(struct ttm_bo_device *bd)
+{
+	return container_of(bd, struct mga_device, ttm.bdev);
+}
+
+static int
+mgag200_ttm_mem_global_init(struct drm_global_reference *ref)
+{
+	return ttm_mem_global_init(ref->object);
+}
+
+static void
+mgag200_ttm_mem_global_release(struct drm_global_reference *ref)
+{
+	ttm_mem_global_release(ref->object);
+}
+
+static int mgag200_ttm_global_init(struct mga_device *ast)
+{
+	struct drm_global_reference *global_ref;
+	int r;
+
+	global_ref = &ast->ttm.mem_global_ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_MEM;
+	global_ref->size = sizeof(struct ttm_mem_global);
+	global_ref->init = &mgag200_ttm_mem_global_init;
+	global_ref->release = &mgag200_ttm_mem_global_release;
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM memory accounting "
+			  "subsystem.\n");
+		return r;
+	}
+
+	ast->ttm.bo_global_ref.mem_glob =
+		ast->ttm.mem_global_ref.object;
+	global_ref = &ast->ttm.bo_global_ref.ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_BO;
+	global_ref->size = sizeof(struct ttm_bo_global);
+	global_ref->init = &ttm_bo_global_init;
+	global_ref->release = &ttm_bo_global_release;
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM BO subsystem.\n");
+		drm_global_item_unref(&ast->ttm.mem_global_ref);
+		return r;
+	}
+	return 0;
+}
+
+void
+mgag200_ttm_global_release(struct mga_device *ast)
+{
+	if (ast->ttm.mem_global_ref.release == NULL)
+		return;
+
+	drm_global_item_unref(&ast->ttm.bo_global_ref.ref);
+	drm_global_item_unref(&ast->ttm.mem_global_ref);
+	ast->ttm.mem_global_ref.release = NULL;
+}
+
+
+static void mgag200_bo_ttm_destroy(struct ttm_buffer_object *tbo)
+{
+	struct mgag200_bo *bo;
+
+	bo = container_of(tbo, struct mgag200_bo, bo);
+
+	drm_gem_object_release(&bo->gem);
+	kfree(bo);
+}
+
+bool mgag200_ttm_bo_is_mgag200_bo(struct ttm_buffer_object *bo)
+{
+	if (bo->destroy == &mgag200_bo_ttm_destroy)
+		return true;
+	return false;
+}
+
+static int
+mgag200_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
+		     struct ttm_mem_type_manager *man)
+{
+	switch (type) {
+	case TTM_PL_SYSTEM:
+		man->flags = TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_MASK_CACHING;
+		man->default_caching = TTM_PL_FLAG_CACHED;
+		break;
+	case TTM_PL_VRAM:
+		man->func = &ttm_bo_manager_func;
+		man->flags = TTM_MEMTYPE_FLAG_FIXED |
+			TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_FLAG_UNCACHED |
+			TTM_PL_FLAG_WC;
+		man->default_caching = TTM_PL_FLAG_WC;
+		break;
+	default:
+		DRM_ERROR("Unsupported memory type %u\n", (unsigned)type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void
+mgag200_bo_evict_flags(struct ttm_buffer_object *bo, struct ttm_placement *pl)
+{
+	struct mgag200_bo *mgabo = mgag200_bo(bo);
+
+	if (!mgag200_ttm_bo_is_mgag200_bo(bo))
+		return;
+
+	mgag200_ttm_placement(mgabo, TTM_PL_FLAG_SYSTEM);
+	*pl = mgabo->placement;
+}
+
+static int mgag200_bo_verify_access(struct ttm_buffer_object *bo, struct file *filp)
+{
+	return 0;
+}
+
+static int mgag200_ttm_io_mem_reserve(struct ttm_bo_device *bdev,
+				  struct ttm_mem_reg *mem)
+{
+	struct ttm_mem_type_manager *man = &bdev->man[mem->mem_type];
+	struct mga_device *mdev = mgag200_bdev(bdev);
+
+	mem->bus.addr = NULL;
+	mem->bus.offset = 0;
+	mem->bus.size = mem->num_pages << PAGE_SHIFT;
+	mem->bus.base = 0;
+	mem->bus.is_iomem = false;
+	if (!(man->flags & TTM_MEMTYPE_FLAG_MAPPABLE))
+		return -EINVAL;
+	switch (mem->mem_type) {
+	case TTM_PL_SYSTEM:
+		/* system memory */
+		return 0;
+	case TTM_PL_VRAM:
+		mem->bus.offset = mem->start << PAGE_SHIFT;
+		mem->bus.base = pci_resource_start(mdev->dev->pdev, 0);
+		mem->bus.is_iomem = true;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
+
+static void mgag200_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
+{
+}
+
+static int mgag200_bo_move(struct ttm_buffer_object *bo,
+		       bool evict, bool interruptible,
+		       bool no_wait_reserve, bool no_wait_gpu,
+		       struct ttm_mem_reg *new_mem)
+{
+	int r;
+	r = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
+	return r;
+}
+
+
+static void mgag200_ttm_backend_destroy(struct ttm_tt *tt)
+{
+	ttm_tt_fini(tt);
+	kfree(tt);
+}
+
+static struct ttm_backend_func mgag200_tt_backend_func = {
+	.destroy = &mgag200_ttm_backend_destroy,
+};
+
+
+struct ttm_tt *mgag200_ttm_tt_create(struct ttm_bo_device *bdev,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
+{
+	struct ttm_tt *tt;
+
+	tt = kzalloc(sizeof(struct ttm_tt), GFP_KERNEL);
+	if (tt == NULL)
+		return NULL;
+	tt->func = &mgag200_tt_backend_func;
+	if (ttm_tt_init(tt, bdev, size, page_flags, dummy_read_page)) {
+		kfree(tt);
+		return NULL;
+	}
+	return tt;
+}
+
+static int mgag200_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	return ttm_pool_populate(ttm);
+}
+
+static void mgag200_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
+}
+
+struct ttm_bo_driver mgag200_bo_driver = {
+	.ttm_tt_create = mgag200_ttm_tt_create,
+	.ttm_tt_populate = mgag200_ttm_tt_populate,
+	.ttm_tt_unpopulate = mgag200_ttm_tt_unpopulate,
+	.init_mem_type = mgag200_bo_init_mem_type,
+	.evict_flags = mgag200_bo_evict_flags,
+	.move = mgag200_bo_move,
+	.verify_access = mgag200_bo_verify_access,
+	.io_mem_reserve = &mgag200_ttm_io_mem_reserve,
+	.io_mem_free = &mgag200_ttm_io_mem_free,
+};
+
+int mgag200_mm_init(struct mga_device *mdev)
+{
+	int ret;
+	struct drm_device *dev = mdev->dev;
+	struct ttm_bo_device *bdev = &mdev->ttm.bdev;
+
+	ret = mgag200_ttm_global_init(mdev);
+	if (ret)
+		return ret;
+
+	ret = ttm_bo_device_init(&mdev->ttm.bdev,
+				 mdev->ttm.bo_global_ref.ref.object,
+				 &mgag200_bo_driver, DRM_FILE_PAGE_OFFSET,
+				 true);
+	if (ret) {
+		DRM_ERROR("Error initialising bo driver; %d\n", ret);
+		return ret;
+	}
+
+	ret = ttm_bo_init_mm(bdev, TTM_PL_VRAM, mdev->mc.vram_size >> PAGE_SHIFT);
+	if (ret) {
+		DRM_ERROR("Failed ttm VRAM init: %d\n", ret);
+		return ret;
+	}
+
+	mdev->fb_mtrr = drm_mtrr_add(pci_resource_start(dev->pdev, 0),
+				    pci_resource_len(dev->pdev, 0),
+				    DRM_MTRR_WC);
+
+	return 0;
+}
+
+void mgag200_mm_fini(struct mga_device *mdev)
+{
+	struct drm_device *dev = mdev->dev;
+	ttm_bo_device_release(&mdev->ttm.bdev);
+
+	mgag200_ttm_global_release(mdev);
+
+	if (mdev->fb_mtrr >= 0) {
+		drm_mtrr_del(mdev->fb_mtrr,
+			     pci_resource_start(dev->pdev, 0),
+			     pci_resource_len(dev->pdev, 0), DRM_MTRR_WC);
+		mdev->fb_mtrr = -1;
+	}
+}
+
+void mgag200_ttm_placement(struct mgag200_bo *bo, int domain)
+{
+	u32 c = 0;
+	bo->placement.fpfn = 0;
+	bo->placement.lpfn = 0;
+	bo->placement.placement = bo->placements;
+	bo->placement.busy_placement = bo->placements;
+	if (domain & TTM_PL_FLAG_VRAM)
+		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+	if (domain & TTM_PL_FLAG_SYSTEM)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	if (!c)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	bo->placement.num_placement = c;
+	bo->placement.num_busy_placement = c;
+}
+
+int mgag200_bo_reserve(struct mgag200_bo *bo, bool no_wait)
+{
+	int ret;
+
+	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("reserve failed %p\n", bo);
+		return ret;
+	}
+	return 0;
+}
+
+void mgag200_bo_unreserve(struct mgag200_bo *bo)
+{
+	ttm_bo_unreserve(&bo->bo);
+}
+
+int mgag200_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct mgag200_bo **pmgabo)
+{
+	struct mga_device *mdev = dev->dev_private;
+	struct mgag200_bo *mgabo;
+	size_t acc_size;
+	int ret;
+
+	mgabo = kzalloc(sizeof(struct mgag200_bo), GFP_KERNEL);
+	if (!mgabo)
+		return -ENOMEM;
+
+	ret = drm_gem_object_init(dev, &mgabo->gem, size);
+	if (ret) {
+		kfree(mgabo);
+		return ret;
+	}
+
+	mgabo->gem.driver_private = NULL;
+	mgabo->bo.bdev = &mdev->ttm.bdev;
+
+	mgag200_ttm_placement(mgabo, TTM_PL_FLAG_VRAM | TTM_PL_FLAG_SYSTEM);
+
+	acc_size = ttm_bo_dma_acc_size(&mdev->ttm.bdev, size,
+				       sizeof(struct mgag200_bo));
+
+	ret = ttm_bo_init(&mdev->ttm.bdev, &mgabo->bo, size,
+			  ttm_bo_type_device, &mgabo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  NULL, mgag200_bo_ttm_destroy);
+	if (ret)
+		return ret;
+
+	*pmgabo = mgabo;
+	return 0;
+}
+
+static inline u64 mgag200_bo_gpu_offset(struct mgag200_bo *bo)
+{
+	return bo->bo.offset;
+}
+
+int mgag200_bo_pin(struct mgag200_bo *bo, u32 pl_flag, u64 *gpu_addr)
+{
+	int i, ret;
+
+	if (bo->pin_count) {
+		bo->pin_count++;
+		if (gpu_addr)
+			*gpu_addr = mgag200_bo_gpu_offset(bo);
+	}
+
+	mgag200_ttm_placement(bo, pl_flag);
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	bo->pin_count = 1;
+	if (gpu_addr)
+		*gpu_addr = mgag200_bo_gpu_offset(bo);
+	return 0;
+}
+
+int mgag200_bo_unpin(struct mgag200_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int mgag200_bo_push_sysram(struct mgag200_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	if (bo->kmap.virtual)
+		ttm_bo_kunmap(&bo->kmap);
+
+	mgag200_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret) {
+		DRM_ERROR("pushing to VRAM failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+int mgag200_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *file_priv;
+	struct mga_device *mdev;
+
+	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
+		return drm_mmap(filp, vma);
+
+	file_priv = filp->private_data;
+	mdev = file_priv->minor->dev->dev_private;
+	return ttm_bo_mmap(filp, vma, &mdev->ttm.bdev);
+}
diff --git a/drivers/gpu/drm/nouveau/Makefile b/drivers/gpu/drm/nouveau/Makefile
index 1a2ad7eb1734..fe5267d06ab5 100644
--- a/drivers/gpu/drm/nouveau/Makefile
+++ b/drivers/gpu/drm/nouveau/Makefile
@@ -16,10 +16,13 @@ nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
              nv04_mc.o nv40_mc.o nv50_mc.o \
              nv04_fb.o nv10_fb.o nv20_fb.o nv30_fb.o nv40_fb.o \
              nv50_fb.o nvc0_fb.o \
-             nv04_fifo.o nv10_fifo.o nv40_fifo.o nv50_fifo.o nvc0_fifo.o \
+             nv04_fifo.o nv10_fifo.o nv17_fifo.o nv40_fifo.o nv50_fifo.o \
+             nv84_fifo.o nvc0_fifo.o nve0_fifo.o \
+             nv04_fence.o nv10_fence.o nv84_fence.o nvc0_fence.o \
+             nv04_software.o nv50_software.o nvc0_software.o \
              nv04_graph.o nv10_graph.o nv20_graph.o \
-             nv40_graph.o nv50_graph.o nvc0_graph.o \
-             nv40_grctx.o nv50_grctx.o nvc0_grctx.o \
+             nv40_graph.o nv50_graph.o nvc0_graph.o nve0_graph.o \
+             nv40_grctx.o nv50_grctx.o nvc0_grctx.o nve0_grctx.o \
              nv84_crypt.o nv98_crypt.o \
              nva3_copy.o nvc0_copy.o \
              nv31_mpeg.o nv50_mpeg.o \
@@ -37,7 +40,7 @@ nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
 	     nv50_calc.o \
 	     nv04_pm.o nv40_pm.o nv50_pm.o nva3_pm.o nvc0_pm.o \
 	     nv50_vram.o nvc0_vram.o \
-	     nv50_vm.o nvc0_vm.o
+	     nv50_vm.o nvc0_vm.o nouveau_prime.o
 
 nouveau-$(CONFIG_DRM_NOUVEAU_DEBUG) += nouveau_debugfs.o
 nouveau-$(CONFIG_COMPAT) += nouveau_ioc32.o
diff --git a/drivers/gpu/drm/nouveau/nouveau_acpi.c b/drivers/gpu/drm/nouveau/nouveau_acpi.c
index 284bd25d5d21..fc841e87b343 100644
--- a/drivers/gpu/drm/nouveau/nouveau_acpi.c
+++ b/drivers/gpu/drm/nouveau/nouveau_acpi.c
@@ -338,7 +338,8 @@ void nouveau_switcheroo_optimus_dsm(void)
 
 void nouveau_unregister_dsm_handler(void)
 {
-	vga_switcheroo_unregister_handler();
+	if (nouveau_dsm_priv.optimus_detected || nouveau_dsm_priv.dsm_detected)
+		vga_switcheroo_unregister_handler();
 }
 
 /* retrieve the ROM in 4k blocks */
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index 0be4a815e706..2f11e16a81a9 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -30,6 +30,7 @@
 #include "nouveau_gpio.h"
 
 #include <linux/io-mapping.h>
+#include <linux/firmware.h>
 
 /* these defines are made up */
 #define NV_CIO_CRE_44_HEADA 0x0
@@ -195,35 +196,24 @@ static void
 bios_shadow_acpi(struct nvbios *bios)
 {
 	struct pci_dev *pdev = bios->dev->pdev;
-	int ptr, len, ret;
-	u8 data[3];
+	int cnt = 65536 / ROM_BIOS_PAGE;
+	int ret;
 
 	if (!nouveau_acpi_rom_supported(pdev))
 		return;
 
-	ret = nouveau_acpi_get_bios_chunk(data, 0, sizeof(data));
-	if (ret != sizeof(data))
-		return;
-
-	bios->length = min(data[2] * 512, 65536);
-	bios->data = kmalloc(bios->length, GFP_KERNEL);
+	bios->data = kmalloc(cnt * ROM_BIOS_PAGE, GFP_KERNEL);
 	if (!bios->data)
 		return;
 
-	len = bios->length;
-	ptr = 0;
-	while (len) {
-		int size = (len > ROM_BIOS_PAGE) ? ROM_BIOS_PAGE : len;
-
-		ret = nouveau_acpi_get_bios_chunk(bios->data, ptr, size);
-		if (ret != size) {
-			kfree(bios->data);
-			bios->data = NULL;
+	bios->length = 0;
+	while (cnt--) {
+		ret = nouveau_acpi_get_bios_chunk(bios->data, bios->length,
+						  ROM_BIOS_PAGE);
+		if (ret != ROM_BIOS_PAGE)
 			return;
-		}
 
-		len -= size;
-		ptr += size;
+		bios->length += ROM_BIOS_PAGE;
 	}
 }
 
@@ -249,8 +239,12 @@ bios_shadow(struct drm_device *dev)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nvbios *bios = &dev_priv->vbios;
 	struct methods *mthd, *best;
+	const struct firmware *fw;
+	char fname[32];
+	int ret;
 
 	if (nouveau_vbios) {
+		/* try to match one of the built-in methods */
 		mthd = shadow_methods;
 		do {
 			if (strcasecmp(nouveau_vbios, mthd->desc))
@@ -263,6 +257,22 @@ bios_shadow(struct drm_device *dev)
 				return true;
 		} while ((++mthd)->shadow);
 
+		/* attempt to load firmware image */
+		snprintf(fname, sizeof(fname), "nouveau/%s", nouveau_vbios);
+		ret = request_firmware(&fw, fname, &dev->pdev->dev);
+		if (ret == 0) {
+			bios->length = fw->size;
+			bios->data   = kmemdup(fw->data, fw->size, GFP_KERNEL);
+			release_firmware(fw);
+
+			NV_INFO(dev, "VBIOS image: %s\n", nouveau_vbios);
+			if (score_vbios(bios, 1))
+				return true;
+
+			kfree(bios->data);
+			bios->data = NULL;
+		}
+
 		NV_ERROR(dev, "VBIOS source \'%s\' invalid\n", nouveau_vbios);
 	}
 
@@ -273,6 +283,7 @@ bios_shadow(struct drm_device *dev)
 		mthd->score = score_vbios(bios, mthd->rw);
 		mthd->size = bios->length;
 		mthd->data = bios->data;
+		bios->data = NULL;
 	} while (mthd->score != 3 && (++mthd)->shadow);
 
 	mthd = shadow_methods;
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 7d15a774f9c9..7f80ed523562 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -35,6 +35,8 @@
 #include "nouveau_dma.h"
 #include "nouveau_mm.h"
 #include "nouveau_vm.h"
+#include "nouveau_fence.h"
+#include "nouveau_ramht.h"
 
 #include <linux/log2.h>
 #include <linux/slab.h>
@@ -89,12 +91,17 @@ nouveau_bo_fixup_align(struct nouveau_bo *nvbo, u32 flags,
 int
 nouveau_bo_new(struct drm_device *dev, int size, int align,
 	       uint32_t flags, uint32_t tile_mode, uint32_t tile_flags,
+	       struct sg_table *sg,
 	       struct nouveau_bo **pnvbo)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_bo *nvbo;
 	size_t acc_size;
 	int ret;
+	int type = ttm_bo_type_device;
+
+	if (sg)
+		type = ttm_bo_type_sg;
 
 	nvbo = kzalloc(sizeof(struct nouveau_bo), GFP_KERNEL);
 	if (!nvbo)
@@ -120,8 +127,8 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 				       sizeof(struct nouveau_bo));
 
 	ret = ttm_bo_init(&dev_priv->ttm.bdev, &nvbo->bo, size,
-			  ttm_bo_type_device, &nvbo->placement,
-			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  type, &nvbo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size, sg,
 			  nouveau_bo_del_ttm);
 	if (ret) {
 		/* ttm will call nouveau_bo_del_ttm if it fails.. */
@@ -473,7 +480,7 @@ nouveau_bo_move_accel_cleanup(struct nouveau_channel *chan,
 	struct nouveau_fence *fence = NULL;
 	int ret;
 
-	ret = nouveau_fence_new(chan, &fence, true);
+	ret = nouveau_fence_new(chan, &fence);
 	if (ret)
 		return ret;
 
@@ -484,6 +491,76 @@ nouveau_bo_move_accel_cleanup(struct nouveau_channel *chan,
 }
 
 static int
+nve0_bo_move_copy(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	int ret = RING_SPACE(chan, 10);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, NvSubCopy, 0x0400, 8);
+		OUT_RING  (chan, upper_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, upper_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, new_mem->num_pages);
+		BEGIN_IMC0(chan, NvSubCopy, 0x0300, 0x0386);
+	}
+	return ret;
+}
+
+static int
+nvc0_bo_move_init(struct nouveau_channel *chan, u32 handle)
+{
+	int ret = RING_SPACE(chan, 2);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, NvSubCopy, 0x0000, 1);
+		OUT_RING  (chan, handle);
+	}
+	return ret;
+}
+
+static int
+nvc0_bo_move_copy(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	u64 src_offset = node->vma[0].offset;
+	u64 dst_offset = node->vma[1].offset;
+	u32 page_count = new_mem->num_pages;
+	int ret;
+
+	page_count = new_mem->num_pages;
+	while (page_count) {
+		int line_count = (page_count > 8191) ? 8191 : page_count;
+
+		ret = RING_SPACE(chan, 11);
+		if (ret)
+			return ret;
+
+		BEGIN_NVC0(chan, NvSubCopy, 0x030c, 8);
+		OUT_RING  (chan, upper_32_bits(src_offset));
+		OUT_RING  (chan, lower_32_bits(src_offset));
+		OUT_RING  (chan, upper_32_bits(dst_offset));
+		OUT_RING  (chan, lower_32_bits(dst_offset));
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, line_count);
+		BEGIN_NVC0(chan, NvSubCopy, 0x0300, 1);
+		OUT_RING  (chan, 0x00000110);
+
+		page_count -= line_count;
+		src_offset += (PAGE_SIZE * line_count);
+		dst_offset += (PAGE_SIZE * line_count);
+	}
+
+	return 0;
+}
+
+static int
 nvc0_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
 {
@@ -501,17 +578,17 @@ nvc0_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		if (ret)
 			return ret;
 
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0238, 2);
+		BEGIN_NVC0(chan, NvSubCopy, 0x0238, 2);
 		OUT_RING  (chan, upper_32_bits(dst_offset));
 		OUT_RING  (chan, lower_32_bits(dst_offset));
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x030c, 6);
+		BEGIN_NVC0(chan, NvSubCopy, 0x030c, 6);
 		OUT_RING  (chan, upper_32_bits(src_offset));
 		OUT_RING  (chan, lower_32_bits(src_offset));
 		OUT_RING  (chan, PAGE_SIZE); /* src_pitch */
 		OUT_RING  (chan, PAGE_SIZE); /* dst_pitch */
 		OUT_RING  (chan, PAGE_SIZE); /* line_length */
 		OUT_RING  (chan, line_count);
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0300, 1);
+		BEGIN_NVC0(chan, NvSubCopy, 0x0300, 1);
 		OUT_RING  (chan, 0x00100110);
 
 		page_count -= line_count;
@@ -523,6 +600,102 @@ nvc0_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 }
 
 static int
+nva3_bo_move_copy(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	u64 src_offset = node->vma[0].offset;
+	u64 dst_offset = node->vma[1].offset;
+	u32 page_count = new_mem->num_pages;
+	int ret;
+
+	page_count = new_mem->num_pages;
+	while (page_count) {
+		int line_count = (page_count > 8191) ? 8191 : page_count;
+
+		ret = RING_SPACE(chan, 11);
+		if (ret)
+			return ret;
+
+		BEGIN_NV04(chan, NvSubCopy, 0x030c, 8);
+		OUT_RING  (chan, upper_32_bits(src_offset));
+		OUT_RING  (chan, lower_32_bits(src_offset));
+		OUT_RING  (chan, upper_32_bits(dst_offset));
+		OUT_RING  (chan, lower_32_bits(dst_offset));
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, line_count);
+		BEGIN_NV04(chan, NvSubCopy, 0x0300, 1);
+		OUT_RING  (chan, 0x00000110);
+
+		page_count -= line_count;
+		src_offset += (PAGE_SIZE * line_count);
+		dst_offset += (PAGE_SIZE * line_count);
+	}
+
+	return 0;
+}
+
+static int
+nv98_bo_move_exec(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, NvSubCopy, 0x0320, 6);
+		OUT_RING  (chan, upper_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, upper_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, 0x00000000 /* COPY */);
+		OUT_RING  (chan, new_mem->num_pages << PAGE_SHIFT);
+	}
+	return ret;
+}
+
+static int
+nv84_bo_move_exec(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, NvSubCopy, 0x0304, 6);
+		OUT_RING  (chan, new_mem->num_pages << PAGE_SHIFT);
+		OUT_RING  (chan, upper_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, upper_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, 0x00000000 /* MODE_COPY, QUERY_NONE */);
+	}
+	return ret;
+}
+
+static int
+nv50_bo_move_init(struct nouveau_channel *chan, u32 handle)
+{
+	int ret = nouveau_notifier_alloc(chan, NvNotify0, 32, 0xfe0, 0x1000,
+					 &chan->m2mf_ntfy);
+	if (ret == 0) {
+		ret = RING_SPACE(chan, 6);
+		if (ret == 0) {
+			BEGIN_NV04(chan, NvSubCopy, 0x0000, 1);
+			OUT_RING  (chan, handle);
+			BEGIN_NV04(chan, NvSubCopy, 0x0180, 3);
+			OUT_RING  (chan, NvNotify0);
+			OUT_RING  (chan, NvDmaFB);
+			OUT_RING  (chan, NvDmaFB);
+		} else {
+			nouveau_ramht_remove(chan, NvNotify0);
+		}
+	}
+
+	return ret;
+}
+
+static int
 nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
 {
@@ -546,7 +719,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x0200, 7);
+			BEGIN_NV04(chan, NvSubCopy, 0x0200, 7);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, stride);
@@ -559,7 +732,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x0200, 1);
+			BEGIN_NV04(chan, NvSubCopy, 0x0200, 1);
 			OUT_RING  (chan, 1);
 		}
 		if (old_mem->mem_type == TTM_PL_VRAM &&
@@ -568,7 +741,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x021c, 7);
+			BEGIN_NV04(chan, NvSubCopy, 0x021c, 7);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, stride);
@@ -581,7 +754,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x021c, 1);
+			BEGIN_NV04(chan, NvSubCopy, 0x021c, 1);
 			OUT_RING  (chan, 1);
 		}
 
@@ -589,10 +762,10 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		if (ret)
 			return ret;
 
-		BEGIN_RING(chan, NvSubM2MF, 0x0238, 2);
+		BEGIN_NV04(chan, NvSubCopy, 0x0238, 2);
 		OUT_RING  (chan, upper_32_bits(src_offset));
 		OUT_RING  (chan, upper_32_bits(dst_offset));
-		BEGIN_RING(chan, NvSubM2MF, 0x030c, 8);
+		BEGIN_NV04(chan, NvSubCopy, 0x030c, 8);
 		OUT_RING  (chan, lower_32_bits(src_offset));
 		OUT_RING  (chan, lower_32_bits(dst_offset));
 		OUT_RING  (chan, stride);
@@ -601,7 +774,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		OUT_RING  (chan, height);
 		OUT_RING  (chan, 0x00000101);
 		OUT_RING  (chan, 0x00000000);
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
+		BEGIN_NV04(chan, NvSubCopy, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
 		OUT_RING  (chan, 0);
 
 		length -= amount;
@@ -612,6 +785,24 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 	return 0;
 }
 
+static int
+nv04_bo_move_init(struct nouveau_channel *chan, u32 handle)
+{
+	int ret = nouveau_notifier_alloc(chan, NvNotify0, 32, 0xfe0, 0x1000,
+					 &chan->m2mf_ntfy);
+	if (ret == 0) {
+		ret = RING_SPACE(chan, 4);
+		if (ret == 0) {
+			BEGIN_NV04(chan, NvSubCopy, 0x0000, 1);
+			OUT_RING  (chan, handle);
+			BEGIN_NV04(chan, NvSubCopy, 0x0180, 1);
+			OUT_RING  (chan, NvNotify0);
+		}
+	}
+
+	return ret;
+}
+
 static inline uint32_t
 nouveau_bo_mem_ctxdma(struct ttm_buffer_object *bo,
 		      struct nouveau_channel *chan, struct ttm_mem_reg *mem)
@@ -634,7 +825,7 @@ nv04_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_DMA_SOURCE, 2);
+	BEGIN_NV04(chan, NvSubCopy, NV_MEMORY_TO_MEMORY_FORMAT_DMA_SOURCE, 2);
 	OUT_RING  (chan, nouveau_bo_mem_ctxdma(bo, chan, old_mem));
 	OUT_RING  (chan, nouveau_bo_mem_ctxdma(bo, chan, new_mem));
 
@@ -646,7 +837,7 @@ nv04_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		if (ret)
 			return ret;
 
-		BEGIN_RING(chan, NvSubM2MF,
+		BEGIN_NV04(chan, NvSubCopy,
 				 NV_MEMORY_TO_MEMORY_FORMAT_OFFSET_IN, 8);
 		OUT_RING  (chan, src_offset);
 		OUT_RING  (chan, dst_offset);
@@ -656,7 +847,7 @@ nv04_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		OUT_RING  (chan, line_count);
 		OUT_RING  (chan, 0x00000101);
 		OUT_RING  (chan, 0x00000000);
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
+		BEGIN_NV04(chan, NvSubCopy, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
 		OUT_RING  (chan, 0);
 
 		page_count -= line_count;
@@ -716,13 +907,7 @@ nouveau_bo_move_m2mf(struct ttm_buffer_object *bo, int evict, bool intr,
 			goto out;
 	}
 
-	if (dev_priv->card_type < NV_50)
-		ret = nv04_bo_move_m2mf(chan, bo, &bo->mem, new_mem);
-	else
-	if (dev_priv->card_type < NV_C0)
-		ret = nv50_bo_move_m2mf(chan, bo, &bo->mem, new_mem);
-	else
-		ret = nvc0_bo_move_m2mf(chan, bo, &bo->mem, new_mem);
+	ret = dev_priv->ttm.move(chan, bo, &bo->mem, new_mem);
 	if (ret == 0) {
 		ret = nouveau_bo_move_accel_cleanup(chan, nvbo, evict,
 						    no_wait_reserve,
@@ -734,6 +919,49 @@ out:
 	return ret;
 }
 
+void
+nouveau_bo_move_init(struct nouveau_channel *chan)
+{
+	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
+	static const struct {
+		const char *name;
+		int engine;
+		u32 oclass;
+		int (*exec)(struct nouveau_channel *,
+			    struct ttm_buffer_object *,
+			    struct ttm_mem_reg *, struct ttm_mem_reg *);
+		int (*init)(struct nouveau_channel *, u32 handle);
+	} _methods[] = {
+		{  "COPY", 0, 0xa0b5, nve0_bo_move_copy, nvc0_bo_move_init },
+		{ "COPY1", 5, 0x90b8, nvc0_bo_move_copy, nvc0_bo_move_init },
+		{ "COPY0", 4, 0x90b5, nvc0_bo_move_copy, nvc0_bo_move_init },
+		{  "COPY", 0, 0x85b5, nva3_bo_move_copy, nv50_bo_move_init },
+		{ "CRYPT", 0, 0x74c1, nv84_bo_move_exec, nv50_bo_move_init },
+		{  "M2MF", 0, 0x9039, nvc0_bo_move_m2mf, nvc0_bo_move_init },
+		{  "M2MF", 0, 0x5039, nv50_bo_move_m2mf, nv50_bo_move_init },
+		{  "M2MF", 0, 0x0039, nv04_bo_move_m2mf, nv04_bo_move_init },
+		{},
+		{ "CRYPT", 0, 0x88b4, nv98_bo_move_exec, nv50_bo_move_init },
+	}, *mthd = _methods;
+	const char *name = "CPU";
+	int ret;
+
+	do {
+		u32 handle = (mthd->engine << 16) | mthd->oclass;
+		ret = nouveau_gpuobj_gr_new(chan, handle, mthd->oclass);
+		if (ret == 0) {
+			ret = mthd->init(chan, handle);
+			if (ret == 0) {
+				dev_priv->ttm.move = mthd->exec;
+				name = mthd->name;
+				break;
+			}
+		}
+	} while ((++mthd)->exec);
+
+	NV_INFO(chan->dev, "MM: using %s for buffer copies\n", name);
+}
+
 static int
 nouveau_bo_move_flipd(struct ttm_buffer_object *bo, bool evict, bool intr,
 		      bool no_wait_reserve, bool no_wait_gpu,
@@ -817,9 +1045,14 @@ nouveau_bo_move_ntfy(struct ttm_buffer_object *bo, struct ttm_mem_reg *new_mem)
 		} else
 		if (new_mem && new_mem->mem_type == TTM_PL_TT &&
 		    nvbo->page_shift == vma->vm->spg_shift) {
-			nouveau_vm_map_sg(vma, 0, new_mem->
-					  num_pages << PAGE_SHIFT,
-					  new_mem->mm_node);
+			if (((struct nouveau_mem *)new_mem->mm_node)->sg)
+				nouveau_vm_map_sg_table(vma, 0, new_mem->
+						  num_pages << PAGE_SHIFT,
+						  new_mem->mm_node);
+			else
+				nouveau_vm_map_sg(vma, 0, new_mem->
+						  num_pages << PAGE_SHIFT,
+						  new_mem->mm_node);
 		} else {
 			nouveau_vm_unmap(vma);
 		}
@@ -885,8 +1118,8 @@ nouveau_bo_move(struct ttm_buffer_object *bo, bool evict, bool intr,
 		goto out;
 	}
 
-	/* Software copy if the card isn't up and running yet. */
-	if (!dev_priv->channel) {
+	/* CPU copy if we have no accelerated method available */
+	if (!dev_priv->ttm.move) {
 		ret = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
 		goto out;
 	}
@@ -1030,26 +1263,10 @@ nouveau_ttm_fault_reserve_notify(struct ttm_buffer_object *bo)
 
 	nvbo->placement.fpfn = 0;
 	nvbo->placement.lpfn = dev_priv->fb_mappable_pages;
-	nouveau_bo_placement_set(nvbo, TTM_PL_VRAM, 0);
+	nouveau_bo_placement_set(nvbo, TTM_PL_FLAG_VRAM, 0);
 	return nouveau_bo_validate(nvbo, false, true, false);
 }
 
-void
-nouveau_bo_fence(struct nouveau_bo *nvbo, struct nouveau_fence *fence)
-{
-	struct nouveau_fence *old_fence;
-
-	if (likely(fence))
-		nouveau_fence_ref(fence);
-
-	spin_lock(&nvbo->bo.bdev->fence_lock);
-	old_fence = nvbo->bo.sync_obj;
-	nvbo->bo.sync_obj = fence;
-	spin_unlock(&nvbo->bo.bdev->fence_lock);
-
-	nouveau_fence_unref(&old_fence);
-}
-
 static int
 nouveau_ttm_tt_populate(struct ttm_tt *ttm)
 {
@@ -1058,10 +1275,19 @@ nouveau_ttm_tt_populate(struct ttm_tt *ttm)
 	struct drm_device *dev;
 	unsigned i;
 	int r;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
 
 	if (ttm->state != tt_unpopulated)
 		return 0;
 
+	if (slave && ttm->sg) {
+		/* make userspace faulting work */
+		drm_prime_sg_to_page_addr_arrays(ttm->sg, ttm->pages,
+						 ttm_dma->dma_address, ttm->num_pages);
+		ttm->state = tt_unbound;
+		return 0;
+	}
+
 	dev_priv = nouveau_bdev(ttm->bdev);
 	dev = dev_priv->dev;
 
@@ -1106,6 +1332,10 @@ nouveau_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	struct drm_nouveau_private *dev_priv;
 	struct drm_device *dev;
 	unsigned i;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
+
+	if (slave)
+		return;
 
 	dev_priv = nouveau_bdev(ttm->bdev);
 	dev = dev_priv->dev;
@@ -1134,6 +1364,52 @@ nouveau_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	ttm_pool_unpopulate(ttm);
 }
 
+void
+nouveau_bo_fence(struct nouveau_bo *nvbo, struct nouveau_fence *fence)
+{
+	struct nouveau_fence *old_fence = NULL;
+
+	if (likely(fence))
+		nouveau_fence_ref(fence);
+
+	spin_lock(&nvbo->bo.bdev->fence_lock);
+	old_fence = nvbo->bo.sync_obj;
+	nvbo->bo.sync_obj = fence;
+	spin_unlock(&nvbo->bo.bdev->fence_lock);
+
+	nouveau_fence_unref(&old_fence);
+}
+
+static void
+nouveau_bo_fence_unref(void **sync_obj)
+{
+	nouveau_fence_unref((struct nouveau_fence **)sync_obj);
+}
+
+static void *
+nouveau_bo_fence_ref(void *sync_obj)
+{
+	return nouveau_fence_ref(sync_obj);
+}
+
+static bool
+nouveau_bo_fence_signalled(void *sync_obj, void *sync_arg)
+{
+	return nouveau_fence_done(sync_obj);
+}
+
+static int
+nouveau_bo_fence_wait(void *sync_obj, void *sync_arg, bool lazy, bool intr)
+{
+	return nouveau_fence_wait(sync_obj, lazy, intr);
+}
+
+static int
+nouveau_bo_fence_flush(void *sync_obj, void *sync_arg)
+{
+	return 0;
+}
+
 struct ttm_bo_driver nouveau_bo_driver = {
 	.ttm_tt_create = &nouveau_ttm_tt_create,
 	.ttm_tt_populate = &nouveau_ttm_tt_populate,
@@ -1144,11 +1420,11 @@ struct ttm_bo_driver nouveau_bo_driver = {
 	.move_notify = nouveau_bo_move_ntfy,
 	.move = nouveau_bo_move,
 	.verify_access = nouveau_bo_verify_access,
-	.sync_obj_signaled = __nouveau_fence_signalled,
-	.sync_obj_wait = __nouveau_fence_wait,
-	.sync_obj_flush = __nouveau_fence_flush,
-	.sync_obj_unref = __nouveau_fence_unref,
-	.sync_obj_ref = __nouveau_fence_ref,
+	.sync_obj_signaled = nouveau_bo_fence_signalled,
+	.sync_obj_wait = nouveau_bo_fence_wait,
+	.sync_obj_flush = nouveau_bo_fence_flush,
+	.sync_obj_unref = nouveau_bo_fence_unref,
+	.sync_obj_ref = nouveau_bo_fence_ref,
 	.fault_reserve_notify = &nouveau_ttm_fault_reserve_notify,
 	.io_mem_reserve = &nouveau_ttm_io_mem_reserve,
 	.io_mem_free = &nouveau_ttm_io_mem_free,
@@ -1181,9 +1457,12 @@ nouveau_bo_vma_add(struct nouveau_bo *nvbo, struct nouveau_vm *vm,
 
 	if (nvbo->bo.mem.mem_type == TTM_PL_VRAM)
 		nouveau_vm_map(vma, nvbo->bo.mem.mm_node);
-	else
-	if (nvbo->bo.mem.mem_type == TTM_PL_TT)
-		nouveau_vm_map_sg(vma, 0, size, node);
+	else if (nvbo->bo.mem.mem_type == TTM_PL_TT) {
+		if (node->sg)
+			nouveau_vm_map_sg_table(vma, 0, size, node);
+		else
+			nouveau_vm_map_sg(vma, 0, size, node);
+	}
 
 	list_add_tail(&vma->head, &nvbo->vma_list);
 	vma->refcount = 1;
diff --git a/drivers/gpu/drm/nouveau/nouveau_channel.c b/drivers/gpu/drm/nouveau/nouveau_channel.c
index 846afb0bfef4..629d8a2df5bd 100644
--- a/drivers/gpu/drm/nouveau/nouveau_channel.c
+++ b/drivers/gpu/drm/nouveau/nouveau_channel.c
@@ -27,7 +27,10 @@
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
 
 static int
 nouveau_channel_pushbuf_init(struct nouveau_channel *chan)
@@ -38,7 +41,7 @@ nouveau_channel_pushbuf_init(struct nouveau_channel *chan)
 	int ret;
 
 	/* allocate buffer object */
-	ret = nouveau_bo_new(dev, 65536, 0, mem, 0, 0, &chan->pushbuf_bo);
+	ret = nouveau_bo_new(dev, 65536, 0, mem, 0, 0, NULL, &chan->pushbuf_bo);
 	if (ret)
 		goto out;
 
@@ -117,8 +120,9 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 		      struct drm_file *file_priv,
 		      uint32_t vram_handle, uint32_t gart_handle)
 {
+	struct nouveau_exec_engine *fence = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_fpriv *fpriv = nouveau_fpriv(file_priv);
 	struct nouveau_channel *chan;
 	unsigned long flags;
@@ -155,10 +159,6 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 	}
 
 	NV_DEBUG(dev, "initialising channel %d\n", chan->id);
-	INIT_LIST_HEAD(&chan->nvsw.vbl_wait);
-	INIT_LIST_HEAD(&chan->nvsw.flip);
-	INIT_LIST_HEAD(&chan->fence.pending);
-	spin_lock_init(&chan->fence.lock);
 
 	/* setup channel's memory and vm */
 	ret = nouveau_gpuobj_channel_init(chan, vram_handle, gart_handle);
@@ -188,20 +188,15 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 	chan->user_put = 0x40;
 	chan->user_get = 0x44;
 	if (dev_priv->card_type >= NV_50)
-                chan->user_get_hi = 0x60;
+		chan->user_get_hi = 0x60;
 
-	/* disable the fifo caches */
-	pfifo->reassign(dev, false);
-
-	/* Construct initial RAMFC for new channel */
-	ret = pfifo->create_context(chan);
+	/* create fifo context */
+	ret = pfifo->base.context_new(chan, NVOBJ_ENGINE_FIFO);
 	if (ret) {
 		nouveau_channel_put(&chan);
 		return ret;
 	}
 
-	pfifo->reassign(dev, true);
-
 	/* Insert NOPs for NOUVEAU_DMA_SKIPS */
 	ret = RING_SPACE(chan, NOUVEAU_DMA_SKIPS);
 	if (ret) {
@@ -211,9 +206,28 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 
 	for (i = 0; i < NOUVEAU_DMA_SKIPS; i++)
 		OUT_RING  (chan, 0x00000000);
+
+	ret = nouveau_gpuobj_gr_new(chan, NvSw, nouveau_software_class(dev));
+	if (ret) {
+		nouveau_channel_put(&chan);
+		return ret;
+	}
+
+	if (dev_priv->card_type < NV_C0) {
+		ret = RING_SPACE(chan, 2);
+		if (ret) {
+			nouveau_channel_put(&chan);
+			return ret;
+		}
+
+		BEGIN_NV04(chan, NvSubSw, NV01_SUBCHAN_OBJECT, 1);
+		OUT_RING  (chan, NvSw);
+		FIRE_RING (chan);
+	}
+
 	FIRE_RING(chan);
 
-	ret = nouveau_fence_channel_init(chan);
+	ret = fence->context_new(chan, NVOBJ_ENGINE_FENCE);
 	if (ret) {
 		nouveau_channel_put(&chan);
 		return ret;
@@ -268,7 +282,6 @@ nouveau_channel_put_unlocked(struct nouveau_channel **pchan)
 	struct nouveau_channel *chan = *pchan;
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	unsigned long flags;
 	int i;
 
@@ -285,24 +298,12 @@ nouveau_channel_put_unlocked(struct nouveau_channel **pchan)
 	/* give it chance to idle */
 	nouveau_channel_idle(chan);
 
-	/* ensure all outstanding fences are signaled.  they should be if the
-	 * above attempts at idling were OK, but if we failed this'll tell TTM
-	 * we're done with the buffers.
-	 */
-	nouveau_fence_channel_fini(chan);
-
-	/* boot it off the hardware */
-	pfifo->reassign(dev, false);
-
 	/* destroy the engine specific contexts */
-	pfifo->destroy_context(chan);
-	for (i = 0; i < NVOBJ_ENGINE_NR; i++) {
+	for (i = NVOBJ_ENGINE_NR - 1; i >= 0; i--) {
 		if (chan->engctx[i])
 			dev_priv->eng[i]->context_del(chan, i);
 	}
 
-	pfifo->reassign(dev, true);
-
 	/* aside from its resources, the channel should now be dead,
 	 * remove it from the channel list
 	 */
@@ -354,38 +355,37 @@ nouveau_channel_ref(struct nouveau_channel *chan,
 	*pchan = chan;
 }
 
-void
+int
 nouveau_channel_idle(struct nouveau_channel *chan)
 {
 	struct drm_device *dev = chan->dev;
 	struct nouveau_fence *fence = NULL;
 	int ret;
 
-	nouveau_fence_update(chan);
-
-	if (chan->fence.sequence != chan->fence.sequence_ack) {
-		ret = nouveau_fence_new(chan, &fence, true);
-		if (!ret) {
-			ret = nouveau_fence_wait(fence, false, false);
-			nouveau_fence_unref(&fence);
-		}
-
-		if (ret)
-			NV_ERROR(dev, "Failed to idle channel %d.\n", chan->id);
+	ret = nouveau_fence_new(chan, &fence);
+	if (!ret) {
+		ret = nouveau_fence_wait(fence, false, false);
+		nouveau_fence_unref(&fence);
 	}
+
+	if (ret)
+		NV_ERROR(dev, "Failed to idle channel %d.\n", chan->id);
+	return ret;
 }
 
 /* cleans up all the fifos from file_priv */
 void
 nouveau_channel_cleanup(struct drm_device *dev, struct drm_file *file_priv)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_engine *engine = &dev_priv->engine;
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_channel *chan;
 	int i;
 
+	if (!pfifo)
+		return;
+
 	NV_DEBUG(dev, "clearing FIFO enables from file_priv\n");
-	for (i = 0; i < engine->fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		chan = nouveau_channel_get(file_priv, i);
 		if (IS_ERR(chan))
 			continue;
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.c b/drivers/gpu/drm/nouveau/nouveau_connector.c
index fa860358add1..7b11edb077d0 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.c
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.c
@@ -654,7 +654,13 @@ nouveau_connector_detect_depth(struct drm_connector *connector)
 	if (nv_connector->edid && connector->display_info.bpc)
 		return;
 
-	/* if not, we're out of options unless we're LVDS, default to 8bpc */
+	/* EDID 1.4 is *supposed* to be supported on eDP, but, Apple... */
+	if (nv_connector->type == DCB_CONNECTOR_eDP) {
+		connector->display_info.bpc = 6;
+		return;
+	}
+
+	/* we're out of options unless we're LVDS, default to 8bpc */
 	if (nv_encoder->dcb->type != OUTPUT_LVDS) {
 		connector->display_info.bpc = 8;
 		return;
diff --git a/drivers/gpu/drm/nouveau/nouveau_debugfs.c b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
index fa2ec491f6a7..188c92b327e2 100644
--- a/drivers/gpu/drm/nouveau/nouveau_debugfs.c
+++ b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
@@ -67,8 +67,6 @@ nouveau_debugfs_channel_info(struct seq_file *m, void *data)
 			   nvchan_rd32(chan, 0x8c));
 	}
 
-	seq_printf(m, "last fence    : %d\n", chan->fence.sequence);
-	seq_printf(m, "last signalled: %d\n", chan->fence.sequence_ack);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index a85e112863d1..69688ef5cf46 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -33,7 +33,9 @@
 #include "nouveau_crtc.h"
 #include "nouveau_dma.h"
 #include "nouveau_connector.h"
+#include "nouveau_software.h"
 #include "nouveau_gpio.h"
+#include "nouveau_fence.h"
 #include "nv50_display.h"
 
 static void
@@ -300,7 +302,7 @@ nouveau_display_create(struct drm_device *dev)
 		disp->color_vibrance_property->values[1] = 200; /* -100..+100 */
 	}
 
-	dev->mode_config.funcs = (void *)&nouveau_mode_config_funcs;
+	dev->mode_config.funcs = &nouveau_mode_config_funcs;
 	dev->mode_config.fb_base = pci_resource_start(dev->pdev, 1);
 
 	dev->mode_config.min_width = 0;
@@ -325,14 +327,21 @@ nouveau_display_create(struct drm_device *dev)
 
 	ret = disp->create(dev);
 	if (ret)
-		return ret;
+		goto disp_create_err;
 
 	if (dev->mode_config.num_crtc) {
 		ret = drm_vblank_init(dev, dev->mode_config.num_crtc);
 		if (ret)
-			return ret;
+			goto vblank_err;
 	}
 
+	return 0;
+
+vblank_err:
+	disp->destroy(dev);
+disp_create_err:
+	drm_kms_helper_poll_fini(dev);
+	drm_mode_config_cleanup(dev);
 	return ret;
 }
 
@@ -425,6 +434,7 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 		       struct nouveau_page_flip_state *s,
 		       struct nouveau_fence **pfence)
 {
+	struct nouveau_software_chan *swch = chan->engctx[NVOBJ_ENGINE_SW];
 	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct drm_device *dev = chan->dev;
 	unsigned long flags;
@@ -432,7 +442,7 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 
 	/* Queue it to the pending list */
 	spin_lock_irqsave(&dev->event_lock, flags);
-	list_add_tail(&s->head, &chan->nvsw.flip);
+	list_add_tail(&s->head, &swch->flip);
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
 	/* Synchronize with the old framebuffer */
@@ -446,17 +456,17 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 		goto fail;
 
 	if (dev_priv->card_type < NV_C0) {
-		BEGIN_RING(chan, NvSubSw, NV_SW_PAGE_FLIP, 1);
+		BEGIN_NV04(chan, NvSubSw, NV_SW_PAGE_FLIP, 1);
 		OUT_RING  (chan, 0x00000000);
 		OUT_RING  (chan, 0x00000000);
 	} else {
-		BEGIN_NVC0(chan, 2, 0, NV10_SUBCHAN_REF_CNT, 1);
-		OUT_RING  (chan, ++chan->fence.sequence);
-		BEGIN_NVC0(chan, 8, 0, NVSW_SUBCHAN_PAGE_FLIP, 0x0000);
+		BEGIN_NVC0(chan, 0, NV10_SUBCHAN_REF_CNT, 1);
+		OUT_RING  (chan, 0);
+		BEGIN_IMC0(chan, 0, NVSW_SUBCHAN_PAGE_FLIP, 0x0000);
 	}
 	FIRE_RING (chan);
 
-	ret = nouveau_fence_new(chan, pfence, true);
+	ret = nouveau_fence_new(chan, pfence);
 	if (ret)
 		goto fail;
 
@@ -477,7 +487,7 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	struct nouveau_bo *old_bo = nouveau_framebuffer(crtc->fb)->nvbo;
 	struct nouveau_bo *new_bo = nouveau_framebuffer(fb)->nvbo;
 	struct nouveau_page_flip_state *s;
-	struct nouveau_channel *chan;
+	struct nouveau_channel *chan = NULL;
 	struct nouveau_fence *fence;
 	int ret;
 
@@ -500,7 +510,9 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		  new_bo->bo.offset };
 
 	/* Choose the channel the flip will be handled in */
-	chan = nouveau_fence_channel(new_bo->bo.sync_obj);
+	fence = new_bo->bo.sync_obj;
+	if (fence)
+		chan = nouveau_channel_get_unlocked(fence->channel);
 	if (!chan)
 		chan = nouveau_channel_get_unlocked(dev_priv->channel);
 	mutex_lock(&chan->mutex);
@@ -540,20 +552,20 @@ int
 nouveau_finish_page_flip(struct nouveau_channel *chan,
 			 struct nouveau_page_flip_state *ps)
 {
+	struct nouveau_software_chan *swch = chan->engctx[NVOBJ_ENGINE_SW];
 	struct drm_device *dev = chan->dev;
 	struct nouveau_page_flip_state *s;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->event_lock, flags);
 
-	if (list_empty(&chan->nvsw.flip)) {
+	if (list_empty(&swch->flip)) {
 		NV_ERROR(dev, "Unexpected pageflip in channel %d.\n", chan->id);
 		spin_unlock_irqrestore(&dev->event_lock, flags);
 		return -EINVAL;
 	}
 
-	s = list_first_entry(&chan->nvsw.flip,
-			     struct nouveau_page_flip_state, head);
+	s = list_first_entry(&swch->flip, struct nouveau_page_flip_state, head);
 	if (s->event) {
 		struct drm_pending_vblank_event *e = s->event;
 		struct timeval now;
diff --git a/drivers/gpu/drm/nouveau/nouveau_dma.h b/drivers/gpu/drm/nouveau/nouveau_dma.h
index 23d4edf992b7..8db68be9544f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dma.h
+++ b/drivers/gpu/drm/nouveau/nouveau_dma.h
@@ -48,12 +48,12 @@ void nv50_dma_push(struct nouveau_channel *, struct nouveau_bo *,
 
 /* Hardcoded object assignments to subchannels (subchannel id). */
 enum {
-	NvSubM2MF	= 0,
+	NvSubCtxSurf2D  = 0,
 	NvSubSw		= 1,
-	NvSub2D		= 2,
-	NvSubCtxSurf2D  = 2,
+	NvSubImageBlit  = 2,
+	NvSub2D		= 3,
 	NvSubGdiRect    = 3,
-	NvSubImageBlit  = 4
+	NvSubCopy	= 4,
 };
 
 /* Object handles. */
@@ -73,6 +73,7 @@ enum {
 	NvSema		= 0x8000000f,
 	NvEvoSema0	= 0x80000010,
 	NvEvoSema1	= 0x80000011,
+	NvNotify1       = 0x80000012,
 
 	/* G80+ display objects */
 	NvEvoVRAM	= 0x01000000,
@@ -127,15 +128,33 @@ extern void
 OUT_RINGp(struct nouveau_channel *chan, const void *data, unsigned nr_dwords);
 
 static inline void
-BEGIN_NVC0(struct nouveau_channel *chan, int op, int subc, int mthd, int size)
+BEGIN_NV04(struct nouveau_channel *chan, int subc, int mthd, int size)
 {
-	OUT_RING(chan, (op << 28) | (size << 16) | (subc << 13) | (mthd >> 2));
+	OUT_RING(chan, 0x00000000 | (subc << 13) | (size << 18) | mthd);
 }
 
 static inline void
-BEGIN_RING(struct nouveau_channel *chan, int subc, int mthd, int size)
+BEGIN_NI04(struct nouveau_channel *chan, int subc, int mthd, int size)
 {
-	OUT_RING(chan, (subc << 13) | (size << 18) | mthd);
+	OUT_RING(chan, 0x40000000 | (subc << 13) | (size << 18) | mthd);
+}
+
+static inline void
+BEGIN_NVC0(struct nouveau_channel *chan, int subc, int mthd, int size)
+{
+	OUT_RING(chan, 0x20000000 | (size << 16) | (subc << 13) | (mthd >> 2));
+}
+
+static inline void
+BEGIN_NIC0(struct nouveau_channel *chan, int subc, int mthd, int size)
+{
+	OUT_RING(chan, 0x60000000 | (size << 16) | (subc << 13) | (mthd >> 2));
+}
+
+static inline void
+BEGIN_IMC0(struct nouveau_channel *chan, int subc, int mthd, u16 data)
+{
+	OUT_RING(chan, 0x80000000 | (data << 16) | (subc << 13) | (mthd >> 2));
 }
 
 #define WRITE_PUT(val) do {                                                    \
diff --git a/drivers/gpu/drm/nouveau/nouveau_dp.c b/drivers/gpu/drm/nouveau/nouveau_dp.c
index d996134b1b28..7e289d2ad8e4 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dp.c
+++ b/drivers/gpu/drm/nouveau/nouveau_dp.c
@@ -510,6 +510,25 @@ nouveau_dp_dpms(struct drm_encoder *encoder, int mode, u32 datarate,
 		nouveau_dp_link_train(encoder, datarate, func);
 }
 
+static void
+nouveau_dp_probe_oui(struct drm_device *dev, struct nouveau_i2c_chan *auxch,
+		     u8 *dpcd)
+{
+	u8 buf[3];
+
+	if (!(dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
+		return;
+
+	if (!auxch_tx(dev, auxch->drive, 9, DP_SINK_OUI, buf, 3))
+		NV_DEBUG_KMS(dev, "Sink OUI: %02hx%02hx%02hx\n",
+			     buf[0], buf[1], buf[2]);
+
+	if (!auxch_tx(dev, auxch->drive, 9, DP_BRANCH_OUI, buf, 3))
+		NV_DEBUG_KMS(dev, "Branch OUI: %02hx%02hx%02hx\n",
+			     buf[0], buf[1], buf[2]);
+
+}
+
 bool
 nouveau_dp_detect(struct drm_encoder *encoder)
 {
@@ -544,6 +563,8 @@ nouveau_dp_detect(struct drm_encoder *encoder)
 	NV_DEBUG_KMS(dev, "maximum: %dx%d\n",
 		     nv_encoder->dp.link_nr, nv_encoder->dp.link_bw);
 
+	nouveau_dp_probe_oui(dev, auxch, dpcd);
+
 	return true;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.c b/drivers/gpu/drm/nouveau/nouveau_drv.c
index 4f2030bd5676..cad254c8e387 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.c
@@ -33,6 +33,7 @@
 #include "nouveau_fb.h"
 #include "nouveau_fbcon.h"
 #include "nouveau_pm.h"
+#include "nouveau_fifo.h"
 #include "nv50_display.h"
 
 #include "drm_pciids.h"
@@ -175,7 +176,7 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
 	struct drm_device *dev = pci_get_drvdata(pdev);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_channel *chan;
 	struct drm_crtc *crtc;
 	int ret, i, e;
@@ -214,17 +215,13 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
 	ttm_bo_evict_mm(&dev_priv->ttm.bdev, TTM_PL_VRAM);
 
 	NV_INFO(dev, "Idling channels...\n");
-	for (i = 0; i < pfifo->channels; i++) {
+	for (i = 0; i < (pfifo ? pfifo->channels : 0); i++) {
 		chan = dev_priv->channels.ptr[i];
 
 		if (chan && chan->pushbuf_bo)
 			nouveau_channel_idle(chan);
 	}
 
-	pfifo->reassign(dev, false);
-	pfifo->disable(dev);
-	pfifo->unload_context(dev);
-
 	for (e = NVOBJ_ENGINE_NR - 1; e >= 0; e--) {
 		if (!dev_priv->eng[e])
 			continue;
@@ -265,8 +262,6 @@ out_abort:
 		if (dev_priv->eng[e])
 			dev_priv->eng[e]->init(dev, e);
 	}
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
 	return ret;
 }
 
@@ -274,6 +269,7 @@ int
 nouveau_pci_resume(struct pci_dev *pdev)
 {
 	struct drm_device *dev = pci_get_drvdata(pdev);
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_engine *engine = &dev_priv->engine;
 	struct drm_crtc *crtc;
@@ -321,7 +317,6 @@ nouveau_pci_resume(struct pci_dev *pdev)
 		if (dev_priv->eng[i])
 			dev_priv->eng[i]->init(dev, i);
 	}
-	engine->fifo.init(dev);
 
 	nouveau_irq_postinstall(dev);
 
@@ -330,7 +325,7 @@ nouveau_pci_resume(struct pci_dev *pdev)
 		struct nouveau_channel *chan;
 		int j;
 
-		for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+		for (i = 0; i < (pfifo ? pfifo->channels : 0); i++) {
 			chan = dev_priv->channels.ptr[i];
 			if (!chan || !chan->pushbuf_bo)
 				continue;
@@ -408,7 +403,7 @@ static struct drm_driver driver = {
 	.driver_features =
 		DRIVER_USE_AGP | DRIVER_PCI_DMA | DRIVER_SG |
 		DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | DRIVER_GEM |
-		DRIVER_MODESET,
+		DRIVER_MODESET | DRIVER_PRIME,
 	.load = nouveau_load,
 	.firstopen = nouveau_firstopen,
 	.lastclose = nouveau_lastclose,
@@ -430,6 +425,12 @@ static struct drm_driver driver = {
 	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = nouveau_ioctls,
 	.fops = &nouveau_driver_fops,
+
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export = nouveau_gem_prime_export,
+	.gem_prime_import = nouveau_gem_prime_import,
+
 	.gem_init_object = nouveau_gem_object_new,
 	.gem_free_object = nouveau_gem_object_del,
 	.gem_open_object = nouveau_gem_object_open,
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.h b/drivers/gpu/drm/nouveau/nouveau_drv.h
index 3aef353a926c..634d222c93de 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.h
@@ -70,7 +70,7 @@ struct nouveau_mem;
 
 #define MAX_NUM_DCB_ENTRIES 16
 
-#define NOUVEAU_MAX_CHANNEL_NR 128
+#define NOUVEAU_MAX_CHANNEL_NR 4096
 #define NOUVEAU_MAX_TILE_NR 15
 
 struct nouveau_mem {
@@ -86,6 +86,7 @@ struct nouveau_mem {
 	u32 memtype;
 	u64 offset;
 	u64 size;
+	struct sg_table *sg;
 };
 
 struct nouveau_tile_reg {
@@ -164,8 +165,10 @@ enum nouveau_flags {
 #define NVOBJ_ENGINE_PPP	NVOBJ_ENGINE_MPEG
 #define NVOBJ_ENGINE_BSP	6
 #define NVOBJ_ENGINE_VP		7
-#define NVOBJ_ENGINE_DISPLAY	15
+#define NVOBJ_ENGINE_FIFO	14
+#define NVOBJ_ENGINE_FENCE	15
 #define NVOBJ_ENGINE_NR		16
+#define NVOBJ_ENGINE_DISPLAY	(NVOBJ_ENGINE_NR + 0) /*XXX*/
 
 #define NVOBJ_FLAG_DONT_MAP             (1 << 0)
 #define NVOBJ_FLAG_ZERO_ALLOC		(1 << 1)
@@ -233,17 +236,6 @@ struct nouveau_channel {
 	uint32_t user_get_hi;
 	uint32_t user_put;
 
-	/* Fencing */
-	struct {
-		/* lock protects the pending list only */
-		spinlock_t lock;
-		struct list_head pending;
-		uint32_t sequence;
-		uint32_t sequence_ack;
-		atomic_t last_sequence_irq;
-		struct nouveau_vma vma;
-	} fence;
-
 	/* DMA push buffer */
 	struct nouveau_gpuobj *pushbuf;
 	struct nouveau_bo     *pushbuf_bo;
@@ -257,8 +249,6 @@ struct nouveau_channel {
 
 	/* PFIFO context */
 	struct nouveau_gpuobj *ramfc;
-	struct nouveau_gpuobj *cache;
-	void *fifo_priv;
 
 	/* Execution engine contexts */
 	void *engctx[NVOBJ_ENGINE_NR];
@@ -292,18 +282,6 @@ struct nouveau_channel {
 		int ib_put;
 	} dma;
 
-	uint32_t sw_subchannel[8];
-
-	struct nouveau_vma dispc_vma[4];
-	struct {
-		struct nouveau_gpuobj *vblsem;
-		uint32_t vblsem_head;
-		uint32_t vblsem_offset;
-		uint32_t vblsem_rval;
-		struct list_head vbl_wait;
-		struct list_head flip;
-	} nvsw;
-
 	struct {
 		bool active;
 		char name[32];
@@ -366,30 +344,6 @@ struct nouveau_fb_engine {
 	void (*free_tile_region)(struct drm_device *dev, int i);
 };
 
-struct nouveau_fifo_engine {
-	void *priv;
-	int  channels;
-
-	struct nouveau_gpuobj *playlist[2];
-	int cur_playlist;
-
-	int  (*init)(struct drm_device *);
-	void (*takedown)(struct drm_device *);
-
-	void (*disable)(struct drm_device *);
-	void (*enable)(struct drm_device *);
-	bool (*reassign)(struct drm_device *, bool enable);
-	bool (*cache_pull)(struct drm_device *dev, bool enable);
-
-	int  (*channel_id)(struct drm_device *);
-
-	int  (*create_context)(struct nouveau_channel *);
-	void (*destroy_context)(struct nouveau_channel *);
-	int  (*load_context)(struct nouveau_channel *);
-	int  (*unload_context)(struct drm_device *);
-	void (*tlb_flush)(struct drm_device *dev);
-};
-
 struct nouveau_display_engine {
 	void *priv;
 	int (*early_init)(struct drm_device *);
@@ -597,7 +551,6 @@ struct nouveau_engine {
 	struct nouveau_mc_engine      mc;
 	struct nouveau_timer_engine   timer;
 	struct nouveau_fb_engine      fb;
-	struct nouveau_fifo_engine    fifo;
 	struct nouveau_display_engine display;
 	struct nouveau_gpio_engine    gpio;
 	struct nouveau_pm_engine      pm;
@@ -740,6 +693,9 @@ struct drm_nouveau_private {
 		struct ttm_bo_global_ref bo_global_ref;
 		struct ttm_bo_device bdev;
 		atomic_t validate_sequence;
+		int (*move)(struct nouveau_channel *,
+			    struct ttm_buffer_object *,
+			    struct ttm_mem_reg *, struct ttm_mem_reg *);
 	} ttm;
 
 	struct {
@@ -977,7 +933,7 @@ extern void nouveau_channel_put_unlocked(struct nouveau_channel **);
 extern void nouveau_channel_put(struct nouveau_channel **);
 extern void nouveau_channel_ref(struct nouveau_channel *chan,
 				struct nouveau_channel **pchan);
-extern void nouveau_channel_idle(struct nouveau_channel *chan);
+extern int  nouveau_channel_idle(struct nouveau_channel *chan);
 
 /* nouveau_object.c */
 #define NVOBJ_ENGINE_ADD(d, e, p) do {                                         \
@@ -1209,56 +1165,6 @@ extern void nv50_fb_vm_trap(struct drm_device *, int display);
 extern int  nvc0_fb_init(struct drm_device *);
 extern void nvc0_fb_takedown(struct drm_device *);
 
-/* nv04_fifo.c */
-extern int  nv04_fifo_init(struct drm_device *);
-extern void nv04_fifo_fini(struct drm_device *);
-extern void nv04_fifo_disable(struct drm_device *);
-extern void nv04_fifo_enable(struct drm_device *);
-extern bool nv04_fifo_reassign(struct drm_device *, bool);
-extern bool nv04_fifo_cache_pull(struct drm_device *, bool);
-extern int  nv04_fifo_channel_id(struct drm_device *);
-extern int  nv04_fifo_create_context(struct nouveau_channel *);
-extern void nv04_fifo_destroy_context(struct nouveau_channel *);
-extern int  nv04_fifo_load_context(struct nouveau_channel *);
-extern int  nv04_fifo_unload_context(struct drm_device *);
-extern void nv04_fifo_isr(struct drm_device *);
-
-/* nv10_fifo.c */
-extern int  nv10_fifo_init(struct drm_device *);
-extern int  nv10_fifo_channel_id(struct drm_device *);
-extern int  nv10_fifo_create_context(struct nouveau_channel *);
-extern int  nv10_fifo_load_context(struct nouveau_channel *);
-extern int  nv10_fifo_unload_context(struct drm_device *);
-
-/* nv40_fifo.c */
-extern int  nv40_fifo_init(struct drm_device *);
-extern int  nv40_fifo_create_context(struct nouveau_channel *);
-extern int  nv40_fifo_load_context(struct nouveau_channel *);
-extern int  nv40_fifo_unload_context(struct drm_device *);
-
-/* nv50_fifo.c */
-extern int  nv50_fifo_init(struct drm_device *);
-extern void nv50_fifo_takedown(struct drm_device *);
-extern int  nv50_fifo_channel_id(struct drm_device *);
-extern int  nv50_fifo_create_context(struct nouveau_channel *);
-extern void nv50_fifo_destroy_context(struct nouveau_channel *);
-extern int  nv50_fifo_load_context(struct nouveau_channel *);
-extern int  nv50_fifo_unload_context(struct drm_device *);
-extern void nv50_fifo_tlb_flush(struct drm_device *dev);
-
-/* nvc0_fifo.c */
-extern int  nvc0_fifo_init(struct drm_device *);
-extern void nvc0_fifo_takedown(struct drm_device *);
-extern void nvc0_fifo_disable(struct drm_device *);
-extern void nvc0_fifo_enable(struct drm_device *);
-extern bool nvc0_fifo_reassign(struct drm_device *, bool);
-extern bool nvc0_fifo_cache_pull(struct drm_device *, bool);
-extern int  nvc0_fifo_channel_id(struct drm_device *);
-extern int  nvc0_fifo_create_context(struct nouveau_channel *);
-extern void nvc0_fifo_destroy_context(struct nouveau_channel *);
-extern int  nvc0_fifo_load_context(struct nouveau_channel *);
-extern int  nvc0_fifo_unload_context(struct drm_device *);
-
 /* nv04_graph.c */
 extern int  nv04_graph_create(struct drm_device *);
 extern int  nv04_graph_object_new(struct nouveau_channel *, int, u32, u16);
@@ -1277,18 +1183,23 @@ extern int  nv20_graph_create(struct drm_device *);
 
 /* nv40_graph.c */
 extern int  nv40_graph_create(struct drm_device *);
-extern void nv40_grctx_init(struct nouveau_grctx *);
+extern void nv40_grctx_init(struct drm_device *, u32 *size);
+extern void nv40_grctx_fill(struct drm_device *, struct nouveau_gpuobj *);
 
 /* nv50_graph.c */
 extern int  nv50_graph_create(struct drm_device *);
-extern int  nv50_grctx_init(struct nouveau_grctx *);
 extern struct nouveau_enum nv50_data_error_names[];
 extern int  nv50_graph_isr_chid(struct drm_device *dev, u64 inst);
+extern int  nv50_grctx_init(struct drm_device *, u32 *, u32, u32 *, u32 *);
+extern void nv50_grctx_fill(struct drm_device *, struct nouveau_gpuobj *);
 
 /* nvc0_graph.c */
 extern int  nvc0_graph_create(struct drm_device *);
 extern int  nvc0_graph_isr_chid(struct drm_device *dev, u64 inst);
 
+/* nve0_graph.c */
+extern int  nve0_graph_create(struct drm_device *);
+
 /* nv84_crypt.c */
 extern int  nv84_crypt_create(struct drm_device *);
 
@@ -1414,9 +1325,12 @@ extern int nv04_crtc_create(struct drm_device *, int index);
 
 /* nouveau_bo.c */
 extern struct ttm_bo_driver nouveau_bo_driver;
+extern void nouveau_bo_move_init(struct nouveau_channel *);
 extern int nouveau_bo_new(struct drm_device *, int size, int align,
 			  uint32_t flags, uint32_t tile_mode,
-			  uint32_t tile_flags, struct nouveau_bo **);
+			  uint32_t tile_flags,
+			  struct sg_table *sg,
+			  struct nouveau_bo **);
 extern int nouveau_bo_pin(struct nouveau_bo *, uint32_t flags);
 extern int nouveau_bo_unpin(struct nouveau_bo *);
 extern int nouveau_bo_map(struct nouveau_bo *);
@@ -1437,50 +1351,6 @@ extern int  nouveau_bo_vma_add(struct nouveau_bo *, struct nouveau_vm *,
 			       struct nouveau_vma *);
 extern void nouveau_bo_vma_del(struct nouveau_bo *, struct nouveau_vma *);
 
-/* nouveau_fence.c */
-struct nouveau_fence;
-extern int nouveau_fence_init(struct drm_device *);
-extern void nouveau_fence_fini(struct drm_device *);
-extern int nouveau_fence_channel_init(struct nouveau_channel *);
-extern void nouveau_fence_channel_fini(struct nouveau_channel *);
-extern void nouveau_fence_update(struct nouveau_channel *);
-extern int nouveau_fence_new(struct nouveau_channel *, struct nouveau_fence **,
-			     bool emit);
-extern int nouveau_fence_emit(struct nouveau_fence *);
-extern void nouveau_fence_work(struct nouveau_fence *fence,
-			       void (*work)(void *priv, bool signalled),
-			       void *priv);
-struct nouveau_channel *nouveau_fence_channel(struct nouveau_fence *);
-
-extern bool __nouveau_fence_signalled(void *obj, void *arg);
-extern int __nouveau_fence_wait(void *obj, void *arg, bool lazy, bool intr);
-extern int __nouveau_fence_flush(void *obj, void *arg);
-extern void __nouveau_fence_unref(void **obj);
-extern void *__nouveau_fence_ref(void *obj);
-
-static inline bool nouveau_fence_signalled(struct nouveau_fence *obj)
-{
-	return __nouveau_fence_signalled(obj, NULL);
-}
-static inline int
-nouveau_fence_wait(struct nouveau_fence *obj, bool lazy, bool intr)
-{
-	return __nouveau_fence_wait(obj, NULL, lazy, intr);
-}
-extern int nouveau_fence_sync(struct nouveau_fence *, struct nouveau_channel *);
-static inline int nouveau_fence_flush(struct nouveau_fence *obj)
-{
-	return __nouveau_fence_flush(obj, NULL);
-}
-static inline void nouveau_fence_unref(struct nouveau_fence **obj)
-{
-	__nouveau_fence_unref((void **)obj);
-}
-static inline struct nouveau_fence *nouveau_fence_ref(struct nouveau_fence *obj)
-{
-	return __nouveau_fence_ref(obj);
-}
-
 /* nouveau_gem.c */
 extern int nouveau_gem_new(struct drm_device *, int size, int align,
 			   uint32_t domain, uint32_t tile_mode,
@@ -1501,6 +1371,11 @@ extern int nouveau_gem_ioctl_cpu_fini(struct drm_device *, void *,
 extern int nouveau_gem_ioctl_info(struct drm_device *, void *,
 				  struct drm_file *);
 
+extern struct dma_buf *nouveau_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *obj, int flags);
+extern struct drm_gem_object *nouveau_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf);
+
 /* nouveau_display.c */
 int nouveau_display_create(struct drm_device *dev);
 void nouveau_display_destroy(struct drm_device *dev);
@@ -1772,6 +1647,7 @@ nv44_graph_class(struct drm_device *dev)
 #define NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_EQUAL                 0x00000001
 #define NV84_SUBCHAN_SEMAPHORE_TRIGGER_WRITE_LONG                    0x00000002
 #define NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_GEQUAL                0x00000004
+#define NVC0_SUBCHAN_SEMAPHORE_TRIGGER_YIELD                         0x00001000
 #define NV84_SUBCHAN_NOTIFY_INTR                                     0x00000020
 #define NV84_SUBCHAN_WRCACHE_FLUSH                                   0x00000024
 #define NV10_SUBCHAN_REF_CNT                                         0x00000050
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index 8113e9201ed9..153b9a15469b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -153,7 +153,7 @@ nouveau_fbcon_sync(struct fb_info *info)
 	struct drm_device *dev = nfbdev->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = dev_priv->channel;
-	int ret, i;
+	int ret;
 
 	if (!chan || !chan->accel_done || in_interrupt() ||
 	    info->state != FBINFO_STATE_RUNNING ||
@@ -163,38 +163,8 @@ nouveau_fbcon_sync(struct fb_info *info)
 	if (!mutex_trylock(&chan->mutex))
 		return 0;
 
-	ret = RING_SPACE(chan, 4);
-	if (ret) {
-		mutex_unlock(&chan->mutex);
-		nouveau_fbcon_gpu_lockup(info);
-		return 0;
-	}
-
-	if (dev_priv->card_type >= NV_C0) {
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x010c, 1);
-		OUT_RING  (chan, 0);
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x0100, 1);
-		OUT_RING  (chan, 0);
-	} else {
-		BEGIN_RING(chan, 0, 0x0104, 1);
-		OUT_RING  (chan, 0);
-		BEGIN_RING(chan, 0, 0x0100, 1);
-		OUT_RING  (chan, 0);
-	}
-
-	nouveau_bo_wr32(chan->notifier_bo, chan->m2mf_ntfy/4 + 3, 0xffffffff);
-	FIRE_RING(chan);
+	ret = nouveau_channel_idle(chan);
 	mutex_unlock(&chan->mutex);
-
-	ret = -EBUSY;
-	for (i = 0; i < 100000; i++) {
-		if (!nouveau_bo_rd32(chan->notifier_bo, chan->m2mf_ntfy/4 + 3)) {
-			ret = 0;
-			break;
-		}
-		DRM_UDELAY(1);
-	}
-
 	if (ret) {
 		nouveau_fbcon_gpu_lockup(info);
 		return 0;
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.c b/drivers/gpu/drm/nouveau/nouveau_fence.c
index c1dc20f6cb85..3c180493dab8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fence.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.c
@@ -32,220 +32,100 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
 #include "nouveau_dma.h"
 
-#define USE_REFCNT(dev) (nouveau_private(dev)->chipset >= 0x10)
-#define USE_SEMA(dev) (nouveau_private(dev)->chipset >= 0x17)
-
-struct nouveau_fence {
-	struct nouveau_channel *channel;
-	struct kref refcount;
-	struct list_head entry;
-
-	uint32_t sequence;
-	bool signalled;
-
-	void (*work)(void *priv, bool signalled);
-	void *priv;
-};
-
-struct nouveau_semaphore {
-	struct kref ref;
-	struct drm_device *dev;
-	struct drm_mm_node *mem;
-};
-
-static inline struct nouveau_fence *
-nouveau_fence(void *sync_obj)
+void
+nouveau_fence_context_del(struct nouveau_fence_chan *fctx)
 {
-	return (struct nouveau_fence *)sync_obj;
+	struct nouveau_fence *fence, *fnext;
+	spin_lock(&fctx->lock);
+	list_for_each_entry_safe(fence, fnext, &fctx->pending, head) {
+		if (fence->work)
+			fence->work(fence->priv, false);
+		fence->channel = NULL;
+		list_del(&fence->head);
+		nouveau_fence_unref(&fence);
+	}
+	spin_unlock(&fctx->lock);
 }
 
-static void
-nouveau_fence_del(struct kref *ref)
+void
+nouveau_fence_context_new(struct nouveau_fence_chan *fctx)
 {
-	struct nouveau_fence *fence =
-		container_of(ref, struct nouveau_fence, refcount);
-
-	nouveau_channel_ref(NULL, &fence->channel);
-	kfree(fence);
+	INIT_LIST_HEAD(&fctx->pending);
+	spin_lock_init(&fctx->lock);
 }
 
 void
 nouveau_fence_update(struct nouveau_channel *chan)
 {
 	struct drm_device *dev = chan->dev;
-	struct nouveau_fence *tmp, *fence;
-	uint32_t sequence;
+	struct nouveau_fence_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	struct nouveau_fence *fence, *fnext;
 
-	spin_lock(&chan->fence.lock);
-
-	/* Fetch the last sequence if the channel is still up and running */
-	if (likely(!list_empty(&chan->fence.pending))) {
-		if (USE_REFCNT(dev))
-			sequence = nvchan_rd32(chan, 0x48);
-		else
-			sequence = atomic_read(&chan->fence.last_sequence_irq);
-
-		if (chan->fence.sequence_ack == sequence)
-			goto out;
-		chan->fence.sequence_ack = sequence;
-	}
-
-	list_for_each_entry_safe(fence, tmp, &chan->fence.pending, entry) {
-		if (fence->sequence > chan->fence.sequence_ack)
+	spin_lock(&fctx->lock);
+	list_for_each_entry_safe(fence, fnext, &fctx->pending, head) {
+		if (priv->read(chan) < fence->sequence)
 			break;
 
-		fence->signalled = true;
-		list_del(&fence->entry);
 		if (fence->work)
 			fence->work(fence->priv, true);
-
-		kref_put(&fence->refcount, nouveau_fence_del);
-	}
-
-out:
-	spin_unlock(&chan->fence.lock);
-}
-
-int
-nouveau_fence_new(struct nouveau_channel *chan, struct nouveau_fence **pfence,
-		  bool emit)
-{
-	struct nouveau_fence *fence;
-	int ret = 0;
-
-	fence = kzalloc(sizeof(*fence), GFP_KERNEL);
-	if (!fence)
-		return -ENOMEM;
-	kref_init(&fence->refcount);
-	nouveau_channel_ref(chan, &fence->channel);
-
-	if (emit)
-		ret = nouveau_fence_emit(fence);
-
-	if (ret)
+		fence->channel = NULL;
+		list_del(&fence->head);
 		nouveau_fence_unref(&fence);
-	*pfence = fence;
-	return ret;
-}
-
-struct nouveau_channel *
-nouveau_fence_channel(struct nouveau_fence *fence)
-{
-	return fence ? nouveau_channel_get_unlocked(fence->channel) : NULL;
+	}
+	spin_unlock(&fctx->lock);
 }
 
 int
-nouveau_fence_emit(struct nouveau_fence *fence)
+nouveau_fence_emit(struct nouveau_fence *fence, struct nouveau_channel *chan)
 {
-	struct nouveau_channel *chan = fence->channel;
 	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_fence_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
 	int ret;
 
-	ret = RING_SPACE(chan, 2);
-	if (ret)
-		return ret;
-
-	if (unlikely(chan->fence.sequence == chan->fence.sequence_ack - 1)) {
-		nouveau_fence_update(chan);
+	fence->channel  = chan;
+	fence->timeout  = jiffies + (3 * DRM_HZ);
+	fence->sequence = ++fctx->sequence;
 
-		BUG_ON(chan->fence.sequence ==
-		       chan->fence.sequence_ack - 1);
+	ret = priv->emit(fence);
+	if (!ret) {
+		kref_get(&fence->kref);
+		spin_lock(&fctx->lock);
+		list_add_tail(&fence->head, &fctx->pending);
+		spin_unlock(&fctx->lock);
 	}
 
-	fence->sequence = ++chan->fence.sequence;
-
-	kref_get(&fence->refcount);
-	spin_lock(&chan->fence.lock);
-	list_add_tail(&fence->entry, &chan->fence.pending);
-	spin_unlock(&chan->fence.lock);
-
-	if (USE_REFCNT(dev)) {
-		if (dev_priv->card_type < NV_C0)
-			BEGIN_RING(chan, 0, NV10_SUBCHAN_REF_CNT, 1);
-		else
-			BEGIN_NVC0(chan, 2, 0, NV10_SUBCHAN_REF_CNT, 1);
-	} else {
-		BEGIN_RING(chan, NvSubSw, 0x0150, 1);
-	}
-	OUT_RING (chan, fence->sequence);
-	FIRE_RING(chan);
-
-	return 0;
-}
-
-void
-nouveau_fence_work(struct nouveau_fence *fence,
-		   void (*work)(void *priv, bool signalled),
-		   void *priv)
-{
-	BUG_ON(fence->work);
-
-	spin_lock(&fence->channel->fence.lock);
-
-	if (fence->signalled) {
-		work(priv, true);
-	} else {
-		fence->work = work;
-		fence->priv = priv;
-	}
-
-	spin_unlock(&fence->channel->fence.lock);
-}
-
-void
-__nouveau_fence_unref(void **sync_obj)
-{
-	struct nouveau_fence *fence = nouveau_fence(*sync_obj);
-
-	if (fence)
-		kref_put(&fence->refcount, nouveau_fence_del);
-	*sync_obj = NULL;
-}
-
-void *
-__nouveau_fence_ref(void *sync_obj)
-{
-	struct nouveau_fence *fence = nouveau_fence(sync_obj);
-
-	kref_get(&fence->refcount);
-	return sync_obj;
+	return ret;
 }
 
 bool
-__nouveau_fence_signalled(void *sync_obj, void *sync_arg)
+nouveau_fence_done(struct nouveau_fence *fence)
 {
-	struct nouveau_fence *fence = nouveau_fence(sync_obj);
-	struct nouveau_channel *chan = fence->channel;
-
-	if (fence->signalled)
-		return true;
-
-	nouveau_fence_update(chan);
-	return fence->signalled;
+	if (fence->channel)
+		nouveau_fence_update(fence->channel);
+	return !fence->channel;
 }
 
 int
-__nouveau_fence_wait(void *sync_obj, void *sync_arg, bool lazy, bool intr)
+nouveau_fence_wait(struct nouveau_fence *fence, bool lazy, bool intr)
 {
-	unsigned long timeout = jiffies + (3 * DRM_HZ);
 	unsigned long sleep_time = NSEC_PER_MSEC / 1000;
 	ktime_t t;
 	int ret = 0;
 
-	while (1) {
-		if (__nouveau_fence_signalled(sync_obj, sync_arg))
-			break;
-
-		if (time_after_eq(jiffies, timeout)) {
+	while (!nouveau_fence_done(fence)) {
+		if (fence->timeout && time_after_eq(jiffies, fence->timeout)) {
 			ret = -EBUSY;
 			break;
 		}
 
-		__set_current_state(intr ? TASK_INTERRUPTIBLE
-			: TASK_UNINTERRUPTIBLE);
+		__set_current_state(intr ? TASK_INTERRUPTIBLE :
+					   TASK_UNINTERRUPTIBLE);
 		if (lazy) {
 			t = ktime_set(0, sleep_time);
 			schedule_hrtimeout(&t, HRTIMER_MODE_REL);
@@ -261,354 +141,72 @@ __nouveau_fence_wait(void *sync_obj, void *sync_arg, bool lazy, bool intr)
 	}
 
 	__set_current_state(TASK_RUNNING);
-
 	return ret;
 }
 
-static struct nouveau_semaphore *
-semaphore_alloc(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_semaphore *sema;
-	int size = (dev_priv->chipset < 0x84) ? 4 : 16;
-	int ret, i;
-
-	if (!USE_SEMA(dev))
-		return NULL;
-
-	sema = kmalloc(sizeof(*sema), GFP_KERNEL);
-	if (!sema)
-		goto fail;
-
-	ret = drm_mm_pre_get(&dev_priv->fence.heap);
-	if (ret)
-		goto fail;
-
-	spin_lock(&dev_priv->fence.lock);
-	sema->mem = drm_mm_search_free(&dev_priv->fence.heap, size, 0, 0);
-	if (sema->mem)
-		sema->mem = drm_mm_get_block_atomic(sema->mem, size, 0);
-	spin_unlock(&dev_priv->fence.lock);
-
-	if (!sema->mem)
-		goto fail;
-
-	kref_init(&sema->ref);
-	sema->dev = dev;
-	for (i = sema->mem->start; i < sema->mem->start + size; i += 4)
-		nouveau_bo_wr32(dev_priv->fence.bo, i / 4, 0);
-
-	return sema;
-fail:
-	kfree(sema);
-	return NULL;
-}
-
-static void
-semaphore_free(struct kref *ref)
-{
-	struct nouveau_semaphore *sema =
-		container_of(ref, struct nouveau_semaphore, ref);
-	struct drm_nouveau_private *dev_priv = sema->dev->dev_private;
-
-	spin_lock(&dev_priv->fence.lock);
-	drm_mm_put_block(sema->mem);
-	spin_unlock(&dev_priv->fence.lock);
-
-	kfree(sema);
-}
-
-static void
-semaphore_work(void *priv, bool signalled)
-{
-	struct nouveau_semaphore *sema = priv;
-	struct drm_nouveau_private *dev_priv = sema->dev->dev_private;
-
-	if (unlikely(!signalled))
-		nouveau_bo_wr32(dev_priv->fence.bo, sema->mem->start / 4, 1);
-
-	kref_put(&sema->ref, semaphore_free);
-}
-
-static int
-semaphore_acquire(struct nouveau_channel *chan, struct nouveau_semaphore *sema)
-{
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_fence *fence = NULL;
-	u64 offset = chan->fence.vma.offset + sema->mem->start;
-	int ret;
-
-	if (dev_priv->chipset < 0x84) {
-		ret = RING_SPACE(chan, 4);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 3);
-		OUT_RING  (chan, NvSema);
-		OUT_RING  (chan, offset);
-		OUT_RING  (chan, 1);
-	} else
-	if (dev_priv->chipset < 0xc0) {
-		ret = RING_SPACE(chan, 7);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
-		OUT_RING  (chan, chan->vram_handle);
-		BEGIN_RING(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 1); /* ACQUIRE_EQ */
-	} else {
-		ret = RING_SPACE(chan, 5);
-		if (ret)
-			return ret;
-
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 0x1001); /* ACQUIRE_EQ */
-	}
-
-	/* Delay semaphore destruction until its work is done */
-	ret = nouveau_fence_new(chan, &fence, true);
-	if (ret)
-		return ret;
-
-	kref_get(&sema->ref);
-	nouveau_fence_work(fence, semaphore_work, sema);
-	nouveau_fence_unref(&fence);
-	return 0;
-}
-
-static int
-semaphore_release(struct nouveau_channel *chan, struct nouveau_semaphore *sema)
-{
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_fence *fence = NULL;
-	u64 offset = chan->fence.vma.offset + sema->mem->start;
-	int ret;
-
-	if (dev_priv->chipset < 0x84) {
-		ret = RING_SPACE(chan, 5);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 2);
-		OUT_RING  (chan, NvSema);
-		OUT_RING  (chan, offset);
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_SEMAPHORE_RELEASE, 1);
-		OUT_RING  (chan, 1);
-	} else
-	if (dev_priv->chipset < 0xc0) {
-		ret = RING_SPACE(chan, 7);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
-		OUT_RING  (chan, chan->vram_handle);
-		BEGIN_RING(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 2); /* RELEASE */
-	} else {
-		ret = RING_SPACE(chan, 5);
-		if (ret)
-			return ret;
-
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 0x1002); /* RELEASE */
-	}
-
-	/* Delay semaphore destruction until its work is done */
-	ret = nouveau_fence_new(chan, &fence, true);
-	if (ret)
-		return ret;
-
-	kref_get(&sema->ref);
-	nouveau_fence_work(fence, semaphore_work, sema);
-	nouveau_fence_unref(&fence);
-	return 0;
-}
-
 int
-nouveau_fence_sync(struct nouveau_fence *fence,
-		   struct nouveau_channel *wchan)
+nouveau_fence_sync(struct nouveau_fence *fence, struct nouveau_channel *chan)
 {
-	struct nouveau_channel *chan = nouveau_fence_channel(fence);
-	struct drm_device *dev = wchan->dev;
-	struct nouveau_semaphore *sema;
+	struct drm_device *dev = chan->dev;
+	struct nouveau_fence_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_channel *prev;
 	int ret = 0;
 
-	if (likely(!chan || chan == wchan ||
-		   nouveau_fence_signalled(fence)))
-		goto out;
-
-	sema = semaphore_alloc(dev);
-	if (!sema) {
-		/* Early card or broken userspace, fall back to
-		 * software sync. */
-		ret = nouveau_fence_wait(fence, true, false);
-		goto out;
-	}
-
-	/* try to take chan's mutex, if we can't take it right away
-	 * we have to fallback to software sync to prevent locking
-	 * order issues
-	 */
-	if (!mutex_trylock(&chan->mutex)) {
-		ret = nouveau_fence_wait(fence, true, false);
-		goto out_unref;
+	prev = fence ? nouveau_channel_get_unlocked(fence->channel) : NULL;
+	if (prev) {
+		if (unlikely(prev != chan && !nouveau_fence_done(fence))) {
+			ret = priv->sync(fence, prev, chan);
+			if (unlikely(ret))
+				ret = nouveau_fence_wait(fence, true, false);
+		}
+		nouveau_channel_put_unlocked(&prev);
 	}
 
-	/* Make wchan wait until it gets signalled */
-	ret = semaphore_acquire(wchan, sema);
-	if (ret)
-		goto out_unlock;
-
-	/* Signal the semaphore from chan */
-	ret = semaphore_release(chan, sema);
-
-out_unlock:
-	mutex_unlock(&chan->mutex);
-out_unref:
-	kref_put(&sema->ref, semaphore_free);
-out:
-	if (chan)
-		nouveau_channel_put_unlocked(&chan);
 	return ret;
 }
 
-int
-__nouveau_fence_flush(void *sync_obj, void *sync_arg)
+static void
+nouveau_fence_del(struct kref *kref)
 {
-	return 0;
+	struct nouveau_fence *fence = container_of(kref, typeof(*fence), kref);
+	kfree(fence);
 }
 
-int
-nouveau_fence_channel_init(struct nouveau_channel *chan)
+void
+nouveau_fence_unref(struct nouveau_fence **pfence)
 {
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpuobj *obj = NULL;
-	int ret;
-
-	if (dev_priv->card_type < NV_C0) {
-		/* Create an NV_SW object for various sync purposes */
-		ret = nouveau_gpuobj_gr_new(chan, NvSw, NV_SW);
-		if (ret)
-			return ret;
-
-		ret = RING_SPACE(chan, 2);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, NvSubSw, NV01_SUBCHAN_OBJECT, 1);
-		OUT_RING  (chan, NvSw);
-		FIRE_RING (chan);
-	}
-
-	/* Setup area of memory shared between all channels for x-chan sync */
-	if (USE_SEMA(dev) && dev_priv->chipset < 0x84) {
-		struct ttm_mem_reg *mem = &dev_priv->fence.bo->bo.mem;
-
-		ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_FROM_MEMORY,
-					     mem->start << PAGE_SHIFT,
-					     mem->size, NV_MEM_ACCESS_RW,
-					     NV_MEM_TARGET_VRAM, &obj);
-		if (ret)
-			return ret;
-
-		ret = nouveau_ramht_insert(chan, NvSema, obj);
-		nouveau_gpuobj_ref(NULL, &obj);
-		if (ret)
-			return ret;
-	} else
-	if (USE_SEMA(dev)) {
-		/* map fence bo into channel's vm */
-		ret = nouveau_bo_vma_add(dev_priv->fence.bo, chan->vm,
-					 &chan->fence.vma);
-		if (ret)
-			return ret;
-	}
-
-	atomic_set(&chan->fence.last_sequence_irq, 0);
-	return 0;
+	if (*pfence)
+		kref_put(&(*pfence)->kref, nouveau_fence_del);
+	*pfence = NULL;
 }
 
-void
-nouveau_fence_channel_fini(struct nouveau_channel *chan)
+struct nouveau_fence *
+nouveau_fence_ref(struct nouveau_fence *fence)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_fence *tmp, *fence;
-
-	spin_lock(&chan->fence.lock);
-	list_for_each_entry_safe(fence, tmp, &chan->fence.pending, entry) {
-		fence->signalled = true;
-		list_del(&fence->entry);
-
-		if (unlikely(fence->work))
-			fence->work(fence->priv, false);
-
-		kref_put(&fence->refcount, nouveau_fence_del);
-	}
-	spin_unlock(&chan->fence.lock);
-
-	nouveau_bo_vma_del(dev_priv->fence.bo, &chan->fence.vma);
+	kref_get(&fence->kref);
+	return fence;
 }
 
 int
-nouveau_fence_init(struct drm_device *dev)
+nouveau_fence_new(struct nouveau_channel *chan, struct nouveau_fence **pfence)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int size = (dev_priv->chipset < 0x84) ? 4096 : 16384;
-	int ret;
-
-	/* Create a shared VRAM heap for cross-channel sync. */
-	if (USE_SEMA(dev)) {
-		ret = nouveau_bo_new(dev, size, 0, TTM_PL_FLAG_VRAM,
-				     0, 0, &dev_priv->fence.bo);
-		if (ret)
-			return ret;
+	struct nouveau_fence *fence;
+	int ret = 0;
 
-		ret = nouveau_bo_pin(dev_priv->fence.bo, TTM_PL_FLAG_VRAM);
-		if (ret)
-			goto fail;
+	if (unlikely(!chan->engctx[NVOBJ_ENGINE_FENCE]))
+		return -ENODEV;
 
-		ret = nouveau_bo_map(dev_priv->fence.bo);
-		if (ret)
-			goto fail;
+	fence = kzalloc(sizeof(*fence), GFP_KERNEL);
+	if (!fence)
+		return -ENOMEM;
+	kref_init(&fence->kref);
 
-		ret = drm_mm_init(&dev_priv->fence.heap, 0,
-				  dev_priv->fence.bo->bo.mem.size);
+	if (chan) {
+		ret = nouveau_fence_emit(fence, chan);
 		if (ret)
-			goto fail;
-
-		spin_lock_init(&dev_priv->fence.lock);
+			nouveau_fence_unref(&fence);
 	}
 
-	return 0;
-fail:
-	nouveau_bo_unmap(dev_priv->fence.bo);
-	nouveau_bo_ref(NULL, &dev_priv->fence.bo);
+	*pfence = fence;
 	return ret;
 }
-
-void
-nouveau_fence_fini(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-
-	if (USE_SEMA(dev)) {
-		drm_mm_takedown(&dev_priv->fence.heap);
-		nouveau_bo_unmap(dev_priv->fence.bo);
-		nouveau_bo_unpin(dev_priv->fence.bo);
-		nouveau_bo_ref(NULL, &dev_priv->fence.bo);
-	}
-}
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.h b/drivers/gpu/drm/nouveau/nouveau_fence.h
new file mode 100644
index 000000000000..82ba733393ae
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.h
@@ -0,0 +1,52 @@
+#ifndef __NOUVEAU_FENCE_H__
+#define __NOUVEAU_FENCE_H__
+
+struct nouveau_fence {
+	struct list_head head;
+	struct kref kref;
+
+	struct nouveau_channel *channel;
+	unsigned long timeout;
+	u32 sequence;
+
+	void (*work)(void *priv, bool signalled);
+	void *priv;
+};
+
+int  nouveau_fence_new(struct nouveau_channel *, struct nouveau_fence **);
+struct nouveau_fence *
+nouveau_fence_ref(struct nouveau_fence *);
+void nouveau_fence_unref(struct nouveau_fence **);
+
+int  nouveau_fence_emit(struct nouveau_fence *, struct nouveau_channel *);
+bool nouveau_fence_done(struct nouveau_fence *);
+int  nouveau_fence_wait(struct nouveau_fence *, bool lazy, bool intr);
+int  nouveau_fence_sync(struct nouveau_fence *, struct nouveau_channel *);
+void nouveau_fence_idle(struct nouveau_channel *);
+void nouveau_fence_update(struct nouveau_channel *);
+
+struct nouveau_fence_chan {
+	struct list_head pending;
+	spinlock_t lock;
+	u32 sequence;
+};
+
+struct nouveau_fence_priv {
+	struct nouveau_exec_engine engine;
+	int (*emit)(struct nouveau_fence *);
+	int (*sync)(struct nouveau_fence *, struct nouveau_channel *,
+		    struct nouveau_channel *);
+	u32 (*read)(struct nouveau_channel *);
+};
+
+void nouveau_fence_context_new(struct nouveau_fence_chan *);
+void nouveau_fence_context_del(struct nouveau_fence_chan *);
+
+int nv04_fence_create(struct drm_device *dev);
+int nv04_fence_mthd(struct nouveau_channel *, u32, u32, u32);
+
+int nv10_fence_create(struct drm_device *dev);
+int nv84_fence_create(struct drm_device *dev);
+int nvc0_fence_create(struct drm_device *dev);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_fifo.h b/drivers/gpu/drm/nouveau/nouveau_fifo.h
new file mode 100644
index 000000000000..ce99cab2f257
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_fifo.h
@@ -0,0 +1,32 @@
+#ifndef __NOUVEAU_FIFO_H__
+#define __NOUVEAU_FIFO_H__
+
+struct nouveau_fifo_priv {
+	struct nouveau_exec_engine base;
+	u32 channels;
+};
+
+struct nouveau_fifo_chan {
+};
+
+bool nv04_fifo_cache_pull(struct drm_device *, bool);
+void nv04_fifo_context_del(struct nouveau_channel *, int);
+int  nv04_fifo_fini(struct drm_device *, int, bool);
+int  nv04_fifo_init(struct drm_device *, int);
+void nv04_fifo_isr(struct drm_device *);
+void nv04_fifo_destroy(struct drm_device *, int);
+
+void nv50_fifo_playlist_update(struct drm_device *);
+void nv50_fifo_destroy(struct drm_device *, int);
+void nv50_fifo_tlb_flush(struct drm_device *, int);
+
+int  nv04_fifo_create(struct drm_device *);
+int  nv10_fifo_create(struct drm_device *);
+int  nv17_fifo_create(struct drm_device *);
+int  nv40_fifo_create(struct drm_device *);
+int  nv50_fifo_create(struct drm_device *);
+int  nv84_fifo_create(struct drm_device *);
+int  nvc0_fifo_create(struct drm_device *);
+int  nve0_fifo_create(struct drm_device *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.c b/drivers/gpu/drm/nouveau/nouveau_gem.c
index ed52a6f41613..30f542316944 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.c
@@ -23,12 +23,14 @@
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  *
  */
+#include <linux/dma-buf.h>
 #include "drmP.h"
 #include "drm.h"
 
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
+#include "nouveau_fence.h"
 
 #define nouveau_gem_pushbuf_sync(chan) 0
 
@@ -53,6 +55,9 @@ nouveau_gem_object_del(struct drm_gem_object *gem)
 		nouveau_bo_unpin(nvbo);
 	}
 
+	if (gem->import_attach)
+		drm_prime_gem_destroy(gem, nvbo->bo.sg);
+
 	ttm_bo_unref(&bo);
 
 	drm_gem_object_release(gem);
@@ -139,7 +144,7 @@ nouveau_gem_new(struct drm_device *dev, int size, int align, uint32_t domain,
 		flags |= TTM_PL_FLAG_SYSTEM;
 
 	ret = nouveau_bo_new(dev, size, align, flags, tile_mode,
-			     tile_flags, pnvbo);
+			     tile_flags, NULL, pnvbo);
 	if (ret)
 		return ret;
 	nvbo = *pnvbo;
@@ -704,7 +709,7 @@ nouveau_gem_ioctl_pushbuf(struct drm_device *dev, void *data,
 	}
 
 	if (chan->dma.ib_max) {
-		ret = nouveau_dma_wait(chan, req->nr_push + 1, 6);
+		ret = nouveau_dma_wait(chan, req->nr_push + 1, 16);
 		if (ret) {
 			NV_INFO(dev, "nv50cal_space: %d\n", ret);
 			goto out;
@@ -774,7 +779,7 @@ nouveau_gem_ioctl_pushbuf(struct drm_device *dev, void *data,
 		}
 	}
 
-	ret = nouveau_fence_new(chan, &fence, true);
+	ret = nouveau_fence_new(chan, &fence);
 	if (ret) {
 		NV_ERROR(dev, "error fencing pushbuf: %d\n", ret);
 		WIND_RING(chan);
diff --git a/drivers/gpu/drm/nouveau/nouveau_gpio.c b/drivers/gpu/drm/nouveau/nouveau_gpio.c
index a580cc62337a..82c19e82ff02 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gpio.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gpio.c
@@ -387,7 +387,7 @@ nouveau_gpio_reset(struct drm_device *dev)
 		if (dev_priv->card_type >= NV_D0) {
 			nv_mask(dev, 0x00d610 + (line * 4), 0xff, unk0);
 			if (unk1--)
-				nv_mask(dev, 0x00d640 + (unk1 * 4), 0xff, line);
+				nv_mask(dev, 0x00d740 + (unk1 * 4), 0xff, line);
 		} else
 		if (dev_priv->card_type >= NV_50) {
 			static const u32 regs[] = { 0xe100, 0xe28c };
diff --git a/drivers/gpu/drm/nouveau/nouveau_grctx.h b/drivers/gpu/drm/nouveau/nouveau_grctx.h
index 86c2e374e938..b0795ececbda 100644
--- a/drivers/gpu/drm/nouveau/nouveau_grctx.h
+++ b/drivers/gpu/drm/nouveau/nouveau_grctx.h
@@ -18,7 +18,6 @@ struct nouveau_grctx {
 	uint32_t ctxvals_base;
 };
 
-#ifdef CP_CTX
 static inline void
 cp_out(struct nouveau_grctx *ctx, uint32_t inst)
 {
@@ -88,10 +87,8 @@ _cp_bra(struct nouveau_grctx *ctx, u32 mod, int flag, int state, int name)
 		    (state ? 0 : CP_BRA_IF_CLEAR));
 }
 #define cp_bra(c, f, s, n) _cp_bra((c), 0, CP_FLAG_##f, CP_FLAG_##f##_##s, n)
-#ifdef CP_BRA_MOD
 #define cp_cal(c, f, s, n) _cp_bra((c), 1, CP_FLAG_##f, CP_FLAG_##f##_##s, n)
 #define cp_ret(c, f, s) _cp_bra((c), 2, CP_FLAG_##f, CP_FLAG_##f##_##s, 0)
-#endif
 
 static inline void
 _cp_wait(struct nouveau_grctx *ctx, int flag, int state)
@@ -128,6 +125,5 @@ gr_def(struct nouveau_grctx *ctx, uint32_t reg, uint32_t val)
 
 	nv_wo32(ctx->data, reg * 4, val);
 }
-#endif
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_hw.c b/drivers/gpu/drm/nouveau/nouveau_hw.c
index ba896e54b799..b87ad3bd7739 100644
--- a/drivers/gpu/drm/nouveau/nouveau_hw.c
+++ b/drivers/gpu/drm/nouveau/nouveau_hw.c
@@ -1018,11 +1018,6 @@ nv_load_state_ext(struct drm_device *dev, int head,
 	}
 
 	NVWriteCRTC(dev, head, NV_PCRTC_START, regp->fb_start);
-
-	/* Enable vblank interrupts. */
-	NVWriteCRTC(dev, head, NV_PCRTC_INTR_EN_0,
-		    (dev->vblank_enabled[head] ? 1 : 0));
-	NVWriteCRTC(dev, head, NV_PCRTC_INTR_0, NV_PCRTC_INTR_0_VBLANK);
 }
 
 static void
diff --git a/drivers/gpu/drm/nouveau/nouveau_mem.c b/drivers/gpu/drm/nouveau/nouveau_mem.c
index b08065f981df..5b498ea32e14 100644
--- a/drivers/gpu/drm/nouveau/nouveau_mem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_mem.c
@@ -39,6 +39,8 @@
 #include "nouveau_pm.h"
 #include "nouveau_mm.h"
 #include "nouveau_vm.h"
+#include "nouveau_fifo.h"
+#include "nouveau_fence.h"
 
 /*
  * NV10-NV40 tiling helpers
@@ -50,7 +52,6 @@ nv10_mem_update_tile_region(struct drm_device *dev,
 			    uint32_t size, uint32_t pitch, uint32_t flags)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_fb_engine *pfb = &dev_priv->engine.fb;
 	int i = tile - dev_priv->tile.reg, j;
 	unsigned long save;
@@ -64,8 +65,8 @@ nv10_mem_update_tile_region(struct drm_device *dev,
 		pfb->init_tile_region(dev, i, addr, size, pitch, flags);
 
 	spin_lock_irqsave(&dev_priv->context_switch_lock, save);
-	pfifo->reassign(dev, false);
-	pfifo->cache_pull(dev, false);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 0);
+	nv04_fifo_cache_pull(dev, false);
 
 	nouveau_wait_for_idle(dev);
 
@@ -75,8 +76,8 @@ nv10_mem_update_tile_region(struct drm_device *dev,
 			dev_priv->eng[j]->set_tile_region(dev, i);
 	}
 
-	pfifo->cache_pull(dev, true);
-	pfifo->reassign(dev, true);
+	nv04_fifo_cache_pull(dev, true);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, save);
 }
 
@@ -89,7 +90,7 @@ nv10_mem_get_tile_region(struct drm_device *dev, int i)
 	spin_lock(&dev_priv->tile.lock);
 
 	if (!tile->used &&
-	    (!tile->fence || nouveau_fence_signalled(tile->fence)))
+	    (!tile->fence || nouveau_fence_done(tile->fence)))
 		tile->used = true;
 	else
 		tile = NULL;
@@ -416,7 +417,7 @@ nouveau_mem_vram_init(struct drm_device *dev)
 
 	if (dev_priv->card_type < NV_50) {
 		ret = nouveau_bo_new(dev, 256*1024, 0, TTM_PL_FLAG_VRAM,
-				     0, 0, &dev_priv->vga_ram);
+				     0, 0, NULL, &dev_priv->vga_ram);
 		if (ret == 0)
 			ret = nouveau_bo_pin(dev_priv->vga_ram,
 					     TTM_PL_FLAG_VRAM);
@@ -843,6 +844,7 @@ nouveau_mem_timing_calc(struct drm_device *dev, u32 freq,
 		ret = nv50_mem_timing_calc(dev, freq, e, len, boot, t);
 		break;
 	case NV_C0:
+	case NV_D0:
 		ret = nvc0_mem_timing_calc(dev, freq, e, len, boot, t);
 		break;
 	default:
@@ -977,6 +979,8 @@ nouveau_mem_exec(struct nouveau_mem_exec_func *exec,
 		break;
 	case NV_MEM_TYPE_DDR3:
 		tDLLK = 12000;
+		tCKSRE = 2000;
+		tXS = 1000;
 		mr1_dlloff = 0x00000001;
 		break;
 	case NV_MEM_TYPE_GDDR3:
@@ -1023,6 +1027,7 @@ nouveau_mem_exec(struct nouveau_mem_exec_func *exec,
 	exec->refresh_self(exec, false);
 	exec->refresh_auto(exec, true);
 	exec->wait(exec, tXS);
+	exec->wait(exec, tXS);
 
 	/* update MRs */
 	if (mr[2] != info->mr[2]) {
diff --git a/drivers/gpu/drm/nouveau/nouveau_object.c b/drivers/gpu/drm/nouveau/nouveau_object.c
index cc419fae794b..b190cc01c820 100644
--- a/drivers/gpu/drm/nouveau/nouveau_object.c
+++ b/drivers/gpu/drm/nouveau/nouveau_object.c
@@ -34,9 +34,10 @@
 #include "drm.h"
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
+#include "nouveau_software.h"
 #include "nouveau_vm.h"
-#include "nv50_display.h"
 
 struct nouveau_gpuobj_method {
 	struct list_head head;
@@ -120,12 +121,13 @@ nouveau_gpuobj_mthd_call2(struct drm_device *dev, int chid,
 			  u32 class, u32 mthd, u32 data)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_channel *chan = NULL;
 	unsigned long flags;
 	int ret = -EINVAL;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	if (chid >= 0 && chid < dev_priv->engine.fifo.channels)
+	if (chid >= 0 && chid < pfifo->channels)
 		chan = dev_priv->channels.ptr[chid];
 	if (chan)
 		ret = nouveau_gpuobj_mthd_call(chan, class, mthd, data);
@@ -133,37 +135,6 @@ nouveau_gpuobj_mthd_call2(struct drm_device *dev, int chid,
 	return ret;
 }
 
-/* NVidia uses context objects to drive drawing operations.
-
-   Context objects can be selected into 8 subchannels in the FIFO,
-   and then used via DMA command buffers.
-
-   A context object is referenced by a user defined handle (CARD32). The HW
-   looks up graphics objects in a hash table in the instance RAM.
-
-   An entry in the hash table consists of 2 CARD32. The first CARD32 contains
-   the handle, the second one a bitfield, that contains the address of the
-   object in instance RAM.
-
-   The format of the second CARD32 seems to be:
-
-   NV4 to NV30:
-
-   15: 0  instance_addr >> 4
-   17:16  engine (here uses 1 = graphics)
-   28:24  channel id (here uses 0)
-   31	  valid (use 1)
-
-   NV40:
-
-   15: 0  instance_addr >> 4   (maybe 19-0)
-   21:20  engine (here uses 1 = graphics)
-   I'm unsure about the other bits, but using 0 seems to work.
-
-   The key into the hash table depends on the object handle and channel id and
-   is given as:
-*/
-
 int
 nouveau_gpuobj_new(struct drm_device *dev, struct nouveau_channel *chan,
 		   uint32_t size, int align, uint32_t flags,
@@ -267,7 +238,7 @@ nouveau_gpuobj_takedown(struct drm_device *dev)
 		kfree(oc);
 	}
 
-	BUG_ON(!list_empty(&dev_priv->gpuobj_list));
+	WARN_ON(!list_empty(&dev_priv->gpuobj_list));
 }
 
 
@@ -361,34 +332,6 @@ nouveau_gpuobj_new_fake(struct drm_device *dev, u32 pinst, u64 vinst,
 	return 0;
 }
 
-/*
-   DMA objects are used to reference a piece of memory in the
-   framebuffer, PCI or AGP address space. Each object is 16 bytes big
-   and looks as follows:
-
-   entry[0]
-   11:0  class (seems like I can always use 0 here)
-   12    page table present?
-   13    page entry linear?
-   15:14 access: 0 rw, 1 ro, 2 wo
-   17:16 target: 0 NV memory, 1 NV memory tiled, 2 PCI, 3 AGP
-   31:20 dma adjust (bits 0-11 of the address)
-   entry[1]
-   dma limit (size of transfer)
-   entry[X]
-   1     0 readonly, 1 readwrite
-   31:12 dma frame address of the page (bits 12-31 of the address)
-   entry[N]
-   page table terminator, same value as the first pte, as does nvidia
-   rivatv uses 0xffffffff
-
-   Non linear page tables need a list of frame addresses afterwards,
-   the rivatv project has some info on this.
-
-   The method below creates a DMA object in instance RAM and returns a handle
-   to it that can be used to set up context objects.
-*/
-
 void
 nv50_gpuobj_dma_init(struct nouveau_gpuobj *obj, u32 offset, int class,
 		     u64 base, u64 size, int target, int access,
@@ -540,82 +483,6 @@ nouveau_gpuobj_dma_new(struct nouveau_channel *chan, int class, u64 base,
 	return 0;
 }
 
-/* Context objects in the instance RAM have the following structure.
- * On NV40 they are 32 byte long, on NV30 and smaller 16 bytes.
-
-   NV4 - NV30:
-
-   entry[0]
-   11:0 class
-   12   chroma key enable
-   13   user clip enable
-   14   swizzle enable
-   17:15 patch config:
-       scrcopy_and, rop_and, blend_and, scrcopy, srccopy_pre, blend_pre
-   18   synchronize enable
-   19   endian: 1 big, 0 little
-   21:20 dither mode
-   23    single step enable
-   24    patch status: 0 invalid, 1 valid
-   25    context_surface 0: 1 valid
-   26    context surface 1: 1 valid
-   27    context pattern: 1 valid
-   28    context rop: 1 valid
-   29,30 context beta, beta4
-   entry[1]
-   7:0   mono format
-   15:8  color format
-   31:16 notify instance address
-   entry[2]
-   15:0  dma 0 instance address
-   31:16 dma 1 instance address
-   entry[3]
-   dma method traps
-
-   NV40:
-   No idea what the exact format is. Here's what can be deducted:
-
-   entry[0]:
-   11:0  class  (maybe uses more bits here?)
-   17    user clip enable
-   21:19 patch config
-   25    patch status valid ?
-   entry[1]:
-   15:0  DMA notifier  (maybe 20:0)
-   entry[2]:
-   15:0  DMA 0 instance (maybe 20:0)
-   24    big endian
-   entry[3]:
-   15:0  DMA 1 instance (maybe 20:0)
-   entry[4]:
-   entry[5]:
-   set to 0?
-*/
-static int
-nouveau_gpuobj_sw_new(struct nouveau_channel *chan, u32 handle, u16 class)
-{
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_gpuobj *gpuobj;
-	int ret;
-
-	gpuobj = kzalloc(sizeof(*gpuobj), GFP_KERNEL);
-	if (!gpuobj)
-		return -ENOMEM;
-	gpuobj->dev = chan->dev;
-	gpuobj->engine = NVOBJ_ENGINE_SW;
-	gpuobj->class = class;
-	kref_init(&gpuobj->refcount);
-	gpuobj->cinst = 0x40;
-
-	spin_lock(&dev_priv->ramin_lock);
-	list_add_tail(&gpuobj->list, &dev_priv->gpuobj_list);
-	spin_unlock(&dev_priv->ramin_lock);
-
-	ret = nouveau_ramht_insert(chan, handle, gpuobj);
-	nouveau_gpuobj_ref(NULL, &gpuobj);
-	return ret;
-}
-
 int
 nouveau_gpuobj_gr_new(struct nouveau_channel *chan, u32 handle, int class)
 {
@@ -632,9 +499,6 @@ nouveau_gpuobj_gr_new(struct nouveau_channel *chan, u32 handle, int class)
 		if (oc->id != class)
 			continue;
 
-		if (oc->engine == NVOBJ_ENGINE_SW)
-			return nouveau_gpuobj_sw_new(chan, handle, class);
-
 		if (!chan->engctx[oc->engine]) {
 			ret = eng->context_new(chan, oc->engine);
 			if (ret)
@@ -644,7 +508,6 @@ nouveau_gpuobj_gr_new(struct nouveau_channel *chan, u32 handle, int class)
 		return eng->object_new(chan, oc->engine, handle, class);
 	}
 
-	NV_ERROR(dev, "illegal object class: 0x%x\n", class);
 	return -EINVAL;
 }
 
@@ -693,11 +556,10 @@ nouveau_gpuobj_channel_init_pramin(struct nouveau_channel *chan)
 static int
 nvc0_gpuobj_channel_init(struct nouveau_channel *chan, struct nouveau_vm *vm)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct drm_device *dev = chan->dev;
 	struct nouveau_gpuobj *pgd = NULL;
 	struct nouveau_vm_pgd *vpgd;
-	int ret, i;
+	int ret;
 
 	ret = nouveau_gpuobj_new(dev, NULL, 4096, 0x1000, 0, &chan->ramin);
 	if (ret)
@@ -722,19 +584,6 @@ nvc0_gpuobj_channel_init(struct nouveau_channel *chan, struct nouveau_vm *vm)
 	nv_wo32(chan->ramin, 0x0208, 0xffffffff);
 	nv_wo32(chan->ramin, 0x020c, 0x000000ff);
 
-	/* map display semaphore buffers into channel's vm */
-	for (i = 0; i < dev->mode_config.num_crtc; i++) {
-		struct nouveau_bo *bo;
-		if (dev_priv->card_type >= NV_D0)
-			bo = nvd0_display_crtc_sema(dev, i);
-		else
-			bo = nv50_display(dev)->crtc[i].sem.bo;
-
-		ret = nouveau_bo_vma_add(bo, chan->vm, &chan->dispc_vma[i]);
-		if (ret)
-			return ret;
-	}
-
 	return 0;
 }
 
@@ -747,7 +596,7 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 	struct nouveau_fpriv *fpriv = nouveau_fpriv(chan->file_priv);
 	struct nouveau_vm *vm = fpriv ? fpriv->vm : dev_priv->chan_vm;
 	struct nouveau_gpuobj *vram = NULL, *tt = NULL;
-	int ret, i;
+	int ret;
 
 	NV_DEBUG(dev, "ch%d vram=0x%08x tt=0x%08x\n", chan->id, vram_h, tt_h);
 	if (dev_priv->card_type >= NV_C0)
@@ -795,25 +644,6 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 		nouveau_gpuobj_ref(NULL, &ramht);
 		if (ret)
 			return ret;
-
-		/* dma objects for display sync channel semaphore blocks */
-		for (i = 0; i < dev->mode_config.num_crtc; i++) {
-			struct nouveau_gpuobj *sem = NULL;
-			struct nv50_display_crtc *dispc =
-				&nv50_display(dev)->crtc[i];
-			u64 offset = dispc->sem.bo->bo.offset;
-
-			ret = nouveau_gpuobj_dma_new(chan, 0x3d, offset, 0xfff,
-						     NV_MEM_ACCESS_RW,
-						     NV_MEM_TARGET_VRAM, &sem);
-			if (ret)
-				return ret;
-
-			ret = nouveau_ramht_insert(chan, NvEvoSema0 + i, sem);
-			nouveau_gpuobj_ref(NULL, &sem);
-			if (ret)
-				return ret;
-		}
 	}
 
 	/* VRAM ctxdma */
@@ -873,25 +703,7 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 void
 nouveau_gpuobj_channel_takedown(struct nouveau_channel *chan)
 {
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int i;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	if (dev_priv->card_type >= NV_D0) {
-		for (i = 0; i < dev->mode_config.num_crtc; i++) {
-			struct nouveau_bo *bo = nvd0_display_crtc_sema(dev, i);
-			nouveau_bo_vma_del(bo, &chan->dispc_vma[i]);
-		}
-	} else
-	if (dev_priv->card_type >= NV_50) {
-		struct nv50_display *disp = nv50_display(dev);
-		for (i = 0; i < dev->mode_config.num_crtc; i++) {
-			struct nv50_display_crtc *dispc = &disp->crtc[i];
-			nouveau_bo_vma_del(dispc->sem.bo, &chan->dispc_vma[i]);
-		}
-	}
+	NV_DEBUG(chan->dev, "ch%d\n", chan->id);
 
 	nouveau_vm_ref(NULL, &chan->vm, chan->vm_pd);
 	nouveau_gpuobj_ref(NULL, &chan->vm_pd);
@@ -956,6 +768,17 @@ int nouveau_ioctl_grobj_alloc(struct drm_device *dev, void *data,
 	if (init->handle == ~0)
 		return -EINVAL;
 
+	/* compatibility with userspace that assumes 506e for all chipsets */
+	if (init->class == 0x506e) {
+		init->class = nouveau_software_class(dev);
+		if (init->class == 0x906e)
+			return 0;
+	} else
+	if (init->class == 0x906e) {
+		NV_ERROR(dev, "906e not supported yet\n");
+		return -EINVAL;
+	}
+
 	chan = nouveau_channel_get(file_priv, init->channel);
 	if (IS_ERR(chan))
 		return PTR_ERR(chan);
diff --git a/drivers/gpu/drm/nouveau/nouveau_perf.c b/drivers/gpu/drm/nouveau/nouveau_perf.c
index 69a528d106e6..ea6acf1c4a78 100644
--- a/drivers/gpu/drm/nouveau/nouveau_perf.c
+++ b/drivers/gpu/drm/nouveau/nouveau_perf.c
@@ -83,7 +83,7 @@ nouveau_perf_entry(struct drm_device *dev, int idx,
 	return NULL;
 }
 
-static u8 *
+u8 *
 nouveau_perf_rammap(struct drm_device *dev, u32 freq,
 		    u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
 {
diff --git a/drivers/gpu/drm/nouveau/nouveau_pm.h b/drivers/gpu/drm/nouveau/nouveau_pm.h
index 3f82dfea61dd..07cac72c72b4 100644
--- a/drivers/gpu/drm/nouveau/nouveau_pm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_pm.h
@@ -61,8 +61,10 @@ int  nouveau_voltage_gpio_set(struct drm_device *, int voltage);
 /* nouveau_perf.c */
 void nouveau_perf_init(struct drm_device *);
 void nouveau_perf_fini(struct drm_device *);
-u8 *nouveau_perf_timing(struct drm_device *, u32 freq, u8 *ver, u8 *len);
+u8 *nouveau_perf_rammap(struct drm_device *, u32 freq, u8 *ver,
+			u8 *hdr, u8 *cnt, u8 *len);
 u8 *nouveau_perf_ramcfg(struct drm_device *, u32 freq, u8 *ver, u8 *len);
+u8 *nouveau_perf_timing(struct drm_device *, u32 freq, u8 *ver, u8 *len);
 
 /* nouveau_mem.c */
 void nouveau_mem_timing_init(struct drm_device *);
diff --git a/drivers/gpu/drm/nouveau/nouveau_prime.c b/drivers/gpu/drm/nouveau/nouveau_prime.c
new file mode 100644
index 000000000000..c58aab7370c5
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_prime.c
@@ -0,0 +1,163 @@
+
+#include "drmP.h"
+#include "drm.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_drm.h"
+#include "nouveau_dma.h"
+
+#include <linux/dma-buf.h>
+
+static struct sg_table *nouveau_gem_map_dma_buf(struct dma_buf_attachment *attachment,
+					  enum dma_data_direction dir)
+{
+	struct nouveau_bo *nvbo = attachment->dmabuf->priv;
+	struct drm_device *dev = nvbo->gem->dev;
+	int npages = nvbo->bo.num_pages;
+	struct sg_table *sg;
+	int nents;
+
+	mutex_lock(&dev->struct_mutex);
+	sg = drm_prime_pages_to_sg(nvbo->bo.ttm->pages, npages);
+	nents = dma_map_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	mutex_unlock(&dev->struct_mutex);
+	return sg;
+}
+
+static void nouveau_gem_unmap_dma_buf(struct dma_buf_attachment *attachment,
+				      struct sg_table *sg, enum dma_data_direction dir)
+{
+	dma_unmap_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	sg_free_table(sg);
+	kfree(sg);
+}
+
+static void nouveau_gem_dmabuf_release(struct dma_buf *dma_buf)
+{
+	struct nouveau_bo *nvbo = dma_buf->priv;
+
+	if (nvbo->gem->export_dma_buf == dma_buf) {
+		nvbo->gem->export_dma_buf = NULL;
+		drm_gem_object_unreference_unlocked(nvbo->gem);
+	}
+}
+
+static void *nouveau_gem_kmap_atomic(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void nouveau_gem_kunmap_atomic(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+static void *nouveau_gem_kmap(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void nouveau_gem_kunmap(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+
+static const struct dma_buf_ops nouveau_dmabuf_ops =  {
+	.map_dma_buf = nouveau_gem_map_dma_buf,
+	.unmap_dma_buf = nouveau_gem_unmap_dma_buf,
+	.release = nouveau_gem_dmabuf_release,
+	.kmap = nouveau_gem_kmap,
+	.kmap_atomic = nouveau_gem_kmap_atomic,
+	.kunmap = nouveau_gem_kunmap,
+	.kunmap_atomic = nouveau_gem_kunmap_atomic,
+};
+
+static int
+nouveau_prime_new(struct drm_device *dev,
+		  size_t size,
+		  struct sg_table *sg,
+		  struct nouveau_bo **pnvbo)
+{
+	struct nouveau_bo *nvbo;
+	u32 flags = 0;
+	int ret;
+
+	flags = TTM_PL_FLAG_TT;
+
+	ret = nouveau_bo_new(dev, size, 0, flags, 0, 0,
+			     sg, pnvbo);
+	if (ret)
+		return ret;
+	nvbo = *pnvbo;
+
+	/* we restrict allowed domains on nv50+ to only the types
+	 * that were requested at creation time.  not possibly on
+	 * earlier chips without busting the ABI.
+	 */
+	nvbo->valid_domains = NOUVEAU_GEM_DOMAIN_GART;
+	nvbo->gem = drm_gem_object_alloc(dev, nvbo->bo.mem.size);
+	if (!nvbo->gem) {
+		nouveau_bo_ref(NULL, pnvbo);
+		return -ENOMEM;
+	}
+
+	nvbo->gem->driver_private = nvbo;
+	return 0;
+}
+
+struct dma_buf *nouveau_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *obj, int flags)
+{
+	struct nouveau_bo *nvbo = nouveau_gem_object(obj);
+	int ret = 0;
+
+	/* pin buffer into GTT */
+	ret = nouveau_bo_pin(nvbo, TTM_PL_FLAG_TT);
+	if (ret)
+		return ERR_PTR(-EINVAL);
+
+	return dma_buf_export(nvbo, &nouveau_dmabuf_ops, obj->size, flags);
+}
+
+struct drm_gem_object *nouveau_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct nouveau_bo *nvbo;
+	int ret;
+
+	if (dma_buf->ops == &nouveau_dmabuf_ops) {
+		nvbo = dma_buf->priv;
+		if (nvbo->gem) {
+			if (nvbo->gem->dev == dev) {
+				drm_gem_object_reference(nvbo->gem);
+				return nvbo->gem;
+			}
+		}
+	}
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_PTR(PTR_ERR(attach));
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	ret = nouveau_prime_new(dev, dma_buf->size, sg, &nvbo);
+	if (ret)
+		goto fail_unmap;
+
+	nvbo->gem->import_attach = attach;
+
+	return nvbo->gem;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
+
diff --git a/drivers/gpu/drm/nouveau/nouveau_sgdma.c b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
index 47f245edf538..38483a042bc2 100644
--- a/drivers/gpu/drm/nouveau/nouveau_sgdma.c
+++ b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
@@ -290,7 +290,10 @@ nv50_sgdma_bind(struct ttm_tt *ttm, struct ttm_mem_reg *mem)
 	struct nouveau_mem *node = mem->mm_node;
 
 	/* noop: bound in move_notify() */
-	node->pages = nvbe->ttm.dma_address;
+	if (ttm->sg) {
+		node->sg = ttm->sg;
+	} else
+		node->pages = nvbe->ttm.dma_address;
 	return 0;
 }
 
@@ -338,10 +341,10 @@ nouveau_sgdma_init(struct drm_device *dev)
 	u32 aper_size, align;
 	int ret;
 
-	if (dev_priv->card_type >= NV_40 && pci_is_pcie(dev->pdev))
+	if (dev_priv->card_type >= NV_40)
 		aper_size = 512 * 1024 * 1024;
 	else
-		aper_size = 64 * 1024 * 1024;
+		aper_size = 128 * 1024 * 1024;
 
 	/* Dear NVIDIA, NV44+ would like proper present bits in PTEs for
 	 * christmas.  The cards before it have them, the cards after
diff --git a/drivers/gpu/drm/nouveau/nouveau_software.h b/drivers/gpu/drm/nouveau/nouveau_software.h
new file mode 100644
index 000000000000..e60bc6ce9003
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_software.h
@@ -0,0 +1,69 @@
+#ifndef __NOUVEAU_SOFTWARE_H__
+#define __NOUVEAU_SOFTWARE_H__
+
+struct nouveau_software_priv {
+	struct nouveau_exec_engine base;
+	struct list_head vblank;
+};
+
+struct nouveau_software_chan {
+	struct list_head flip;
+	struct {
+		struct list_head list;
+		struct nouveau_bo *bo;
+		u32 offset;
+		u32 value;
+		u32 head;
+	} vblank;
+};
+
+static inline void
+nouveau_software_vblank(struct drm_device *dev, int crtc)
+{
+	struct nouveau_software_priv *psw = nv_engine(dev, NVOBJ_ENGINE_SW);
+	struct nouveau_software_chan *pch, *tmp;
+
+	list_for_each_entry_safe(pch, tmp, &psw->vblank, vblank.list) {
+		if (pch->vblank.head != crtc)
+			continue;
+
+		nouveau_bo_wr32(pch->vblank.bo, pch->vblank.offset,
+						pch->vblank.value);
+		list_del(&pch->vblank.list);
+		drm_vblank_put(dev, crtc);
+	}
+}
+
+static inline void
+nouveau_software_context_new(struct nouveau_software_chan *pch)
+{
+	INIT_LIST_HEAD(&pch->flip);
+}
+
+static inline void
+nouveau_software_create(struct nouveau_software_priv *psw)
+{
+	INIT_LIST_HEAD(&psw->vblank);
+}
+
+static inline u16
+nouveau_software_class(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	if (dev_priv->card_type <= NV_04)
+		return 0x006e;
+	if (dev_priv->card_type <= NV_40)
+		return 0x016e;
+	if (dev_priv->card_type <= NV_50)
+		return 0x506e;
+	if (dev_priv->card_type <= NV_E0)
+		return 0x906e;
+	return 0x0000;
+}
+
+int nv04_software_create(struct drm_device *);
+int nv50_software_create(struct drm_device *);
+int nvc0_software_create(struct drm_device *);
+u64 nvc0_software_crtc(struct nouveau_channel *, int crtc);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_state.c b/drivers/gpu/drm/nouveau/nouveau_state.c
index c2a8511e855a..19706f0532ea 100644
--- a/drivers/gpu/drm/nouveau/nouveau_state.c
+++ b/drivers/gpu/drm/nouveau/nouveau_state.c
@@ -39,6 +39,9 @@
 #include "nouveau_gpio.h"
 #include "nouveau_pm.h"
 #include "nv50_display.h"
+#include "nouveau_fifo.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
 
 static void nouveau_stub_takedown(struct drm_device *dev) {}
 static int nouveau_stub_init(struct drm_device *dev) { return 0; }
@@ -66,18 +69,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nv04_fb_init;
 		engine->fb.takedown		= nv04_fb_takedown;
-		engine->fifo.channels		= 16;
-		engine->fifo.init		= nv04_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv04_fifo_channel_id;
-		engine->fifo.create_context	= nv04_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv04_fifo_load_context;
-		engine->fifo.unload_context	= nv04_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -111,18 +102,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv10_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv10_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv10_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv10_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv10_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv10_fifo_load_context;
-		engine->fifo.unload_context	= nv10_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -162,18 +141,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv20_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv20_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv20_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv10_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv10_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv10_fifo_load_context;
-		engine->fifo.unload_context	= nv10_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -209,18 +176,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv30_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv10_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv30_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv10_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv10_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv10_fifo_load_context;
-		engine->fifo.unload_context	= nv10_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -259,18 +214,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv30_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv40_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv30_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv40_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv40_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv40_fifo_load_context;
-		engine->fifo.unload_context	= nv40_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -317,18 +260,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nv50_fb_init;
 		engine->fb.takedown		= nv50_fb_takedown;
-		engine->fifo.channels		= 128;
-		engine->fifo.init		= nv50_fifo_init;
-		engine->fifo.takedown		= nv50_fifo_takedown;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.channel_id		= nv50_fifo_channel_id;
-		engine->fifo.create_context	= nv50_fifo_create_context;
-		engine->fifo.destroy_context	= nv50_fifo_destroy_context;
-		engine->fifo.load_context	= nv50_fifo_load_context;
-		engine->fifo.unload_context	= nv50_fifo_unload_context;
-		engine->fifo.tlb_flush		= nv50_fifo_tlb_flush;
 		engine->display.early_init	= nv50_display_early_init;
 		engine->display.late_takedown	= nv50_display_late_takedown;
 		engine->display.create		= nv50_display_create;
@@ -392,17 +323,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nvc0_fb_init;
 		engine->fb.takedown		= nvc0_fb_takedown;
-		engine->fifo.channels		= 128;
-		engine->fifo.init		= nvc0_fifo_init;
-		engine->fifo.takedown		= nvc0_fifo_takedown;
-		engine->fifo.disable		= nvc0_fifo_disable;
-		engine->fifo.enable		= nvc0_fifo_enable;
-		engine->fifo.reassign		= nvc0_fifo_reassign;
-		engine->fifo.channel_id		= nvc0_fifo_channel_id;
-		engine->fifo.create_context	= nvc0_fifo_create_context;
-		engine->fifo.destroy_context	= nvc0_fifo_destroy_context;
-		engine->fifo.load_context	= nvc0_fifo_load_context;
-		engine->fifo.unload_context	= nvc0_fifo_unload_context;
 		engine->display.early_init	= nv50_display_early_init;
 		engine->display.late_takedown	= nv50_display_late_takedown;
 		engine->display.create		= nv50_display_create;
@@ -445,17 +365,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nvc0_fb_init;
 		engine->fb.takedown		= nvc0_fb_takedown;
-		engine->fifo.channels		= 128;
-		engine->fifo.init		= nvc0_fifo_init;
-		engine->fifo.takedown		= nvc0_fifo_takedown;
-		engine->fifo.disable		= nvc0_fifo_disable;
-		engine->fifo.enable		= nvc0_fifo_enable;
-		engine->fifo.reassign		= nvc0_fifo_reassign;
-		engine->fifo.channel_id		= nvc0_fifo_channel_id;
-		engine->fifo.create_context	= nvc0_fifo_create_context;
-		engine->fifo.destroy_context	= nvc0_fifo_destroy_context;
-		engine->fifo.load_context	= nvc0_fifo_load_context;
-		engine->fifo.unload_context	= nvc0_fifo_unload_context;
 		engine->display.early_init	= nouveau_stub_init;
 		engine->display.late_takedown	= nouveau_stub_takedown;
 		engine->display.create		= nvd0_display_create;
@@ -496,13 +405,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nvc0_fb_init;
 		engine->fb.takedown		= nvc0_fb_takedown;
-		engine->fifo.channels		= 0;
-		engine->fifo.init		= nouveau_stub_init;
-		engine->fifo.takedown		= nouveau_stub_takedown;
-		engine->fifo.disable		= nvc0_fifo_disable;
-		engine->fifo.enable		= nvc0_fifo_enable;
-		engine->fifo.reassign		= nvc0_fifo_reassign;
-		engine->fifo.unload_context	= nouveau_stub_init;
 		engine->display.early_init	= nouveau_stub_init;
 		engine->display.late_takedown	= nouveau_stub_takedown;
 		engine->display.create		= nvd0_display_create;
@@ -607,61 +509,24 @@ nouveau_card_channel_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan;
-	int ret, oclass;
+	int ret;
 
 	ret = nouveau_channel_alloc(dev, &chan, NULL, NvDmaFB, NvDmaTT);
 	dev_priv->channel = chan;
 	if (ret)
 		return ret;
-
 	mutex_unlock(&dev_priv->channel->mutex);
 
-	if (dev_priv->card_type <= NV_50) {
-		if (dev_priv->card_type < NV_50)
-			oclass = 0x0039;
-		else
-			oclass = 0x5039;
-
-		ret = nouveau_gpuobj_gr_new(chan, NvM2MF, oclass);
-		if (ret)
-			goto error;
-
-		ret = nouveau_notifier_alloc(chan, NvNotify0, 32, 0xfe0, 0x1000,
-					     &chan->m2mf_ntfy);
-		if (ret)
-			goto error;
-
-		ret = RING_SPACE(chan, 6);
-		if (ret)
-			goto error;
-
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_NAME, 1);
-		OUT_RING  (chan, NvM2MF);
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_DMA_NOTIFY, 3);
-		OUT_RING  (chan, NvNotify0);
-		OUT_RING  (chan, chan->vram_handle);
-		OUT_RING  (chan, chan->gart_handle);
-	} else
-	if (dev_priv->card_type <= NV_D0) {
-		ret = nouveau_gpuobj_gr_new(chan, 0x9039, 0x9039);
-		if (ret)
-			goto error;
-
-		ret = RING_SPACE(chan, 2);
-		if (ret)
-			goto error;
-
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0000, 1);
-		OUT_RING  (chan, 0x00009039);
-	}
-
-	FIRE_RING (chan);
-error:
-	if (ret)
-		nouveau_card_channel_fini(dev);
-	return ret;
+	nouveau_bo_move_init(chan);
+	return 0;
 }
 
+static const struct vga_switcheroo_client_ops nouveau_switcheroo_ops = {
+	.set_gpu_state = nouveau_switcheroo_set_state,
+	.reprobe = nouveau_switcheroo_reprobe,
+	.can_switch = nouveau_switcheroo_can_switch,
+};
+
 int
 nouveau_card_init(struct drm_device *dev)
 {
@@ -670,9 +535,7 @@ nouveau_card_init(struct drm_device *dev)
 	int ret, e = 0;
 
 	vga_client_register(dev->pdev, dev, NULL, nouveau_vga_set_decode);
-	vga_switcheroo_register_client(dev->pdev, nouveau_switcheroo_set_state,
-				       nouveau_switcheroo_reprobe,
-				       nouveau_switcheroo_can_switch);
+	vga_switcheroo_register_client(dev->pdev, &nouveau_switcheroo_ops);
 
 	/* Initialise internal driver API hooks */
 	ret = nouveau_init_engine_ptrs(dev);
@@ -745,6 +608,81 @@ nouveau_card_init(struct drm_device *dev)
 	if (!dev_priv->noaccel) {
 		switch (dev_priv->card_type) {
 		case NV_04:
+			nv04_fifo_create(dev);
+			break;
+		case NV_10:
+		case NV_20:
+		case NV_30:
+			if (dev_priv->chipset < 0x17)
+				nv10_fifo_create(dev);
+			else
+				nv17_fifo_create(dev);
+			break;
+		case NV_40:
+			nv40_fifo_create(dev);
+			break;
+		case NV_50:
+			if (dev_priv->chipset == 0x50)
+				nv50_fifo_create(dev);
+			else
+				nv84_fifo_create(dev);
+			break;
+		case NV_C0:
+		case NV_D0:
+			nvc0_fifo_create(dev);
+			break;
+		case NV_E0:
+			nve0_fifo_create(dev);
+			break;
+		default:
+			break;
+		}
+
+		switch (dev_priv->card_type) {
+		case NV_04:
+			nv04_fence_create(dev);
+			break;
+		case NV_10:
+		case NV_20:
+		case NV_30:
+		case NV_40:
+		case NV_50:
+			if (dev_priv->chipset < 0x84)
+				nv10_fence_create(dev);
+			else
+				nv84_fence_create(dev);
+			break;
+		case NV_C0:
+		case NV_D0:
+		case NV_E0:
+			nvc0_fence_create(dev);
+			break;
+		default:
+			break;
+		}
+
+		switch (dev_priv->card_type) {
+		case NV_04:
+		case NV_10:
+		case NV_20:
+		case NV_30:
+		case NV_40:
+			nv04_software_create(dev);
+			break;
+		case NV_50:
+			nv50_software_create(dev);
+			break;
+		case NV_C0:
+		case NV_D0:
+		case NV_E0:
+			nvc0_software_create(dev);
+			break;
+		default:
+			break;
+		}
+
+		switch (dev_priv->card_type) {
+		case NV_04:
 			nv04_graph_create(dev);
 			break;
 		case NV_10:
@@ -764,6 +702,9 @@ nouveau_card_init(struct drm_device *dev)
 		case NV_D0:
 			nvc0_graph_create(dev);
 			break;
+		case NV_E0:
+			nve0_graph_create(dev);
+			break;
 		default:
 			break;
 		}
@@ -796,8 +737,9 @@ nouveau_card_init(struct drm_device *dev)
 			}
 			break;
 		case NV_C0:
-			nvc0_copy_create(dev, 0);
 			nvc0_copy_create(dev, 1);
+		case NV_D0:
+			nvc0_copy_create(dev, 0);
 			break;
 		default:
 			break;
@@ -830,16 +772,11 @@ nouveau_card_init(struct drm_device *dev)
 					goto out_engine;
 			}
 		}
-
-		/* PFIFO */
-		ret = engine->fifo.init(dev);
-		if (ret)
-			goto out_engine;
 	}
 
 	ret = nouveau_irq_init(dev);
 	if (ret)
-		goto out_fifo;
+		goto out_engine;
 
 	ret = nouveau_display_create(dev);
 	if (ret)
@@ -848,14 +785,10 @@ nouveau_card_init(struct drm_device *dev)
 	nouveau_backlight_init(dev);
 	nouveau_pm_init(dev);
 
-	ret = nouveau_fence_init(dev);
-	if (ret)
-		goto out_pm;
-
 	if (dev_priv->eng[NVOBJ_ENGINE_GR]) {
 		ret = nouveau_card_channel_init(dev);
 		if (ret)
-			goto out_fence;
+			goto out_pm;
 	}
 
 	if (dev->mode_config.num_crtc) {
@@ -870,17 +803,12 @@ nouveau_card_init(struct drm_device *dev)
 
 out_chan:
 	nouveau_card_channel_fini(dev);
-out_fence:
-	nouveau_fence_fini(dev);
 out_pm:
 	nouveau_pm_fini(dev);
 	nouveau_backlight_exit(dev);
 	nouveau_display_destroy(dev);
 out_irq:
 	nouveau_irq_fini(dev);
-out_fifo:
-	if (!dev_priv->noaccel)
-		engine->fifo.takedown(dev);
 out_engine:
 	if (!dev_priv->noaccel) {
 		for (e = e - 1; e >= 0; e--) {
@@ -912,6 +840,7 @@ out_bios:
 out_display_early:
 	engine->display.late_takedown(dev);
 out:
+	vga_switcheroo_unregister_client(dev->pdev);
 	vga_client_register(dev->pdev, NULL, NULL, NULL);
 	return ret;
 }
@@ -928,13 +857,11 @@ static void nouveau_card_takedown(struct drm_device *dev)
 	}
 
 	nouveau_card_channel_fini(dev);
-	nouveau_fence_fini(dev);
 	nouveau_pm_fini(dev);
 	nouveau_backlight_exit(dev);
 	nouveau_display_destroy(dev);
 
 	if (!dev_priv->noaccel) {
-		engine->fifo.takedown(dev);
 		for (e = NVOBJ_ENGINE_NR - 1; e >= 0; e--) {
 			if (dev_priv->eng[e]) {
 				dev_priv->eng[e]->fini(dev, e, false);
@@ -969,6 +896,7 @@ static void nouveau_card_takedown(struct drm_device *dev)
 
 	nouveau_irq_fini(dev);
 
+	vga_switcheroo_unregister_client(dev->pdev);
 	vga_client_register(dev->pdev, NULL, NULL, NULL);
 }
 
@@ -1176,7 +1104,7 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
 		goto err_priv;
 	}
 
-	NV_INFO(dev, "Detected an NV%2x generation card (0x%08x)\n",
+	NV_INFO(dev, "Detected an NV%02x generation card (0x%08x)\n",
 		     dev_priv->card_type, reg0);
 
 	/* map the mmio regs, limiting the amount to preserve vmap space */
@@ -1219,6 +1147,8 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
 	if (nouveau_noaccel == -1) {
 		switch (dev_priv->chipset) {
 		case 0xd9: /* known broken */
+		case 0xe4: /* needs binary driver firmware */
+		case 0xe7: /* needs binary driver firmware */
 			NV_INFO(dev, "acceleration disabled by default, pass "
 				     "noaccel=0 to force enable\n");
 			dev_priv->noaccel = true;
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.c b/drivers/gpu/drm/nouveau/nouveau_vm.c
index 2bf6c0350b4b..11edd5e91a0a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.c
@@ -77,6 +77,63 @@ nouveau_vm_map(struct nouveau_vma *vma, struct nouveau_mem *node)
 }
 
 void
+nouveau_vm_map_sg_table(struct nouveau_vma *vma, u64 delta, u64 length,
+			struct nouveau_mem *mem)
+{
+	struct nouveau_vm *vm = vma->vm;
+	int big = vma->node->type != vm->spg_shift;
+	u32 offset = vma->node->offset + (delta >> 12);
+	u32 bits = vma->node->type - 12;
+	u32 num  = length >> vma->node->type;
+	u32 pde  = (offset >> vm->pgt_bits) - vm->fpde;
+	u32 pte  = (offset & ((1 << vm->pgt_bits) - 1)) >> bits;
+	u32 max  = 1 << (vm->pgt_bits - bits);
+	unsigned m, sglen;
+	u32 end, len;
+	int i;
+	struct scatterlist *sg;
+
+	for_each_sg(mem->sg->sgl, sg, mem->sg->nents, i) {
+		struct nouveau_gpuobj *pgt = vm->pgt[pde].obj[big];
+		sglen = sg_dma_len(sg) >> PAGE_SHIFT;
+
+		end = pte + sglen;
+		if (unlikely(end >= max))
+			end = max;
+		len = end - pte;
+
+		for (m = 0; m < len; m++) {
+			dma_addr_t addr = sg_dma_address(sg) + (m << PAGE_SHIFT);
+
+			vm->map_sg(vma, pgt, mem, pte, 1, &addr);
+			num--;
+			pte++;
+
+			if (num == 0)
+				goto finish;
+		}
+		if (unlikely(end >= max)) {
+			pde++;
+			pte = 0;
+		}
+		if (m < sglen) {
+			for (; m < sglen; m++) {
+				dma_addr_t addr = sg_dma_address(sg) + (m << PAGE_SHIFT);
+
+				vm->map_sg(vma, pgt, mem, pte, 1, &addr);
+				num--;
+				pte++;
+				if (num == 0)
+					goto finish;
+			}
+		}
+
+	}
+finish:
+	vm->flush(vm);
+}
+
+void
 nouveau_vm_map_sg(struct nouveau_vma *vma, u64 delta, u64 length,
 		  struct nouveau_mem *mem)
 {
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.h b/drivers/gpu/drm/nouveau/nouveau_vm.h
index 4fb6e728734d..a8246e7e4a89 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.h
@@ -72,6 +72,9 @@ struct nouveau_vm {
 		    u64 phys, u64 delta);
 	void (*map_sg)(struct nouveau_vma *, struct nouveau_gpuobj *,
 		       struct nouveau_mem *, u32 pte, u32 cnt, dma_addr_t *);
+
+	void (*map_sg_table)(struct nouveau_vma *, struct nouveau_gpuobj *,
+			     struct nouveau_mem *, u32 pte, u32 cnt, dma_addr_t *);
 	void (*unmap)(struct nouveau_gpuobj *pgt, u32 pte, u32 cnt);
 	void (*flush)(struct nouveau_vm *);
 };
@@ -90,7 +93,8 @@ void nouveau_vm_unmap(struct nouveau_vma *);
 void nouveau_vm_unmap_at(struct nouveau_vma *, u64 offset, u64 length);
 void nouveau_vm_map_sg(struct nouveau_vma *, u64 offset, u64 length,
 		       struct nouveau_mem *);
-
+void nouveau_vm_map_sg_table(struct nouveau_vma *vma, u64 delta, u64 length,
+			     struct nouveau_mem *mem);
 /* nv50_vm.c */
 void nv50_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
 		     struct nouveau_gpuobj *pgt[2]);
diff --git a/drivers/gpu/drm/nouveau/nv04_crtc.c b/drivers/gpu/drm/nouveau/nv04_crtc.c
index 728d07584d39..4c31c63e5528 100644
--- a/drivers/gpu/drm/nouveau/nv04_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv04_crtc.c
@@ -1047,7 +1047,7 @@ nv04_crtc_create(struct drm_device *dev, int crtc_num)
 	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nv04_display.c b/drivers/gpu/drm/nouveau/nv04_display.c
index 7047d37e8dab..44488e3a257d 100644
--- a/drivers/gpu/drm/nouveau/nv04_display.c
+++ b/drivers/gpu/drm/nouveau/nv04_display.c
@@ -98,6 +98,13 @@ nv04_display_early_init(struct drm_device *dev)
 		NVSetOwner(dev, 0);
 	}
 
+	/* ensure vblank interrupts are off, they can't be enabled until
+	 * drm_vblank has been initialised
+	 */
+	NVWriteCRTC(dev, 0, NV_PCRTC_INTR_EN_0, 0);
+	if (nv_two_heads(dev))
+		NVWriteCRTC(dev, 1, NV_PCRTC_INTR_EN_0, 0);
+
 	return 0;
 }
 
@@ -246,6 +253,10 @@ nv04_display_init(struct drm_device *dev)
 void
 nv04_display_fini(struct drm_device *dev)
 {
+	/* disable vblank interrupts */
+	NVWriteCRTC(dev, 0, NV_PCRTC_INTR_EN_0, 0);
+	if (nv_two_heads(dev))
+		NVWriteCRTC(dev, 1, NV_PCRTC_INTR_EN_0, 0);
 }
 
 static void
diff --git a/drivers/gpu/drm/nouveau/nv04_fbcon.c b/drivers/gpu/drm/nouveau/nv04_fbcon.c
index 7a1189371096..7cd7857347ef 100644
--- a/drivers/gpu/drm/nouveau/nv04_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nv04_fbcon.c
@@ -41,7 +41,7 @@ nv04_fbcon_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSubImageBlit, 0x0300, 3);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x0300, 3);
 	OUT_RING(chan, (region->sy << 16) | region->sx);
 	OUT_RING(chan, (region->dy << 16) | region->dx);
 	OUT_RING(chan, (region->height << 16) | region->width);
@@ -62,15 +62,15 @@ nv04_fbcon_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSubGdiRect, 0x02fc, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x02fc, 1);
 	OUT_RING(chan, (rect->rop != ROP_COPY) ? 1 : 3);
-	BEGIN_RING(chan, NvSubGdiRect, 0x03fc, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x03fc, 1);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR)
 		OUT_RING(chan, ((uint32_t *)info->pseudo_palette)[rect->color]);
 	else
 		OUT_RING(chan, rect->color);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0400, 2);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0400, 2);
 	OUT_RING(chan, (rect->dx << 16) | rect->dy);
 	OUT_RING(chan, (rect->width << 16) | rect->height);
 	FIRE_RING(chan);
@@ -110,7 +110,7 @@ nv04_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		bg = image->bg_color;
 	}
 
-	BEGIN_RING(chan, NvSubGdiRect, 0x0be4, 7);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0be4, 7);
 	OUT_RING(chan, (image->dy << 16) | (image->dx & 0xffff));
 	OUT_RING(chan, ((image->dy + image->height) << 16) |
 			 ((image->dx + image->width) & 0xffff));
@@ -127,7 +127,7 @@ nv04_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		if (ret)
 			return ret;
 
-		BEGIN_RING(chan, NvSubGdiRect, 0x0c00, iter_len);
+		BEGIN_NV04(chan, NvSubGdiRect, 0x0c00, iter_len);
 		OUT_RINGp(chan, data, iter_len);
 		data += iter_len;
 		dsize -= iter_len;
@@ -209,25 +209,25 @@ nv04_fbcon_accel_init(struct fb_info *info)
 		return 0;
 	}
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvCtxSurf2D);
-	BEGIN_RING(chan, sub, 0x0184, 2);
+	BEGIN_NV04(chan, sub, 0x0184, 2);
 	OUT_RING(chan, NvDmaFB);
 	OUT_RING(chan, NvDmaFB);
-	BEGIN_RING(chan, sub, 0x0300, 4);
+	BEGIN_NV04(chan, sub, 0x0300, 4);
 	OUT_RING(chan, surface_fmt);
 	OUT_RING(chan, info->fix.line_length | (info->fix.line_length << 16));
 	OUT_RING(chan, info->fix.smem_start - dev->mode_config.fb_base);
 	OUT_RING(chan, info->fix.smem_start - dev->mode_config.fb_base);
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvRop);
-	BEGIN_RING(chan, sub, 0x0300, 1);
+	BEGIN_NV04(chan, sub, 0x0300, 1);
 	OUT_RING(chan, 0x55);
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvImagePatt);
-	BEGIN_RING(chan, sub, 0x0300, 8);
+	BEGIN_NV04(chan, sub, 0x0300, 8);
 	OUT_RING(chan, pattern_fmt);
 #ifdef __BIG_ENDIAN
 	OUT_RING(chan, 2);
@@ -241,31 +241,31 @@ nv04_fbcon_accel_init(struct fb_info *info)
 	OUT_RING(chan, ~0);
 	OUT_RING(chan, ~0);
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvClipRect);
-	BEGIN_RING(chan, sub, 0x0300, 2);
+	BEGIN_NV04(chan, sub, 0x0300, 2);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, (info->var.yres_virtual << 16) | info->var.xres_virtual);
 
-	BEGIN_RING(chan, NvSubImageBlit, 0x0000, 1);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x0000, 1);
 	OUT_RING(chan, NvImageBlit);
-	BEGIN_RING(chan, NvSubImageBlit, 0x019c, 1);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x019c, 1);
 	OUT_RING(chan, NvCtxSurf2D);
-	BEGIN_RING(chan, NvSubImageBlit, 0x02fc, 1);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x02fc, 1);
 	OUT_RING(chan, 3);
 
-	BEGIN_RING(chan, NvSubGdiRect, 0x0000, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0000, 1);
 	OUT_RING(chan, NvGdiRect);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0198, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0198, 1);
 	OUT_RING(chan, NvCtxSurf2D);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0188, 2);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0188, 2);
 	OUT_RING(chan, NvImagePatt);
 	OUT_RING(chan, NvRop);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0304, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0304, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0300, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0300, 1);
 	OUT_RING(chan, rect_fmt);
-	BEGIN_RING(chan, NvSubGdiRect, 0x02fc, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x02fc, 1);
 	OUT_RING(chan, 3);
 
 	FIRE_RING(chan);
diff --git a/drivers/gpu/drm/nouveau/nv04_fence.c b/drivers/gpu/drm/nouveau/nv04_fence.c
new file mode 100644
index 000000000000..abe89db6de24
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv04_fence.c
@@ -0,0 +1,140 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nv04_fence_chan {
+	struct nouveau_fence_chan base;
+	atomic_t sequence;
+};
+
+struct nv04_fence_priv {
+	struct nouveau_fence_priv base;
+};
+
+static int
+nv04_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	int ret = RING_SPACE(chan, 2);
+	if (ret == 0) {
+		BEGIN_NV04(chan, NvSubSw, 0x0150, 1);
+		OUT_RING  (chan, fence->sequence);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+static int
+nv04_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	return -ENODEV;
+}
+
+int
+nv04_fence_mthd(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv04_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	atomic_set(&fctx->sequence, data);
+	return 0;
+}
+
+static u32
+nv04_fence_read(struct nouveau_channel *chan)
+{
+	struct nv04_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	return atomic_read(&fctx->sequence);
+}
+
+static void
+nv04_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_fence_chan *fctx = chan->engctx[engine];
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv04_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_fence_chan *fctx = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (fctx) {
+		nouveau_fence_context_new(&fctx->base);
+		atomic_set(&fctx->sequence, 0);
+		chan->engctx[engine] = fctx;
+		return 0;
+	}
+	return -ENOMEM;
+}
+
+static int
+nv04_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nv04_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nv04_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fence_priv *priv = nv_engine(dev, engine);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv04_fence_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fence_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nv04_fence_destroy;
+	priv->base.engine.init = nv04_fence_init;
+	priv->base.engine.fini = nv04_fence_fini;
+	priv->base.engine.context_new = nv04_fence_context_new;
+	priv->base.engine.context_del = nv04_fence_context_del;
+	priv->base.emit = nv04_fence_emit;
+	priv->base.sync = nv04_fence_sync;
+	priv->base.read = nv04_fence_read;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv04_fifo.c b/drivers/gpu/drm/nouveau/nv04_fifo.c
index db465a3ee1b2..a6295cd00ec7 100644
--- a/drivers/gpu/drm/nouveau/nv04_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv04_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -27,49 +27,38 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
-#include "nouveau_ramht.h"
+#include "nouveau_fifo.h"
 #include "nouveau_util.h"
-
-#define NV04_RAMFC(c) (dev_priv->ramfc->pinst + ((c) * NV04_RAMFC__SIZE))
-#define NV04_RAMFC__SIZE 32
-#define NV04_RAMFC_DMA_PUT                                       0x00
-#define NV04_RAMFC_DMA_GET                                       0x04
-#define NV04_RAMFC_DMA_INSTANCE                                  0x08
-#define NV04_RAMFC_DMA_STATE                                     0x0C
-#define NV04_RAMFC_DMA_FETCH                                     0x10
-#define NV04_RAMFC_ENGINE                                        0x14
-#define NV04_RAMFC_PULL1_ENGINE                                  0x18
-
-#define RAMFC_WR(offset, val) nv_wo32(chan->ramfc, NV04_RAMFC_##offset, (val))
-#define RAMFC_RD(offset)      nv_ro32(chan->ramfc, NV04_RAMFC_##offset)
-
-void
-nv04_fifo_disable(struct drm_device *dev)
-{
-	uint32_t tmp;
-
-	tmp = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUSH);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, tmp & ~1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 0);
-	tmp = nv_rd32(dev, NV03_PFIFO_CACHE1_PULL1);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, tmp & ~1);
-}
-
-void
-nv04_fifo_enable(struct drm_device *dev)
-{
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
-}
-
-bool
-nv04_fifo_reassign(struct drm_device *dev, bool enable)
-{
-	uint32_t reassign = nv_rd32(dev, NV03_PFIFO_CACHES);
-
-	nv_wr32(dev, NV03_PFIFO_CACHES, enable ? 1 : 0);
-	return (reassign == 1);
-}
+#include "nouveau_ramht.h"
+#include "nouveau_software.h"
+
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv04_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 16,  0, 0x08,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 16, 16, 0x08,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x18,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{}
+};
+
+struct nv04_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv04_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
 
 bool
 nv04_fifo_cache_pull(struct drm_device *dev, bool enable)
@@ -86,13 +75,13 @@ nv04_fifo_cache_pull(struct drm_device *dev, bool enable)
 		 * invalidate the most recently calculated instance.
 		 */
 		if (!nv_wait(dev, NV04_PFIFO_CACHE1_PULL0,
-			     NV04_PFIFO_CACHE1_PULL0_HASH_BUSY, 0))
+				  NV04_PFIFO_CACHE1_PULL0_HASH_BUSY, 0))
 			NV_ERROR(dev, "Timeout idling the PFIFO puller.\n");
 
 		if (nv_rd32(dev, NV04_PFIFO_CACHE1_PULL0) &
-		    NV04_PFIFO_CACHE1_PULL0_HASH_FAILED)
+				 NV04_PFIFO_CACHE1_PULL0_HASH_FAILED)
 			nv_wr32(dev, NV03_PFIFO_INTR_0,
-				NV_PFIFO_INTR_CACHE_ERROR);
+				     NV_PFIFO_INTR_CACHE_ERROR);
 
 		nv_wr32(dev, NV04_PFIFO_CACHE1_HASH, 0);
 	}
@@ -100,242 +89,182 @@ nv04_fifo_cache_pull(struct drm_device *dev, bool enable)
 	return pull & 1;
 }
 
-int
-nv04_fifo_channel_id(struct drm_device *dev)
-{
-	return nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
-			NV03_PFIFO_CACHE1_PUSH1_CHID_MASK;
-}
-
-#ifdef __BIG_ENDIAN
-#define DMA_FETCH_ENDIANNESS NV_PFIFO_CACHE1_BIG_ENDIAN
-#else
-#define DMA_FETCH_ENDIANNESS 0
-#endif
-
-int
-nv04_fifo_create_context(struct nouveau_channel *chan)
+static int
+nv04_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv04_fifo_chan *fctx;
 	unsigned long flags;
 	int ret;
 
-	ret = nouveau_gpuobj_new_fake(dev, NV04_RAMFC(chan->id), ~0,
-						NV04_RAMFC__SIZE,
-						NVOBJ_FLAG_ZERO_ALLOC |
-						NVOBJ_FLAG_ZERO_FREE,
-						&chan->ramfc);
-	if (ret)
-		return ret;
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
 
+	/* map channel control registers */
 	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
 			     NV03_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
-		return -ENOMEM;
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-
-	/* Setup initial state */
-	RAMFC_WR(DMA_PUT, chan->pushbuf_base);
-	RAMFC_WR(DMA_GET, chan->pushbuf_base);
-	RAMFC_WR(DMA_INSTANCE, chan->pushbuf->pinst >> 4);
-	RAMFC_WR(DMA_FETCH, (NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
-			     NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
-			     NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8 |
-			     DMA_FETCH_ENDIANNESS));
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	/* enable the fifo dma operation */
-	nv_wr32(dev, NV04_PFIFO_MODE,
-		nv_rd32(dev, NV04_PFIFO_MODE) | (1 << chan->id));
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 32, ~0, 32,
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x08, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x0c, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x10, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
+#ifdef __BIG_ENDIAN
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
+#endif
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+	nv_wo32(fctx->ramfc, 0x14, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x18, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x1c, 0x00000000);
 
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-	return 0;
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
 void
-nv04_fifo_destroy_context(struct nouveau_channel *chan)
+nv04_fifo_context_del(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	struct nv04_fifo_priv *priv = nv_engine(chan->dev, engine);
+	struct nv04_fifo_chan *fctx = chan->engctx[engine];
+	struct ramfc_desc *c = priv->ramfc_desc;
 	unsigned long flags;
+	int chid;
 
+	/* prevent fifo context switches */
 	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	pfifo->reassign(dev, false);
-
-	/* Unload the context if it's the currently active one */
-	if (pfifo->channel_id(dev) == chan->id) {
-		pfifo->disable(dev);
-		pfifo->unload_context(dev);
-		pfifo->enable(dev);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 0);
+
+	/* if this channel is active, replace it with a null context */
+	chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) & priv->base.channels;
+	if (chid == chan->id) {
+		nv_mask(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 0x00000001, 0);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 0);
+		nv_mask(dev, NV04_PFIFO_CACHE1_PULL0, 0x00000001, 0);
+
+		do {
+			u32 mask = ((1ULL << c->bits) - 1) << c->regs;
+			nv_mask(dev, c->regp, mask, 0x00000000);
+		} while ((++c)->bits);
+
+		nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+		nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
 	}
 
-	/* Keep it from being rescheduled */
+	/* restore normal operation, after disabling dma mode */
 	nv_mask(dev, NV04_PFIFO_MODE, 1 << chan->id, 0);
-
-	pfifo->reassign(dev, true);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	/* Free the channel resources */
+	/* clean up */
+	nouveau_gpuobj_ref(NULL, &fctx->ramfc);
+	nouveau_gpuobj_ref(NULL, &chan->ramfc); /*XXX: nv40 */
 	if (chan->user) {
 		iounmap(chan->user);
 		chan->user = NULL;
 	}
-	nouveau_gpuobj_ref(NULL, &chan->ramfc);
-}
-
-static void
-nv04_fifo_do_load_context(struct drm_device *dev, int chid)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV04_RAMFC(chid), tmp;
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUT, nv_ri32(dev, fc + 0));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_GET, nv_ri32(dev, fc + 4));
-	tmp = nv_ri32(dev, fc + 8);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE, tmp & 0xFFFF);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT, tmp >> 16);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_STATE, nv_ri32(dev, fc + 12));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_FETCH, nv_ri32(dev, fc + 16));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_ENGINE, nv_ri32(dev, fc + 20));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL1, nv_ri32(dev, fc + 24));
-
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
-}
-
-int
-nv04_fifo_load_context(struct nouveau_channel *chan)
-{
-	uint32_t tmp;
-
-	nv_wr32(chan->dev, NV03_PFIFO_CACHE1_PUSH1,
-			   NV03_PFIFO_CACHE1_PUSH1_DMA | chan->id);
-	nv04_fifo_do_load_context(chan->dev, chan->id);
-	nv_wr32(chan->dev, NV04_PFIFO_CACHE1_DMA_PUSH, 1);
-
-	/* Reset NV04_PFIFO_CACHE1_DMA_CTL_AT_INFO to INVALID */
-	tmp = nv_rd32(chan->dev, NV04_PFIFO_CACHE1_DMA_CTL) & ~(1 << 31);
-	nv_wr32(chan->dev, NV04_PFIFO_CACHE1_DMA_CTL, tmp);
-
-	return 0;
 }
 
 int
-nv04_fifo_unload_context(struct drm_device *dev)
+nv04_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nouveau_channel *chan = NULL;
-	uint32_t tmp;
-	int chid;
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 0 || chid >= dev_priv->engine.fifo.channels)
-		return 0;
-
-	chan = dev_priv->channels.ptr[chid];
-	if (!chan) {
-		NV_ERROR(dev, "Inactive channel on PFIFO: %d\n", chid);
-		return -EINVAL;
-	}
-
-	RAMFC_WR(DMA_PUT, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUT));
-	RAMFC_WR(DMA_GET, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT) << 16;
-	tmp |= nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE);
-	RAMFC_WR(DMA_INSTANCE, tmp);
-	RAMFC_WR(DMA_STATE, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_STATE));
-	RAMFC_WR(DMA_FETCH, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_FETCH));
-	RAMFC_WR(ENGINE, nv_rd32(dev, NV04_PFIFO_CACHE1_ENGINE));
-	RAMFC_WR(PULL1_ENGINE, nv_rd32(dev, NV04_PFIFO_CACHE1_PULL1));
-
-	nv04_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-	return 0;
-}
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
 
-static void
-nv04_fifo_init_reset(struct drm_device *dev)
-{
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) & ~NV_PMC_ENABLE_PFIFO);
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) |  NV_PMC_ENABLE_PFIFO);
-
-	nv_wr32(dev, 0x003224, 0x000f0078);
-	nv_wr32(dev, 0x002044, 0x0101ffff);
-	nv_wr32(dev, 0x002040, 0x000000ff);
-	nv_wr32(dev, 0x002500, 0x00000000);
-	nv_wr32(dev, 0x003000, 0x00000000);
-	nv_wr32(dev, 0x003050, 0x00000000);
-	nv_wr32(dev, 0x003200, 0x00000000);
-	nv_wr32(dev, 0x003250, 0x00000000);
-	nv_wr32(dev, 0x003220, 0x00000000);
-
-	nv_wr32(dev, 0x003250, 0x00000000);
-	nv_wr32(dev, 0x003270, 0x00000000);
-	nv_wr32(dev, 0x003210, 0x00000000);
-}
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, 0);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, NV_PMC_ENABLE_PFIFO);
 
-static void
-nv04_fifo_init_ramxx(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	nv_wr32(dev, NV04_PFIFO_DELAY_0, 0x000000ff);
+	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, 0x0101ffff);
 
 	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
 				       ((dev_priv->ramht->bits - 9) << 16) |
 				       (dev_priv->ramht->gpuobj->pinst >> 8));
 	nv_wr32(dev, NV03_PFIFO_RAMRO, dev_priv->ramro->pinst >> 8);
 	nv_wr32(dev, NV03_PFIFO_RAMFC, dev_priv->ramfc->pinst >> 8);
-}
 
-static void
-nv04_fifo_init_intr(struct drm_device *dev)
-{
-	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, 0x002100, 0xffffffff);
-	nv_wr32(dev, 0x002140, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+
+	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xffffffff);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (dev_priv->channels.ptr[i])
+			nv_mask(dev, NV04_PFIFO_MODE, (1 << i), (1 << i));
+	}
+
+	return 0;
 }
 
 int
-nv04_fifo_init(struct drm_device *dev)
+nv04_fifo_fini(struct drm_device *dev, int engine, bool suspend)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int i;
-
-	nv04_fifo_init_reset(dev);
-	nv04_fifo_init_ramxx(dev);
-
-	nv04_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+	struct nouveau_channel *chan;
+	int chid;
 
-	nv04_fifo_init_intr(dev);
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
+	/* prevent context switches and halt fifo operation */
+	nv_wr32(dev, NV03_PFIFO_CACHES, 0);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 0);
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 0);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 0);
 
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		if (dev_priv->channels.ptr[i]) {
-			uint32_t mode = nv_rd32(dev, NV04_PFIFO_MODE);
-			nv_wr32(dev, NV04_PFIFO_MODE, mode | (1 << i));
-		}
+	/* store current fifo context in ramfc */
+	chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) & priv->base.channels;
+	chan = dev_priv->channels.ptr[chid];
+	if (suspend && chid != priv->base.channels && chan) {
+		struct nv04_fifo_chan *fctx = chan->engctx[engine];
+		struct nouveau_gpuobj *ctx = fctx->ramfc;
+		struct ramfc_desc *c = priv->ramfc_desc;
+		do {
+			u32 rm = ((1ULL << c->bits) - 1) << c->regs;
+			u32 cm = ((1ULL << c->bits) - 1) << c->ctxs;
+			u32 rv = (nv_rd32(dev, c->regp) &  rm) >> c->regs;
+			u32 cv = (nv_ro32(ctx, c->ctxp) & ~cm);
+			nv_wo32(ctx, c->ctxp, cv | (rv << c->ctxs));
+		} while ((++c)->bits);
 	}
 
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0x00000000);
 	return 0;
 }
 
-void
-nv04_fifo_fini(struct drm_device *dev)
-{
-	nv_wr32(dev, 0x2140, 0x00000000);
-	nouveau_irq_unregister(dev, 8);
-}
-
 static bool
 nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = NULL;
 	struct nouveau_gpuobj *obj;
@@ -346,7 +275,7 @@ nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 	u32 engine;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	if (likely(chid >= 0 && chid < dev_priv->engine.fifo.channels))
+	if (likely(chid >= 0 && chid < pfifo->channels))
 		chan = dev_priv->channels.ptr[chid];
 	if (unlikely(!chan))
 		goto out;
@@ -357,7 +286,6 @@ nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 		if (unlikely(!obj || obj->engine != NVOBJ_ENGINE_SW))
 			break;
 
-		chan->sw_subchannel[subc] = obj->class;
 		engine = 0x0000000f << (subc * 4);
 
 		nv_mask(dev, NV04_PFIFO_CACHE1_ENGINE, engine, 0x00000000);
@@ -368,7 +296,7 @@ nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 		if (unlikely(((engine >> (subc * 4)) & 0xf) != 0))
 			break;
 
-		if (!nouveau_gpuobj_mthd_call(chan, chan->sw_subchannel[subc],
+		if (!nouveau_gpuobj_mthd_call(chan, nouveau_software_class(dev),
 					      mthd, data))
 			handled = true;
 		break;
@@ -391,8 +319,8 @@ static const char *nv_dma_state_err(u32 state)
 void
 nv04_fifo_isr(struct drm_device *dev)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_engine *engine = &dev_priv->engine;
 	uint32_t status, reassign;
 	int cnt = 0;
 
@@ -402,7 +330,7 @@ nv04_fifo_isr(struct drm_device *dev)
 
 		nv_wr32(dev, NV03_PFIFO_CACHES, 0);
 
-		chid = engine->fifo.channel_id(dev);
+		chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) & pfifo->channels;
 		get  = nv_rd32(dev, NV03_PFIFO_CACHE1_GET);
 
 		if (status & NV_PFIFO_INTR_CACHE_ERROR) {
@@ -541,3 +469,38 @@ nv04_fifo_isr(struct drm_device *dev)
 
 	nv_wr32(dev, NV03_PMC_INTR_0, NV_PMC_INTR_0_PFIFO_PENDING);
 }
+
+void
+nv04_fifo_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+
+	nouveau_irq_unregister(dev, 8);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv04_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fifo_priv *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv04_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv04_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 15;
+	priv->ramfc_desc = nv04_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv04_graph.c b/drivers/gpu/drm/nouveau/nv04_graph.c
index dbdea8ed3925..72f1a62903b3 100644
--- a/drivers/gpu/drm/nouveau/nv04_graph.c
+++ b/drivers/gpu/drm/nouveau/nv04_graph.c
@@ -356,12 +356,12 @@ static struct nouveau_channel *
 nv04_graph_channel(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int chid = dev_priv->engine.fifo.channels;
+	int chid = 15;
 
 	if (nv_rd32(dev, NV04_PGRAPH_CTX_CONTROL) & 0x00010000)
 		chid = nv_rd32(dev, NV04_PGRAPH_CTX_USER) >> 24;
 
-	if (chid >= dev_priv->engine.fifo.channels)
+	if (chid > 15)
 		return NULL;
 
 	return dev_priv->channels.ptr[chid];
@@ -404,7 +404,6 @@ nv04_graph_load_context(struct nouveau_channel *chan)
 static int
 nv04_graph_unload_context(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = NULL;
 	struct graph_state *ctx;
 	uint32_t tmp;
@@ -420,7 +419,7 @@ nv04_graph_unload_context(struct drm_device *dev)
 
 	nv_wr32(dev, NV04_PGRAPH_CTX_CONTROL, 0x10000000);
 	tmp  = nv_rd32(dev, NV04_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (dev_priv->engine.fifo.channels - 1) << 24;
+	tmp |= 15 << 24;
 	nv_wr32(dev, NV04_PGRAPH_CTX_USER, tmp);
 	return 0;
 }
@@ -495,7 +494,6 @@ nv04_graph_object_new(struct nouveau_channel *chan, int engine,
 static int
 nv04_graph_init(struct drm_device *dev, int engine)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	uint32_t tmp;
 
 	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) &
@@ -527,7 +525,7 @@ nv04_graph_init(struct drm_device *dev, int engine)
 	nv_wr32(dev, NV04_PGRAPH_STATE        , 0xFFFFFFFF);
 	nv_wr32(dev, NV04_PGRAPH_CTX_CONTROL  , 0x10000100);
 	tmp  = nv_rd32(dev, NV04_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (dev_priv->engine.fifo.channels - 1) << 24;
+	tmp |= 15 << 24;
 	nv_wr32(dev, NV04_PGRAPH_CTX_USER, tmp);
 
 	/* These don't belong here, they're part of a per-channel context */
@@ -550,28 +548,6 @@ nv04_graph_fini(struct drm_device *dev, int engine, bool suspend)
 	return 0;
 }
 
-static int
-nv04_graph_mthd_set_ref(struct nouveau_channel *chan,
-			u32 class, u32 mthd, u32 data)
-{
-	atomic_set(&chan->fence.last_sequence_irq, data);
-	return 0;
-}
-
-int
-nv04_graph_mthd_page_flip(struct nouveau_channel *chan,
-			  u32 class, u32 mthd, u32 data)
-{
-	struct drm_device *dev = chan->dev;
-	struct nouveau_page_flip_state s;
-
-	if (!nouveau_finish_page_flip(chan, &s))
-		nv_set_crtc_base(dev, s.crtc,
-				 s.offset + s.y * s.pitch + s.x * s.bpp / 8);
-
-	return 0;
-}
-
 /*
  * Software methods, why they are needed, and how they all work:
  *
@@ -1020,7 +996,8 @@ nv04_graph_context_switch(struct drm_device *dev)
 	nv04_graph_unload_context(dev);
 
 	/* Load context for next channel */
-	chid = dev_priv->engine.fifo.channel_id(dev);
+	chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
+			    NV03_PFIFO_CACHE1_PUSH1_CHID_MASK;
 	chan = dev_priv->channels.ptr[chid];
 	if (chan)
 		nv04_graph_load_context(chan);
@@ -1345,9 +1322,5 @@ nv04_graph_create(struct drm_device *dev)
 	NVOBJ_MTHD (dev, 0x005e, 0x0198, nv04_graph_mthd_bind_surf2d);
 	NVOBJ_MTHD (dev, 0x005e, 0x02fc, nv04_graph_mthd_set_operation);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0150, nv04_graph_mthd_set_ref);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv04_instmem.c b/drivers/gpu/drm/nouveau/nv04_instmem.c
index c1248e0740a3..ef7a934a499a 100644
--- a/drivers/gpu/drm/nouveau/nv04_instmem.c
+++ b/drivers/gpu/drm/nouveau/nv04_instmem.c
@@ -1,6 +1,8 @@
 #include "drmP.h"
 #include "drm.h"
+
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 
 /* returns the size of fifo context */
@@ -10,12 +12,15 @@ nouveau_fifo_ctx_size(struct drm_device *dev)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 
 	if (dev_priv->chipset >= 0x40)
-		return 128;
+		return 128 * 32;
 	else
 	if (dev_priv->chipset >= 0x17)
-		return 64;
+		return 64 * 32;
+	else
+	if (dev_priv->chipset >= 0x10)
+		return 32 * 32;
 
-	return 32;
+	return 32 * 16;
 }
 
 int nv04_instmem_init(struct drm_device *dev)
@@ -39,14 +44,10 @@ int nv04_instmem_init(struct drm_device *dev)
 		else if (nv44_graph_class(dev))	    rsvd = 0x4980 * vs;
 		else				    rsvd = 0x4a40 * vs;
 		rsvd += 16 * 1024;
-		rsvd *= dev_priv->engine.fifo.channels;
-
-		/* pciegart table */
-		if (pci_is_pcie(dev->pdev))
-			rsvd += 512 * 1024;
+		rsvd *= 32; /* per-channel */
 
-		/* object storage */
-		rsvd += 512 * 1024;
+		rsvd += 512 * 1024; /* pci(e)gart table */
+		rsvd += 512 * 1024; /* object storage */
 
 		dev_priv->ramin_rsvd_vram = round_up(rsvd, 4096);
 	} else {
@@ -71,7 +72,7 @@ int nv04_instmem_init(struct drm_device *dev)
 		return ret;
 
 	/* And RAMFC */
-	length = dev_priv->engine.fifo.channels * nouveau_fifo_ctx_size(dev);
+	length = nouveau_fifo_ctx_size(dev);
 	switch (dev_priv->card_type) {
 	case NV_40:
 		offset = 0x20000;
diff --git a/drivers/gpu/drm/nouveau/nv04_software.c b/drivers/gpu/drm/nouveau/nv04_software.c
new file mode 100644
index 000000000000..0c41abf48774
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv04_software.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
+#include "nouveau_hw.h"
+
+struct nv04_software_priv {
+	struct nouveau_software_priv base;
+};
+
+struct nv04_software_chan {
+	struct nouveau_software_chan base;
+};
+
+static int
+mthd_flip(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+
+	struct nouveau_page_flip_state state;
+
+	if (!nouveau_finish_page_flip(chan, &state)) {
+		nv_set_crtc_base(chan->dev, state.crtc, state.offset +
+				 state.y * state.pitch +
+				 state.x * state.bpp / 8);
+	}
+
+	return 0;
+}
+
+static int
+nv04_software_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_software_chan *pch;
+
+	pch = kzalloc(sizeof(*pch), GFP_KERNEL);
+	if (!pch)
+		return -ENOMEM;
+
+	nouveau_software_context_new(&pch->base);
+	chan->engctx[engine] = pch;
+	return 0;
+}
+
+static void
+nv04_software_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_software_chan *pch = chan->engctx[engine];
+	chan->engctx[engine] = NULL;
+	kfree(pch);
+}
+
+static int
+nv04_software_object_new(struct nouveau_channel *chan, int engine,
+			 u32 handle, u16 class)
+{
+	struct drm_device *dev = chan->dev;
+	struct nouveau_gpuobj *obj = NULL;
+	int ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 16, 16, 0, &obj);
+	if (ret)
+		return ret;
+	obj->engine = 0;
+	obj->class  = class;
+
+	ret = nouveau_ramht_insert(chan, handle, obj);
+	nouveau_gpuobj_ref(NULL, &obj);
+	return ret;
+}
+
+static int
+nv04_software_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static int
+nv04_software_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nv04_software_destroy(struct drm_device *dev, int engine)
+{
+	struct nv04_software_priv *psw = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, SW);
+	kfree(psw);
+}
+
+int
+nv04_software_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_software_priv *psw;
+
+	psw = kzalloc(sizeof(*psw), GFP_KERNEL);
+	if (!psw)
+		return -ENOMEM;
+
+	psw->base.base.destroy = nv04_software_destroy;
+	psw->base.base.init = nv04_software_init;
+	psw->base.base.fini = nv04_software_fini;
+	psw->base.base.context_new = nv04_software_context_new;
+	psw->base.base.context_del = nv04_software_context_del;
+	psw->base.base.object_new = nv04_software_object_new;
+	nouveau_software_create(&psw->base);
+
+	NVOBJ_ENGINE_ADD(dev, SW, &psw->base.base);
+	if (dev_priv->card_type <= NV_04) {
+		NVOBJ_CLASS(dev, 0x006e, SW);
+		NVOBJ_MTHD (dev, 0x006e, 0x0150, nv04_fence_mthd);
+		NVOBJ_MTHD (dev, 0x006e, 0x0500, mthd_flip);
+	} else {
+		NVOBJ_CLASS(dev, 0x016e, SW);
+		NVOBJ_MTHD (dev, 0x016e, 0x0500, mthd_flip);
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv10_fence.c b/drivers/gpu/drm/nouveau/nv10_fence.c
new file mode 100644
index 000000000000..8a1b75009185
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv10_fence.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs <bskeggs@redhat.com>
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nv10_fence_chan {
+	struct nouveau_fence_chan base;
+};
+
+struct nv10_fence_priv {
+	struct nouveau_fence_priv base;
+	struct nouveau_bo *bo;
+	spinlock_t lock;
+	u32 sequence;
+};
+
+static int
+nv10_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	int ret = RING_SPACE(chan, 2);
+	if (ret == 0) {
+		BEGIN_NV04(chan, 0, NV10_SUBCHAN_REF_CNT, 1);
+		OUT_RING  (chan, fence->sequence);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+
+static int
+nv10_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	return -ENODEV;
+}
+
+static int
+nv17_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	struct nv10_fence_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_FENCE);
+	u32 value;
+	int ret;
+
+	if (!mutex_trylock(&prev->mutex))
+		return -EBUSY;
+
+	spin_lock(&priv->lock);
+	value = priv->sequence;
+	priv->sequence += 2;
+	spin_unlock(&priv->lock);
+
+	ret = RING_SPACE(prev, 5);
+	if (!ret) {
+		BEGIN_NV04(prev, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 4);
+		OUT_RING  (prev, NvSema);
+		OUT_RING  (prev, 0);
+		OUT_RING  (prev, value + 0);
+		OUT_RING  (prev, value + 1);
+		FIRE_RING (prev);
+	}
+
+	if (!ret && !(ret = RING_SPACE(chan, 5))) {
+		BEGIN_NV04(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 4);
+		OUT_RING  (chan, NvSema);
+		OUT_RING  (chan, 0);
+		OUT_RING  (chan, value + 1);
+		OUT_RING  (chan, value + 2);
+		FIRE_RING (chan);
+	}
+
+	mutex_unlock(&prev->mutex);
+	return 0;
+}
+
+static u32
+nv10_fence_read(struct nouveau_channel *chan)
+{
+	return nvchan_rd32(chan, 0x0048);
+}
+
+static void
+nv10_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv10_fence_chan *fctx = chan->engctx[engine];
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv10_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv10_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nv10_fence_chan *fctx;
+	struct nouveau_gpuobj *obj;
+	int ret = 0;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	nouveau_fence_context_new(&fctx->base);
+
+	if (priv->bo) {
+		struct ttm_mem_reg *mem = &priv->bo->bo.mem;
+
+		ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_FROM_MEMORY,
+					     mem->start * PAGE_SIZE, mem->size,
+					     NV_MEM_ACCESS_RW,
+					     NV_MEM_TARGET_VRAM, &obj);
+		if (!ret) {
+			ret = nouveau_ramht_insert(chan, NvSema, obj);
+			nouveau_gpuobj_ref(NULL, &obj);
+		}
+	}
+
+	if (ret)
+		nv10_fence_context_del(chan, engine);
+	return ret;
+}
+
+static int
+nv10_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nv10_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nv10_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fence_priv *priv = nv_engine(dev, engine);
+
+	nouveau_bo_ref(NULL, &priv->bo);
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv10_fence_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fence_priv *priv;
+	int ret = 0;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nv10_fence_destroy;
+	priv->base.engine.init = nv10_fence_init;
+	priv->base.engine.fini = nv10_fence_fini;
+	priv->base.engine.context_new = nv10_fence_context_new;
+	priv->base.engine.context_del = nv10_fence_context_del;
+	priv->base.emit = nv10_fence_emit;
+	priv->base.read = nv10_fence_read;
+	priv->base.sync = nv10_fence_sync;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+	spin_lock_init(&priv->lock);
+
+	if (dev_priv->chipset >= 0x17) {
+		ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
+				     0, 0x0000, NULL, &priv->bo);
+		if (!ret) {
+			ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
+			if (!ret)
+				ret = nouveau_bo_map(priv->bo);
+			if (ret)
+				nouveau_bo_ref(NULL, &priv->bo);
+		}
+
+		if (ret == 0) {
+			nouveau_bo_wr32(priv->bo, 0x000, 0x00000000);
+			priv->base.sync = nv17_fence_sync;
+		}
+	}
+
+	if (ret)
+		nv10_fence_destroy(dev, NVOBJ_ENGINE_FENCE);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv10_fifo.c b/drivers/gpu/drm/nouveau/nv10_fifo.c
index d2ecbff4bee1..f1fe7d758241 100644
--- a/drivers/gpu/drm/nouveau/nv10_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv10_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -27,220 +27,112 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
+#include "nouveau_util.h"
 #include "nouveau_ramht.h"
 
-#define NV10_RAMFC(c) (dev_priv->ramfc->pinst + ((c) * NV10_RAMFC__SIZE))
-#define NV10_RAMFC__SIZE ((dev_priv->chipset) >= 0x17 ? 64 : 32)
-
-int
-nv10_fifo_channel_id(struct drm_device *dev)
-{
-	return nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
-			NV10_PFIFO_CACHE1_PUSH1_CHID_MASK;
-}
-
-int
-nv10_fifo_create_context(struct nouveau_channel *chan)
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv10_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 32,  0, 0x08,  0, NV10_PFIFO_CACHE1_REF_CNT },
+	{ 16,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 16, 16, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{ 32,  0, 0x18,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x1c,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{}
+};
+
+struct nv10_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv10_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
+
+static int
+nv10_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct drm_device *dev = chan->dev;
-	uint32_t fc = NV10_RAMFC(chan->id);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv10_fifo_chan *fctx;
+	unsigned long flags;
 	int ret;
 
-	ret = nouveau_gpuobj_new_fake(dev, NV10_RAMFC(chan->id), ~0,
-				      NV10_RAMFC__SIZE, NVOBJ_FLAG_ZERO_ALLOC |
-				      NVOBJ_FLAG_ZERO_FREE, &chan->ramfc);
-	if (ret)
-		return ret;
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
 
+	/* map channel control registers */
 	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
 			     NV03_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
-		return -ENOMEM;
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	/* Fill entries that are seen filled in dumps of nvidia driver just
-	 * after channel's is put into DMA mode
-	 */
-	nv_wi32(dev, fc +  0, chan->pushbuf_base);
-	nv_wi32(dev, fc +  4, chan->pushbuf_base);
-	nv_wi32(dev, fc + 12, chan->pushbuf->pinst >> 4);
-	nv_wi32(dev, fc + 20, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8 |
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 32, ~0, 32,
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x08, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x0c, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x10, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x14, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
 #ifdef __BIG_ENDIAN
-			      NV_PFIFO_CACHE1_BIG_ENDIAN |
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
 #endif
-			      0);
-
-	/* enable the fifo dma operation */
-	nv_wr32(dev, NV04_PFIFO_MODE,
-		nv_rd32(dev, NV04_PFIFO_MODE) | (1 << chan->id));
-	return 0;
-}
-
-static void
-nv10_fifo_do_load_context(struct drm_device *dev, int chid)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV10_RAMFC(chid), tmp;
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUT, nv_ri32(dev, fc + 0));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_GET, nv_ri32(dev, fc + 4));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_REF_CNT, nv_ri32(dev, fc + 8));
-
-	tmp = nv_ri32(dev, fc + 12);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE, tmp & 0xFFFF);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT, tmp >> 16);
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_STATE, nv_ri32(dev, fc + 16));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_FETCH, nv_ri32(dev, fc + 20));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_ENGINE, nv_ri32(dev, fc + 24));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL1, nv_ri32(dev, fc + 28));
-
-	if (dev_priv->chipset < 0x17)
-		goto out;
-
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE, nv_ri32(dev, fc + 32));
-	tmp = nv_ri32(dev, fc + 36);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP, tmp);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT, nv_ri32(dev, fc + 40));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_SEMAPHORE, nv_ri32(dev, fc + 44));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_DMA_SUBROUTINE, nv_ri32(dev, fc + 48));
-
-out:
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
-}
-
-int
-nv10_fifo_load_context(struct nouveau_channel *chan)
-{
-	struct drm_device *dev = chan->dev;
-	uint32_t tmp;
-
-	nv10_fifo_do_load_context(dev, chan->id);
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+	nv_wo32(fctx->ramfc, 0x18, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x1c, 0x00000000);
 
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1,
-		     NV03_PFIFO_CACHE1_PUSH1_DMA | chan->id);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 1);
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	/* Reset NV04_PFIFO_CACHE1_DMA_CTL_AT_INFO to INVALID */
-	tmp = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_CTL) & ~(1 << 31);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_CTL, tmp);
-
-	return 0;
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
 int
-nv10_fifo_unload_context(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	uint32_t fc, tmp;
-	int chid;
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 0 || chid >= dev_priv->engine.fifo.channels)
-		return 0;
-	fc = NV10_RAMFC(chid);
-
-	nv_wi32(dev, fc +  0, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUT));
-	nv_wi32(dev, fc +  4, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	nv_wi32(dev, fc +  8, nv_rd32(dev, NV10_PFIFO_CACHE1_REF_CNT));
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE) & 0xFFFF;
-	tmp |= (nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT) << 16);
-	nv_wi32(dev, fc + 12, tmp);
-	nv_wi32(dev, fc + 16, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_STATE));
-	nv_wi32(dev, fc + 20, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_FETCH));
-	nv_wi32(dev, fc + 24, nv_rd32(dev, NV04_PFIFO_CACHE1_ENGINE));
-	nv_wi32(dev, fc + 28, nv_rd32(dev, NV04_PFIFO_CACHE1_PULL1));
-
-	if (dev_priv->chipset < 0x17)
-		goto out;
-
-	nv_wi32(dev, fc + 32, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE));
-	tmp = nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP);
-	nv_wi32(dev, fc + 36, tmp);
-	nv_wi32(dev, fc + 40, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT));
-	nv_wi32(dev, fc + 44, nv_rd32(dev, NV10_PFIFO_CACHE1_SEMAPHORE));
-	nv_wi32(dev, fc + 48, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-
-out:
-	nv10_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-	return 0;
-}
-
-static void
-nv10_fifo_init_reset(struct drm_device *dev)
-{
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) & ~NV_PMC_ENABLE_PFIFO);
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) |  NV_PMC_ENABLE_PFIFO);
-
-	nv_wr32(dev, 0x003224, 0x000f0078);
-	nv_wr32(dev, 0x002044, 0x0101ffff);
-	nv_wr32(dev, 0x002040, 0x000000ff);
-	nv_wr32(dev, 0x002500, 0x00000000);
-	nv_wr32(dev, 0x003000, 0x00000000);
-	nv_wr32(dev, 0x003050, 0x00000000);
-
-	nv_wr32(dev, 0x003258, 0x00000000);
-	nv_wr32(dev, 0x003210, 0x00000000);
-	nv_wr32(dev, 0x003270, 0x00000000);
-}
-
-static void
-nv10_fifo_init_ramxx(struct drm_device *dev)
+nv10_fifo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fifo_priv *priv;
 
-	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
-				       ((dev_priv->ramht->bits - 9) << 16) |
-				       (dev_priv->ramht->gpuobj->pinst >> 8));
-	nv_wr32(dev, NV03_PFIFO_RAMRO, dev_priv->ramro->pinst >> 8);
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
-	if (dev_priv->chipset < 0x17) {
-		nv_wr32(dev, NV03_PFIFO_RAMFC, dev_priv->ramfc->pinst >> 8);
-	} else {
-		nv_wr32(dev, NV03_PFIFO_RAMFC, (dev_priv->ramfc->pinst >> 8) |
-					       (1 << 16) /* 64 Bytes entry*/);
-		/* XXX nvidia blob set bit 18, 21,23 for nv20 & nv30 */
-	}
-}
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv04_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv10_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 31;
+	priv->ramfc_desc = nv10_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
 
-static void
-nv10_fifo_init_intr(struct drm_device *dev)
-{
 	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, 0x002100, 0xffffffff);
-	nv_wr32(dev, 0x002140, 0xffffffff);
-}
-
-int
-nv10_fifo_init(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int i;
-
-	nv10_fifo_init_reset(dev);
-	nv10_fifo_init_ramxx(dev);
-
-	nv10_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-
-	nv10_fifo_init_intr(dev);
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
-
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		if (dev_priv->channels.ptr[i]) {
-			uint32_t mode = nv_rd32(dev, NV04_PFIFO_MODE);
-			nv_wr32(dev, NV04_PFIFO_MODE, mode | (1 << i));
-		}
-	}
-
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv10_graph.c b/drivers/gpu/drm/nouveau/nv10_graph.c
index 7255e4a4d3f3..fb1d88a951de 100644
--- a/drivers/gpu/drm/nouveau/nv10_graph.c
+++ b/drivers/gpu/drm/nouveau/nv10_graph.c
@@ -759,7 +759,6 @@ static int
 nv10_graph_unload_context(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_channel *chan;
 	struct graph_state *ctx;
 	uint32_t tmp;
@@ -782,7 +781,7 @@ nv10_graph_unload_context(struct drm_device *dev)
 
 	nv_wr32(dev, NV10_PGRAPH_CTX_CONTROL, 0x10000000);
 	tmp  = nv_rd32(dev, NV10_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (pfifo->channels - 1) << 24;
+	tmp |= 31 << 24;
 	nv_wr32(dev, NV10_PGRAPH_CTX_USER, tmp);
 	return 0;
 }
@@ -822,12 +821,12 @@ struct nouveau_channel *
 nv10_graph_channel(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int chid = dev_priv->engine.fifo.channels;
+	int chid = 31;
 
 	if (nv_rd32(dev, NV10_PGRAPH_CTX_CONTROL) & 0x00010000)
 		chid = nv_rd32(dev, NV10_PGRAPH_CTX_USER) >> 24;
 
-	if (chid >= dev_priv->engine.fifo.channels)
+	if (chid >= 31)
 		return NULL;
 
 	return dev_priv->channels.ptr[chid];
@@ -948,7 +947,7 @@ nv10_graph_init(struct drm_device *dev, int engine)
 	nv_wr32(dev, NV10_PGRAPH_STATE, 0xFFFFFFFF);
 
 	tmp  = nv_rd32(dev, NV10_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (dev_priv->engine.fifo.channels - 1) << 24;
+	tmp |= 31 << 24;
 	nv_wr32(dev, NV10_PGRAPH_CTX_USER, tmp);
 	nv_wr32(dev, NV10_PGRAPH_CTX_CONTROL, 0x10000100);
 	nv_wr32(dev, NV10_PGRAPH_FFINTFC_ST2, 0x08000000);
@@ -1153,10 +1152,6 @@ nv10_graph_create(struct drm_device *dev)
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	nouveau_irq_register(dev, 12, nv10_graph_isr);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
-
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x0039, GR); /* m2mf */
 	NVOBJ_CLASS(dev, 0x004a, GR); /* gdirect */
diff --git a/drivers/gpu/drm/nouveau/nv17_fifo.c b/drivers/gpu/drm/nouveau/nv17_fifo.c
new file mode 100644
index 000000000000..d9e482e4abee
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv17_fifo.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2012 Ben Skeggs.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial
+ * portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE COPYRIGHT OWNER(S) AND/OR ITS SUPPLIERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "nouveau_drv.h"
+#include "nouveau_fifo.h"
+#include "nouveau_util.h"
+#include "nouveau_ramht.h"
+
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv17_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 32,  0, 0x08,  0, NV10_PFIFO_CACHE1_REF_CNT },
+	{ 16,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 16, 16, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{ 32,  0, 0x18,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x1c,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{ 32,  0, 0x20,  0, NV10_PFIFO_CACHE1_ACQUIRE_VALUE },
+	{ 32,  0, 0x24,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP },
+	{ 32,  0, 0x28,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT },
+	{ 32,  0, 0x2c,  0, NV10_PFIFO_CACHE1_SEMAPHORE },
+	{ 32,  0, 0x30,  0, NV10_PFIFO_CACHE1_DMA_SUBROUTINE },
+	{}
+};
+
+struct nv17_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv17_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
+
+static int
+nv17_fifo_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv17_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv17_fifo_chan *fctx;
+	unsigned long flags;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	/* map channel control registers */
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV03_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 64, ~0, 64,
+				      NVOBJ_FLAG_ZERO_ALLOC |
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x0c, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x14, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
+#ifdef __BIG_ENDIAN
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
+#endif
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static int
+nv17_fifo_init(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv17_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, 0);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, NV_PMC_ENABLE_PFIFO);
+
+	nv_wr32(dev, NV04_PFIFO_DELAY_0, 0x000000ff);
+	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, 0x0101ffff);
+
+	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
+				       ((dev_priv->ramht->bits - 9) << 16) |
+				       (dev_priv->ramht->gpuobj->pinst >> 8));
+	nv_wr32(dev, NV03_PFIFO_RAMRO, dev_priv->ramro->pinst >> 8);
+	nv_wr32(dev, NV03_PFIFO_RAMFC, 0x00010000 |
+				       dev_priv->ramfc->pinst >> 8);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+
+	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xffffffff);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (dev_priv->channels.ptr[i])
+			nv_mask(dev, NV04_PFIFO_MODE, (1 << i), (1 << i));
+	}
+
+	return 0;
+}
+
+int
+nv17_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv17_fifo_priv *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv17_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv17_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 31;
+	priv->ramfc_desc = nv17_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv20_graph.c b/drivers/gpu/drm/nouveau/nv20_graph.c
index 183e37512ef9..e34ea30758f6 100644
--- a/drivers/gpu/drm/nouveau/nv20_graph.c
+++ b/drivers/gpu/drm/nouveau/nv20_graph.c
@@ -43,8 +43,6 @@ struct nv20_graph_engine {
 int
 nv20_graph_unload_context(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_channel *chan;
 	struct nouveau_gpuobj *grctx;
 	u32 tmp;
@@ -62,7 +60,7 @@ nv20_graph_unload_context(struct drm_device *dev)
 
 	nv_wr32(dev, NV10_PGRAPH_CTX_CONTROL, 0x10000000);
 	tmp  = nv_rd32(dev, NV10_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (pfifo->channels - 1) << 24;
+	tmp |= 31 << 24;
 	nv_wr32(dev, NV10_PGRAPH_CTX_USER, tmp);
 	return 0;
 }
@@ -796,10 +794,6 @@ nv20_graph_create(struct drm_device *dev)
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	nouveau_irq_register(dev, 12, nv20_graph_isr);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
-
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x0039, GR); /* m2mf */
 	NVOBJ_CLASS(dev, 0x004a, GR); /* gdirect */
diff --git a/drivers/gpu/drm/nouveau/nv31_mpeg.c b/drivers/gpu/drm/nouveau/nv31_mpeg.c
index 6f06a0713f00..5f239bf658c4 100644
--- a/drivers/gpu/drm/nouveau/nv31_mpeg.c
+++ b/drivers/gpu/drm/nouveau/nv31_mpeg.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 
 struct nv31_mpeg_engine {
@@ -208,6 +209,7 @@ nv31_mpeg_mthd_dma(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
 static int
 nv31_mpeg_isr_chid(struct drm_device *dev, u32 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *ctx;
 	unsigned long flags;
@@ -218,7 +220,7 @@ nv31_mpeg_isr_chid(struct drm_device *dev, u32 inst)
 		return 0;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		if (!dev_priv->channels.ptr[i])
 			continue;
 
diff --git a/drivers/gpu/drm/nouveau/nv40_fifo.c b/drivers/gpu/drm/nouveau/nv40_fifo.c
index 68cb2d991c88..cdc818479b0a 100644
--- a/drivers/gpu/drm/nouveau/nv40_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv40_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -25,215 +25,123 @@
  */
 
 #include "drmP.h"
+#include "drm.h"
 #include "nouveau_drv.h"
-#include "nouveau_drm.h"
+#include "nouveau_fifo.h"
+#include "nouveau_util.h"
 #include "nouveau_ramht.h"
 
-#define NV40_RAMFC(c) (dev_priv->ramfc->pinst + ((c) * NV40_RAMFC__SIZE))
-#define NV40_RAMFC__SIZE 128
-
-int
-nv40_fifo_create_context(struct nouveau_channel *chan)
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv40_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 32,  0, 0x08,  0, NV10_PFIFO_CACHE1_REF_CNT },
+	{ 32,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 28,  0, 0x18,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{  2, 28, 0x18, 28, 0x002058 },
+	{ 32,  0, 0x1c,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x20,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{ 32,  0, 0x24,  0, NV10_PFIFO_CACHE1_ACQUIRE_VALUE },
+	{ 32,  0, 0x28,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP },
+	{ 32,  0, 0x2c,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT },
+	{ 32,  0, 0x30,  0, NV10_PFIFO_CACHE1_SEMAPHORE },
+	{ 32,  0, 0x34,  0, NV10_PFIFO_CACHE1_DMA_SUBROUTINE },
+	{ 32,  0, 0x38,  0, NV40_PFIFO_GRCTX_INSTANCE },
+	{ 17,  0, 0x3c,  0, NV04_PFIFO_DMA_TIMESLICE },
+	{ 32,  0, 0x40,  0, 0x0032e4 },
+	{ 32,  0, 0x44,  0, 0x0032e8 },
+	{ 32,  0, 0x4c,  0, 0x002088 },
+	{ 32,  0, 0x50,  0, 0x003300 },
+	{ 32,  0, 0x54,  0, 0x00330c },
+	{}
+};
+
+struct nv40_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv40_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
+
+static int
+nv40_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV40_RAMFC(chan->id);
+	struct nv40_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv40_fifo_chan *fctx;
 	unsigned long flags;
 	int ret;
 
-	ret = nouveau_gpuobj_new_fake(dev, NV40_RAMFC(chan->id), ~0,
-				      NV40_RAMFC__SIZE, NVOBJ_FLAG_ZERO_ALLOC |
-				      NVOBJ_FLAG_ZERO_FREE, &chan->ramfc);
-	if (ret)
-		return ret;
-
-	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
-			     NV40_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
 		return -ENOMEM;
 
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	/* map channel control registers */
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV03_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	nv_wi32(dev, fc +  0, chan->pushbuf_base);
-	nv_wi32(dev, fc +  4, chan->pushbuf_base);
-	nv_wi32(dev, fc + 12, chan->pushbuf->pinst >> 4);
-	nv_wi32(dev, fc + 24, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8 |
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 128, ~0, 128,
+				      NVOBJ_FLAG_ZERO_ALLOC |
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x0c, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x18, 0x30000000 |
+				   NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
 #ifdef __BIG_ENDIAN
-			      NV_PFIFO_CACHE1_BIG_ENDIAN |
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
 #endif
-			      0x30000000 /* no idea.. */);
-	nv_wi32(dev, fc + 60, 0x0001FFFF);
-
-	/* enable the fifo dma operation */
-	nv_wr32(dev, NV04_PFIFO_MODE,
-		nv_rd32(dev, NV04_PFIFO_MODE) | (1 << chan->id));
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+	nv_wo32(fctx->ramfc, 0x3c, 0x0001ffff);
 
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-	return 0;
-}
-
-static void
-nv40_fifo_do_load_context(struct drm_device *dev, int chid)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV40_RAMFC(chid), tmp, tmp2;
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUT, nv_ri32(dev, fc + 0));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_GET, nv_ri32(dev, fc + 4));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_REF_CNT, nv_ri32(dev, fc + 8));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE, nv_ri32(dev, fc + 12));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT, nv_ri32(dev, fc + 16));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_STATE, nv_ri32(dev, fc + 20));
-
-	/* No idea what 0x2058 is.. */
-	tmp   = nv_ri32(dev, fc + 24);
-	tmp2  = nv_rd32(dev, 0x2058) & 0xFFF;
-	tmp2 |= (tmp & 0x30000000);
-	nv_wr32(dev, 0x2058, tmp2);
-	tmp  &= ~0x30000000;
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_FETCH, tmp);
 
-	nv_wr32(dev, NV04_PFIFO_CACHE1_ENGINE, nv_ri32(dev, fc + 28));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL1, nv_ri32(dev, fc + 32));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE, nv_ri32(dev, fc + 36));
-	tmp = nv_ri32(dev, fc + 40);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP, tmp);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT, nv_ri32(dev, fc + 44));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_SEMAPHORE, nv_ri32(dev, fc + 48));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_DMA_SUBROUTINE, nv_ri32(dev, fc + 52));
-	nv_wr32(dev, NV40_PFIFO_GRCTX_INSTANCE, nv_ri32(dev, fc + 56));
+	/*XXX: remove this later, need fifo engine context commit hook */
+	nouveau_gpuobj_ref(fctx->ramfc, &chan->ramfc);
 
-	/* Don't clobber the TIMEOUT_ENABLED flag when restoring from RAMFC */
-	tmp  = nv_rd32(dev, NV04_PFIFO_DMA_TIMESLICE) & ~0x1FFFF;
-	tmp |= nv_ri32(dev, fc + 60) & 0x1FFFF;
-	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, tmp);
-
-	nv_wr32(dev, 0x32e4, nv_ri32(dev, fc + 64));
-	/* NVIDIA does this next line twice... */
-	nv_wr32(dev, 0x32e8, nv_ri32(dev, fc + 68));
-	nv_wr32(dev, 0x2088, nv_ri32(dev, fc + 76));
-	nv_wr32(dev, 0x3300, nv_ri32(dev, fc + 80));
-	nv_wr32(dev, 0x330c, nv_ri32(dev, fc + 84));
-
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
-}
-
-int
-nv40_fifo_load_context(struct nouveau_channel *chan)
-{
-	struct drm_device *dev = chan->dev;
-	uint32_t tmp;
-
-	nv40_fifo_do_load_context(dev, chan->id);
-
-	/* Set channel active, and in DMA mode */
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1,
-		     NV40_PFIFO_CACHE1_PUSH1_DMA | chan->id);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 1);
-
-	/* Reset DMA_CTL_AT_INFO to INVALID */
-	tmp = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_CTL) & ~(1 << 31);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_CTL, tmp);
-
-	return 0;
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
-int
-nv40_fifo_unload_context(struct drm_device *dev)
+static int
+nv40_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	uint32_t fc, tmp;
-	int chid;
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 0 || chid >= dev_priv->engine.fifo.channels)
-		return 0;
-	fc = NV40_RAMFC(chid);
-
-	nv_wi32(dev, fc + 0, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUT));
-	nv_wi32(dev, fc + 4, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	nv_wi32(dev, fc + 8, nv_rd32(dev, NV10_PFIFO_CACHE1_REF_CNT));
-	nv_wi32(dev, fc + 12, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE));
-	nv_wi32(dev, fc + 16, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT));
-	nv_wi32(dev, fc + 20, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_STATE));
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_FETCH);
-	tmp |= nv_rd32(dev, 0x2058) & 0x30000000;
-	nv_wi32(dev, fc + 24, tmp);
-	nv_wi32(dev, fc + 28, nv_rd32(dev, NV04_PFIFO_CACHE1_ENGINE));
-	nv_wi32(dev, fc + 32, nv_rd32(dev, NV04_PFIFO_CACHE1_PULL1));
-	nv_wi32(dev, fc + 36, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE));
-	tmp = nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP);
-	nv_wi32(dev, fc + 40, tmp);
-	nv_wi32(dev, fc + 44, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT));
-	nv_wi32(dev, fc + 48, nv_rd32(dev, NV10_PFIFO_CACHE1_SEMAPHORE));
-	/* NVIDIA read 0x3228 first, then write DMA_GET here.. maybe something
-	 * more involved depending on the value of 0x3228?
-	 */
-	nv_wi32(dev, fc + 52, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	nv_wi32(dev, fc + 56, nv_rd32(dev, NV40_PFIFO_GRCTX_INSTANCE));
-	nv_wi32(dev, fc + 60, nv_rd32(dev, NV04_PFIFO_DMA_TIMESLICE) & 0x1ffff);
-	/* No idea what the below is for exactly, ripped from a mmio-trace */
-	nv_wi32(dev, fc + 64, nv_rd32(dev, NV40_PFIFO_UNK32E4));
-	/* NVIDIA do this next line twice.. bug? */
-	nv_wi32(dev, fc + 68, nv_rd32(dev, 0x32e8));
-	nv_wi32(dev, fc + 76, nv_rd32(dev, 0x2088));
-	nv_wi32(dev, fc + 80, nv_rd32(dev, 0x3300));
-#if 0 /* no real idea which is PUT/GET in UNK_48.. */
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_GET);
-	tmp |= (nv_rd32(dev, NV04_PFIFO_CACHE1_PUT) << 16);
-	nv_wi32(dev, fc + 72, tmp);
-#endif
-	nv_wi32(dev, fc + 84, nv_rd32(dev, 0x330c));
-
-	nv40_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1,
-		     NV40_PFIFO_CACHE1_PUSH1_DMA | (pfifo->channels - 1));
-	return 0;
-}
-
-static void
-nv40_fifo_init_reset(struct drm_device *dev)
-{
+	struct nv40_fifo_priv *priv = nv_engine(dev, engine);
 	int i;
 
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) & ~NV_PMC_ENABLE_PFIFO);
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) |  NV_PMC_ENABLE_PFIFO);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, 0);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, NV_PMC_ENABLE_PFIFO);
 
-	nv_wr32(dev, 0x003224, 0x000f0078);
-	nv_wr32(dev, 0x003210, 0x00000000);
-	nv_wr32(dev, 0x003270, 0x00000000);
-	nv_wr32(dev, 0x003240, 0x00000000);
-	nv_wr32(dev, 0x003244, 0x00000000);
-	nv_wr32(dev, 0x003258, 0x00000000);
-	nv_wr32(dev, 0x002504, 0x00000000);
-	for (i = 0; i < 16; i++)
-		nv_wr32(dev, 0x002510 + (i * 4), 0x00000000);
-	nv_wr32(dev, 0x00250c, 0x0000ffff);
-	nv_wr32(dev, 0x002048, 0x00000000);
-	nv_wr32(dev, 0x003228, 0x00000000);
-	nv_wr32(dev, 0x0032e8, 0x00000000);
-	nv_wr32(dev, 0x002410, 0x00000000);
-	nv_wr32(dev, 0x002420, 0x00000000);
-	nv_wr32(dev, 0x002058, 0x00000001);
-	nv_wr32(dev, 0x00221c, 0x00000000);
-	/* something with 0x2084, read/modify/write, no change */
 	nv_wr32(dev, 0x002040, 0x000000ff);
-	nv_wr32(dev, 0x002500, 0x00000000);
-	nv_wr32(dev, 0x003200, 0x00000000);
-
-	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, 0x2101ffff);
-}
-
-static void
-nv40_fifo_init_ramxx(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	nv_wr32(dev, 0x002044, 0x2101ffff);
+	nv_wr32(dev, 0x002058, 0x00000001);
 
 	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
 				       ((dev_priv->ramht->bits - 9) << 16) |
@@ -244,64 +152,59 @@ nv40_fifo_init_ramxx(struct drm_device *dev)
 	case 0x47:
 	case 0x49:
 	case 0x4b:
-		nv_wr32(dev, 0x2230, 1);
-		break;
-	default:
-		break;
-	}
-
-	switch (dev_priv->chipset) {
+		nv_wr32(dev, 0x002230, 0x00000001);
 	case 0x40:
 	case 0x41:
 	case 0x42:
 	case 0x43:
 	case 0x45:
-	case 0x47:
 	case 0x48:
-	case 0x49:
-	case 0x4b:
-		nv_wr32(dev, NV40_PFIFO_RAMFC, 0x30002);
+		nv_wr32(dev, 0x002220, 0x00030002);
 		break;
 	default:
-		nv_wr32(dev, 0x2230, 0);
-		nv_wr32(dev, NV40_PFIFO_RAMFC,
-			((dev_priv->vram_size - 512 * 1024 +
-			  dev_priv->ramfc->pinst) >> 16) | (3 << 16));
+		nv_wr32(dev, 0x002230, 0x00000000);
+		nv_wr32(dev, 0x002220, ((dev_priv->vram_size - 512 * 1024 +
+					 dev_priv->ramfc->pinst) >> 16) |
+				       0x00030000);
 		break;
 	}
-}
 
-static void
-nv40_fifo_init_intr(struct drm_device *dev)
-{
-	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, 0x002100, 0xffffffff);
-	nv_wr32(dev, 0x002140, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+
+	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xffffffff);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (dev_priv->channels.ptr[i])
+			nv_mask(dev, NV04_PFIFO_MODE, (1 << i), (1 << i));
+	}
+
+	return 0;
 }
 
 int
-nv40_fifo_init(struct drm_device *dev)
+nv40_fifo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int i;
-
-	nv40_fifo_init_reset(dev);
-	nv40_fifo_init_ramxx(dev);
+	struct nv40_fifo_priv *priv;
 
-	nv40_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-
-	nv40_fifo_init_intr(dev);
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		if (dev_priv->channels.ptr[i]) {
-			uint32_t mode = nv_rd32(dev, NV04_PFIFO_MODE);
-			nv_wr32(dev, NV04_PFIFO_MODE, mode | (1 << i));
-		}
-	}
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv40_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv40_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 31;
+	priv->ramfc_desc = nv40_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
 
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv40_graph.c b/drivers/gpu/drm/nouveau/nv40_graph.c
index ba14a93d8afa..aa9e2df64a26 100644
--- a/drivers/gpu/drm/nouveau/nv40_graph.c
+++ b/drivers/gpu/drm/nouveau/nv40_graph.c
@@ -27,7 +27,7 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
-#include "nouveau_grctx.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 
 struct nv40_graph_engine {
@@ -42,7 +42,6 @@ nv40_graph_context_new(struct nouveau_channel *chan, int engine)
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *grctx = NULL;
-	struct nouveau_grctx ctx = {};
 	unsigned long flags;
 	int ret;
 
@@ -52,11 +51,7 @@ nv40_graph_context_new(struct nouveau_channel *chan, int engine)
 		return ret;
 
 	/* Initialise default context values */
-	ctx.dev = chan->dev;
-	ctx.mode = NOUVEAU_GRCTX_VALS;
-	ctx.data = grctx;
-	nv40_grctx_init(&ctx);
-
+	nv40_grctx_fill(dev, grctx);
 	nv_wo32(grctx, 0, grctx->vinst);
 
 	/* init grctx pointer in ramfc, and on PFIFO if channel is
@@ -184,8 +179,7 @@ nv40_graph_init(struct drm_device *dev, int engine)
 	struct nv40_graph_engine *pgraph = nv_engine(dev, engine);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_fb_engine *pfb = &dev_priv->engine.fb;
-	struct nouveau_grctx ctx = {};
-	uint32_t vramsz, *cp;
+	uint32_t vramsz;
 	int i, j;
 
 	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) &
@@ -193,22 +187,8 @@ nv40_graph_init(struct drm_device *dev, int engine)
 	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) |
 			 NV_PMC_ENABLE_PGRAPH);
 
-	cp = kmalloc(sizeof(*cp) * 256, GFP_KERNEL);
-	if (!cp)
-		return -ENOMEM;
-
-	ctx.dev = dev;
-	ctx.mode = NOUVEAU_GRCTX_PROG;
-	ctx.data = cp;
-	ctx.ctxprog_max = 256;
-	nv40_grctx_init(&ctx);
-	pgraph->grctx_size = ctx.ctxvals_pos * 4;
-
-	nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_INDEX, 0);
-	for (i = 0; i < ctx.ctxprog_len; i++)
-		nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_DATA, cp[i]);
-
-	kfree(cp);
+	/* generate and upload context program */
+	nv40_grctx_init(dev, &pgraph->grctx_size);
 
 	/* No context present currently */
 	nv_wr32(dev, NV40_PGRAPH_CTXCTL_CUR, 0x00000000);
@@ -366,13 +346,14 @@ nv40_graph_fini(struct drm_device *dev, int engine, bool suspend)
 static int
 nv40_graph_isr_chid(struct drm_device *dev, u32 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *grctx;
 	unsigned long flags;
 	int i;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		if (!dev_priv->channels.ptr[i])
 			continue;
 		grctx = dev_priv->channels.ptr[i]->engctx[NVOBJ_ENGINE_GR];
@@ -460,7 +441,6 @@ nv40_graph_create(struct drm_device *dev)
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	nouveau_irq_register(dev, 12, nv40_graph_isr);
 
-	NVOBJ_CLASS(dev, 0x506e, SW); /* nvsw */
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x0039, GR); /* m2mf */
 	NVOBJ_CLASS(dev, 0x004a, GR); /* gdirect */
@@ -483,8 +463,5 @@ nv40_graph_create(struct drm_device *dev)
 	else
 		NVOBJ_CLASS(dev, 0x4097, GR);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv40_grctx.c b/drivers/gpu/drm/nouveau/nv40_grctx.c
index f70447d131d7..be0a74750fb1 100644
--- a/drivers/gpu/drm/nouveau/nv40_grctx.c
+++ b/drivers/gpu/drm/nouveau/nv40_grctx.c
@@ -595,8 +595,8 @@ nv40_graph_construct_shader(struct nouveau_grctx *ctx)
 	}
 }
 
-void
-nv40_grctx_init(struct nouveau_grctx *ctx)
+static void
+nv40_grctx_generate(struct nouveau_grctx *ctx)
 {
 	/* decide whether we're loading/unloading the context */
 	cp_bra (ctx, AUTO_SAVE, PENDING, cp_setup_save);
@@ -660,3 +660,31 @@ nv40_grctx_init(struct nouveau_grctx *ctx)
 	cp_out (ctx, CP_END);
 }
 
+void
+nv40_grctx_fill(struct drm_device *dev, struct nouveau_gpuobj *mem)
+{
+	nv40_grctx_generate(&(struct nouveau_grctx) {
+			     .dev = dev,
+			     .mode = NOUVEAU_GRCTX_VALS,
+			     .data = mem,
+			   });
+}
+
+void
+nv40_grctx_init(struct drm_device *dev, u32 *size)
+{
+	u32 ctxprog[256], i;
+	struct nouveau_grctx ctx = {
+		.dev = dev,
+		.mode = NOUVEAU_GRCTX_PROG,
+		.data = ctxprog,
+		.ctxprog_max = ARRAY_SIZE(ctxprog)
+	};
+
+	nv40_grctx_generate(&ctx);
+
+	nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_INDEX, 0);
+	for (i = 0; i < ctx.ctxprog_len; i++)
+		nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_DATA, ctxprog[i]);
+	*size = ctx.ctxvals_pos * 4;
+}
diff --git a/drivers/gpu/drm/nouveau/nv40_pm.c b/drivers/gpu/drm/nouveau/nv40_pm.c
index c7615381c5d9..e66273aff493 100644
--- a/drivers/gpu/drm/nouveau/nv40_pm.c
+++ b/drivers/gpu/drm/nouveau/nv40_pm.c
@@ -27,6 +27,7 @@
 #include "nouveau_bios.h"
 #include "nouveau_pm.h"
 #include "nouveau_hw.h"
+#include "nouveau_fifo.h"
 
 #define min2(a,b) ((a) < (b) ? (a) : (b))
 
diff --git a/drivers/gpu/drm/nouveau/nv50_crtc.c b/drivers/gpu/drm/nouveau/nv50_crtc.c
index 701b927998bf..97a477b3d52d 100644
--- a/drivers/gpu/drm/nouveau/nv50_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv50_crtc.c
@@ -79,15 +79,15 @@ nv50_crtc_blank(struct nouveau_crtc *nv_crtc, bool blanked)
 			NV_ERROR(dev, "no space while blanking crtc\n");
 			return ret;
 		}
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
 		OUT_RING(evo, NV50_EVO_CRTC_CLUT_MODE_BLANK);
 		OUT_RING(evo, 0);
 		if (dev_priv->chipset != 0x50) {
-			BEGIN_RING(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
+			BEGIN_NV04(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
 			OUT_RING(evo, NV84_EVO_CRTC_CLUT_DMA_HANDLE_NONE);
 		}
 
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
 		OUT_RING(evo, NV50_EVO_CRTC_FB_DMA_HANDLE_NONE);
 	} else {
 		if (nv_crtc->cursor.visible)
@@ -100,20 +100,20 @@ nv50_crtc_blank(struct nouveau_crtc *nv_crtc, bool blanked)
 			NV_ERROR(dev, "no space while unblanking crtc\n");
 			return ret;
 		}
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
 		OUT_RING(evo, nv_crtc->lut.depth == 8 ?
 				NV50_EVO_CRTC_CLUT_MODE_OFF :
 				NV50_EVO_CRTC_CLUT_MODE_ON);
 		OUT_RING(evo, nv_crtc->lut.nvbo->bo.offset >> 8);
 		if (dev_priv->chipset != 0x50) {
-			BEGIN_RING(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
+			BEGIN_NV04(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
 			OUT_RING(evo, NvEvoVRAM);
 		}
 
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, FB_OFFSET), 2);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, FB_OFFSET), 2);
 		OUT_RING(evo, nv_crtc->fb.offset >> 8);
 		OUT_RING(evo, 0);
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
 		if (dev_priv->chipset != 0x50)
 			if (nv_crtc->fb.tile_flags == 0x7a00 ||
 			    nv_crtc->fb.tile_flags == 0xfe00)
@@ -158,10 +158,10 @@ nv50_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool update)
 
 	ret = RING_SPACE(evo, 2 + (update ? 2 : 0));
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(head, DITHER_CTRL), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(head, DITHER_CTRL), 1);
 		OUT_RING  (evo, mode);
 		if (update) {
-			BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+			BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 			OUT_RING  (evo, 0);
 			FIRE_RING (evo);
 		}
@@ -193,11 +193,11 @@ nv50_crtc_set_color_vibrance(struct nouveau_crtc *nv_crtc, bool update)
 
 	hue = ((nv_crtc->vibrant_hue * 2047) / 100) & 0xfff;
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, COLOR_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, COLOR_CTRL), 1);
 	OUT_RING  (evo, (hue << 20) | (vib << 8));
 
 	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING  (evo, 0);
 		FIRE_RING (evo);
 	}
@@ -311,9 +311,9 @@ nv50_crtc_set_scale(struct nouveau_crtc *nv_crtc, bool update)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_CTRL), 1);
 	OUT_RING  (evo, ctrl);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_RES1), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_RES1), 2);
 	OUT_RING  (evo, oY << 16 | oX);
 	OUT_RING  (evo, oY << 16 | oX);
 
@@ -383,23 +383,15 @@ nv50_crtc_set_clock(struct drm_device *dev, int head, int pclk)
 static void
 nv50_crtc_destroy(struct drm_crtc *crtc)
 {
-	struct drm_device *dev;
-	struct nouveau_crtc *nv_crtc;
-
-	if (!crtc)
-		return;
-
-	dev = crtc->dev;
-	nv_crtc = nouveau_crtc(crtc);
-
-	NV_DEBUG_KMS(dev, "\n");
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 
-	drm_crtc_cleanup(&nv_crtc->base);
+	NV_DEBUG_KMS(crtc->dev, "\n");
 
 	nouveau_bo_unmap(nv_crtc->lut.nvbo);
 	nouveau_bo_ref(NULL, &nv_crtc->lut.nvbo);
 	nouveau_bo_unmap(nv_crtc->cursor.nvbo);
 	nouveau_bo_ref(NULL, &nv_crtc->cursor.nvbo);
+	drm_crtc_cleanup(&nv_crtc->base);
 	kfree(nv_crtc);
 }
 
@@ -593,7 +585,7 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 		if (ret)
 			return ret;
 
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_DMA), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_DMA), 1);
 		OUT_RING  (evo, fb->r_dma);
 	}
 
@@ -601,18 +593,18 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 	if (ret)
 		return ret;
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_OFFSET), 5);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_OFFSET), 5);
 	OUT_RING  (evo, nv_crtc->fb.offset >> 8);
 	OUT_RING  (evo, 0);
 	OUT_RING  (evo, (drm_fb->height << 16) | drm_fb->width);
 	OUT_RING  (evo, fb->r_pitch);
 	OUT_RING  (evo, fb->r_format);
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CLUT_MODE), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CLUT_MODE), 1);
 	OUT_RING  (evo, fb->base.depth == 8 ?
 		   NV50_EVO_CRTC_CLUT_MODE_OFF : NV50_EVO_CRTC_CLUT_MODE_ON);
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_POS), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_POS), 1);
 	OUT_RING  (evo, (y << 16) | x);
 
 	if (nv_crtc->lut.depth != fb->base.depth) {
@@ -672,23 +664,23 @@ nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
 
 	ret = RING_SPACE(evo, 18);
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, 0x0804 + head, 2);
+		BEGIN_NV04(evo, 0, 0x0804 + head, 2);
 		OUT_RING  (evo, 0x00800000 | mode->clock);
 		OUT_RING  (evo, (ilace == 2) ? 2 : 0);
-		BEGIN_RING(evo, 0, 0x0810 + head, 6);
+		BEGIN_NV04(evo, 0, 0x0810 + head, 6);
 		OUT_RING  (evo, 0x00000000); /* border colour */
 		OUT_RING  (evo, (vactive << 16) | hactive);
 		OUT_RING  (evo, ( vsynce << 16) | hsynce);
 		OUT_RING  (evo, (vblanke << 16) | hblanke);
 		OUT_RING  (evo, (vblanks << 16) | hblanks);
 		OUT_RING  (evo, (vblan2e << 16) | vblan2s);
-		BEGIN_RING(evo, 0, 0x082c + head, 1);
+		BEGIN_NV04(evo, 0, 0x082c + head, 1);
 		OUT_RING  (evo, 0x00000000);
-		BEGIN_RING(evo, 0, 0x0900 + head, 1);
+		BEGIN_NV04(evo, 0, 0x0900 + head, 1);
 		OUT_RING  (evo, 0x00000311); /* makes sync channel work */
-		BEGIN_RING(evo, 0, 0x08c8 + head, 1);
+		BEGIN_NV04(evo, 0, 0x08c8 + head, 1);
 		OUT_RING  (evo, (umode->vdisplay << 16) | umode->hdisplay);
-		BEGIN_RING(evo, 0, 0x08d4 + head, 1);
+		BEGIN_NV04(evo, 0, 0x08d4 + head, 1);
 		OUT_RING  (evo, 0x00000000); /* screen position */
 	}
 
@@ -755,21 +747,25 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	if (!nv_crtc)
 		return -ENOMEM;
 
+	nv_crtc->index = index;
+	nv_crtc->set_dither = nv50_crtc_set_dither;
+	nv_crtc->set_scale = nv50_crtc_set_scale;
+	nv_crtc->set_color_vibrance = nv50_crtc_set_color_vibrance;
 	nv_crtc->color_vibrance = 50;
 	nv_crtc->vibrant_hue = 0;
-
-	/* Default CLUT parameters, will be activated on the hw upon
-	 * first mode set.
-	 */
+	nv_crtc->lut.depth = 0;
 	for (i = 0; i < 256; i++) {
 		nv_crtc->lut.r[i] = i << 8;
 		nv_crtc->lut.g[i] = i << 8;
 		nv_crtc->lut.b[i] = i << 8;
 	}
-	nv_crtc->lut.depth = 0;
+
+	drm_crtc_init(dev, &nv_crtc->base, &nv50_crtc_funcs);
+	drm_crtc_helper_add(&nv_crtc->base, &nv50_crtc_helper_funcs);
+	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, 4096, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->lut.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->lut.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->lut.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -778,24 +774,12 @@ nv50_crtc_create(struct drm_device *dev, int index)
 			nouveau_bo_ref(NULL, &nv_crtc->lut.nvbo);
 	}
 
-	if (ret) {
-		kfree(nv_crtc);
-		return ret;
-	}
-
-	nv_crtc->index = index;
+	if (ret)
+		goto out;
 
-	/* set function pointers */
-	nv_crtc->set_dither = nv50_crtc_set_dither;
-	nv_crtc->set_scale = nv50_crtc_set_scale;
-	nv_crtc->set_color_vibrance = nv50_crtc_set_color_vibrance;
-
-	drm_crtc_init(dev, &nv_crtc->base, &nv50_crtc_funcs);
-	drm_crtc_helper_add(&nv_crtc->base, &nv50_crtc_helper_funcs);
-	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -804,6 +788,12 @@ nv50_crtc_create(struct drm_device *dev, int index)
 			nouveau_bo_ref(NULL, &nv_crtc->cursor.nvbo);
 	}
 
+	if (ret)
+		goto out;
+
 	nv50_cursor_init(nv_crtc);
-	return 0;
+out:
+	if (ret)
+		nv50_crtc_destroy(&nv_crtc->base);
+	return ret;
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_cursor.c b/drivers/gpu/drm/nouveau/nv50_cursor.c
index adfc9b607a50..af4ec7bf3670 100644
--- a/drivers/gpu/drm/nouveau/nv50_cursor.c
+++ b/drivers/gpu/drm/nouveau/nv50_cursor.c
@@ -53,15 +53,15 @@ nv50_cursor_show(struct nouveau_crtc *nv_crtc, bool update)
 	}
 
 	if (dev_priv->chipset != 0x50) {
-		BEGIN_RING(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
+		BEGIN_NV04(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
 		OUT_RING(evo, NvEvoVRAM);
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
 	OUT_RING(evo, NV50_EVO_CRTC_CURSOR_CTRL_SHOW);
 	OUT_RING(evo, nv_crtc->cursor.offset >> 8);
 
 	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING(evo, 0);
 		FIRE_RING(evo);
 		nv_crtc->cursor.visible = true;
@@ -86,16 +86,16 @@ nv50_cursor_hide(struct nouveau_crtc *nv_crtc, bool update)
 		NV_ERROR(dev, "no space while hiding cursor\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
 	OUT_RING(evo, NV50_EVO_CRTC_CURSOR_CTRL_HIDE);
 	OUT_RING(evo, 0);
 	if (dev_priv->chipset != 0x50) {
-		BEGIN_RING(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
+		BEGIN_NV04(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
 		OUT_RING(evo, NV84_EVO_CRTC_CURSOR_DMA_HANDLE_NONE);
 	}
 
 	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING(evo, 0);
 		FIRE_RING(evo);
 		nv_crtc->cursor.visible = false;
diff --git a/drivers/gpu/drm/nouveau/nv50_dac.c b/drivers/gpu/drm/nouveau/nv50_dac.c
index 55c56330be6d..eb216a446b89 100644
--- a/drivers/gpu/drm/nouveau/nv50_dac.c
+++ b/drivers/gpu/drm/nouveau/nv50_dac.c
@@ -55,9 +55,9 @@ nv50_dac_disconnect(struct drm_encoder *encoder)
 		NV_ERROR(dev, "no space while disconnecting DAC\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 1);
 	OUT_RING  (evo, 0);
-	BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 	OUT_RING  (evo, 0);
 
 	nv_encoder->crtc = NULL;
@@ -240,7 +240,7 @@ nv50_dac_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		NV_ERROR(dev, "no space while connecting DAC\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 2);
 	OUT_RING(evo, mode_ctl);
 	OUT_RING(evo, mode_ctl2);
 
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 8b78b9cfa383..5c41612723b4 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -32,6 +32,7 @@
 #include "nouveau_fb.h"
 #include "nouveau_fbcon.h"
 #include "nouveau_ramht.h"
+#include "nouveau_software.h"
 #include "drm_crtc_helper.h"
 
 static void nv50_display_isr(struct drm_device *);
@@ -140,11 +141,11 @@ nv50_display_sync(struct drm_device *dev)
 
 	ret = RING_SPACE(evo, 6);
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x80000000);
-		BEGIN_RING(evo, 0, 0x0080, 1);
+		BEGIN_NV04(evo, 0, 0x0080, 1);
 		OUT_RING  (evo, 0);
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x00000000);
 
 		nv_wo32(disp->ntfy, 0x000, 0x00000000);
@@ -267,7 +268,7 @@ nv50_display_init(struct drm_device *dev)
 	ret = RING_SPACE(evo, 3);
 	if (ret)
 		return ret;
-	BEGIN_RING(evo, 0, NV50_EVO_UNK84, 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_UNK84, 2);
 	OUT_RING  (evo, NV50_EVO_UNK84_NOTIFY_DISABLED);
 	OUT_RING  (evo, NvEvoSync);
 
@@ -292,7 +293,7 @@ nv50_display_fini(struct drm_device *dev)
 
 	ret = RING_SPACE(evo, 2);
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING(evo, 0);
 	}
 	FIRE_RING(evo);
@@ -358,8 +359,11 @@ nv50_display_create(struct drm_device *dev)
 	dev_priv->engine.display.priv = priv;
 
 	/* Create CRTC objects */
-	for (i = 0; i < 2; i++)
-		nv50_crtc_create(dev, i);
+	for (i = 0; i < 2; i++) {
+		ret = nv50_crtc_create(dev, i);
+		if (ret)
+			return ret;
+	}
 
 	/* We setup the encoders from the BIOS table */
 	for (i = 0 ; i < dcb->entries; i++) {
@@ -438,13 +442,13 @@ nv50_display_flip_stop(struct drm_crtc *crtc)
 		return;
 	}
 
-	BEGIN_RING(evo, 0, 0x0084, 1);
+	BEGIN_NV04(evo, 0, 0x0084, 1);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x0094, 1);
+	BEGIN_NV04(evo, 0, 0x0094, 1);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x00c0, 1);
+	BEGIN_NV04(evo, 0, 0x00c0, 1);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x0080, 1);
+	BEGIN_NV04(evo, 0, 0x0080, 1);
 	OUT_RING  (evo, 0x00000000);
 	FIRE_RING (evo);
 }
@@ -474,28 +478,28 @@ nv50_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		}
 
 		if (dev_priv->chipset < 0xc0) {
-			BEGIN_RING(chan, 0, 0x0060, 2);
+			BEGIN_NV04(chan, 0, 0x0060, 2);
 			OUT_RING  (chan, NvEvoSema0 + nv_crtc->index);
 			OUT_RING  (chan, dispc->sem.offset);
-			BEGIN_RING(chan, 0, 0x006c, 1);
+			BEGIN_NV04(chan, 0, 0x006c, 1);
 			OUT_RING  (chan, 0xf00d0000 | dispc->sem.value);
-			BEGIN_RING(chan, 0, 0x0064, 2);
+			BEGIN_NV04(chan, 0, 0x0064, 2);
 			OUT_RING  (chan, dispc->sem.offset ^ 0x10);
 			OUT_RING  (chan, 0x74b1e000);
-			BEGIN_RING(chan, 0, 0x0060, 1);
+			BEGIN_NV04(chan, 0, 0x0060, 1);
 			if (dev_priv->chipset < 0x84)
 				OUT_RING  (chan, NvSema);
 			else
 				OUT_RING  (chan, chan->vram_handle);
 		} else {
-			u64 offset = chan->dispc_vma[nv_crtc->index].offset;
+			u64 offset = nvc0_software_crtc(chan, nv_crtc->index);
 			offset += dispc->sem.offset;
-			BEGIN_NVC0(chan, 2, 0, 0x0010, 4);
+			BEGIN_NVC0(chan, 0, 0x0010, 4);
 			OUT_RING  (chan, upper_32_bits(offset));
 			OUT_RING  (chan, lower_32_bits(offset));
 			OUT_RING  (chan, 0xf00d0000 | dispc->sem.value);
 			OUT_RING  (chan, 0x1002);
-			BEGIN_NVC0(chan, 2, 0, 0x0010, 4);
+			BEGIN_NVC0(chan, 0, 0x0010, 4);
 			OUT_RING  (chan, upper_32_bits(offset));
 			OUT_RING  (chan, lower_32_bits(offset ^ 0x10));
 			OUT_RING  (chan, 0x74b1e000);
@@ -508,40 +512,40 @@ nv50_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	}
 
 	/* queue the flip on the crtc's "display sync" channel */
-	BEGIN_RING(evo, 0, 0x0100, 1);
+	BEGIN_NV04(evo, 0, 0x0100, 1);
 	OUT_RING  (evo, 0xfffe0000);
 	if (chan) {
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x00000100);
 	} else {
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x00000010);
 		/* allows gamma somehow, PDISP will bitch at you if
 		 * you don't wait for vblank before changing this..
 		 */
-		BEGIN_RING(evo, 0, 0x00e0, 1);
+		BEGIN_NV04(evo, 0, 0x00e0, 1);
 		OUT_RING  (evo, 0x40000000);
 	}
-	BEGIN_RING(evo, 0, 0x0088, 4);
+	BEGIN_NV04(evo, 0, 0x0088, 4);
 	OUT_RING  (evo, dispc->sem.offset);
 	OUT_RING  (evo, 0xf00d0000 | dispc->sem.value);
 	OUT_RING  (evo, 0x74b1e000);
 	OUT_RING  (evo, NvEvoSync);
-	BEGIN_RING(evo, 0, 0x00a0, 2);
+	BEGIN_NV04(evo, 0, 0x00a0, 2);
 	OUT_RING  (evo, 0x00000000);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x00c0, 1);
+	BEGIN_NV04(evo, 0, 0x00c0, 1);
 	OUT_RING  (evo, nv_fb->r_dma);
-	BEGIN_RING(evo, 0, 0x0110, 2);
+	BEGIN_NV04(evo, 0, 0x0110, 2);
 	OUT_RING  (evo, 0x00000000);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x0800, 5);
+	BEGIN_NV04(evo, 0, 0x0800, 5);
 	OUT_RING  (evo, nv_fb->nvbo->bo.offset >> 8);
 	OUT_RING  (evo, 0);
 	OUT_RING  (evo, (fb->height << 16) | fb->width);
 	OUT_RING  (evo, nv_fb->r_pitch);
 	OUT_RING  (evo, nv_fb->r_format);
-	BEGIN_RING(evo, 0, 0x0080, 1);
+	BEGIN_NV04(evo, 0, 0x0080, 1);
 	OUT_RING  (evo, 0x00000000);
 	FIRE_RING (evo);
 
@@ -642,20 +646,7 @@ nv50_display_script_select(struct drm_device *dev, struct dcb_entry *dcb,
 static void
 nv50_display_vblank_crtc_handler(struct drm_device *dev, int crtc)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *chan, *tmp;
-
-	list_for_each_entry_safe(chan, tmp, &dev_priv->vbl_waiting,
-				 nvsw.vbl_wait) {
-		if (chan->nvsw.vblsem_head != crtc)
-			continue;
-
-		nouveau_bo_wr32(chan->notifier_bo, chan->nvsw.vblsem_offset,
-						chan->nvsw.vblsem_rval);
-		list_del(&chan->nvsw.vbl_wait);
-		drm_vblank_put(dev, crtc);
-	}
-
+	nouveau_software_vblank(dev, crtc);
 	drm_handle_vblank(dev, crtc);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nv50_display.h b/drivers/gpu/drm/nouveau/nv50_display.h
index 5d3dd14d2837..e9db9b97f041 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.h
+++ b/drivers/gpu/drm/nouveau/nv50_display.h
@@ -33,6 +33,7 @@
 #include "nouveau_dma.h"
 #include "nouveau_reg.h"
 #include "nouveau_crtc.h"
+#include "nouveau_software.h"
 #include "nv50_evo.h"
 
 struct nv50_display_crtc {
diff --git a/drivers/gpu/drm/nouveau/nv50_evo.c b/drivers/gpu/drm/nouveau/nv50_evo.c
index 9b962e989d7c..ddcd55595824 100644
--- a/drivers/gpu/drm/nouveau/nv50_evo.c
+++ b/drivers/gpu/drm/nouveau/nv50_evo.c
@@ -117,7 +117,7 @@ nv50_evo_channel_new(struct drm_device *dev, int chid,
 	evo->user_get = 4;
 	evo->user_put = 0;
 
-	ret = nouveau_bo_new(dev, 4096, 0, TTM_PL_FLAG_VRAM, 0, 0,
+	ret = nouveau_bo_new(dev, 4096, 0, TTM_PL_FLAG_VRAM, 0, 0, NULL,
 			     &evo->pushbuf_bo);
 	if (ret == 0)
 		ret = nouveau_bo_pin(evo->pushbuf_bo, TTM_PL_FLAG_VRAM);
@@ -333,7 +333,7 @@ nv50_evo_create(struct drm_device *dev)
 			goto err;
 
 		ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
-				     0, 0x0000, &dispc->sem.bo);
+				     0, 0x0000, NULL, &dispc->sem.bo);
 		if (!ret) {
 			ret = nouveau_bo_pin(dispc->sem.bo, TTM_PL_FLAG_VRAM);
 			if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nv50_fb.c b/drivers/gpu/drm/nouveau/nv50_fb.c
index bdd2afe29205..f1e4b9e07d14 100644
--- a/drivers/gpu/drm/nouveau/nv50_fb.c
+++ b/drivers/gpu/drm/nouveau/nv50_fb.c
@@ -2,6 +2,7 @@
 #include "drm.h"
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
+#include "nouveau_fifo.h"
 
 struct nv50_fb_priv {
 	struct page *r100c08_page;
@@ -212,6 +213,7 @@ static struct nouveau_enum vm_fault[] = {
 void
 nv50_fb_vm_trap(struct drm_device *dev, int display)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	const struct nouveau_enum *en, *cl;
 	unsigned long flags;
@@ -236,7 +238,7 @@ nv50_fb_vm_trap(struct drm_device *dev, int display)
 	/* lookup channel id */
 	chinst = (trap[2] << 16) | trap[1];
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (ch = 0; ch < dev_priv->engine.fifo.channels; ch++) {
+	for (ch = 0; ch < pfifo->channels; ch++) {
 		struct nouveau_channel *chan = dev_priv->channels.ptr[ch];
 
 		if (!chan || !chan->ramin)
diff --git a/drivers/gpu/drm/nouveau/nv50_fbcon.c b/drivers/gpu/drm/nouveau/nv50_fbcon.c
index dc75a7206524..e3c8b05dcae4 100644
--- a/drivers/gpu/drm/nouveau/nv50_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nv50_fbcon.c
@@ -43,22 +43,22 @@ nv50_fbcon_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 		return ret;
 
 	if (rect->rop != ROP_COPY) {
-		BEGIN_RING(chan, NvSub2D, 0x02ac, 1);
+		BEGIN_NV04(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING(chan, 1);
 	}
-	BEGIN_RING(chan, NvSub2D, 0x0588, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0588, 1);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR)
 		OUT_RING(chan, ((uint32_t *)info->pseudo_palette)[rect->color]);
 	else
 		OUT_RING(chan, rect->color);
-	BEGIN_RING(chan, NvSub2D, 0x0600, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x0600, 4);
 	OUT_RING(chan, rect->dx);
 	OUT_RING(chan, rect->dy);
 	OUT_RING(chan, rect->dx + rect->width);
 	OUT_RING(chan, rect->dy + rect->height);
 	if (rect->rop != ROP_COPY) {
-		BEGIN_RING(chan, NvSub2D, 0x02ac, 1);
+		BEGIN_NV04(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING(chan, 3);
 	}
 	FIRE_RING(chan);
@@ -78,14 +78,14 @@ nv50_fbcon_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSub2D, 0x0110, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0110, 1);
 	OUT_RING(chan, 0);
-	BEGIN_RING(chan, NvSub2D, 0x08b0, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x08b0, 4);
 	OUT_RING(chan, region->dx);
 	OUT_RING(chan, region->dy);
 	OUT_RING(chan, region->width);
 	OUT_RING(chan, region->height);
-	BEGIN_RING(chan, NvSub2D, 0x08d0, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x08d0, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, region->sx);
 	OUT_RING(chan, 0);
@@ -116,7 +116,7 @@ nv50_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 	width = ALIGN(image->width, 32);
 	dwords = (width * image->height) >> 5;
 
-	BEGIN_RING(chan, NvSub2D, 0x0814, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0814, 2);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
 		OUT_RING(chan, palette[image->bg_color] | mask);
@@ -125,10 +125,10 @@ nv50_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		OUT_RING(chan, image->bg_color);
 		OUT_RING(chan, image->fg_color);
 	}
-	BEGIN_RING(chan, NvSub2D, 0x0838, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0838, 2);
 	OUT_RING(chan, image->width);
 	OUT_RING(chan, image->height);
-	BEGIN_RING(chan, NvSub2D, 0x0850, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x0850, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, image->dx);
 	OUT_RING(chan, 0);
@@ -143,7 +143,7 @@ nv50_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 
 		dwords -= push;
 
-		BEGIN_RING(chan, NvSub2D, 0x40000860, push);
+		BEGIN_NI04(chan, NvSub2D, 0x0860, push);
 		OUT_RINGp(chan, data, push);
 		data += push;
 	}
@@ -199,60 +199,59 @@ nv50_fbcon_accel_init(struct fb_info *info)
 		return ret;
 	}
 
-	BEGIN_RING(chan, NvSub2D, 0x0000, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0000, 1);
 	OUT_RING(chan, Nv2D);
-	BEGIN_RING(chan, NvSub2D, 0x0180, 4);
-	OUT_RING(chan, NvNotify0);
+	BEGIN_NV04(chan, NvSub2D, 0x0184, 3);
 	OUT_RING(chan, chan->vram_handle);
 	OUT_RING(chan, chan->vram_handle);
 	OUT_RING(chan, chan->vram_handle);
-	BEGIN_RING(chan, NvSub2D, 0x0290, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0290, 1);
 	OUT_RING(chan, 0);
-	BEGIN_RING(chan, NvSub2D, 0x0888, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0888, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x02ac, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x02ac, 1);
 	OUT_RING(chan, 3);
-	BEGIN_RING(chan, NvSub2D, 0x02a0, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x02a0, 1);
 	OUT_RING(chan, 0x55);
-	BEGIN_RING(chan, NvSub2D, 0x08c0, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x08c0, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0580, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0580, 2);
 	OUT_RING(chan, 4);
 	OUT_RING(chan, format);
-	BEGIN_RING(chan, NvSub2D, 0x02e8, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x02e8, 2);
 	OUT_RING(chan, 2);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0804, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0804, 1);
 	OUT_RING(chan, format);
-	BEGIN_RING(chan, NvSub2D, 0x0800, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0800, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0808, 3);
+	BEGIN_NV04(chan, NvSub2D, 0x0808, 3);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x081c, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x081c, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0840, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x0840, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0200, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0200, 2);
 	OUT_RING(chan, format);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0214, 5);
+	BEGIN_NV04(chan, NvSub2D, 0x0214, 5);
 	OUT_RING(chan, info->fix.line_length);
 	OUT_RING(chan, info->var.xres_virtual);
 	OUT_RING(chan, info->var.yres_virtual);
 	OUT_RING(chan, upper_32_bits(fb->vma.offset));
 	OUT_RING(chan, lower_32_bits(fb->vma.offset));
-	BEGIN_RING(chan, NvSub2D, 0x0230, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0230, 2);
 	OUT_RING(chan, format);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0244, 5);
+	BEGIN_NV04(chan, NvSub2D, 0x0244, 5);
 	OUT_RING(chan, info->fix.line_length);
 	OUT_RING(chan, info->var.xres_virtual);
 	OUT_RING(chan, info->var.yres_virtual);
diff --git a/drivers/gpu/drm/nouveau/nv50_fifo.c b/drivers/gpu/drm/nouveau/nv50_fifo.c
index 3bc2a565c20b..55383b85db0b 100644
--- a/drivers/gpu/drm/nouveau/nv50_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv50_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -27,480 +27,268 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 #include "nouveau_vm.h"
 
-static void
+struct nv50_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct nouveau_gpuobj *playlist[2];
+	int cur_playlist;
+};
+
+struct nv50_fifo_chan {
+	struct nouveau_fifo_chan base;
+};
+
+void
 nv50_fifo_playlist_update(struct drm_device *dev)
 {
+	struct nv50_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_gpuobj *cur;
-	int i, nr;
-
-	NV_DEBUG(dev, "\n");
+	int i, p;
 
-	cur = pfifo->playlist[pfifo->cur_playlist];
-	pfifo->cur_playlist = !pfifo->cur_playlist;
+	cur = priv->playlist[priv->cur_playlist];
+	priv->cur_playlist = !priv->cur_playlist;
 
-	/* We never schedule channel 0 or 127 */
-	for (i = 1, nr = 0; i < 127; i++) {
-		if (dev_priv->channels.ptr[i] &&
-		    dev_priv->channels.ptr[i]->ramfc) {
-			nv_wo32(cur, (nr * 4), i);
-			nr++;
-		}
+	for (i = 0, p = 0; i < priv->base.channels; i++) {
+		if (nv_rd32(dev, 0x002600 + (i * 4)) & 0x80000000)
+			nv_wo32(cur, p++ * 4, i);
 	}
-	dev_priv->engine.instmem.flush(dev);
-
-	nv_wr32(dev, 0x32f4, cur->vinst >> 12);
-	nv_wr32(dev, 0x32ec, nr);
-	nv_wr32(dev, 0x2500, 0x101);
-}
 
-static void
-nv50_fifo_channel_enable(struct drm_device *dev, int channel)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *chan = dev_priv->channels.ptr[channel];
-	uint32_t inst;
-
-	NV_DEBUG(dev, "ch%d\n", channel);
-
-	if (dev_priv->chipset == 0x50)
-		inst = chan->ramfc->vinst >> 12;
-	else
-		inst = chan->ramfc->vinst >> 8;
+	dev_priv->engine.instmem.flush(dev);
 
-	nv_wr32(dev, NV50_PFIFO_CTX_TABLE(channel), inst |
-		     NV50_PFIFO_CTX_TABLE_CHANNEL_ENABLED);
+	nv_wr32(dev, 0x0032f4, cur->vinst >> 12);
+	nv_wr32(dev, 0x0032ec, p);
+	nv_wr32(dev, 0x002500, 0x00000101);
 }
 
-static void
-nv50_fifo_channel_disable(struct drm_device *dev, int channel)
+static int
+nv50_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
+	struct nv50_fifo_priv *priv = nv_engine(chan->dev, engine);
+	struct nv50_fifo_chan *fctx;
+	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t inst;
-
-	NV_DEBUG(dev, "ch%d\n", channel);
+	u64 ib_offset = chan->pushbuf_base + chan->dma.ib_base * 4;
+	u64 instance = chan->ramin->vinst >> 12;
+	unsigned long flags;
+	int ret = 0, i;
 
-	if (dev_priv->chipset == 0x50)
-		inst = NV50_PFIFO_CTX_TABLE_INSTANCE_MASK_G80;
-	else
-		inst = NV50_PFIFO_CTX_TABLE_INSTANCE_MASK_G84;
-	nv_wr32(dev, NV50_PFIFO_CTX_TABLE(channel), inst);
-}
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+	atomic_inc(&chan->vm->engref[engine]);
 
-static void
-nv50_fifo_init_reset(struct drm_device *dev)
-{
-	uint32_t pmc_e = NV_PMC_ENABLE_PFIFO;
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV50_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	NV_DEBUG(dev, "\n");
+	for (i = 0; i < 0x100; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+	nv_wo32(chan->ramin, 0x3c, 0x403f6078);
+	nv_wo32(chan->ramin, 0x40, 0x00000000);
+	nv_wo32(chan->ramin, 0x44, 0x01003fff);
+	nv_wo32(chan->ramin, 0x48, chan->pushbuf->cinst >> 4);
+	nv_wo32(chan->ramin, 0x50, lower_32_bits(ib_offset));
+	nv_wo32(chan->ramin, 0x54, upper_32_bits(ib_offset) |
+				   drm_order(chan->dma.ib_max + 1) << 16);
+	nv_wo32(chan->ramin, 0x60, 0x7fffffff);
+	nv_wo32(chan->ramin, 0x78, 0x00000000);
+	nv_wo32(chan->ramin, 0x7c, 0x30000001);
+	nv_wo32(chan->ramin, 0x80, ((chan->ramht->bits - 9) << 27) |
+				   (4 << 24) /* SEARCH_FULL */ |
+				   (chan->ramht->gpuobj->cinst >> 4));
 
-	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) & ~pmc_e);
-	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) |  pmc_e);
-}
+	dev_priv->engine.instmem.flush(dev);
 
-static void
-nv50_fifo_init_intr(struct drm_device *dev)
-{
-	NV_DEBUG(dev, "\n");
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x80000000 | instance);
+	nv50_fifo_playlist_update(dev);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xFFFFFFFF);
-	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xFFFFFFFF);
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
-static void
-nv50_fifo_init_context_table(struct drm_device *dev)
+static bool
+nv50_fifo_kickoff(struct nouveau_channel *chan)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int i;
-
-	NV_DEBUG(dev, "\n");
-
-	for (i = 0; i < NV50_PFIFO_CTX_TABLE__SIZE; i++) {
-		if (dev_priv->channels.ptr[i])
-			nv50_fifo_channel_enable(dev, i);
-		else
-			nv50_fifo_channel_disable(dev, i);
+	struct drm_device *dev = chan->dev;
+	bool done = true;
+	u32 me;
+
+	/* HW bug workaround:
+	 *
+	 * PFIFO will hang forever if the connected engines don't report
+	 * that they've processed the context switch request.
+	 *
+	 * In order for the kickoff to work, we need to ensure all the
+	 * connected engines are in a state where they can answer.
+	 *
+	 * Newer chipsets don't seem to suffer from this issue, and well,
+	 * there's also a "ignore these engines" bitmask reg we can use
+	 * if we hit the issue there..
+	 */
+
+	/* PME: make sure engine is enabled */
+	me = nv_mask(dev, 0x00b860, 0x00000001, 0x00000001);
+
+	/* do the kickoff... */
+	nv_wr32(dev, 0x0032fc, chan->ramin->vinst >> 12);
+	if (!nv_wait_ne(dev, 0x0032fc, 0xffffffff, 0xffffffff)) {
+		NV_INFO(dev, "PFIFO: channel %d unload timeout\n", chan->id);
+		done = false;
 	}
 
-	nv50_fifo_playlist_update(dev);
+	/* restore any engine states we changed, and exit */
+	nv_wr32(dev, 0x00b860, me);
+	return done;
 }
 
 static void
-nv50_fifo_init_regs__nv(struct drm_device *dev)
-{
-	NV_DEBUG(dev, "\n");
-
-	nv_wr32(dev, 0x250c, 0x6f3cfc34);
-}
-
-static void
-nv50_fifo_init_regs(struct drm_device *dev)
-{
-	NV_DEBUG(dev, "\n");
-
-	nv_wr32(dev, 0x2500, 0);
-	nv_wr32(dev, 0x3250, 0);
-	nv_wr32(dev, 0x3220, 0);
-	nv_wr32(dev, 0x3204, 0);
-	nv_wr32(dev, 0x3210, 0);
-	nv_wr32(dev, 0x3270, 0);
-	nv_wr32(dev, 0x2044, 0x01003fff);
-
-	/* Enable dummy channels setup by nv50_instmem.c */
-	nv50_fifo_channel_enable(dev, 0);
-	nv50_fifo_channel_enable(dev, 127);
-}
-
-int
-nv50_fifo_init(struct drm_device *dev)
+nv50_fifo_context_del(struct nouveau_channel *chan, int engine)
 {
+	struct nv50_fifo_chan *fctx = chan->engctx[engine];
+	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int ret;
+	unsigned long flags;
 
-	NV_DEBUG(dev, "\n");
+	/* remove channel from playlist, will context switch if active */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, 0x002600 + (chan->id * 4), 0x80000000, 0x00000000);
+	nv50_fifo_playlist_update(dev);
 
-	if (pfifo->playlist[0]) {
-		pfifo->cur_playlist = !pfifo->cur_playlist;
-		goto just_reset;
-	}
+	/* tell any engines on this channel to unload their contexts */
+	nv50_fifo_kickoff(chan);
 
-	ret = nouveau_gpuobj_new(dev, NULL, 128*4, 0x1000,
-				 NVOBJ_FLAG_ZERO_ALLOC,
-				 &pfifo->playlist[0]);
-	if (ret) {
-		NV_ERROR(dev, "error creating playlist 0: %d\n", ret);
-		return ret;
-	}
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x00000000);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	ret = nouveau_gpuobj_new(dev, NULL, 128*4, 0x1000,
-				 NVOBJ_FLAG_ZERO_ALLOC,
-				 &pfifo->playlist[1]);
-	if (ret) {
-		nouveau_gpuobj_ref(NULL, &pfifo->playlist[0]);
-		NV_ERROR(dev, "error creating playlist 1: %d\n", ret);
-		return ret;
+	/* clean up */
+	if (chan->user) {
+		iounmap(chan->user);
+		chan->user = NULL;
 	}
 
-just_reset:
-	nv50_fifo_init_reset(dev);
-	nv50_fifo_init_intr(dev);
-	nv50_fifo_init_context_table(dev);
-	nv50_fifo_init_regs__nv(dev);
-	nv50_fifo_init_regs(dev);
-	dev_priv->engine.fifo.enable(dev);
-	dev_priv->engine.fifo.reassign(dev, true);
-
-	return 0;
+	atomic_dec(&chan->vm->engref[engine]);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
 }
 
-void
-nv50_fifo_takedown(struct drm_device *dev)
+static int
+nv50_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	u32 instance;
+	int i;
 
-	NV_DEBUG(dev, "\n");
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000100);
+	nv_wr32(dev, 0x00250c, 0x6f3cfc34);
+	nv_wr32(dev, 0x002044, 0x01003fff);
 
-	if (!pfifo->playlist[0])
-		return;
+	nv_wr32(dev, 0x002100, 0xffffffff);
+	nv_wr32(dev, 0x002140, 0xffffffff);
 
-	nv_wr32(dev, 0x2140, 0x00000000);
-	nouveau_irq_unregister(dev, 8);
+	for (i = 0; i < 128; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan && chan->engctx[engine])
+			instance = 0x80000000 | chan->ramin->vinst >> 12;
+		else
+			instance = 0x00000000;
+		nv_wr32(dev, 0x002600 + (i * 4), instance);
+	}
 
-	nouveau_gpuobj_ref(NULL, &pfifo->playlist[0]);
-	nouveau_gpuobj_ref(NULL, &pfifo->playlist[1]);
-}
+	nv50_fifo_playlist_update(dev);
 
-int
-nv50_fifo_channel_id(struct drm_device *dev)
-{
-	return nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
-			NV50_PFIFO_CACHE1_PUSH1_CHID_MASK;
+	nv_wr32(dev, 0x003200, 1);
+	nv_wr32(dev, 0x003250, 1);
+	nv_wr32(dev, 0x002500, 1);
+	return 0;
 }
 
-int
-nv50_fifo_create_context(struct nouveau_channel *chan)
+static int
+nv50_fifo_fini(struct drm_device *dev, int engine, bool suspend)
 {
-	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpuobj *ramfc = NULL;
-        uint64_t ib_offset = chan->pushbuf_base + chan->dma.ib_base * 4;
-	unsigned long flags;
-	int ret;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	if (dev_priv->chipset == 0x50) {
-		ret = nouveau_gpuobj_new_fake(dev, chan->ramin->pinst,
-					      chan->ramin->vinst, 0x100,
-					      NVOBJ_FLAG_ZERO_ALLOC |
-					      NVOBJ_FLAG_ZERO_FREE,
-					      &chan->ramfc);
-		if (ret)
-			return ret;
-
-		ret = nouveau_gpuobj_new_fake(dev, chan->ramin->pinst + 0x0400,
-					      chan->ramin->vinst + 0x0400,
-					      4096, 0, &chan->cache);
-		if (ret)
-			return ret;
-	} else {
-		ret = nouveau_gpuobj_new(dev, chan, 0x100, 256,
-					 NVOBJ_FLAG_ZERO_ALLOC |
-					 NVOBJ_FLAG_ZERO_FREE, &chan->ramfc);
-		if (ret)
-			return ret;
-
-		ret = nouveau_gpuobj_new(dev, chan, 4096, 1024,
-					 0, &chan->cache);
-		if (ret)
-			return ret;
-	}
-	ramfc = chan->ramfc;
+	struct nv50_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
 
-	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
-			     NV50_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
-		return -ENOMEM;
+	/* set playlist length to zero, fifo will unload context */
+	nv_wr32(dev, 0x0032ec, 0);
 
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-
-	nv_wo32(ramfc, 0x48, chan->pushbuf->cinst >> 4);
-	nv_wo32(ramfc, 0x80, ((chan->ramht->bits - 9) << 27) |
-			     (4 << 24) /* SEARCH_FULL */ |
-			     (chan->ramht->gpuobj->cinst >> 4));
-	nv_wo32(ramfc, 0x44, 0x01003fff);
-	nv_wo32(ramfc, 0x60, 0x7fffffff);
-	nv_wo32(ramfc, 0x40, 0x00000000);
-	nv_wo32(ramfc, 0x7c, 0x30000001);
-	nv_wo32(ramfc, 0x78, 0x00000000);
-	nv_wo32(ramfc, 0x3c, 0x403f6078);
-	nv_wo32(ramfc, 0x50, lower_32_bits(ib_offset));
-	nv_wo32(ramfc, 0x54, upper_32_bits(ib_offset) |
-                drm_order(chan->dma.ib_max + 1) << 16);
-
-	if (dev_priv->chipset != 0x50) {
-		nv_wo32(chan->ramin, 0, chan->id);
-		nv_wo32(chan->ramin, 4, chan->ramfc->vinst >> 8);
-
-		nv_wo32(ramfc, 0x88, chan->cache->vinst >> 10);
-		nv_wo32(ramfc, 0x98, chan->ramin->vinst >> 12);
+	/* tell all connected engines to unload their contexts */
+	for (i = 0; i < priv->base.channels; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan && !nv50_fifo_kickoff(chan))
+			return -EBUSY;
 	}
 
-	dev_priv->engine.instmem.flush(dev);
-
-	nv50_fifo_channel_enable(dev, chan->id);
-	nv50_fifo_playlist_update(dev);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+	nv_wr32(dev, 0x002140, 0);
 	return 0;
 }
 
 void
-nv50_fifo_destroy_context(struct nouveau_channel *chan)
+nv50_fifo_tlb_flush(struct drm_device *dev, int engine)
 {
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nouveau_gpuobj *ramfc = NULL;
-	unsigned long flags;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	pfifo->reassign(dev, false);
-
-	/* Unload the context if it's the currently active one */
-	if (pfifo->channel_id(dev) == chan->id) {
-		pfifo->disable(dev);
-		pfifo->unload_context(dev);
-		pfifo->enable(dev);
-	}
-
-	/* This will ensure the channel is seen as disabled. */
-	nouveau_gpuobj_ref(chan->ramfc, &ramfc);
-	nouveau_gpuobj_ref(NULL, &chan->ramfc);
-	nv50_fifo_channel_disable(dev, chan->id);
-
-	/* Dummy channel, also used on ch 127 */
-	if (chan->id == 0)
-		nv50_fifo_channel_disable(dev, 127);
-	nv50_fifo_playlist_update(dev);
-
-	pfifo->reassign(dev, true);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-
-	/* Free the channel resources */
-	if (chan->user) {
-		iounmap(chan->user);
-		chan->user = NULL;
-	}
-	nouveau_gpuobj_ref(NULL, &ramfc);
-	nouveau_gpuobj_ref(NULL, &chan->cache);
+	nv50_vm_flush_engine(dev, 5);
 }
 
-int
-nv50_fifo_load_context(struct nouveau_channel *chan)
+void
+nv50_fifo_destroy(struct drm_device *dev, int engine)
 {
-	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpuobj *ramfc = chan->ramfc;
-	struct nouveau_gpuobj *cache = chan->cache;
-	int ptr, cnt;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	nv_wr32(dev, 0x3330, nv_ro32(ramfc, 0x00));
-	nv_wr32(dev, 0x3334, nv_ro32(ramfc, 0x04));
-	nv_wr32(dev, 0x3240, nv_ro32(ramfc, 0x08));
-	nv_wr32(dev, 0x3320, nv_ro32(ramfc, 0x0c));
-	nv_wr32(dev, 0x3244, nv_ro32(ramfc, 0x10));
-	nv_wr32(dev, 0x3328, nv_ro32(ramfc, 0x14));
-	nv_wr32(dev, 0x3368, nv_ro32(ramfc, 0x18));
-	nv_wr32(dev, 0x336c, nv_ro32(ramfc, 0x1c));
-	nv_wr32(dev, 0x3370, nv_ro32(ramfc, 0x20));
-	nv_wr32(dev, 0x3374, nv_ro32(ramfc, 0x24));
-	nv_wr32(dev, 0x3378, nv_ro32(ramfc, 0x28));
-	nv_wr32(dev, 0x337c, nv_ro32(ramfc, 0x2c));
-	nv_wr32(dev, 0x3228, nv_ro32(ramfc, 0x30));
-	nv_wr32(dev, 0x3364, nv_ro32(ramfc, 0x34));
-	nv_wr32(dev, 0x32a0, nv_ro32(ramfc, 0x38));
-	nv_wr32(dev, 0x3224, nv_ro32(ramfc, 0x3c));
-	nv_wr32(dev, 0x324c, nv_ro32(ramfc, 0x40));
-	nv_wr32(dev, 0x2044, nv_ro32(ramfc, 0x44));
-	nv_wr32(dev, 0x322c, nv_ro32(ramfc, 0x48));
-	nv_wr32(dev, 0x3234, nv_ro32(ramfc, 0x4c));
-	nv_wr32(dev, 0x3340, nv_ro32(ramfc, 0x50));
-	nv_wr32(dev, 0x3344, nv_ro32(ramfc, 0x54));
-	nv_wr32(dev, 0x3280, nv_ro32(ramfc, 0x58));
-	nv_wr32(dev, 0x3254, nv_ro32(ramfc, 0x5c));
-	nv_wr32(dev, 0x3260, nv_ro32(ramfc, 0x60));
-	nv_wr32(dev, 0x3264, nv_ro32(ramfc, 0x64));
-	nv_wr32(dev, 0x3268, nv_ro32(ramfc, 0x68));
-	nv_wr32(dev, 0x326c, nv_ro32(ramfc, 0x6c));
-	nv_wr32(dev, 0x32e4, nv_ro32(ramfc, 0x70));
-	nv_wr32(dev, 0x3248, nv_ro32(ramfc, 0x74));
-	nv_wr32(dev, 0x2088, nv_ro32(ramfc, 0x78));
-	nv_wr32(dev, 0x2058, nv_ro32(ramfc, 0x7c));
-	nv_wr32(dev, 0x2210, nv_ro32(ramfc, 0x80));
-
-	cnt = nv_ro32(ramfc, 0x84);
-	for (ptr = 0; ptr < cnt; ptr++) {
-		nv_wr32(dev, NV40_PFIFO_CACHE1_METHOD(ptr),
-			nv_ro32(cache, (ptr * 8) + 0));
-		nv_wr32(dev, NV40_PFIFO_CACHE1_DATA(ptr),
-			nv_ro32(cache, (ptr * 8) + 4));
-	}
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, cnt << 2);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-
-	/* guessing that all the 0x34xx regs aren't on NV50 */
-	if (dev_priv->chipset != 0x50) {
-		nv_wr32(dev, 0x340c, nv_ro32(ramfc, 0x88));
-		nv_wr32(dev, 0x3400, nv_ro32(ramfc, 0x8c));
-		nv_wr32(dev, 0x3404, nv_ro32(ramfc, 0x90));
-		nv_wr32(dev, 0x3408, nv_ro32(ramfc, 0x94));
-		nv_wr32(dev, 0x3410, nv_ro32(ramfc, 0x98));
-	}
+	struct nv50_fifo_priv *priv = nv_engine(dev, engine);
 
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, chan->id | (1<<16));
-	return 0;
+	nouveau_irq_unregister(dev, 8);
+
+	nouveau_gpuobj_ref(NULL, &priv->playlist[0]);
+	nouveau_gpuobj_ref(NULL, &priv->playlist[1]);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
 }
 
 int
-nv50_fifo_unload_context(struct drm_device *dev)
+nv50_fifo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nouveau_gpuobj *ramfc, *cache;
-	struct nouveau_channel *chan = NULL;
-	int chid, get, put, ptr;
-
-	NV_DEBUG(dev, "\n");
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 1 || chid >= dev_priv->engine.fifo.channels - 1)
-		return 0;
-
-	chan = dev_priv->channels.ptr[chid];
-	if (!chan) {
-		NV_ERROR(dev, "Inactive channel on PFIFO: %d\n", chid);
-		return -EINVAL;
-	}
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-	ramfc = chan->ramfc;
-	cache = chan->cache;
-
-	nv_wo32(ramfc, 0x00, nv_rd32(dev, 0x3330));
-	nv_wo32(ramfc, 0x04, nv_rd32(dev, 0x3334));
-	nv_wo32(ramfc, 0x08, nv_rd32(dev, 0x3240));
-	nv_wo32(ramfc, 0x0c, nv_rd32(dev, 0x3320));
-	nv_wo32(ramfc, 0x10, nv_rd32(dev, 0x3244));
-	nv_wo32(ramfc, 0x14, nv_rd32(dev, 0x3328));
-	nv_wo32(ramfc, 0x18, nv_rd32(dev, 0x3368));
-	nv_wo32(ramfc, 0x1c, nv_rd32(dev, 0x336c));
-	nv_wo32(ramfc, 0x20, nv_rd32(dev, 0x3370));
-	nv_wo32(ramfc, 0x24, nv_rd32(dev, 0x3374));
-	nv_wo32(ramfc, 0x28, nv_rd32(dev, 0x3378));
-	nv_wo32(ramfc, 0x2c, nv_rd32(dev, 0x337c));
-	nv_wo32(ramfc, 0x30, nv_rd32(dev, 0x3228));
-	nv_wo32(ramfc, 0x34, nv_rd32(dev, 0x3364));
-	nv_wo32(ramfc, 0x38, nv_rd32(dev, 0x32a0));
-	nv_wo32(ramfc, 0x3c, nv_rd32(dev, 0x3224));
-	nv_wo32(ramfc, 0x40, nv_rd32(dev, 0x324c));
-	nv_wo32(ramfc, 0x44, nv_rd32(dev, 0x2044));
-	nv_wo32(ramfc, 0x48, nv_rd32(dev, 0x322c));
-	nv_wo32(ramfc, 0x4c, nv_rd32(dev, 0x3234));
-	nv_wo32(ramfc, 0x50, nv_rd32(dev, 0x3340));
-	nv_wo32(ramfc, 0x54, nv_rd32(dev, 0x3344));
-	nv_wo32(ramfc, 0x58, nv_rd32(dev, 0x3280));
-	nv_wo32(ramfc, 0x5c, nv_rd32(dev, 0x3254));
-	nv_wo32(ramfc, 0x60, nv_rd32(dev, 0x3260));
-	nv_wo32(ramfc, 0x64, nv_rd32(dev, 0x3264));
-	nv_wo32(ramfc, 0x68, nv_rd32(dev, 0x3268));
-	nv_wo32(ramfc, 0x6c, nv_rd32(dev, 0x326c));
-	nv_wo32(ramfc, 0x70, nv_rd32(dev, 0x32e4));
-	nv_wo32(ramfc, 0x74, nv_rd32(dev, 0x3248));
-	nv_wo32(ramfc, 0x78, nv_rd32(dev, 0x2088));
-	nv_wo32(ramfc, 0x7c, nv_rd32(dev, 0x2058));
-	nv_wo32(ramfc, 0x80, nv_rd32(dev, 0x2210));
-
-	put = (nv_rd32(dev, NV03_PFIFO_CACHE1_PUT) & 0x7ff) >> 2;
-	get = (nv_rd32(dev, NV03_PFIFO_CACHE1_GET) & 0x7ff) >> 2;
-	ptr = 0;
-	while (put != get) {
-		nv_wo32(cache, ptr + 0,
-			nv_rd32(dev, NV40_PFIFO_CACHE1_METHOD(get)));
-		nv_wo32(cache, ptr + 4,
-			nv_rd32(dev, NV40_PFIFO_CACHE1_DATA(get)));
-		get = (get + 1) & 0x1ff;
-		ptr += 8;
-	}
-
-	/* guessing that all the 0x34xx regs aren't on NV50 */
-	if (dev_priv->chipset != 0x50) {
-		nv_wo32(ramfc, 0x84, ptr >> 3);
-		nv_wo32(ramfc, 0x88, nv_rd32(dev, 0x340c));
-		nv_wo32(ramfc, 0x8c, nv_rd32(dev, 0x3400));
-		nv_wo32(ramfc, 0x90, nv_rd32(dev, 0x3404));
-		nv_wo32(ramfc, 0x94, nv_rd32(dev, 0x3408));
-		nv_wo32(ramfc, 0x98, nv_rd32(dev, 0x3410));
-	}
+	struct nv50_fifo_priv *priv;
+	int ret;
 
-	dev_priv->engine.instmem.flush(dev);
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
-	/*XXX: probably reload ch127 (NULL) state back too */
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, 127);
-	return 0;
-}
+	priv->base.base.destroy = nv50_fifo_destroy;
+	priv->base.base.init = nv50_fifo_init;
+	priv->base.base.fini = nv50_fifo_fini;
+	priv->base.base.context_new = nv50_fifo_context_new;
+	priv->base.base.context_del = nv50_fifo_context_del;
+	priv->base.base.tlb_flush = nv50_fifo_tlb_flush;
+	priv->base.channels = 127;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[0]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[1]);
+	if (ret)
+		goto error;
 
-void
-nv50_fifo_tlb_flush(struct drm_device *dev)
-{
-	nv50_vm_flush_engine(dev, 5);
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_graph.c b/drivers/gpu/drm/nouveau/nv50_graph.c
index 33d5711a918d..d9cc2f2638d6 100644
--- a/drivers/gpu/drm/nouveau/nv50_graph.c
+++ b/drivers/gpu/drm/nouveau/nv50_graph.c
@@ -27,8 +27,8 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
-#include "nouveau_grctx.h"
 #include "nouveau_dma.h"
 #include "nouveau_vm.h"
 #include "nv50_evo.h"
@@ -40,86 +40,6 @@ struct nv50_graph_engine {
 	u32 grctx_size;
 };
 
-static void
-nv50_graph_fifo_access(struct drm_device *dev, bool enabled)
-{
-	const uint32_t mask = 0x00010001;
-
-	if (enabled)
-		nv_wr32(dev, 0x400500, nv_rd32(dev, 0x400500) | mask);
-	else
-		nv_wr32(dev, 0x400500, nv_rd32(dev, 0x400500) & ~mask);
-}
-
-static struct nouveau_channel *
-nv50_graph_channel(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t inst;
-	int i;
-
-	/* Be sure we're not in the middle of a context switch or bad things
-	 * will happen, such as unloading the wrong pgraph context.
-	 */
-	if (!nv_wait(dev, 0x400300, 0x00000001, 0x00000000))
-		NV_ERROR(dev, "Ctxprog is still running\n");
-
-	inst = nv_rd32(dev, NV50_PGRAPH_CTXCTL_CUR);
-	if (!(inst & NV50_PGRAPH_CTXCTL_CUR_LOADED))
-		return NULL;
-	inst = (inst & NV50_PGRAPH_CTXCTL_CUR_INSTANCE) << 12;
-
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
-
-		if (chan && chan->ramin && chan->ramin->vinst == inst)
-			return chan;
-	}
-
-	return NULL;
-}
-
-static int
-nv50_graph_do_load_context(struct drm_device *dev, uint32_t inst)
-{
-	uint32_t fifo = nv_rd32(dev, 0x400500);
-
-	nv_wr32(dev, 0x400500, fifo & ~1);
-	nv_wr32(dev, 0x400784, inst);
-	nv_wr32(dev, 0x400824, nv_rd32(dev, 0x400824) | 0x40);
-	nv_wr32(dev, 0x400320, nv_rd32(dev, 0x400320) | 0x11);
-	nv_wr32(dev, 0x400040, 0xffffffff);
-	(void)nv_rd32(dev, 0x400040);
-	nv_wr32(dev, 0x400040, 0x00000000);
-	nv_wr32(dev, 0x400304, nv_rd32(dev, 0x400304) | 1);
-
-	if (nouveau_wait_for_idle(dev))
-		nv_wr32(dev, 0x40032c, inst | (1<<31));
-	nv_wr32(dev, 0x400500, fifo);
-
-	return 0;
-}
-
-static int
-nv50_graph_unload_context(struct drm_device *dev)
-{
-	uint32_t inst;
-
-	inst  = nv_rd32(dev, NV50_PGRAPH_CTXCTL_CUR);
-	if (!(inst & NV50_PGRAPH_CTXCTL_CUR_LOADED))
-		return 0;
-	inst &= NV50_PGRAPH_CTXCTL_CUR_INSTANCE;
-
-	nouveau_wait_for_idle(dev);
-	nv_wr32(dev, 0x400784, inst);
-	nv_wr32(dev, 0x400824, nv_rd32(dev, 0x400824) | 0x20);
-	nv_wr32(dev, 0x400304, nv_rd32(dev, 0x400304) | 0x01);
-	nouveau_wait_for_idle(dev);
-
-	nv_wr32(dev, NV50_PGRAPH_CTXCTL_CUR, inst);
-	return 0;
-}
-
 static int
 nv50_graph_init(struct drm_device *dev, int engine)
 {
@@ -211,12 +131,6 @@ nv50_graph_init(struct drm_device *dev, int engine)
 static int
 nv50_graph_fini(struct drm_device *dev, int engine, bool suspend)
 {
-	nv_mask(dev, 0x400500, 0x00010001, 0x00000000);
-	if (!nv_wait(dev, 0x400700, ~0, 0) && suspend) {
-		nv_mask(dev, 0x400500, 0x00010001, 0x00010001);
-		return -EBUSY;
-	}
-	nv50_graph_unload_context(dev);
 	nv_wr32(dev, 0x40013c, 0x00000000);
 	return 0;
 }
@@ -229,7 +143,6 @@ nv50_graph_context_new(struct nouveau_channel *chan, int engine)
 	struct nouveau_gpuobj *ramin = chan->ramin;
 	struct nouveau_gpuobj *grctx = NULL;
 	struct nv50_graph_engine *pgraph = nv_engine(dev, engine);
-	struct nouveau_grctx ctx = {};
 	int hdr, ret;
 
 	NV_DEBUG(dev, "ch%d\n", chan->id);
@@ -248,11 +161,7 @@ nv50_graph_context_new(struct nouveau_channel *chan, int engine)
 	nv_wo32(ramin, hdr + 0x10, 0);
 	nv_wo32(ramin, hdr + 0x14, 0x00010000);
 
-	ctx.dev = chan->dev;
-	ctx.mode = NOUVEAU_GRCTX_VALS;
-	ctx.data = grctx;
-	nv50_grctx_init(&ctx);
-
+	nv50_grctx_fill(dev, grctx);
 	nv_wo32(grctx, 0x00000, chan->ramin->vinst >> 12);
 
 	dev_priv->engine.instmem.flush(dev);
@@ -268,33 +177,14 @@ nv50_graph_context_del(struct nouveau_channel *chan, int engine)
 	struct nouveau_gpuobj *grctx = chan->engctx[engine];
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	int i, hdr = (dev_priv->chipset == 0x50) ? 0x200 : 0x20;
-	unsigned long flags;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	if (!chan->ramin)
-		return;
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	pfifo->reassign(dev, false);
-	nv50_graph_fifo_access(dev, false);
-
-	if (nv50_graph_channel(dev) == chan)
-		nv50_graph_unload_context(dev);
 
 	for (i = hdr; i < hdr + 24; i += 4)
 		nv_wo32(chan->ramin, i, 0);
 	dev_priv->engine.instmem.flush(dev);
 
-	nv50_graph_fifo_access(dev, true);
-	pfifo->reassign(dev, true);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-
-	nouveau_gpuobj_ref(NULL, &grctx);
-
 	atomic_dec(&chan->vm->engref[engine]);
+	nouveau_gpuobj_ref(NULL, &grctx);
 	chan->engctx[engine] = NULL;
 }
 
@@ -325,85 +215,6 @@ nv50_graph_object_new(struct nouveau_channel *chan, int engine,
 }
 
 static void
-nv50_graph_context_switch(struct drm_device *dev)
-{
-	uint32_t inst;
-
-	nv50_graph_unload_context(dev);
-
-	inst  = nv_rd32(dev, NV50_PGRAPH_CTXCTL_NEXT);
-	inst &= NV50_PGRAPH_CTXCTL_NEXT_INSTANCE;
-	nv50_graph_do_load_context(dev, inst);
-
-	nv_wr32(dev, NV40_PGRAPH_INTR_EN, nv_rd32(dev,
-		NV40_PGRAPH_INTR_EN) | NV_PGRAPH_INTR_CONTEXT_SWITCH);
-}
-
-static int
-nv50_graph_nvsw_dma_vblsem(struct nouveau_channel *chan,
-			   u32 class, u32 mthd, u32 data)
-{
-	struct nouveau_gpuobj *gpuobj;
-
-	gpuobj = nouveau_ramht_find(chan, data);
-	if (!gpuobj)
-		return -ENOENT;
-
-	if (nouveau_notifier_offset(gpuobj, NULL))
-		return -EINVAL;
-
-	chan->nvsw.vblsem = gpuobj;
-	chan->nvsw.vblsem_offset = ~0;
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_vblsem_offset(struct nouveau_channel *chan,
-			      u32 class, u32 mthd, u32 data)
-{
-	if (nouveau_notifier_offset(chan->nvsw.vblsem, &data))
-		return -ERANGE;
-
-	chan->nvsw.vblsem_offset = data >> 2;
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_vblsem_release_val(struct nouveau_channel *chan,
-				   u32 class, u32 mthd, u32 data)
-{
-	chan->nvsw.vblsem_rval = data;
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_vblsem_release(struct nouveau_channel *chan,
-			       u32 class, u32 mthd, u32 data)
-{
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-
-	if (!chan->nvsw.vblsem || chan->nvsw.vblsem_offset == ~0 || data > 1)
-		return -EINVAL;
-
-	drm_vblank_get(dev, data);
-
-	chan->nvsw.vblsem_head = data;
-	list_add(&chan->nvsw.vbl_wait, &dev_priv->vbl_waiting);
-
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_mthd_page_flip(struct nouveau_channel *chan,
-			       u32 class, u32 mthd, u32 data)
-{
-	nouveau_finish_page_flip(chan, NULL);
-	return 0;
-}
-
-
-static void
 nv50_graph_tlb_flush(struct drm_device *dev, int engine)
 {
 	nv50_vm_flush_engine(dev, 0);
@@ -514,6 +325,7 @@ struct nouveau_enum nv50_data_error_names[] = {
 	{ 0x0000001f, "RT_BPP128_WITH_MS8", NULL },
 	{ 0x00000021, "Z_OUT_OF_BOUNDS", NULL },
 	{ 0x00000023, "XY_OUT_OF_BOUNDS", NULL },
+	{ 0x00000024, "VP_ZERO_INPUTS", NULL },
 	{ 0x00000027, "CP_MORE_PARAMS_THAN_SHARED", NULL },
 	{ 0x00000028, "CP_NO_REG_SPACE_STRIPED", NULL },
 	{ 0x00000029, "CP_NO_REG_SPACE_PACKED", NULL },
@@ -900,13 +712,14 @@ nv50_pgraph_trap_handler(struct drm_device *dev, u32 display, u64 inst, u32 chid
 int
 nv50_graph_isr_chid(struct drm_device *dev, u64 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan;
 	unsigned long flags;
 	int i;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		chan = dev_priv->channels.ptr[i];
 		if (!chan || !chan->ramin)
 			continue;
@@ -939,15 +752,6 @@ nv50_graph_isr(struct drm_device *dev)
 				show &= ~0x00000010;
 		}
 
-		if (stat & 0x00001000) {
-			nv_wr32(dev, 0x400500, 0x00000000);
-			nv_wr32(dev, 0x400100, 0x00001000);
-			nv_mask(dev, 0x40013c, 0x00001000, 0x00000000);
-			nv50_graph_context_switch(dev);
-			stat &= ~0x00001000;
-			show &= ~0x00001000;
-		}
-
 		show = (show && nouveau_ratelimit()) ? show : 0;
 
 		if (show & 0x00100000) {
@@ -996,28 +800,21 @@ nv50_graph_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nv50_graph_engine *pgraph;
-	struct nouveau_grctx ctx = {};
 	int ret;
 
 	pgraph = kzalloc(sizeof(*pgraph),GFP_KERNEL);
 	if (!pgraph)
 		return -ENOMEM;
 
-	ctx.dev = dev;
-	ctx.mode = NOUVEAU_GRCTX_PROG;
-	ctx.data = pgraph->ctxprog;
-	ctx.ctxprog_max = ARRAY_SIZE(pgraph->ctxprog);
-
-	ret = nv50_grctx_init(&ctx);
+	ret = nv50_grctx_init(dev, pgraph->ctxprog, ARRAY_SIZE(pgraph->ctxprog),
+				  &pgraph->ctxprog_size,
+				  &pgraph->grctx_size);
 	if (ret) {
 		NV_ERROR(dev, "PGRAPH: ctxprog build failed\n");
 		kfree(pgraph);
 		return 0;
 	}
 
-	pgraph->grctx_size = ctx.ctxvals_pos * 4;
-	pgraph->ctxprog_size = ctx.ctxprog_len;
-
 	pgraph->base.destroy = nv50_graph_destroy;
 	pgraph->base.init = nv50_graph_init;
 	pgraph->base.fini = nv50_graph_fini;
@@ -1031,14 +828,6 @@ nv50_graph_create(struct drm_device *dev)
 
 	nouveau_irq_register(dev, 12, nv50_graph_isr);
 
-	/* NVSW really doesn't live here... */
-	NVOBJ_CLASS(dev, 0x506e, SW); /* nvsw */
-	NVOBJ_MTHD (dev, 0x506e, 0x018c, nv50_graph_nvsw_dma_vblsem);
-	NVOBJ_MTHD (dev, 0x506e, 0x0400, nv50_graph_nvsw_vblsem_offset);
-	NVOBJ_MTHD (dev, 0x506e, 0x0404, nv50_graph_nvsw_vblsem_release_val);
-	NVOBJ_MTHD (dev, 0x506e, 0x0408, nv50_graph_nvsw_vblsem_release);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv50_graph_nvsw_mthd_page_flip);
-
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x5039, GR); /* m2mf */
diff --git a/drivers/gpu/drm/nouveau/nv50_grctx.c b/drivers/gpu/drm/nouveau/nv50_grctx.c
index 4b46d6968566..881e22b249fc 100644
--- a/drivers/gpu/drm/nouveau/nv50_grctx.c
+++ b/drivers/gpu/drm/nouveau/nv50_grctx.c
@@ -172,8 +172,8 @@ static void nv50_graph_construct_xfer2(struct nouveau_grctx *ctx);
 
 /* Main function: construct the ctxprog skeleton, call the other functions. */
 
-int
-nv50_grctx_init(struct nouveau_grctx *ctx)
+static int
+nv50_grctx_generate(struct nouveau_grctx *ctx)
 {
 	struct drm_nouveau_private *dev_priv = ctx->dev->dev_private;
 
@@ -210,7 +210,7 @@ nv50_grctx_init(struct nouveau_grctx *ctx)
 	cp_name(ctx, cp_check_load);
 	cp_bra (ctx, AUTO_LOAD, PENDING, cp_setup_auto_load);
 	cp_bra (ctx, USER_LOAD, PENDING, cp_setup_load);
-	cp_bra (ctx, ALWAYS, TRUE, cp_exit);
+	cp_bra (ctx, ALWAYS, TRUE, cp_prepare_exit);
 
 	/* setup for context load */
 	cp_name(ctx, cp_setup_auto_load);
@@ -277,6 +277,33 @@ nv50_grctx_init(struct nouveau_grctx *ctx)
 	return 0;
 }
 
+void
+nv50_grctx_fill(struct drm_device *dev, struct nouveau_gpuobj *mem)
+{
+	nv50_grctx_generate(&(struct nouveau_grctx) {
+			     .dev = dev,
+			     .mode = NOUVEAU_GRCTX_VALS,
+			     .data = mem,
+			   });
+}
+
+int
+nv50_grctx_init(struct drm_device *dev, u32 *data, u32 max, u32 *len, u32 *cnt)
+{
+	struct nouveau_grctx ctx = {
+		.dev = dev,
+		.mode = NOUVEAU_GRCTX_PROG,
+		.data = data,
+		.ctxprog_max = max
+	};
+	int ret;
+
+	ret = nv50_grctx_generate(&ctx);
+	*cnt = ctx.ctxvals_pos * 4;
+	*len = ctx.ctxprog_len;
+	return ret;
+}
+
 /*
  * Constructs MMIO part of ctxprog and ctxvals. Just a matter of knowing which
  * registers to save/restore and the default values for them.
diff --git a/drivers/gpu/drm/nouveau/nv50_instmem.c b/drivers/gpu/drm/nouveau/nv50_instmem.c
index a7c12c94a5a6..0bba54f11800 100644
--- a/drivers/gpu/drm/nouveau/nv50_instmem.c
+++ b/drivers/gpu/drm/nouveau/nv50_instmem.c
@@ -83,7 +83,7 @@ nv50_channel_new(struct drm_device *dev, u32 size, struct nouveau_vm *vm,
 		return ret;
 	}
 
-	ret = drm_mm_init(&chan->ramin_heap, 0x6000, chan->ramin->size);
+	ret = drm_mm_init(&chan->ramin_heap, 0x6000, chan->ramin->size - 0x6000);
 	if (ret) {
 		nv50_channel_del(&chan);
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/nv50_mpeg.c b/drivers/gpu/drm/nouveau/nv50_mpeg.c
index b57a2d180ad2..90e8ed22cfcb 100644
--- a/drivers/gpu/drm/nouveau/nv50_mpeg.c
+++ b/drivers/gpu/drm/nouveau/nv50_mpeg.c
@@ -77,27 +77,13 @@ nv50_mpeg_context_new(struct nouveau_channel *chan, int engine)
 static void
 nv50_mpeg_context_del(struct nouveau_channel *chan, int engine)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct nouveau_gpuobj *ctx = chan->engctx[engine];
 	struct drm_device *dev = chan->dev;
-	unsigned long flags;
-	u32 inst, i;
-
-	if (!chan->ramin)
-		return;
-
-	inst  = chan->ramin->vinst >> 12;
-	inst |= 0x80000000;
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	nv_mask(dev, 0x00b32c, 0x00000001, 0x00000000);
-	if (nv_rd32(dev, 0x00b318) == inst)
-		nv_mask(dev, 0x00b318, 0x80000000, 0x00000000);
-	nv_mask(dev, 0x00b32c, 0x00000001, 0x00000001);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+	int i;
 
 	for (i = 0x00; i <= 0x14; i += 4)
 		nv_wo32(chan->ramin, CTX_PTR(dev, i), 0x00000000);
+
 	nouveau_gpuobj_ref(NULL, &ctx);
 	chan->engctx[engine] = NULL;
 }
@@ -162,7 +148,6 @@ nv50_mpeg_init(struct drm_device *dev, int engine)
 static int
 nv50_mpeg_fini(struct drm_device *dev, int engine, bool suspend)
 {
-	/*XXX: context save for s/r */
 	nv_mask(dev, 0x00b32c, 0x00000001, 0x00000000);
 	nv_wr32(dev, 0x00b140, 0x00000000);
 	return 0;
diff --git a/drivers/gpu/drm/nouveau/nv50_software.c b/drivers/gpu/drm/nouveau/nv50_software.c
new file mode 100644
index 000000000000..114d2517d4a8
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv50_software.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_ramht.h"
+#include "nouveau_software.h"
+
+#include "nv50_display.h"
+
+struct nv50_software_priv {
+	struct nouveau_software_priv base;
+};
+
+struct nv50_software_chan {
+	struct nouveau_software_chan base;
+	struct {
+		struct nouveau_gpuobj *object;
+	} vblank;
+};
+
+static int
+mthd_dma_vblsem(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	struct nouveau_gpuobj *gpuobj;
+
+	gpuobj = nouveau_ramht_find(chan, data);
+	if (!gpuobj)
+		return -ENOENT;
+
+	if (nouveau_notifier_offset(gpuobj, NULL))
+		return -EINVAL;
+
+	pch->vblank.object = gpuobj;
+	pch->base.vblank.offset = ~0;
+	return 0;
+}
+
+static int
+mthd_vblsem_offset(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+
+	if (nouveau_notifier_offset(pch->vblank.object, &data))
+		return -ERANGE;
+
+	pch->base.vblank.offset = data >> 2;
+	return 0;
+}
+
+static int
+mthd_vblsem_value(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	pch->base.vblank.value = data;
+	return 0;
+}
+
+static int
+mthd_vblsem_release(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_priv *psw = nv_engine(chan->dev, NVOBJ_ENGINE_SW);
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	struct drm_device *dev = chan->dev;
+
+	if (!pch->vblank.object || pch->base.vblank.offset == ~0 || data > 1)
+		return -EINVAL;
+
+	drm_vblank_get(dev, data);
+
+	pch->base.vblank.head = data;
+	list_add(&pch->base.vblank.list, &psw->base.vblank);
+	return 0;
+}
+
+static int
+mthd_flip(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	nouveau_finish_page_flip(chan, NULL);
+	return 0;
+}
+
+static int
+nv50_software_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv50_software_priv *psw = nv_engine(chan->dev, NVOBJ_ENGINE_SW);
+	struct nv50_display *pdisp = nv50_display(chan->dev);
+	struct nv50_software_chan *pch;
+	int ret = 0, i;
+
+	pch = kzalloc(sizeof(*pch), GFP_KERNEL);
+	if (!pch)
+		return -ENOMEM;
+
+	nouveau_software_context_new(&pch->base);
+	pch->base.vblank.bo = chan->notifier_bo;
+	chan->engctx[engine] = pch;
+
+	/* dma objects for display sync channel semaphore blocks */
+	for (i = 0; i < chan->dev->mode_config.num_crtc; i++) {
+		struct nv50_display_crtc *dispc = &pdisp->crtc[i];
+		struct nouveau_gpuobj *obj = NULL;
+
+		ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_IN_MEMORY,
+					     dispc->sem.bo->bo.offset, 0x1000,
+					     NV_MEM_ACCESS_RW,
+					     NV_MEM_TARGET_VRAM, &obj);
+		if (ret)
+			break;
+
+		ret = nouveau_ramht_insert(chan, NvEvoSema0 + i, obj);
+		nouveau_gpuobj_ref(NULL, &obj);
+	}
+
+	if (ret)
+		psw->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nv50_software_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv50_software_chan *pch = chan->engctx[engine];
+	chan->engctx[engine] = NULL;
+	kfree(pch);
+}
+
+static int
+nv50_software_object_new(struct nouveau_channel *chan, int engine,
+			 u32 handle, u16 class)
+{
+	struct drm_device *dev = chan->dev;
+	struct nouveau_gpuobj *obj = NULL;
+	int ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 16, 16, 0, &obj);
+	if (ret)
+		return ret;
+	obj->engine = 0;
+	obj->class  = class;
+
+	ret = nouveau_ramht_insert(chan, handle, obj);
+	nouveau_gpuobj_ref(NULL, &obj);
+	return ret;
+}
+
+static int
+nv50_software_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static int
+nv50_software_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nv50_software_destroy(struct drm_device *dev, int engine)
+{
+	struct nv50_software_priv *psw = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, SW);
+	kfree(psw);
+}
+
+int
+nv50_software_create(struct drm_device *dev)
+{
+	struct nv50_software_priv *psw = kzalloc(sizeof(*psw), GFP_KERNEL);
+	if (!psw)
+		return -ENOMEM;
+
+	psw->base.base.destroy = nv50_software_destroy;
+	psw->base.base.init = nv50_software_init;
+	psw->base.base.fini = nv50_software_fini;
+	psw->base.base.context_new = nv50_software_context_new;
+	psw->base.base.context_del = nv50_software_context_del;
+	psw->base.base.object_new = nv50_software_object_new;
+	nouveau_software_create(&psw->base);
+
+	NVOBJ_ENGINE_ADD(dev, SW, &psw->base.base);
+	NVOBJ_CLASS(dev, 0x506e, SW);
+	NVOBJ_MTHD (dev, 0x506e, 0x018c, mthd_dma_vblsem);
+	NVOBJ_MTHD (dev, 0x506e, 0x0400, mthd_vblsem_offset);
+	NVOBJ_MTHD (dev, 0x506e, 0x0404, mthd_vblsem_value);
+	NVOBJ_MTHD (dev, 0x506e, 0x0408, mthd_vblsem_release);
+	NVOBJ_MTHD (dev, 0x506e, 0x0500, mthd_flip);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv50_sor.c b/drivers/gpu/drm/nouveau/nv50_sor.c
index 274640212475..a9514eaa74c1 100644
--- a/drivers/gpu/drm/nouveau/nv50_sor.c
+++ b/drivers/gpu/drm/nouveau/nv50_sor.c
@@ -242,9 +242,9 @@ nv50_sor_disconnect(struct drm_encoder *encoder)
 		NV_ERROR(dev, "no space while disconnecting SOR\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
 	OUT_RING  (evo, 0);
-	BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 	OUT_RING  (evo, 0);
 
 	nouveau_hdmi_mode_set(encoder, NULL);
@@ -430,7 +430,7 @@ nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 		nv_encoder->crtc = NULL;
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
 	OUT_RING(evo, mode_ctl);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nv50_vm.c b/drivers/gpu/drm/nouveau/nv50_vm.c
index 44fbac9c7d93..179bb42a635c 100644
--- a/drivers/gpu/drm/nouveau/nv50_vm.c
+++ b/drivers/gpu/drm/nouveau/nv50_vm.c
@@ -147,7 +147,6 @@ nv50_vm_flush(struct nouveau_vm *vm)
 {
 	struct drm_nouveau_private *dev_priv = vm->dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	int i;
 
 	pinstmem->flush(vm->dev);
@@ -158,7 +157,6 @@ nv50_vm_flush(struct nouveau_vm *vm)
 		return;
 	}
 
-	pfifo->tlb_flush(vm->dev);
 	for (i = 0; i < NVOBJ_ENGINE_NR; i++) {
 		if (atomic_read(&vm->engref[i]))
 			dev_priv->eng[i]->tlb_flush(vm->dev, i);
diff --git a/drivers/gpu/drm/nouveau/nv84_fence.c b/drivers/gpu/drm/nouveau/nv84_fence.c
new file mode 100644
index 000000000000..c2f889b0d340
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv84_fence.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_fifo.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nv84_fence_chan {
+	struct nouveau_fence_chan base;
+};
+
+struct nv84_fence_priv {
+	struct nouveau_fence_priv base;
+	struct nouveau_gpuobj *mem;
+};
+
+static int
+nv84_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
+		OUT_RING  (chan, NvSema);
+		BEGIN_NV04(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(chan->id * 16));
+		OUT_RING  (chan, lower_32_bits(chan->id * 16));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_WRITE_LONG);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+
+static int
+nv84_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
+		OUT_RING  (chan, NvSema);
+		BEGIN_NV04(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(prev->id * 16));
+		OUT_RING  (chan, lower_32_bits(prev->id * 16));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_GEQUAL);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+static u32
+nv84_fence_read(struct nouveau_channel *chan)
+{
+	struct nv84_fence_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_FENCE);
+	return nv_ro32(priv->mem, chan->id * 16);
+}
+
+static void
+nv84_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fence_chan *fctx = chan->engctx[engine];
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv84_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nv84_fence_chan *fctx;
+	struct nouveau_gpuobj *obj;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	nouveau_fence_context_new(&fctx->base);
+
+	ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_FROM_MEMORY,
+				     priv->mem->vinst, priv->mem->size,
+				     NV_MEM_ACCESS_RW,
+				     NV_MEM_TARGET_VRAM, &obj);
+	if (ret == 0) {
+		ret = nouveau_ramht_insert(chan, NvSema, obj);
+		nouveau_gpuobj_ref(NULL, &obj);
+		nv_wo32(priv->mem, chan->id * 16, 0x00000000);
+	}
+
+	if (ret)
+		nv84_fence_context_del(chan, engine);
+	return ret;
+}
+
+static int
+nv84_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nv84_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nv84_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fence_priv *priv = nv_engine(dev, engine);
+
+	nouveau_gpuobj_ref(NULL, &priv->mem);
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv84_fence_create(struct drm_device *dev)
+{
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fence_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nv84_fence_destroy;
+	priv->base.engine.init = nv84_fence_init;
+	priv->base.engine.fini = nv84_fence_fini;
+	priv->base.engine.context_new = nv84_fence_context_new;
+	priv->base.engine.context_del = nv84_fence_context_del;
+	priv->base.emit = nv84_fence_emit;
+	priv->base.sync = nv84_fence_sync;
+	priv->base.read = nv84_fence_read;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 16 * pfifo->channels,
+				 0x1000, 0, &priv->mem);
+	if (ret)
+		goto out;
+
+out:
+	if (ret)
+		nv84_fence_destroy(dev, NVOBJ_ENGINE_FENCE);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv84_fifo.c b/drivers/gpu/drm/nouveau/nv84_fifo.c
new file mode 100644
index 000000000000..cc82d799fc3b
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv84_fifo.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright (C) 2012 Ben Skeggs.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial
+ * portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE COPYRIGHT OWNER(S) AND/OR ITS SUPPLIERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "nouveau_drv.h"
+#include "nouveau_fifo.h"
+#include "nouveau_ramht.h"
+#include "nouveau_vm.h"
+
+struct nv84_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct nouveau_gpuobj *playlist[2];
+	int cur_playlist;
+};
+
+struct nv84_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+	struct nouveau_gpuobj *cache;
+};
+
+static int
+nv84_fifo_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fifo_priv *priv = nv_engine(chan->dev, engine);
+	struct nv84_fifo_chan *fctx;
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+        u64 ib_offset = chan->pushbuf_base + chan->dma.ib_base * 4;
+	u64 instance;
+	unsigned long flags;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+	atomic_inc(&chan->vm->engref[engine]);
+
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV50_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	ret = nouveau_gpuobj_new(dev, chan, 256, 256, NVOBJ_FLAG_ZERO_ALLOC |
+				 NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	instance = fctx->ramfc->vinst >> 8;
+
+	ret = nouveau_gpuobj_new(dev, chan, 4096, 1024, 0, &fctx->cache);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x3c, 0x403f6078);
+	nv_wo32(fctx->ramfc, 0x40, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x44, 0x01003fff);
+	nv_wo32(fctx->ramfc, 0x48, chan->pushbuf->cinst >> 4);
+	nv_wo32(fctx->ramfc, 0x50, lower_32_bits(ib_offset));
+	nv_wo32(fctx->ramfc, 0x54, upper_32_bits(ib_offset) |
+				   drm_order(chan->dma.ib_max + 1) << 16);
+	nv_wo32(fctx->ramfc, 0x60, 0x7fffffff);
+	nv_wo32(fctx->ramfc, 0x78, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x7c, 0x30000001);
+	nv_wo32(fctx->ramfc, 0x80, ((chan->ramht->bits - 9) << 27) |
+				   (4 << 24) /* SEARCH_FULL */ |
+				   (chan->ramht->gpuobj->cinst >> 4));
+	nv_wo32(fctx->ramfc, 0x88, fctx->cache->vinst >> 10);
+	nv_wo32(fctx->ramfc, 0x98, chan->ramin->vinst >> 12);
+
+	nv_wo32(chan->ramin, 0x00, chan->id);
+	nv_wo32(chan->ramin, 0x04, fctx->ramfc->vinst >> 8);
+
+	dev_priv->engine.instmem.flush(dev);
+
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x80000000 | instance);
+	nv50_fifo_playlist_update(dev);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nv84_fifo_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fifo_chan *fctx = chan->engctx[engine];
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	/* remove channel from playlist, will context switch if active */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, 0x002600 + (chan->id * 4), 0x80000000, 0x00000000);
+	nv50_fifo_playlist_update(dev);
+
+	/* tell any engines on this channel to unload their contexts */
+	nv_wr32(dev, 0x0032fc, chan->ramin->vinst >> 12);
+	if (!nv_wait_ne(dev, 0x0032fc, 0xffffffff, 0xffffffff))
+		NV_INFO(dev, "PFIFO: channel %d unload timeout\n", chan->id);
+
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x00000000);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+
+	/* clean up */
+	if (chan->user) {
+		iounmap(chan->user);
+		chan->user = NULL;
+	}
+
+	nouveau_gpuobj_ref(NULL, &fctx->ramfc);
+	nouveau_gpuobj_ref(NULL, &fctx->cache);
+
+	atomic_dec(&chan->vm->engref[engine]);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv84_fifo_init(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fifo_chan *fctx;
+	u32 instance;
+	int i;
+
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000100);
+	nv_wr32(dev, 0x00250c, 0x6f3cfc34);
+	nv_wr32(dev, 0x002044, 0x01003fff);
+
+	nv_wr32(dev, 0x002100, 0xffffffff);
+	nv_wr32(dev, 0x002140, 0xffffffff);
+
+	for (i = 0; i < 128; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan && (fctx = chan->engctx[engine]))
+			instance = 0x80000000 | fctx->ramfc->vinst >> 8;
+		else
+			instance = 0x00000000;
+		nv_wr32(dev, 0x002600 + (i * 4), instance);
+	}
+
+	nv50_fifo_playlist_update(dev);
+
+	nv_wr32(dev, 0x003200, 1);
+	nv_wr32(dev, 0x003250, 1);
+	nv_wr32(dev, 0x002500, 1);
+	return 0;
+}
+
+static int
+nv84_fifo_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	/* set playlist length to zero, fifo will unload context */
+	nv_wr32(dev, 0x0032ec, 0);
+
+	/* tell all connected engines to unload their contexts */
+	for (i = 0; i < priv->base.channels; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan)
+			nv_wr32(dev, 0x0032fc, chan->ramin->vinst >> 12);
+		if (!nv_wait_ne(dev, 0x0032fc, 0xffffffff, 0xffffffff)) {
+			NV_INFO(dev, "PFIFO: channel %d unload timeout\n", i);
+			return -EBUSY;
+		}
+	}
+
+	nv_wr32(dev, 0x002140, 0);
+	return 0;
+}
+
+int
+nv84_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fifo_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nv50_fifo_destroy;
+	priv->base.base.init = nv84_fifo_init;
+	priv->base.base.fini = nv84_fifo_fini;
+	priv->base.base.context_new = nv84_fifo_context_new;
+	priv->base.base.context_del = nv84_fifo_context_del;
+	priv->base.base.tlb_flush = nv50_fifo_tlb_flush;
+	priv->base.channels = 127;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[0]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[1]);
+	if (ret)
+		goto error;
+
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv98_crypt.c b/drivers/gpu/drm/nouveau/nv98_crypt.c
index db94ff0a9fab..e25e13fb894e 100644
--- a/drivers/gpu/drm/nouveau/nv98_crypt.c
+++ b/drivers/gpu/drm/nouveau/nv98_crypt.c
@@ -23,21 +23,93 @@
  */
 
 #include "drmP.h"
+
 #include "nouveau_drv.h"
 #include "nouveau_util.h"
 #include "nouveau_vm.h"
 #include "nouveau_ramht.h"
 
-struct nv98_crypt_engine {
+#include "nv98_crypt.fuc.h"
+
+struct nv98_crypt_priv {
 	struct nouveau_exec_engine base;
 };
 
+struct nv98_crypt_chan {
+	struct nouveau_gpuobj *mem;
+};
+
 static int
-nv98_crypt_fini(struct drm_device *dev, int engine, bool suspend)
+nv98_crypt_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv98_crypt_priv *priv = nv_engine(dev, engine);
+	struct nv98_crypt_chan *cctx;
+	int ret;
+
+	cctx = chan->engctx[engine] = kzalloc(sizeof(*cctx), GFP_KERNEL);
+	if (!cctx)
+		return -ENOMEM;
+
+	atomic_inc(&chan->vm->engref[engine]);
+
+	ret = nouveau_gpuobj_new(dev, chan, 256, 0, NVOBJ_FLAG_ZERO_ALLOC |
+				 NVOBJ_FLAG_ZERO_FREE, &cctx->mem);
+	if (ret)
+		goto error;
+
+	nv_wo32(chan->ramin, 0xa0, 0x00190000);
+	nv_wo32(chan->ramin, 0xa4, cctx->mem->vinst + cctx->mem->size - 1);
+	nv_wo32(chan->ramin, 0xa8, cctx->mem->vinst);
+	nv_wo32(chan->ramin, 0xac, 0x00000000);
+	nv_wo32(chan->ramin, 0xb0, 0x00000000);
+	nv_wo32(chan->ramin, 0xb4, 0x00000000);
+	dev_priv->engine.instmem.flush(dev);
+
+error:
+	if (ret)
+		priv->base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nv98_crypt_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv98_crypt_chan *cctx = chan->engctx[engine];
+	int i;
+
+	for (i = 0xa0; i < 0xb4; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+
+	nouveau_gpuobj_ref(NULL, &cctx->mem);
+
+	atomic_dec(&chan->vm->engref[engine]);
+	chan->engctx[engine] = NULL;
+	kfree(cctx);
+}
+
+static int
+nv98_crypt_object_new(struct nouveau_channel *chan, int engine,
+		     u32 handle, u16 class)
 {
-	if (!(nv_rd32(dev, 0x000200) & 0x00004000))
-		return 0;
+	struct nv98_crypt_chan *cctx = chan->engctx[engine];
+
+	/* fuc engine doesn't need an object, our ramht code does.. */
+	cctx->mem->engine = 5;
+	cctx->mem->class  = class;
+	return nouveau_ramht_insert(chan, handle, cctx->mem);
+}
 
+static void
+nv98_crypt_tlb_flush(struct drm_device *dev, int engine)
+{
+	nv50_vm_flush_engine(dev, 0x0a);
+}
+
+static int
+nv98_crypt_fini(struct drm_device *dev, int engine, bool suspend)
+{
 	nv_mask(dev, 0x000200, 0x00004000, 0x00000000);
 	return 0;
 }
@@ -45,34 +117,100 @@ nv98_crypt_fini(struct drm_device *dev, int engine, bool suspend)
 static int
 nv98_crypt_init(struct drm_device *dev, int engine)
 {
+	int i;
+
+	/* reset! */
 	nv_mask(dev, 0x000200, 0x00004000, 0x00000000);
 	nv_mask(dev, 0x000200, 0x00004000, 0x00004000);
+
+	/* wait for exit interrupt to signal */
+	nv_wait(dev, 0x087008, 0x00000010, 0x00000010);
+	nv_wr32(dev, 0x087004, 0x00000010);
+
+	/* upload microcode code and data segments */
+	nv_wr32(dev, 0x087ff8, 0x00100000);
+	for (i = 0; i < ARRAY_SIZE(nv98_pcrypt_code); i++)
+		nv_wr32(dev, 0x087ff4, nv98_pcrypt_code[i]);
+
+	nv_wr32(dev, 0x087ff8, 0x00000000);
+	for (i = 0; i < ARRAY_SIZE(nv98_pcrypt_data); i++)
+		nv_wr32(dev, 0x087ff4, nv98_pcrypt_data[i]);
+
+	/* start it running */
+	nv_wr32(dev, 0x08710c, 0x00000000);
+	nv_wr32(dev, 0x087104, 0x00000000); /* ENTRY */
+	nv_wr32(dev, 0x087100, 0x00000002); /* TRIGGER */
 	return 0;
 }
 
+static struct nouveau_enum nv98_crypt_isr_error_name[] = {
+	{ 0x0000, "ILLEGAL_MTHD" },
+	{ 0x0001, "INVALID_BITFIELD" },
+	{ 0x0002, "INVALID_ENUM" },
+	{ 0x0003, "QUERY" },
+	{}
+};
+
+static void
+nv98_crypt_isr(struct drm_device *dev)
+{
+	u32 disp = nv_rd32(dev, 0x08701c);
+	u32 stat = nv_rd32(dev, 0x087008) & disp & ~(disp >> 16);
+	u32 inst = nv_rd32(dev, 0x087050) & 0x3fffffff;
+	u32 ssta = nv_rd32(dev, 0x087040) & 0x0000ffff;
+	u32 addr = nv_rd32(dev, 0x087040) >> 16;
+	u32 mthd = (addr & 0x07ff) << 2;
+	u32 subc = (addr & 0x3800) >> 11;
+	u32 data = nv_rd32(dev, 0x087044);
+	int chid = nv50_graph_isr_chid(dev, inst);
+
+	if (stat & 0x00000040) {
+		NV_INFO(dev, "PCRYPT: DISPATCH_ERROR [");
+		nouveau_enum_print(nv98_crypt_isr_error_name, ssta);
+		printk("] ch %d [0x%08x] subc %d mthd 0x%04x data 0x%08x\n",
+			chid, inst, subc, mthd, data);
+		nv_wr32(dev, 0x087004, 0x00000040);
+		stat &= ~0x00000040;
+	}
+
+	if (stat) {
+		NV_INFO(dev, "PCRYPT: unhandled intr 0x%08x\n", stat);
+		nv_wr32(dev, 0x087004, stat);
+	}
+
+	nv50_fb_vm_trap(dev, 1);
+}
+
 static void
 nv98_crypt_destroy(struct drm_device *dev, int engine)
 {
-	struct nv98_crypt_engine *pcrypt = nv_engine(dev, engine);
+	struct nv98_crypt_priv *priv = nv_engine(dev, engine);
 
+	nouveau_irq_unregister(dev, 14);
 	NVOBJ_ENGINE_DEL(dev, CRYPT);
-
-	kfree(pcrypt);
+	kfree(priv);
 }
 
 int
 nv98_crypt_create(struct drm_device *dev)
 {
-	struct nv98_crypt_engine *pcrypt;
+	struct nv98_crypt_priv *priv;
 
-	pcrypt = kzalloc(sizeof(*pcrypt), GFP_KERNEL);
-	if (!pcrypt)
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
 		return -ENOMEM;
 
-	pcrypt->base.destroy = nv98_crypt_destroy;
-	pcrypt->base.init = nv98_crypt_init;
-	pcrypt->base.fini = nv98_crypt_fini;
+	priv->base.destroy = nv98_crypt_destroy;
+	priv->base.init = nv98_crypt_init;
+	priv->base.fini = nv98_crypt_fini;
+	priv->base.context_new = nv98_crypt_context_new;
+	priv->base.context_del = nv98_crypt_context_del;
+	priv->base.object_new = nv98_crypt_object_new;
+	priv->base.tlb_flush = nv98_crypt_tlb_flush;
+
+	nouveau_irq_register(dev, 14, nv98_crypt_isr);
 
-	NVOBJ_ENGINE_ADD(dev, CRYPT, &pcrypt->base);
+	NVOBJ_ENGINE_ADD(dev, CRYPT, &priv->base);
+	NVOBJ_CLASS(dev, 0x88b4, CRYPT);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv98_crypt.fuc b/drivers/gpu/drm/nouveau/nv98_crypt.fuc
new file mode 100644
index 000000000000..7393813044de
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv98_crypt.fuc
@@ -0,0 +1,698 @@
+/*
+ *  fuc microcode for nv98 pcrypt engine
+ *  Copyright (C) 2010  Marcin Kościelnicki
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+.section #nv98_pcrypt_data
+
+ctx_dma:
+ctx_dma_query:		.b32 0
+ctx_dma_src:		.b32 0
+ctx_dma_dst:		.b32 0
+.equ #dma_count 3
+ctx_query_address_high:	.b32 0
+ctx_query_address_low:	.b32 0
+ctx_query_counter:	.b32 0
+ctx_cond_address_high:	.b32 0
+ctx_cond_address_low:	.b32 0
+ctx_cond_off:		.b32 0
+ctx_src_address_high:	.b32 0
+ctx_src_address_low:	.b32 0
+ctx_dst_address_high:	.b32 0
+ctx_dst_address_low:	.b32 0
+ctx_mode:		.b32 0
+.align 16
+ctx_key:		.skip 16
+ctx_iv:			.skip 16
+
+.align 0x80
+swap:
+.skip 32
+
+.align 8
+common_cmd_dtable:
+.b32 #ctx_query_address_high + 0x20000 ~0xff
+.b32 #ctx_query_address_low + 0x20000 ~0xfffffff0
+.b32 #ctx_query_counter + 0x20000 ~0xffffffff
+.b32 #cmd_query_get + 0x00000 ~1
+.b32 #ctx_cond_address_high + 0x20000 ~0xff
+.b32 #ctx_cond_address_low + 0x20000 ~0xfffffff0
+.b32 #cmd_cond_mode + 0x00000 ~7
+.b32 #cmd_wrcache_flush + 0x00000 ~0
+.equ #common_cmd_max 0x88
+
+
+.align 8
+engine_cmd_dtable:
+.b32 #ctx_key + 0x0 + 0x20000 ~0xffffffff
+.b32 #ctx_key + 0x4 + 0x20000 ~0xffffffff
+.b32 #ctx_key + 0x8 + 0x20000 ~0xffffffff
+.b32 #ctx_key + 0xc + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0x0 + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0x4 + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0x8 + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0xc + 0x20000 ~0xffffffff
+.b32 #ctx_src_address_high + 0x20000 ~0xff
+.b32 #ctx_src_address_low + 0x20000 ~0xfffffff0
+.b32 #ctx_dst_address_high + 0x20000 ~0xff
+.b32 #ctx_dst_address_low + 0x20000 ~0xfffffff0
+.b32 #crypt_cmd_mode + 0x00000 ~0xf
+.b32 #crypt_cmd_length + 0x10000 ~0x0ffffff0
+.equ #engine_cmd_max 0xce
+
+.align 4
+crypt_dtable:
+.b16 #crypt_copy_prep #crypt_do_inout
+.b16 #crypt_store_prep #crypt_do_out
+.b16 #crypt_ecb_e_prep #crypt_do_inout
+.b16 #crypt_ecb_d_prep #crypt_do_inout
+.b16 #crypt_cbc_e_prep #crypt_do_inout
+.b16 #crypt_cbc_d_prep #crypt_do_inout
+.b16 #crypt_pcbc_e_prep #crypt_do_inout
+.b16 #crypt_pcbc_d_prep #crypt_do_inout
+.b16 #crypt_cfb_e_prep #crypt_do_inout
+.b16 #crypt_cfb_d_prep #crypt_do_inout
+.b16 #crypt_ofb_prep #crypt_do_inout
+.b16 #crypt_ctr_prep #crypt_do_inout
+.b16 #crypt_cbc_mac_prep #crypt_do_in
+.b16 #crypt_cmac_finish_complete_prep #crypt_do_in
+.b16 #crypt_cmac_finish_partial_prep #crypt_do_in
+
+.align 0x100
+
+.section #nv98_pcrypt_code
+
+	// $r0 is always set to 0 in our code - this allows some space savings.
+	clear b32 $r0
+
+	// set up the interrupt handler
+	mov $r1 #ih
+	mov $iv0 $r1
+
+	// init stack pointer
+	mov $sp $r0
+
+	// set interrupt dispatch - route timer, fifo, ctxswitch to i0, others to host
+	movw $r1 0xfff0
+	sethi $r1 0
+	mov $r2 0x400
+	iowr I[$r2 + 0x300] $r1
+
+	// enable the interrupts
+	or $r1 0xc
+	iowr I[$r2] $r1
+
+	// enable fifo access and context switching
+	mov $r1 3
+	mov $r2 0x1200
+	iowr I[$r2] $r1
+
+	// enable i0 delivery
+	bset $flags ie0
+
+	// sleep forver, waking only for interrupts.
+	bset $flags $p0
+	spin:
+	sleep $p0
+	bra #spin
+
+// i0 handler
+ih:
+	// see which interrupts we got
+	iord $r1 I[$r0 + 0x200]
+
+	and $r2 $r1 0x8
+	cmpu b32 $r2 0
+	bra e #noctx
+
+		// context switch... prepare the regs for xfer
+		mov $r2 0x7700
+		mov $xtargets $r2
+		mov $xdbase $r0
+		// 128-byte context.
+		mov $r2 0
+		sethi $r2 0x50000
+
+		// read current channel
+		mov $r3 0x1400
+		iord $r4 I[$r3]
+		// if bit 30 set, it's active, so we have to unload it first.
+		shl b32 $r5 $r4 1
+		cmps b32 $r5 0
+		bra nc #ctxload
+
+			// unload the current channel - save the context
+			xdst $r0 $r2
+			xdwait
+			// and clear bit 30, then write back
+			bclr $r4 0x1e
+			iowr I[$r3] $r4
+			// tell PFIFO we unloaded
+			mov $r4 1
+			iowr I[$r3 + 0x200] $r4
+
+		bra #noctx
+
+		ctxload:
+			// no channel loaded - perhaps we're requested to load one
+			iord $r4 I[$r3 + 0x100]
+			shl b32 $r15 $r4 1
+			cmps b32 $r15 0
+			// if bit 30 of next channel not set, probably PFIFO is just
+			// killing a context. do a faux load, without the active bit.
+			bra nc #dummyload
+
+				// ok, do a real context load.
+				xdld $r0 $r2
+				xdwait
+				mov $r5 #ctx_dma
+				mov $r6 #dma_count - 1
+				ctxload_dma_loop:
+					ld b32 $r7 D[$r5 + $r6 * 4]
+					add b32 $r8 $r6 0x180
+					shl b32 $r8 8
+					iowr I[$r8] $r7
+					sub b32 $r6 1
+				bra nc #ctxload_dma_loop
+
+			dummyload:
+			// tell PFIFO we're done
+			mov $r5 2
+			iowr I[$r3 + 0x200] $r5
+
+	noctx:
+	and $r2 $r1 0x4
+	cmpu b32 $r2 0
+	bra e #nocmd
+
+		// incoming fifo command.
+		mov $r3 0x1900
+		iord $r2 I[$r3 + 0x100]
+		iord $r3 I[$r3]
+		// extract the method
+		and $r4 $r2 0x7ff
+		// shift the addr to proper position if we need to interrupt later
+		shl b32 $r2 0x10
+
+		// mthd 0 and 0x100 [NAME, NOP]: ignore
+		and $r5 $r4 0x7bf
+		cmpu b32 $r5 0
+		bra e #cmddone
+
+		mov $r5 #engine_cmd_dtable - 0xc0 * 8
+		mov $r6 #engine_cmd_max
+		cmpu b32 $r4 0xc0
+		bra nc #dtable_cmd
+		mov $r5 #common_cmd_dtable - 0x80 * 8
+		mov $r6 #common_cmd_max
+		cmpu b32 $r4 0x80
+		bra nc #dtable_cmd
+		cmpu b32 $r4 0x60
+		bra nc #dma_cmd
+		cmpu b32 $r4 0x50
+		bra ne #illegal_mthd
+
+			// mthd 0x140: PM_TRIGGER
+			mov $r2 0x2200
+			clear b32 $r3
+			sethi $r3 0x20000
+			iowr I[$r2] $r3
+			bra #cmddone
+
+		dma_cmd:
+			// mthd 0x180...: DMA_*
+			cmpu b32 $r4 0x60+#dma_count
+			bra nc #illegal_mthd
+			shl b32 $r5 $r4 2
+			add b32 $r5 (#ctx_dma - 0x60 * 4) & 0xffff
+			bset $r3 0x1e
+			st b32 D[$r5] $r3
+			add b32 $r4 0x180 - 0x60
+			shl b32 $r4 8
+			iowr I[$r4] $r3
+			bra #cmddone
+
+		dtable_cmd:
+			cmpu b32 $r4 $r6
+			bra nc #illegal_mthd
+			shl b32 $r4 3
+			add b32 $r4 $r5
+			ld b32 $r5 D[$r4 + 4]
+			and $r5 $r3
+			cmpu b32 $r5 0
+			bra ne #invalid_bitfield
+			ld b16 $r5 D[$r4]
+			ld b16 $r6 D[$r4 + 2]
+			cmpu b32 $r6 2
+			bra e #cmd_setctx
+			ld b32 $r7 D[$r0 + #ctx_cond_off]
+			and $r6 $r7
+			cmpu b32 $r6 1
+			bra e #cmddone
+			call $r5
+			bra $p1 #dispatch_error
+			bra #cmddone
+
+		cmd_setctx:
+			st b32 D[$r5] $r3
+			bra #cmddone
+
+
+		invalid_bitfield:
+			or $r2 1
+		dispatch_error:
+		illegal_mthd:
+			mov $r4 0x1000
+			iowr I[$r4] $r2
+			iowr I[$r4 + 0x100] $r3
+			mov $r4 0x40
+			iowr I[$r0] $r4
+
+			im_loop:
+				iord $r4 I[$r0 + 0x200]
+				and $r4 0x40
+				cmpu b32 $r4 0
+			bra ne #im_loop
+
+		cmddone:
+		// remove the command from FIFO
+		mov $r3 0x1d00
+		mov $r4 1
+		iowr I[$r3] $r4
+
+	nocmd:
+	// ack the processed interrupts
+	and $r1 $r1 0xc
+	iowr I[$r0 + 0x100] $r1
+iret
+
+cmd_query_get:
+	// if bit 0 of param set, trigger interrupt afterwards.
+	setp $p1 $r3
+	or $r2 3
+
+	// read PTIMER, beware of races...
+	mov $r4 0xb00
+	ptimer_retry:
+		iord $r6 I[$r4 + 0x100]
+		iord $r5 I[$r4]
+		iord $r7 I[$r4 + 0x100]
+		cmpu b32 $r6 $r7
+	bra ne #ptimer_retry
+
+	// prepare the query structure
+	ld b32 $r4 D[$r0 + #ctx_query_counter]
+	st b32 D[$r0 + #swap + 0x0] $r4
+	st b32 D[$r0 + #swap + 0x4] $r0
+	st b32 D[$r0 + #swap + 0x8] $r5
+	st b32 D[$r0 + #swap + 0xc] $r6
+
+	// will use target 0, DMA_QUERY.
+	mov $xtargets $r0
+
+	ld b32 $r4 D[$r0 + #ctx_query_address_high]
+	shl b32 $r4 0x18
+	mov $xdbase $r4
+
+	ld b32 $r4 D[$r0 + #ctx_query_address_low]
+	mov $r5 #swap
+	sethi $r5 0x20000
+	xdst $r4 $r5
+	xdwait
+
+	ret
+
+cmd_cond_mode:
+	// if >= 5, INVALID_ENUM
+	bset $flags $p1
+	or $r2 2
+	cmpu b32 $r3 5
+	bra nc #return
+
+	// otherwise, no error.
+	bclr $flags $p1
+
+	// if < 2, no QUERY object is involved
+	cmpu b32 $r3 2
+	bra nc #cmd_cond_mode_queryful
+
+		xor $r3 1
+		st b32 D[$r0 + #ctx_cond_off] $r3
+	return:
+		ret
+
+	cmd_cond_mode_queryful:
+	// ok, will need to pull a QUERY object, prepare offsets
+	ld b32 $r4 D[$r0 + #ctx_cond_address_high]
+	ld b32 $r5 D[$r0 + #ctx_cond_address_low]
+	and $r6 $r5 0xff
+	shr b32 $r5 8
+	shl b32 $r4 0x18
+	or $r4 $r5
+	mov $xdbase $r4
+	mov $xtargets $r0
+
+	// pull the first one
+	mov $r5 #swap
+	sethi $r5 0x20000
+	xdld $r6 $r5
+
+	// if == 2, only a single QUERY is involved...
+	cmpu b32 $r3 2
+	bra ne #cmd_cond_mode_double
+
+		xdwait
+		ld b32 $r4 D[$r0 + #swap + 4]
+		cmpu b32 $r4 0
+		xbit $r4 $flags z
+		st b32 D[$r0 + #ctx_cond_off] $r4
+		ret
+
+	// ok, we'll need to pull second one too
+	cmd_cond_mode_double:
+	add b32 $r6 0x10
+	add b32 $r5 0x10
+	xdld $r6 $r5
+	xdwait
+
+	// compare COUNTERs
+	ld b32 $r5 D[$r0 + #swap + 0x00]
+	ld b32 $r6 D[$r0 + #swap + 0x10]
+	cmpu b32 $r5 $r6
+	xbit $r4 $flags z
+
+	// compare RESen
+	ld b32 $r5 D[$r0 + #swap + 0x04]
+	ld b32 $r6 D[$r0 + #swap + 0x14]
+	cmpu b32 $r5 $r6
+	xbit $r5 $flags z
+	and $r4 $r5
+
+	// and negate or not, depending on mode
+	cmpu b32 $r3 3
+	xbit $r5 $flags z
+	xor $r4 $r5
+	st b32 D[$r0 + #ctx_cond_off] $r4
+	ret
+
+cmd_wrcache_flush:
+	bclr $flags $p1
+	mov $r2 0x2200
+	clear b32 $r3
+	sethi $r3 0x10000
+	iowr I[$r2] $r3
+	ret
+
+crypt_cmd_mode:
+	// if >= 0xf, INVALID_ENUM
+	bset $flags $p1
+	or $r2 2
+	cmpu b32 $r3 0xf
+	bra nc #crypt_cmd_mode_return
+
+		bclr $flags $p1
+		st b32 D[$r0 + #ctx_mode] $r3
+
+	crypt_cmd_mode_return:
+	ret
+
+crypt_cmd_length:
+	// nop if length == 0
+	cmpu b32 $r3 0
+	bra e #crypt_cmd_mode_return
+
+	// init key, IV
+	cxset 3
+	mov $r4 #ctx_key
+	sethi $r4 0x70000
+	xdst $r0 $r4
+	mov $r4 #ctx_iv
+	sethi $r4 0x60000
+	xdst $r0 $r4
+	xdwait
+	ckeyreg $c7
+
+	// prepare the targets
+	mov $r4 0x2100
+	mov $xtargets $r4
+
+	// prepare src address
+	ld b32 $r4 D[$r0 + #ctx_src_address_high]
+	ld b32 $r5 D[$r0 + #ctx_src_address_low]
+	shr b32 $r8 $r5 8
+	shl b32 $r4 0x18
+	or $r4 $r8
+	and $r5 $r5 0xff
+
+	// prepare dst address
+	ld b32 $r6 D[$r0 + #ctx_dst_address_high]
+	ld b32 $r7 D[$r0 + #ctx_dst_address_low]
+	shr b32 $r8 $r7 8
+	shl b32 $r6 0x18
+	or $r6 $r8
+	and $r7 $r7 0xff
+
+	// find the proper prep & do functions
+	ld b32 $r8 D[$r0 + #ctx_mode]
+	shl b32 $r8 2
+
+	// run prep
+	ld b16 $r9 D[$r8 + #crypt_dtable]
+	call $r9
+
+	// do it
+	ld b16 $r9 D[$r8 + #crypt_dtable + 2]
+	call $r9
+	cxset 1
+	xdwait
+	cxset 0x61
+	xdwait
+	xdwait
+
+	// update src address
+	shr b32 $r8 $r4 0x18
+	shl b32 $r9 $r4 8
+	add b32 $r9 $r5
+	adc b32 $r8 0
+	st b32 D[$r0 + #ctx_src_address_high] $r8
+	st b32 D[$r0 + #ctx_src_address_low] $r9
+
+	// update dst address
+	shr b32 $r8 $r6 0x18
+	shl b32 $r9 $r6 8
+	add b32 $r9 $r7
+	adc b32 $r8 0
+	st b32 D[$r0 + #ctx_dst_address_high] $r8
+	st b32 D[$r0 + #ctx_dst_address_low] $r9
+
+	// pull updated IV
+	cxset 2
+	mov $r4 #ctx_iv
+	sethi $r4 0x60000
+	xdld $r0 $r4
+	xdwait
+
+	ret
+
+
+crypt_copy_prep:
+	cs0begin 2
+		cxsin $c0
+		cxsout $c0
+	ret
+
+crypt_store_prep:
+	cs0begin 1
+		cxsout $c6
+	ret
+
+crypt_ecb_e_prep:
+	cs0begin 3
+		cxsin $c0
+		cenc $c0 $c0
+		cxsout $c0
+	ret
+
+crypt_ecb_d_prep:
+	ckexp $c7 $c7
+	cs0begin 3
+		cxsin $c0
+		cdec $c0 $c0
+		cxsout $c0
+	ret
+
+crypt_cbc_e_prep:
+	cs0begin 4
+		cxsin $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+		cxsout $c6
+	ret
+
+crypt_cbc_d_prep:
+	ckexp $c7 $c7
+	cs0begin 5
+		cmov $c2 $c6
+		cxsin $c6
+		cdec $c0 $c6
+		cxor $c0 $c2
+		cxsout $c0
+	ret
+
+crypt_pcbc_e_prep:
+	cs0begin 5
+		cxsin $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+		cxsout $c6
+		cxor $c6 $c0
+	ret
+
+crypt_pcbc_d_prep:
+	ckexp $c7 $c7
+	cs0begin 5
+		cxsin $c0
+		cdec $c1 $c0
+		cxor $c6 $c1
+		cxsout $c6
+		cxor $c6 $c0
+	ret
+
+crypt_cfb_e_prep:
+	cs0begin 4
+		cenc $c6 $c6
+		cxsin $c0
+		cxor $c6 $c0
+		cxsout $c6
+	ret
+
+crypt_cfb_d_prep:
+	cs0begin 4
+		cenc $c0 $c6
+		cxsin $c6
+		cxor $c0 $c6
+		cxsout $c0
+	ret
+
+crypt_ofb_prep:
+	cs0begin 4
+		cenc $c6 $c6
+		cxsin $c0
+		cxor $c0 $c6
+		cxsout $c0
+	ret
+
+crypt_ctr_prep:
+	cs0begin 5
+		cenc $c1 $c6
+		cadd $c6 1
+		cxsin $c0
+		cxor $c0 $c1
+		cxsout $c0
+	ret
+
+crypt_cbc_mac_prep:
+	cs0begin 3
+		cxsin $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+	ret
+
+crypt_cmac_finish_complete_prep:
+	cs0begin 7
+		cxsin $c0
+		cxor $c6 $c0
+		cxor $c0 $c0
+		cenc $c0 $c0
+		cprecmac $c0 $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+	ret
+
+crypt_cmac_finish_partial_prep:
+	cs0begin 8
+		cxsin $c0
+		cxor $c6 $c0
+		cxor $c0 $c0
+		cenc $c0 $c0
+		cprecmac $c0 $c0
+		cprecmac $c0 $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+	ret
+
+// TODO
+crypt_do_in:
+	add b32 $r3 $r5
+	mov $xdbase $r4
+	mov $r9 #swap
+	sethi $r9 0x20000
+	crypt_do_in_loop:
+		xdld $r5 $r9
+		xdwait
+		cxset 0x22
+		xdst $r0 $r9
+		cs0exec 1
+		xdwait
+		add b32 $r5 0x10
+		cmpu b32 $r5 $r3
+	bra ne #crypt_do_in_loop
+	cxset 1
+	xdwait
+	ret
+
+crypt_do_out:
+	add b32 $r3 $r7
+	mov $xdbase $r6
+	mov $r9 #swap
+	sethi $r9 0x20000
+	crypt_do_out_loop:
+		cs0exec 1
+		cxset 0x61
+		xdld $r7 $r9
+		xdst $r7 $r9
+		cxset 1
+		xdwait
+		add b32 $r7 0x10
+		cmpu b32 $r7 $r3
+	bra ne #crypt_do_out_loop
+	ret
+
+crypt_do_inout:
+	add b32 $r3 $r5
+	mov $r9 #swap
+	sethi $r9 0x20000
+	crypt_do_inout_loop:
+		mov $xdbase $r4
+		xdld $r5 $r9
+		xdwait
+		cxset 0x21
+		xdst $r0 $r9
+		cs0exec 1
+		cxset 0x61
+		mov $xdbase $r6
+		xdld $r7 $r9
+		xdst $r7 $r9
+		cxset 1
+		xdwait
+		add b32 $r5 0x10
+		add b32 $r7 0x10
+		cmpu b32 $r5 $r3
+	bra ne #crypt_do_inout_loop
+	ret
+
+.align 0x100
diff --git a/drivers/gpu/drm/nouveau/nv98_crypt.fuc.h b/drivers/gpu/drm/nouveau/nv98_crypt.fuc.h
new file mode 100644
index 000000000000..38676c74e6e0
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv98_crypt.fuc.h
@@ -0,0 +1,584 @@
+uint32_t nv98_pcrypt_data[] = {
+/* 0x0000: ctx_dma */
+/* 0x0000: ctx_dma_query */
+	0x00000000,
+/* 0x0004: ctx_dma_src */
+	0x00000000,
+/* 0x0008: ctx_dma_dst */
+	0x00000000,
+/* 0x000c: ctx_query_address_high */
+	0x00000000,
+/* 0x0010: ctx_query_address_low */
+	0x00000000,
+/* 0x0014: ctx_query_counter */
+	0x00000000,
+/* 0x0018: ctx_cond_address_high */
+	0x00000000,
+/* 0x001c: ctx_cond_address_low */
+	0x00000000,
+/* 0x0020: ctx_cond_off */
+	0x00000000,
+/* 0x0024: ctx_src_address_high */
+	0x00000000,
+/* 0x0028: ctx_src_address_low */
+	0x00000000,
+/* 0x002c: ctx_dst_address_high */
+	0x00000000,
+/* 0x0030: ctx_dst_address_low */
+	0x00000000,
+/* 0x0034: ctx_mode */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0040: ctx_key */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0050: ctx_iv */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0080: swap */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x00a0: common_cmd_dtable */
+	0x0002000c,
+	0xffffff00,
+	0x00020010,
+	0x0000000f,
+	0x00020014,
+	0x00000000,
+	0x00000192,
+	0xfffffffe,
+	0x00020018,
+	0xffffff00,
+	0x0002001c,
+	0x0000000f,
+	0x000001d7,
+	0xfffffff8,
+	0x00000260,
+	0xffffffff,
+/* 0x00e0: engine_cmd_dtable */
+	0x00020040,
+	0x00000000,
+	0x00020044,
+	0x00000000,
+	0x00020048,
+	0x00000000,
+	0x0002004c,
+	0x00000000,
+	0x00020050,
+	0x00000000,
+	0x00020054,
+	0x00000000,
+	0x00020058,
+	0x00000000,
+	0x0002005c,
+	0x00000000,
+	0x00020024,
+	0xffffff00,
+	0x00020028,
+	0x0000000f,
+	0x0002002c,
+	0xffffff00,
+	0x00020030,
+	0x0000000f,
+	0x00000271,
+	0xfffffff0,
+	0x00010285,
+	0xf000000f,
+/* 0x0150: crypt_dtable */
+	0x04db0321,
+	0x04b1032f,
+	0x04db0339,
+	0x04db034b,
+	0x04db0361,
+	0x04db0377,
+	0x04db0395,
+	0x04db03af,
+	0x04db03cd,
+	0x04db03e3,
+	0x04db03f9,
+	0x04db040f,
+	0x04830429,
+	0x0483043b,
+	0x0483045d,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+};
+
+uint32_t nv98_pcrypt_code[] = {
+	0x17f004bd,
+	0x0010fe35,
+	0xf10004fe,
+	0xf0fff017,
+	0x27f10013,
+	0x21d00400,
+	0x0c15f0c0,
+	0xf00021d0,
+	0x27f10317,
+	0x21d01200,
+	0x1031f400,
+/* 0x002f: spin */
+	0xf40031f4,
+	0x0ef40028,
+/* 0x0035: ih */
+	0x8001cffd,
+	0xb00812c4,
+	0x0bf40024,
+	0x0027f167,
+	0x002bfe77,
+	0xf00007fe,
+	0x23f00027,
+	0x0037f105,
+	0x0034cf14,
+	0xb0014594,
+	0x18f40055,
+	0x0602fa17,
+	0x4af003f8,
+	0x0034d01e,
+	0xd00147f0,
+	0x0ef48034,
+/* 0x0075: ctxload */
+	0x4034cf33,
+	0xb0014f94,
+	0x18f400f5,
+	0x0502fa21,
+	0x57f003f8,
+	0x0267f000,
+/* 0x008c: ctxload_dma_loop */
+	0xa07856bc,
+	0xb6018068,
+	0x87d00884,
+	0x0162b600,
+/* 0x009f: dummyload */
+	0xf0f018f4,
+	0x35d00257,
+/* 0x00a5: noctx */
+	0x0412c480,
+	0xf50024b0,
+	0xf100df0b,
+	0xcf190037,
+	0x33cf4032,
+	0xff24e400,
+	0x1024b607,
+	0x07bf45e4,
+	0xf50054b0,
+	0xf100b90b,
+	0xf1fae057,
+	0xb000ce67,
+	0x18f4c044,
+	0xa057f14d,
+	0x8867f1fc,
+	0x8044b000,
+	0xb03f18f4,
+	0x18f46044,
+	0x5044b019,
+	0xf1741bf4,
+	0xbd220027,
+	0x0233f034,
+	0xf50023d0,
+/* 0x0103: dma_cmd */
+	0xb000810e,
+	0x18f46344,
+	0x0245945e,
+	0xfe8050b7,
+	0x801e39f0,
+	0x40b70053,
+	0x44b60120,
+	0x0043d008,
+/* 0x0123: dtable_cmd */
+	0xb8600ef4,
+	0x18f40446,
+	0x0344b63e,
+	0x980045bb,
+	0x53fd0145,
+	0x0054b004,
+	0x58291bf4,
+	0x46580045,
+	0x0264b001,
+	0x98170bf4,
+	0x67fd0807,
+	0x0164b004,
+	0xf9300bf4,
+	0x0f01f455,
+/* 0x015b: cmd_setctx */
+	0x80280ef4,
+	0x0ef40053,
+/* 0x0161: invalid_bitfield */
+	0x0125f022,
+/* 0x0164: dispatch_error */
+/* 0x0164: illegal_mthd */
+	0x100047f1,
+	0xd00042d0,
+	0x47f04043,
+	0x0004d040,
+/* 0x0174: im_loop */
+	0xf08004cf,
+	0x44b04044,
+	0xf71bf400,
+/* 0x0180: cmddone */
+	0x1d0037f1,
+	0xd00147f0,
+/* 0x018a: nocmd */
+	0x11c40034,
+	0x4001d00c,
+/* 0x0192: cmd_query_get */
+	0x38f201f8,
+	0x0325f001,
+	0x0b0047f1,
+/* 0x019c: ptimer_retry */
+	0xcf4046cf,
+	0x47cf0045,
+	0x0467b840,
+	0x98f41bf4,
+	0x04800504,
+	0x21008020,
+	0x80220580,
+	0x0bfe2306,
+	0x03049800,
+	0xfe1844b6,
+	0x04980047,
+	0x8057f104,
+	0x0253f000,
+	0xf80645fa,
+/* 0x01d7: cmd_cond_mode */
+	0xf400f803,
+	0x25f00131,
+	0x0534b002,
+	0xf41218f4,
+	0x34b00132,
+	0x0b18f402,
+	0x800136f0,
+/* 0x01f2: return */
+	0x00f80803,
+/* 0x01f4: cmd_cond_mode_queryful */
+	0x98060498,
+	0x56c40705,
+	0x0855b6ff,
+	0xfd1844b6,
+	0x47fe0545,
+	0x000bfe00,
+	0x008057f1,
+	0xfa0253f0,
+	0x34b00565,
+	0x131bf402,
+	0x049803f8,
+	0x0044b021,
+	0x800b4cf0,
+	0x00f80804,
+/* 0x022c: cmd_cond_mode_double */
+	0xb61060b6,
+	0x65fa1050,
+	0x9803f805,
+	0x06982005,
+	0x0456b824,
+	0x980b4cf0,
+	0x06982105,
+	0x0456b825,
+	0xfd0b5cf0,
+	0x34b00445,
+	0x0b5cf003,
+	0x800645fd,
+	0x00f80804,
+/* 0x0260: cmd_wrcache_flush */
+	0xf10132f4,
+	0xbd220027,
+	0x0133f034,
+	0xf80023d0,
+/* 0x0271: crypt_cmd_mode */
+	0x0131f400,
+	0xb00225f0,
+	0x18f40f34,
+	0x0132f409,
+/* 0x0283: crypt_cmd_mode_return */
+	0xf80d0380,
+/* 0x0285: crypt_cmd_length */
+	0x0034b000,
+	0xf4fb0bf4,
+	0x47f0033c,
+	0x0743f040,
+	0xf00604fa,
+	0x43f05047,
+	0x0604fa06,
+	0x3cf503f8,
+	0x47f1c407,
+	0x4bfe2100,
+	0x09049800,
+	0x950a0598,
+	0x44b60858,
+	0x0548fd18,
+	0x98ff55c4,
+	0x07980b06,
+	0x0878950c,
+	0xfd1864b6,
+	0x77c40568,
+	0x0d0898ff,
+	0x580284b6,
+	0x95f9a889,
+	0xf9a98958,
+	0x013cf495,
+	0x3cf403f8,
+	0xf803f861,
+	0x18489503,
+	0xbb084994,
+	0x81b60095,
+	0x09088000,
+	0x950a0980,
+	0x69941868,
+	0x0097bb08,
+	0x800081b6,
+	0x09800b08,
+	0x023cf40c,
+	0xf05047f0,
+	0x04fa0643,
+	0xf803f805,
+/* 0x0321: crypt_copy_prep */
+	0x203cf500,
+	0x003cf594,
+	0x003cf588,
+/* 0x032f: crypt_store_prep */
+	0xf500f88c,
+	0xf594103c,
+	0xf88c063c,
+/* 0x0339: crypt_ecb_e_prep */
+	0x303cf500,
+	0x003cf594,
+	0x003cf588,
+	0x003cf5d0,
+/* 0x034b: crypt_ecb_d_prep */
+	0xf500f88c,
+	0xf5c8773c,
+	0xf594303c,
+	0xf588003c,
+	0xf5d4003c,
+	0xf88c003c,
+/* 0x0361: crypt_cbc_e_prep */
+	0x403cf500,
+	0x003cf594,
+	0x063cf588,
+	0x663cf5ac,
+	0x063cf5d0,
+/* 0x0377: crypt_cbc_d_prep */
+	0xf500f88c,
+	0xf5c8773c,
+	0xf594503c,
+	0xf584623c,
+	0xf588063c,
+	0xf5d4603c,
+	0xf5ac203c,
+	0xf88c003c,
+/* 0x0395: crypt_pcbc_e_prep */
+	0x503cf500,
+	0x003cf594,
+	0x063cf588,
+	0x663cf5ac,
+	0x063cf5d0,
+	0x063cf58c,
+/* 0x03af: crypt_pcbc_d_prep */
+	0xf500f8ac,
+	0xf5c8773c,
+	0xf594503c,
+	0xf588003c,
+	0xf5d4013c,
+	0xf5ac163c,
+	0xf58c063c,
+	0xf8ac063c,
+/* 0x03cd: crypt_cfb_e_prep */
+	0x403cf500,
+	0x663cf594,
+	0x003cf5d0,
+	0x063cf588,
+	0x063cf5ac,
+/* 0x03e3: crypt_cfb_d_prep */
+	0xf500f88c,
+	0xf594403c,
+	0xf5d0603c,
+	0xf588063c,
+	0xf5ac603c,
+	0xf88c003c,
+/* 0x03f9: crypt_ofb_prep */
+	0x403cf500,
+	0x663cf594,
+	0x003cf5d0,
+	0x603cf588,
+	0x003cf5ac,
+/* 0x040f: crypt_ctr_prep */
+	0xf500f88c,
+	0xf594503c,
+	0xf5d0613c,
+	0xf5b0163c,
+	0xf588003c,
+	0xf5ac103c,
+	0xf88c003c,
+/* 0x0429: crypt_cbc_mac_prep */
+	0x303cf500,
+	0x003cf594,
+	0x063cf588,
+	0x663cf5ac,
+/* 0x043b: crypt_cmac_finish_complete_prep */
+	0xf500f8d0,
+	0xf594703c,
+	0xf588003c,
+	0xf5ac063c,
+	0xf5ac003c,
+	0xf5d0003c,
+	0xf5bc003c,
+	0xf5ac063c,
+	0xf8d0663c,
+/* 0x045d: crypt_cmac_finish_partial_prep */
+	0x803cf500,
+	0x003cf594,
+	0x063cf588,
+	0x003cf5ac,
+	0x003cf5ac,
+	0x003cf5d0,
+	0x003cf5bc,
+	0x063cf5bc,
+	0x663cf5ac,
+/* 0x0483: crypt_do_in */
+	0xbb00f8d0,
+	0x47fe0035,
+	0x8097f100,
+	0x0293f000,
+/* 0x0490: crypt_do_in_loop */
+	0xf80559fa,
+	0x223cf403,
+	0xf50609fa,
+	0xf898103c,
+	0x1050b603,
+	0xf40453b8,
+	0x3cf4e91b,
+	0xf803f801,
+/* 0x04b1: crypt_do_out */
+	0x0037bb00,
+	0xf10067fe,
+	0xf0008097,
+/* 0x04be: crypt_do_out_loop */
+	0x3cf50293,
+	0x3cf49810,
+	0x0579fa61,
+	0xf40679fa,
+	0x03f8013c,
+	0xb81070b6,
+	0x1bf40473,
+/* 0x04db: crypt_do_inout */
+	0xbb00f8e8,
+	0x97f10035,
+	0x93f00080,
+/* 0x04e5: crypt_do_inout_loop */
+	0x0047fe02,
+	0xf80559fa,
+	0x213cf403,
+	0xf50609fa,
+	0xf498103c,
+	0x67fe613c,
+	0x0579fa00,
+	0xf40679fa,
+	0x03f8013c,
+	0xb61050b6,
+	0x53b81070,
+	0xd41bf404,
+	0x000000f8,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+};
diff --git a/drivers/gpu/drm/nouveau/nva3_copy.c b/drivers/gpu/drm/nouveau/nva3_copy.c
index 8f356d58e409..0387dc7f4f42 100644
--- a/drivers/gpu/drm/nouveau/nva3_copy.c
+++ b/drivers/gpu/drm/nouveau/nva3_copy.c
@@ -79,29 +79,13 @@ static void
 nva3_copy_context_del(struct nouveau_channel *chan, int engine)
 {
 	struct nouveau_gpuobj *ctx = chan->engctx[engine];
-	struct drm_device *dev = chan->dev;
-	u32 inst;
-
-	inst  = (chan->ramin->vinst >> 12);
-	inst |= 0x40000000;
-
-	/* disable fifo access */
-	nv_wr32(dev, 0x104048, 0x00000000);
-	/* mark channel as unloaded if it's currently active */
-	if (nv_rd32(dev, 0x104050) == inst)
-		nv_mask(dev, 0x104050, 0x40000000, 0x00000000);
-	/* mark next channel as invalid if it's about to be loaded */
-	if (nv_rd32(dev, 0x104054) == inst)
-		nv_mask(dev, 0x104054, 0x40000000, 0x00000000);
-	/* restore fifo access */
-	nv_wr32(dev, 0x104048, 0x00000003);
+	int i;
 
-	for (inst = 0xc0; inst <= 0xd4; inst += 4)
-		nv_wo32(chan->ramin, inst, 0x00000000);
-
-	nouveau_gpuobj_ref(NULL, &ctx);
+	for (i = 0xc0; i <= 0xd4; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
 
 	atomic_dec(&chan->vm->engref[engine]);
+	nouveau_gpuobj_ref(NULL, &ctx);
 	chan->engctx[engine] = ctx;
 }
 
@@ -143,13 +127,6 @@ static int
 nva3_copy_fini(struct drm_device *dev, int engine, bool suspend)
 {
 	nv_mask(dev, 0x104048, 0x00000003, 0x00000000);
-
-	/* trigger fuc context unload */
-	nv_wait(dev, 0x104008, 0x0000000c, 0x00000000);
-	nv_mask(dev, 0x104054, 0x40000000, 0x00000000);
-	nv_wr32(dev, 0x104000, 0x00000008);
-	nv_wait(dev, 0x104008, 0x00000008, 0x00000000);
-
 	nv_wr32(dev, 0x104014, 0xffffffff);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nva3_pm.c b/drivers/gpu/drm/nouveau/nva3_pm.c
index 9e636e6ef6d7..798829353fb6 100644
--- a/drivers/gpu/drm/nouveau/nva3_pm.c
+++ b/drivers/gpu/drm/nouveau/nva3_pm.c
@@ -98,7 +98,9 @@ read_pll(struct drm_device *dev, int clk, u32 pll)
 		sclk = read_clk(dev, 0x10 + clk, false);
 	}
 
-	return sclk * N / (M * P);
+	if (M * P)
+		return sclk * N / (M * P);
+	return 0;
 }
 
 struct creg {
@@ -182,23 +184,26 @@ prog_pll(struct drm_device *dev, int clk, u32 pll, struct creg *reg)
 	const u32 src1 = 0x004160 + (clk * 4);
 	const u32 ctrl = pll + 0;
 	const u32 coef = pll + 4;
-	u32 cntl;
 
 	if (!reg->clk && !reg->pll) {
 		NV_DEBUG(dev, "no clock for %02x\n", clk);
 		return;
 	}
 
-	cntl = nv_rd32(dev, ctrl) & 0xfffffff2;
 	if (reg->pll) {
 		nv_mask(dev, src0, 0x00000101, 0x00000101);
 		nv_wr32(dev, coef, reg->pll);
-		nv_wr32(dev, ctrl, cntl | 0x00000015);
+		nv_mask(dev, ctrl, 0x00000015, 0x00000015);
+		nv_mask(dev, ctrl, 0x00000010, 0x00000000);
+		nv_wait(dev, ctrl, 0x00020000, 0x00020000);
+		nv_mask(dev, ctrl, 0x00000010, 0x00000010);
+		nv_mask(dev, ctrl, 0x00000008, 0x00000000);
 		nv_mask(dev, src1, 0x00000100, 0x00000000);
 		nv_mask(dev, src1, 0x00000001, 0x00000000);
 	} else {
 		nv_mask(dev, src1, 0x003f3141, 0x00000101 | reg->clk);
-		nv_wr32(dev, ctrl, cntl | 0x0000001d);
+		nv_mask(dev, ctrl, 0x00000018, 0x00000018);
+		udelay(20);
 		nv_mask(dev, ctrl, 0x00000001, 0x00000000);
 		nv_mask(dev, src0, 0x00000100, 0x00000000);
 		nv_mask(dev, src0, 0x00000001, 0x00000000);
@@ -230,17 +235,28 @@ nva3_pm_clocks_get(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 }
 
 struct nva3_pm_state {
+	struct nouveau_pm_level *perflvl;
+
 	struct creg nclk;
 	struct creg sclk;
-	struct creg mclk;
 	struct creg vdec;
 	struct creg unka0;
+
+	struct creg mclk;
+	u8 *rammap;
+	u8  rammap_ver;
+	u8  rammap_len;
+	u8 *ramcfg;
+	u8  ramcfg_len;
+	u32 r004018;
+	u32 r100760;
 };
 
 void *
 nva3_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
 	struct nva3_pm_state *info;
+	u8 ramcfg_cnt;
 	int ret;
 
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
@@ -267,6 +283,20 @@ nva3_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 	if (ret < 0)
 		goto out;
 
+	info->rammap = nouveau_perf_rammap(dev, perflvl->memory,
+					   &info->rammap_ver,
+					   &info->rammap_len,
+					   &ramcfg_cnt, &info->ramcfg_len);
+	if (info->rammap_ver != 0x10 || info->rammap_len < 5)
+		info->rammap = NULL;
+
+	info->ramcfg = nouveau_perf_ramcfg(dev, perflvl->memory,
+					   &info->rammap_ver,
+					   &info->ramcfg_len);
+	if (info->rammap_ver != 0x10)
+		info->ramcfg = NULL;
+
+	info->perflvl = perflvl;
 out:
 	if (ret < 0) {
 		kfree(info);
@@ -287,6 +317,240 @@ nva3_pm_grcp_idle(void *data)
 	return false;
 }
 
+static void
+mclk_precharge(struct nouveau_mem_exec_func *exec)
+{
+	nv_wr32(exec->dev, 0x1002d4, 0x00000001);
+}
+
+static void
+mclk_refresh(struct nouveau_mem_exec_func *exec)
+{
+	nv_wr32(exec->dev, 0x1002d0, 0x00000001);
+}
+
+static void
+mclk_refresh_auto(struct nouveau_mem_exec_func *exec, bool enable)
+{
+	nv_wr32(exec->dev, 0x100210, enable ? 0x80000000 : 0x00000000);
+}
+
+static void
+mclk_refresh_self(struct nouveau_mem_exec_func *exec, bool enable)
+{
+	nv_wr32(exec->dev, 0x1002dc, enable ? 0x00000001 : 0x00000000);
+}
+
+static void
+mclk_wait(struct nouveau_mem_exec_func *exec, u32 nsec)
+{
+	volatile u32 post = nv_rd32(exec->dev, 0); (void)post;
+	udelay((nsec + 500) / 1000);
+}
+
+static u32
+mclk_mrg(struct nouveau_mem_exec_func *exec, int mr)
+{
+	if (mr <= 1)
+		return nv_rd32(exec->dev, 0x1002c0 + ((mr - 0) * 4));
+	if (mr <= 3)
+		return nv_rd32(exec->dev, 0x1002e0 + ((mr - 2) * 4));
+	return 0;
+}
+
+static void
+mclk_mrs(struct nouveau_mem_exec_func *exec, int mr, u32 data)
+{
+	struct drm_nouveau_private *dev_priv = exec->dev->dev_private;
+
+	if (mr <= 1) {
+		if (dev_priv->vram_rank_B)
+			nv_wr32(exec->dev, 0x1002c8 + ((mr - 0) * 4), data);
+		nv_wr32(exec->dev, 0x1002c0 + ((mr - 0) * 4), data);
+	} else
+	if (mr <= 3) {
+		if (dev_priv->vram_rank_B)
+			nv_wr32(exec->dev, 0x1002e8 + ((mr - 2) * 4), data);
+		nv_wr32(exec->dev, 0x1002e0 + ((mr - 2) * 4), data);
+	}
+}
+
+static void
+mclk_clock_set(struct nouveau_mem_exec_func *exec)
+{
+	struct drm_device *dev = exec->dev;
+	struct nva3_pm_state *info = exec->priv;
+	u32 ctrl;
+
+	ctrl = nv_rd32(dev, 0x004000);
+	if (!(ctrl & 0x00000008) && info->mclk.pll) {
+		nv_wr32(dev, 0x004000, (ctrl |=  0x00000008));
+		nv_mask(dev, 0x1110e0, 0x00088000, 0x00088000);
+		nv_wr32(dev, 0x004018, 0x00001000);
+		nv_wr32(dev, 0x004000, (ctrl &= ~0x00000001));
+		nv_wr32(dev, 0x004004, info->mclk.pll);
+		nv_wr32(dev, 0x004000, (ctrl |=  0x00000001));
+		udelay(64);
+		nv_wr32(dev, 0x004018, 0x00005000 | info->r004018);
+		udelay(20);
+	} else
+	if (!info->mclk.pll) {
+		nv_mask(dev, 0x004168, 0x003f3040, info->mclk.clk);
+		nv_wr32(dev, 0x004000, (ctrl |= 0x00000008));
+		nv_mask(dev, 0x1110e0, 0x00088000, 0x00088000);
+		nv_wr32(dev, 0x004018, 0x0000d000 | info->r004018);
+	}
+
+	if (info->rammap) {
+		if (info->ramcfg && (info->rammap[4] & 0x08)) {
+			u32 unk5a0 = (ROM16(info->ramcfg[5]) << 8) |
+				      info->ramcfg[5];
+			u32 unk5a4 = ROM16(info->ramcfg[7]);
+			u32 unk804 = (info->ramcfg[9] & 0xf0) << 16 |
+				     (info->ramcfg[3] & 0x0f) << 16 |
+				     (info->ramcfg[9] & 0x0f) |
+				     0x80000000;
+			nv_wr32(dev, 0x1005a0, unk5a0);
+			nv_wr32(dev, 0x1005a4, unk5a4);
+			nv_wr32(dev, 0x10f804, unk804);
+			nv_mask(dev, 0x10053c, 0x00001000, 0x00000000);
+		} else {
+			nv_mask(dev, 0x10053c, 0x00001000, 0x00001000);
+			nv_mask(dev, 0x10f804, 0x80000000, 0x00000000);
+			nv_mask(dev, 0x100760, 0x22222222, info->r100760);
+			nv_mask(dev, 0x1007a0, 0x22222222, info->r100760);
+			nv_mask(dev, 0x1007e0, 0x22222222, info->r100760);
+		}
+	}
+
+	if (info->mclk.pll) {
+		nv_mask(dev, 0x1110e0, 0x00088000, 0x00011000);
+		nv_wr32(dev, 0x004000, (ctrl &= ~0x00000008));
+	}
+}
+
+static void
+mclk_timing_set(struct nouveau_mem_exec_func *exec)
+{
+	struct drm_device *dev = exec->dev;
+	struct nva3_pm_state *info = exec->priv;
+	struct nouveau_pm_level *perflvl = info->perflvl;
+	int i;
+
+	for (i = 0; i < 9; i++)
+		nv_wr32(dev, 0x100220 + (i * 4), perflvl->timing.reg[i]);
+
+	if (info->ramcfg) {
+		u32 data = (info->ramcfg[2] & 0x08) ? 0x00000000 : 0x00001000;
+		nv_mask(dev, 0x100200, 0x00001000, data);
+	}
+
+	if (info->ramcfg) {
+		u32 unk714 = nv_rd32(dev, 0x100714) & ~0xf0000010;
+		u32 unk718 = nv_rd32(dev, 0x100718) & ~0x00000100;
+		u32 unk71c = nv_rd32(dev, 0x10071c) & ~0x00000100;
+		if ( (info->ramcfg[2] & 0x20))
+			unk714 |= 0xf0000000;
+		if (!(info->ramcfg[2] & 0x04))
+			unk714 |= 0x00000010;
+		nv_wr32(dev, 0x100714, unk714);
+
+		if (info->ramcfg[2] & 0x01)
+			unk71c |= 0x00000100;
+		nv_wr32(dev, 0x10071c, unk71c);
+
+		if (info->ramcfg[2] & 0x02)
+			unk718 |= 0x00000100;
+		nv_wr32(dev, 0x100718, unk718);
+
+		if (info->ramcfg[2] & 0x10)
+			nv_wr32(dev, 0x111100, 0x48000000); /*XXX*/
+	}
+}
+
+static void
+prog_mem(struct drm_device *dev, struct nva3_pm_state *info)
+{
+	struct nouveau_mem_exec_func exec = {
+		.dev = dev,
+		.precharge = mclk_precharge,
+		.refresh = mclk_refresh,
+		.refresh_auto = mclk_refresh_auto,
+		.refresh_self = mclk_refresh_self,
+		.wait = mclk_wait,
+		.mrg = mclk_mrg,
+		.mrs = mclk_mrs,
+		.clock_set = mclk_clock_set,
+		.timing_set = mclk_timing_set,
+		.priv = info
+	};
+	u32 ctrl;
+
+	/* XXX: where the fuck does 750MHz come from? */
+	if (info->perflvl->memory <= 750000) {
+		info->r004018 = 0x10000000;
+		info->r100760 = 0x22222222;
+	}
+
+	ctrl = nv_rd32(dev, 0x004000);
+	if (ctrl & 0x00000008) {
+		if (info->mclk.pll) {
+			nv_mask(dev, 0x004128, 0x00000101, 0x00000101);
+			nv_wr32(dev, 0x004004, info->mclk.pll);
+			nv_wr32(dev, 0x004000, (ctrl |= 0x00000001));
+			nv_wr32(dev, 0x004000, (ctrl &= 0xffffffef));
+			nv_wait(dev, 0x004000, 0x00020000, 0x00020000);
+			nv_wr32(dev, 0x004000, (ctrl |= 0x00000010));
+			nv_wr32(dev, 0x004018, 0x00005000 | info->r004018);
+			nv_wr32(dev, 0x004000, (ctrl |= 0x00000004));
+		}
+	} else {
+		u32 ssel = 0x00000101;
+		if (info->mclk.clk)
+			ssel |= info->mclk.clk;
+		else
+			ssel |= 0x00080000; /* 324MHz, shouldn't matter... */
+		nv_mask(dev, 0x004168, 0x003f3141, ctrl);
+	}
+
+	if (info->ramcfg) {
+		if (info->ramcfg[2] & 0x10) {
+			nv_mask(dev, 0x111104, 0x00000600, 0x00000000);
+		} else {
+			nv_mask(dev, 0x111100, 0x40000000, 0x40000000);
+			nv_mask(dev, 0x111104, 0x00000180, 0x00000000);
+		}
+	}
+	if (info->rammap && !(info->rammap[4] & 0x02))
+		nv_mask(dev, 0x100200, 0x00000800, 0x00000000);
+	nv_wr32(dev, 0x611200, 0x00003300);
+	if (!(info->ramcfg[2] & 0x10))
+		nv_wr32(dev, 0x111100, 0x4c020000); /*XXX*/
+
+	nouveau_mem_exec(&exec, info->perflvl);
+
+	nv_wr32(dev, 0x611200, 0x00003330);
+	if (info->rammap && (info->rammap[4] & 0x02))
+		nv_mask(dev, 0x100200, 0x00000800, 0x00000800);
+	if (info->ramcfg) {
+		if (info->ramcfg[2] & 0x10) {
+			nv_mask(dev, 0x111104, 0x00000180, 0x00000180);
+			nv_mask(dev, 0x111100, 0x40000000, 0x00000000);
+		} else {
+			nv_mask(dev, 0x111104, 0x00000600, 0x00000600);
+		}
+	}
+
+	if (info->mclk.pll) {
+		nv_mask(dev, 0x004168, 0x00000001, 0x00000000);
+		nv_mask(dev, 0x004168, 0x00000100, 0x00000000);
+	} else {
+		nv_mask(dev, 0x004000, 0x00000001, 0x00000000);
+		nv_mask(dev, 0x004128, 0x00000001, 0x00000000);
+		nv_mask(dev, 0x004128, 0x00000100, 0x00000000);
+	}
+}
+
 int
 nva3_pm_clocks_set(struct drm_device *dev, void *pre_state)
 {
@@ -316,18 +580,8 @@ nva3_pm_clocks_set(struct drm_device *dev, void *pre_state)
 	prog_clk(dev, 0x20, &info->unka0);
 	prog_clk(dev, 0x21, &info->vdec);
 
-	if (info->mclk.clk || info->mclk.pll) {
-		nv_wr32(dev, 0x100210, 0);
-		nv_wr32(dev, 0x1002dc, 1);
-		nv_wr32(dev, 0x004018, 0x00001000);
-		prog_pll(dev, 0x02, 0x004000, &info->mclk);
-		if (nv_rd32(dev, 0x4000) & 0x00000008)
-			nv_wr32(dev, 0x004018, 0x1000d000);
-		else
-			nv_wr32(dev, 0x004018, 0x10005000);
-		nv_wr32(dev, 0x1002dc, 0);
-		nv_wr32(dev, 0x100210, 0x80000000);
-	}
+	if (info->mclk.clk || info->mclk.pll)
+		prog_mem(dev, info);
 
 	ret = 0;
 
diff --git a/drivers/gpu/drm/nouveau/nvc0_fbcon.c b/drivers/gpu/drm/nouveau/nvc0_fbcon.c
index a495e48197ca..797159e7b7a6 100644
--- a/drivers/gpu/drm/nouveau/nvc0_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nvc0_fbcon.c
@@ -43,22 +43,22 @@ nvc0_fbcon_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 		return ret;
 
 	if (rect->rop != ROP_COPY) {
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x02ac, 1);
+		BEGIN_NVC0(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING  (chan, 1);
 	}
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0588, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0588, 1);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR)
 		OUT_RING  (chan, ((uint32_t *)info->pseudo_palette)[rect->color]);
 	else
 		OUT_RING  (chan, rect->color);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0600, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x0600, 4);
 	OUT_RING  (chan, rect->dx);
 	OUT_RING  (chan, rect->dy);
 	OUT_RING  (chan, rect->dx + rect->width);
 	OUT_RING  (chan, rect->dy + rect->height);
 	if (rect->rop != ROP_COPY) {
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x02ac, 1);
+		BEGIN_NVC0(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING  (chan, 3);
 	}
 	FIRE_RING(chan);
@@ -78,14 +78,14 @@ nvc0_fbcon_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (ret)
 		return ret;
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0110, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0110, 1);
 	OUT_RING  (chan, 0);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x08b0, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x08b0, 4);
 	OUT_RING  (chan, region->dx);
 	OUT_RING  (chan, region->dy);
 	OUT_RING  (chan, region->width);
 	OUT_RING  (chan, region->height);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x08d0, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x08d0, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, region->sx);
 	OUT_RING  (chan, 0);
@@ -116,7 +116,7 @@ nvc0_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 	width = ALIGN(image->width, 32);
 	dwords = (width * image->height) >> 5;
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0814, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0814, 2);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
 		OUT_RING  (chan, palette[image->bg_color] | mask);
@@ -125,10 +125,10 @@ nvc0_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		OUT_RING  (chan, image->bg_color);
 		OUT_RING  (chan, image->fg_color);
 	}
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0838, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0838, 2);
 	OUT_RING  (chan, image->width);
 	OUT_RING  (chan, image->height);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0850, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x0850, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, image->dx);
 	OUT_RING  (chan, 0);
@@ -143,7 +143,7 @@ nvc0_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 
 		dwords -= push;
 
-		BEGIN_NVC0(chan, 6, NvSub2D, 0x0860, push);
+		BEGIN_NIC0(chan, NvSub2D, 0x0860, push);
 		OUT_RINGp(chan, data, push);
 		data += push;
 	}
@@ -200,47 +200,47 @@ nvc0_fbcon_accel_init(struct fb_info *info)
 		return ret;
 	}
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0000, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0000, 1);
 	OUT_RING  (chan, 0x0000902d);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0104, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0104, 2);
 	OUT_RING  (chan, upper_32_bits(chan->notifier_vma.offset));
 	OUT_RING  (chan, lower_32_bits(chan->notifier_vma.offset));
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0290, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0290, 1);
 	OUT_RING  (chan, 0);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0888, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0888, 1);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x02ac, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x02ac, 1);
 	OUT_RING  (chan, 3);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x02a0, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x02a0, 1);
 	OUT_RING  (chan, 0x55);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x08c0, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x08c0, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0580, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0580, 2);
 	OUT_RING  (chan, 4);
 	OUT_RING  (chan, format);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x02e8, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x02e8, 2);
 	OUT_RING  (chan, 2);
 	OUT_RING  (chan, 1);
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0804, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0804, 1);
 	OUT_RING  (chan, format);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0800, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0800, 1);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0808, 3);
+	BEGIN_NVC0(chan, NvSub2D, 0x0808, 3);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x081c, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x081c, 1);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0840, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x0840, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0200, 10);
+	BEGIN_NVC0(chan, NvSub2D, 0x0200, 10);
 	OUT_RING  (chan, format);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
@@ -251,7 +251,7 @@ nvc0_fbcon_accel_init(struct fb_info *info)
 	OUT_RING  (chan, info->var.yres_virtual);
 	OUT_RING  (chan, upper_32_bits(fb->vma.offset));
 	OUT_RING  (chan, lower_32_bits(fb->vma.offset));
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0230, 10);
+	BEGIN_NVC0(chan, NvSub2D, 0x0230, 10);
 	OUT_RING  (chan, format);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
diff --git a/drivers/gpu/drm/nouveau/nvc0_fence.c b/drivers/gpu/drm/nouveau/nvc0_fence.c
new file mode 100644
index 000000000000..47ab388a606e
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvc0_fence.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_fifo.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nvc0_fence_priv {
+	struct nouveau_fence_priv base;
+	struct nouveau_bo *bo;
+};
+
+struct nvc0_fence_chan {
+	struct nouveau_fence_chan base;
+	struct nouveau_vma vma;
+};
+
+static int
+nvc0_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	struct nvc0_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	u64 addr = fctx->vma.offset + chan->id * 16;
+	int ret;
+
+	ret = RING_SPACE(chan, 5);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(addr));
+		OUT_RING  (chan, lower_32_bits(addr));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_WRITE_LONG);
+		FIRE_RING (chan);
+	}
+
+	return ret;
+}
+
+static int
+nvc0_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	struct nvc0_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	u64 addr = fctx->vma.offset + prev->id * 16;
+	int ret;
+
+	ret = RING_SPACE(chan, 5);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(addr));
+		OUT_RING  (chan, lower_32_bits(addr));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_GEQUAL |
+				 NVC0_SUBCHAN_SEMAPHORE_TRIGGER_YIELD);
+		FIRE_RING (chan);
+	}
+
+	return ret;
+}
+
+static u32
+nvc0_fence_read(struct nouveau_channel *chan)
+{
+	struct nvc0_fence_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_FENCE);
+	return nouveau_bo_rd32(priv->bo, chan->id * 16/4);
+}
+
+static void
+nvc0_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nvc0_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nvc0_fence_chan *fctx = chan->engctx[engine];
+
+	nouveau_bo_vma_del(priv->bo, &fctx->vma);
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nvc0_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nvc0_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nvc0_fence_chan *fctx;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	nouveau_fence_context_new(&fctx->base);
+
+	ret = nouveau_bo_vma_add(priv->bo, chan->vm, &fctx->vma);
+	if (ret)
+		nvc0_fence_context_del(chan, engine);
+
+	nouveau_bo_wr32(priv->bo, chan->id * 16/4, 0x00000000);
+	return ret;
+}
+
+static int
+nvc0_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nvc0_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nvc0_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_fence_priv *priv = nv_engine(dev, engine);
+
+	nouveau_bo_unmap(priv->bo);
+	nouveau_bo_ref(NULL, &priv->bo);
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nvc0_fence_create(struct drm_device *dev)
+{
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_fence_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nvc0_fence_destroy;
+	priv->base.engine.init = nvc0_fence_init;
+	priv->base.engine.fini = nvc0_fence_fini;
+	priv->base.engine.context_new = nvc0_fence_context_new;
+	priv->base.engine.context_del = nvc0_fence_context_del;
+	priv->base.emit = nvc0_fence_emit;
+	priv->base.sync = nvc0_fence_sync;
+	priv->base.read = nvc0_fence_read;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+
+	ret = nouveau_bo_new(dev, 16 * pfifo->channels, 0, TTM_PL_FLAG_VRAM,
+			     0, 0, NULL, &priv->bo);
+	if (ret == 0) {
+		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
+		if (ret == 0)
+			ret = nouveau_bo_map(priv->bo);
+		if (ret)
+			nouveau_bo_ref(NULL, &priv->bo);
+	}
+
+	if (ret)
+		nvc0_fence_destroy(dev, NVOBJ_ENGINE_FENCE);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nvc0_fifo.c b/drivers/gpu/drm/nouveau/nvc0_fifo.c
index 50d68a7a1379..7d85553d518c 100644
--- a/drivers/gpu/drm/nouveau/nvc0_fifo.c
+++ b/drivers/gpu/drm/nouveau/nvc0_fifo.c
@@ -26,10 +26,12 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_mm.h"
+#include "nouveau_fifo.h"
 
 static void nvc0_fifo_isr(struct drm_device *);
 
 struct nvc0_fifo_priv {
+	struct nouveau_fifo_priv base;
 	struct nouveau_gpuobj *playlist[2];
 	int cur_playlist;
 	struct nouveau_vma user_vma;
@@ -37,8 +39,8 @@ struct nvc0_fifo_priv {
 };
 
 struct nvc0_fifo_chan {
+	struct nouveau_fifo_chan base;
 	struct nouveau_gpuobj *user;
-	struct nouveau_gpuobj *ramfc;
 };
 
 static void
@@ -46,8 +48,7 @@ nvc0_fifo_playlist_update(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv = pfifo->priv;
+	struct nvc0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_gpuobj *cur;
 	int i, p;
 
@@ -69,59 +70,20 @@ nvc0_fifo_playlist_update(struct drm_device *dev)
 		NV_ERROR(dev, "PFIFO - playlist update failed\n");
 }
 
-void
-nvc0_fifo_disable(struct drm_device *dev)
-{
-}
-
-void
-nvc0_fifo_enable(struct drm_device *dev)
-{
-}
-
-bool
-nvc0_fifo_reassign(struct drm_device *dev, bool enable)
-{
-	return false;
-}
-
-bool
-nvc0_fifo_cache_pull(struct drm_device *dev, bool enable)
-{
-	return false;
-}
-
-int
-nvc0_fifo_channel_id(struct drm_device *dev)
-{
-	return 127;
-}
-
-int
-nvc0_fifo_create_context(struct nouveau_channel *chan)
+static int
+nvc0_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv = pfifo->priv;
-	struct nvc0_fifo_chan *fifoch;
+	struct nvc0_fifo_priv *priv = nv_engine(dev, engine);
+	struct nvc0_fifo_chan *fctx;
 	u64 ib_virt = chan->pushbuf_base + chan->dma.ib_base * 4;
-	int ret;
+	int ret, i;
 
-	chan->fifo_priv = kzalloc(sizeof(*fifoch), GFP_KERNEL);
-	if (!chan->fifo_priv)
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
 		return -ENOMEM;
-	fifoch = chan->fifo_priv;
-
-	/* allocate vram for control regs, map into polling area */
-	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000,
-				 NVOBJ_FLAG_ZERO_ALLOC, &fifoch->user);
-	if (ret)
-		goto error;
-
-	nouveau_vm_map_at(&priv->user_vma, chan->id * 0x1000,
-			  *(struct nouveau_mem **)fifoch->user->node);
 
 	chan->user = ioremap_wc(pci_resource_start(dev->pdev, 1) +
 				priv->user_vma.offset + (chan->id * 0x1000),
@@ -131,176 +93,77 @@ nvc0_fifo_create_context(struct nouveau_channel *chan)
 		goto error;
 	}
 
-	/* ramfc */
-	ret = nouveau_gpuobj_new_fake(dev, chan->ramin->pinst,
-				      chan->ramin->vinst, 0x100,
-				      NVOBJ_FLAG_ZERO_ALLOC, &fifoch->ramfc);
+	/* allocate vram for control regs, map into polling area */
+	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &fctx->user);
 	if (ret)
 		goto error;
 
-	nv_wo32(fifoch->ramfc, 0x08, lower_32_bits(fifoch->user->vinst));
-	nv_wo32(fifoch->ramfc, 0x0c, upper_32_bits(fifoch->user->vinst));
-	nv_wo32(fifoch->ramfc, 0x10, 0x0000face);
-	nv_wo32(fifoch->ramfc, 0x30, 0xfffff902);
-	nv_wo32(fifoch->ramfc, 0x48, lower_32_bits(ib_virt));
-	nv_wo32(fifoch->ramfc, 0x4c, drm_order(chan->dma.ib_max + 1) << 16 |
+	nouveau_vm_map_at(&priv->user_vma, chan->id * 0x1000,
+			  *(struct nouveau_mem **)fctx->user->node);
+
+	for (i = 0; i < 0x100; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+	nv_wo32(chan->ramin, 0x08, lower_32_bits(fctx->user->vinst));
+	nv_wo32(chan->ramin, 0x0c, upper_32_bits(fctx->user->vinst));
+	nv_wo32(chan->ramin, 0x10, 0x0000face);
+	nv_wo32(chan->ramin, 0x30, 0xfffff902);
+	nv_wo32(chan->ramin, 0x48, lower_32_bits(ib_virt));
+	nv_wo32(chan->ramin, 0x4c, drm_order(chan->dma.ib_max + 1) << 16 |
 				   upper_32_bits(ib_virt));
-	nv_wo32(fifoch->ramfc, 0x54, 0x00000002);
-	nv_wo32(fifoch->ramfc, 0x84, 0x20400000);
-	nv_wo32(fifoch->ramfc, 0x94, 0x30000001);
-	nv_wo32(fifoch->ramfc, 0x9c, 0x00000100);
-	nv_wo32(fifoch->ramfc, 0xa4, 0x1f1f1f1f);
-	nv_wo32(fifoch->ramfc, 0xa8, 0x1f1f1f1f);
-	nv_wo32(fifoch->ramfc, 0xac, 0x0000001f);
-	nv_wo32(fifoch->ramfc, 0xb8, 0xf8000000);
-	nv_wo32(fifoch->ramfc, 0xf8, 0x10003080); /* 0x002310 */
-	nv_wo32(fifoch->ramfc, 0xfc, 0x10000010); /* 0x002350 */
+	nv_wo32(chan->ramin, 0x54, 0x00000002);
+	nv_wo32(chan->ramin, 0x84, 0x20400000);
+	nv_wo32(chan->ramin, 0x94, 0x30000001);
+	nv_wo32(chan->ramin, 0x9c, 0x00000100);
+	nv_wo32(chan->ramin, 0xa4, 0x1f1f1f1f);
+	nv_wo32(chan->ramin, 0xa8, 0x1f1f1f1f);
+	nv_wo32(chan->ramin, 0xac, 0x0000001f);
+	nv_wo32(chan->ramin, 0xb8, 0xf8000000);
+	nv_wo32(chan->ramin, 0xf8, 0x10003080); /* 0x002310 */
+	nv_wo32(chan->ramin, 0xfc, 0x10000010); /* 0x002350 */
 	pinstmem->flush(dev);
 
 	nv_wr32(dev, 0x003000 + (chan->id * 8), 0xc0000000 |
 						(chan->ramin->vinst >> 12));
 	nv_wr32(dev, 0x003004 + (chan->id * 8), 0x001f0001);
 	nvc0_fifo_playlist_update(dev);
-	return 0;
 
 error:
-	pfifo->destroy_context(chan);
+	if (ret)
+		priv->base.base.context_del(chan, engine);
 	return ret;
 }
 
-void
-nvc0_fifo_destroy_context(struct nouveau_channel *chan)
+static void
+nvc0_fifo_context_del(struct nouveau_channel *chan, int engine)
 {
+	struct nvc0_fifo_chan *fctx = chan->engctx[engine];
 	struct drm_device *dev = chan->dev;
-	struct nvc0_fifo_chan *fifoch;
 
 	nv_mask(dev, 0x003004 + (chan->id * 8), 0x00000001, 0x00000000);
 	nv_wr32(dev, 0x002634, chan->id);
 	if (!nv_wait(dev, 0x0002634, 0xffffffff, chan->id))
 		NV_WARN(dev, "0x2634 != chid: 0x%08x\n", nv_rd32(dev, 0x2634));
-
 	nvc0_fifo_playlist_update(dev);
-
 	nv_wr32(dev, 0x003000 + (chan->id * 8), 0x00000000);
 
+	nouveau_gpuobj_ref(NULL, &fctx->user);
 	if (chan->user) {
 		iounmap(chan->user);
 		chan->user = NULL;
 	}
 
-	fifoch = chan->fifo_priv;
-	chan->fifo_priv = NULL;
-	if (!fifoch)
-		return;
-
-	nouveau_gpuobj_ref(NULL, &fifoch->ramfc);
-	nouveau_gpuobj_ref(NULL, &fifoch->user);
-	kfree(fifoch);
-}
-
-int
-nvc0_fifo_load_context(struct nouveau_channel *chan)
-{
-	return 0;
-}
-
-int
-nvc0_fifo_unload_context(struct drm_device *dev)
-{
-	int i;
-
-	for (i = 0; i < 128; i++) {
-		if (!(nv_rd32(dev, 0x003004 + (i * 8)) & 1))
-			continue;
-
-		nv_mask(dev, 0x003004 + (i * 8), 0x00000001, 0x00000000);
-		nv_wr32(dev, 0x002634, i);
-		if (!nv_wait(dev, 0x002634, 0xffffffff, i)) {
-			NV_INFO(dev, "PFIFO: kick ch %d failed: 0x%08x\n",
-				i, nv_rd32(dev, 0x002634));
-			return -EBUSY;
-		}
-	}
-
-	return 0;
-}
-
-static void
-nvc0_fifo_destroy(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv;
-
-	priv = pfifo->priv;
-	if (!priv)
-		return;
-
-	nouveau_vm_put(&priv->user_vma);
-	nouveau_gpuobj_ref(NULL, &priv->playlist[1]);
-	nouveau_gpuobj_ref(NULL, &priv->playlist[0]);
-	kfree(priv);
-}
-
-void
-nvc0_fifo_takedown(struct drm_device *dev)
-{
-	nv_wr32(dev, 0x002140, 0x00000000);
-	nvc0_fifo_destroy(dev);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
 }
 
 static int
-nvc0_fifo_create(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv;
-	int ret;
-
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-	pfifo->priv = priv;
-
-	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000, 0,
-				 &priv->playlist[0]);
-	if (ret)
-		goto error;
-
-	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000, 0,
-				 &priv->playlist[1]);
-	if (ret)
-		goto error;
-
-	ret = nouveau_vm_get(dev_priv->bar1_vm, pfifo->channels * 0x1000,
-			     12, NV_MEM_ACCESS_RW, &priv->user_vma);
-	if (ret)
-		goto error;
-
-	nouveau_irq_register(dev, 8, nvc0_fifo_isr);
-	NVOBJ_CLASS(dev, 0x506e, SW); /* nvsw */
-	return 0;
-
-error:
-	nvc0_fifo_destroy(dev);
-	return ret;
-}
-
-int
-nvc0_fifo_init(struct drm_device *dev)
+nvc0_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	struct nvc0_fifo_priv *priv = nv_engine(dev, engine);
 	struct nouveau_channel *chan;
-	struct nvc0_fifo_priv *priv;
-	int ret, i;
-
-	if (!pfifo->priv) {
-		ret = nvc0_fifo_create(dev);
-		if (ret)
-			return ret;
-	}
-	priv = pfifo->priv;
+	int i;
 
 	/* reset PFIFO, enable all available PSUBFIFO areas */
 	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
@@ -338,7 +201,7 @@ nvc0_fifo_init(struct drm_device *dev)
 	/* restore PFIFO context table */
 	for (i = 0; i < 128; i++) {
 		chan = dev_priv->channels.ptr[i];
-		if (!chan || !chan->fifo_priv)
+		if (!chan || !chan->engctx[engine])
 			continue;
 
 		nv_wr32(dev, 0x003000 + (i * 8), 0xc0000000 |
@@ -350,6 +213,29 @@ nvc0_fifo_init(struct drm_device *dev)
 	return 0;
 }
 
+static int
+nvc0_fifo_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	int i;
+
+	for (i = 0; i < 128; i++) {
+		if (!(nv_rd32(dev, 0x003004 + (i * 8)) & 1))
+			continue;
+
+		nv_mask(dev, 0x003004 + (i * 8), 0x00000001, 0x00000000);
+		nv_wr32(dev, 0x002634, i);
+		if (!nv_wait(dev, 0x002634, 0xffffffff, i)) {
+			NV_INFO(dev, "PFIFO: kick ch %d failed: 0x%08x\n",
+				i, nv_rd32(dev, 0x002634));
+			return -EBUSY;
+		}
+	}
+
+	nv_wr32(dev, 0x002140, 0x00000000);
+	return 0;
+}
+
+
 struct nouveau_enum nvc0_fifo_fault_unit[] = {
 	{ 0x00, "PGRAPH" },
 	{ 0x03, "PEEPHOLE" },
@@ -439,13 +325,14 @@ nvc0_fifo_isr_vm_fault(struct drm_device *dev, int unit)
 static int
 nvc0_fifo_page_flip(struct drm_device *dev, u32 chid)
 {
+	struct nvc0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = NULL;
 	unsigned long flags;
 	int ret = -EINVAL;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	if (likely(chid >= 0 && chid < dev_priv->engine.fifo.channels)) {
+	if (likely(chid >= 0 && chid < priv->base.channels)) {
 		chan = dev_priv->channels.ptr[chid];
 		if (likely(chan))
 			ret = nouveau_finish_page_flip(chan, NULL);
@@ -534,3 +421,56 @@ nvc0_fifo_isr(struct drm_device *dev)
 		nv_wr32(dev, 0x002140, 0);
 	}
 }
+
+static void
+nvc0_fifo_destroy(struct drm_device *dev, int engine)
+{
+	struct nvc0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	nouveau_vm_put(&priv->user_vma);
+	nouveau_gpuobj_ref(NULL, &priv->playlist[1]);
+	nouveau_gpuobj_ref(NULL, &priv->playlist[0]);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nvc0_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_fifo_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nvc0_fifo_destroy;
+	priv->base.base.init = nvc0_fifo_init;
+	priv->base.base.fini = nvc0_fifo_fini;
+	priv->base.base.context_new = nvc0_fifo_context_new;
+	priv->base.base.context_del = nvc0_fifo_context_del;
+	priv->base.channels = 128;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 4096, 4096, 0, &priv->playlist[0]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 4096, 4096, 0, &priv->playlist[1]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_vm_get(dev_priv->bar1_vm, priv->base.channels * 0x1000,
+			     12, NV_MEM_ACCESS_RW, &priv->user_vma);
+	if (ret)
+		goto error;
+
+	nouveau_irq_register(dev, 8, nvc0_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nvc0_graph.c b/drivers/gpu/drm/nouveau/nvc0_graph.c
index 9066102d1159..2a01e6e47724 100644
--- a/drivers/gpu/drm/nouveau/nvc0_graph.c
+++ b/drivers/gpu/drm/nouveau/nvc0_graph.c
@@ -29,6 +29,7 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_mm.h"
+#include "nouveau_fifo.h"
 
 #include "nvc0_graph.h"
 #include "nvc0_grhub.fuc.h"
@@ -620,13 +621,14 @@ nvc0_graph_init(struct drm_device *dev, int engine)
 int
 nvc0_graph_isr_chid(struct drm_device *dev, u64 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan;
 	unsigned long flags;
 	int i;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		chan = dev_priv->channels.ptr[i];
 		if (!chan || !chan->ramin)
 			continue;
diff --git a/drivers/gpu/drm/nouveau/nvc0_pm.c b/drivers/gpu/drm/nouveau/nvc0_pm.c
index ce65f81bb871..7c95c44e2887 100644
--- a/drivers/gpu/drm/nouveau/nvc0_pm.c
+++ b/drivers/gpu/drm/nouveau/nvc0_pm.c
@@ -164,7 +164,9 @@ struct nvc0_pm_clock {
 };
 
 struct nvc0_pm_state {
+	struct nouveau_pm_level *perflvl;
 	struct nvc0_pm_clock eng[16];
+	struct nvc0_pm_clock mem;
 };
 
 static u32
@@ -303,6 +305,48 @@ calc_clk(struct drm_device *dev, int clk, struct nvc0_pm_clock *info, u32 freq)
 	return 0;
 }
 
+static int
+calc_mem(struct drm_device *dev, struct nvc0_pm_clock *info, u32 freq)
+{
+	struct pll_lims pll;
+	int N, M, P, ret;
+	u32 ctrl;
+
+	/* mclk pll input freq comes from another pll, make sure it's on */
+	ctrl = nv_rd32(dev, 0x132020);
+	if (!(ctrl & 0x00000001)) {
+		/* if not, program it to 567MHz.  nfi where this value comes
+		 * from - it looks like it's in the pll limits table for
+		 * 132000 but the binary driver ignores all my attempts to
+		 * change this value.
+		 */
+		nv_wr32(dev, 0x137320, 0x00000103);
+		nv_wr32(dev, 0x137330, 0x81200606);
+		nv_wait(dev, 0x132020, 0x00010000, 0x00010000);
+		nv_wr32(dev, 0x132024, 0x0001150f);
+		nv_mask(dev, 0x132020, 0x00000001, 0x00000001);
+		nv_wait(dev, 0x137390, 0x00020000, 0x00020000);
+		nv_mask(dev, 0x132020, 0x00000004, 0x00000004);
+	}
+
+	/* for the moment, until the clock tree is better understood, use
+	 * pll mode for all clock frequencies
+	 */
+	ret = get_pll_limits(dev, 0x132000, &pll);
+	if (ret == 0) {
+		pll.refclk = read_pll(dev, 0x132020);
+		if (pll.refclk) {
+			ret = nva3_calc_pll(dev, &pll, freq, &N, NULL, &M, &P);
+			if (ret > 0) {
+				info->coef = (P << 16) | (N << 8) | M;
+				return 0;
+			}
+		}
+	}
+
+	return -EINVAL;
+}
+
 void *
 nvc0_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
@@ -335,6 +379,15 @@ nvc0_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 		return ERR_PTR(ret);
 	}
 
+	if (perflvl->memory) {
+		ret = calc_mem(dev, &info->mem, perflvl->memory);
+		if (ret) {
+			kfree(info);
+			return ERR_PTR(ret);
+		}
+	}
+
+	info->perflvl = perflvl;
 	return info;
 }
 
@@ -375,12 +428,148 @@ prog_clk(struct drm_device *dev, int clk, struct nvc0_pm_clock *info)
 	nv_mask(dev, 0x137250 + (clk * 0x04), 0x00003f3f, info->mdiv);
 }
 
+static void
+mclk_precharge(struct nouveau_mem_exec_func *exec)
+{
+}
+
+static void
+mclk_refresh(struct nouveau_mem_exec_func *exec)
+{
+}
+
+static void
+mclk_refresh_auto(struct nouveau_mem_exec_func *exec, bool enable)
+{
+	nv_wr32(exec->dev, 0x10f210, enable ? 0x80000000 : 0x00000000);
+}
+
+static void
+mclk_refresh_self(struct nouveau_mem_exec_func *exec, bool enable)
+{
+}
+
+static void
+mclk_wait(struct nouveau_mem_exec_func *exec, u32 nsec)
+{
+	udelay((nsec + 500) / 1000);
+}
+
+static u32
+mclk_mrg(struct nouveau_mem_exec_func *exec, int mr)
+{
+	struct drm_device *dev = exec->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	if (dev_priv->vram_type != NV_MEM_TYPE_GDDR5) {
+		if (mr <= 1)
+			return nv_rd32(dev, 0x10f300 + ((mr - 0) * 4));
+		return nv_rd32(dev, 0x10f320 + ((mr - 2) * 4));
+	} else {
+		if (mr == 0)
+			return nv_rd32(dev, 0x10f300 + (mr * 4));
+		else
+		if (mr <= 7)
+			return nv_rd32(dev, 0x10f32c + (mr * 4));
+		return nv_rd32(dev, 0x10f34c);
+	}
+}
+
+static void
+mclk_mrs(struct nouveau_mem_exec_func *exec, int mr, u32 data)
+{
+	struct drm_device *dev = exec->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	if (dev_priv->vram_type != NV_MEM_TYPE_GDDR5) {
+		if (mr <= 1) {
+			nv_wr32(dev, 0x10f300 + ((mr - 0) * 4), data);
+			if (dev_priv->vram_rank_B)
+				nv_wr32(dev, 0x10f308 + ((mr - 0) * 4), data);
+		} else
+		if (mr <= 3) {
+			nv_wr32(dev, 0x10f320 + ((mr - 2) * 4), data);
+			if (dev_priv->vram_rank_B)
+				nv_wr32(dev, 0x10f328 + ((mr - 2) * 4), data);
+		}
+	} else {
+		if      (mr ==  0) nv_wr32(dev, 0x10f300 + (mr * 4), data);
+		else if (mr <=  7) nv_wr32(dev, 0x10f32c + (mr * 4), data);
+		else if (mr == 15) nv_wr32(dev, 0x10f34c, data);
+	}
+}
+
+static void
+mclk_clock_set(struct nouveau_mem_exec_func *exec)
+{
+	struct nvc0_pm_state *info = exec->priv;
+	struct drm_device *dev = exec->dev;
+	u32 ctrl = nv_rd32(dev, 0x132000);
+
+	nv_wr32(dev, 0x137360, 0x00000001);
+	nv_wr32(dev, 0x137370, 0x00000000);
+	nv_wr32(dev, 0x137380, 0x00000000);
+	if (ctrl & 0x00000001)
+		nv_wr32(dev, 0x132000, (ctrl &= ~0x00000001));
+
+	nv_wr32(dev, 0x132004, info->mem.coef);
+	nv_wr32(dev, 0x132000, (ctrl |= 0x00000001));
+	nv_wait(dev, 0x137390, 0x00000002, 0x00000002);
+	nv_wr32(dev, 0x132018, 0x00005000);
+
+	nv_wr32(dev, 0x137370, 0x00000001);
+	nv_wr32(dev, 0x137380, 0x00000001);
+	nv_wr32(dev, 0x137360, 0x00000000);
+}
+
+static void
+mclk_timing_set(struct nouveau_mem_exec_func *exec)
+{
+	struct nvc0_pm_state *info = exec->priv;
+	struct nouveau_pm_level *perflvl = info->perflvl;
+	int i;
+
+	for (i = 0; i < 5; i++)
+		nv_wr32(exec->dev, 0x10f290 + (i * 4), perflvl->timing.reg[i]);
+}
+
+static void
+prog_mem(struct drm_device *dev, struct nvc0_pm_state *info)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_mem_exec_func exec = {
+		.dev = dev,
+		.precharge = mclk_precharge,
+		.refresh = mclk_refresh,
+		.refresh_auto = mclk_refresh_auto,
+		.refresh_self = mclk_refresh_self,
+		.wait = mclk_wait,
+		.mrg = mclk_mrg,
+		.mrs = mclk_mrs,
+		.clock_set = mclk_clock_set,
+		.timing_set = mclk_timing_set,
+		.priv = info
+	};
+
+	if (dev_priv->chipset < 0xd0)
+		nv_wr32(dev, 0x611200, 0x00003300);
+	else
+		nv_wr32(dev, 0x62c000, 0x03030000);
+
+	nouveau_mem_exec(&exec, info->perflvl);
+
+	if (dev_priv->chipset < 0xd0)
+		nv_wr32(dev, 0x611200, 0x00003300);
+	else
+		nv_wr32(dev, 0x62c000, 0x03030300);
+}
 int
 nvc0_pm_clocks_set(struct drm_device *dev, void *data)
 {
 	struct nvc0_pm_state *info = data;
 	int i;
 
+	if (info->mem.coef)
+		prog_mem(dev, info);
+
 	for (i = 0; i < 16; i++) {
 		if (!info->eng[i].freq)
 			continue;
diff --git a/drivers/gpu/drm/nouveau/nvc0_software.c b/drivers/gpu/drm/nouveau/nvc0_software.c
new file mode 100644
index 000000000000..93e8c164fec6
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvc0_software.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_ramht.h"
+#include "nouveau_software.h"
+
+#include "nv50_display.h"
+
+struct nvc0_software_priv {
+	struct nouveau_software_priv base;
+};
+
+struct nvc0_software_chan {
+	struct nouveau_software_chan base;
+	struct nouveau_vma dispc_vma[4];
+};
+
+u64
+nvc0_software_crtc(struct nouveau_channel *chan, int crtc)
+{
+	struct nvc0_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	return pch->dispc_vma[crtc].offset;
+}
+
+static int
+nvc0_software_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_software_priv *psw = nv_engine(dev, NVOBJ_ENGINE_SW);
+	struct nvc0_software_chan *pch;
+	int ret = 0, i;
+
+	pch = kzalloc(sizeof(*pch), GFP_KERNEL);
+	if (!pch)
+		return -ENOMEM;
+
+	nouveau_software_context_new(&pch->base);
+	chan->engctx[engine] = pch;
+
+	/* map display semaphore buffers into channel's vm */
+	for (i = 0; !ret && i < dev->mode_config.num_crtc; i++) {
+		struct nouveau_bo *bo;
+		if (dev_priv->card_type >= NV_D0)
+			bo = nvd0_display_crtc_sema(dev, i);
+		else
+			bo = nv50_display(dev)->crtc[i].sem.bo;
+
+		ret = nouveau_bo_vma_add(bo, chan->vm, &pch->dispc_vma[i]);
+	}
+
+	if (ret)
+		psw->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nvc0_software_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_software_chan *pch = chan->engctx[engine];
+	int i;
+
+	if (dev_priv->card_type >= NV_D0) {
+		for (i = 0; i < dev->mode_config.num_crtc; i++) {
+			struct nouveau_bo *bo = nvd0_display_crtc_sema(dev, i);
+			nouveau_bo_vma_del(bo, &pch->dispc_vma[i]);
+		}
+	} else
+	if (dev_priv->card_type >= NV_50) {
+		struct nv50_display *disp = nv50_display(dev);
+		for (i = 0; i < dev->mode_config.num_crtc; i++) {
+			struct nv50_display_crtc *dispc = &disp->crtc[i];
+			nouveau_bo_vma_del(dispc->sem.bo, &pch->dispc_vma[i]);
+		}
+	}
+
+	chan->engctx[engine] = NULL;
+	kfree(pch);
+}
+
+static int
+nvc0_software_object_new(struct nouveau_channel *chan, int engine,
+			 u32 handle, u16 class)
+{
+	return 0;
+}
+
+static int
+nvc0_software_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static int
+nvc0_software_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nvc0_software_destroy(struct drm_device *dev, int engine)
+{
+	struct nvc0_software_priv *psw = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, SW);
+	kfree(psw);
+}
+
+int
+nvc0_software_create(struct drm_device *dev)
+{
+	struct nvc0_software_priv *psw = kzalloc(sizeof(*psw), GFP_KERNEL);
+	if (!psw)
+		return -ENOMEM;
+
+	psw->base.base.destroy = nvc0_software_destroy;
+	psw->base.base.init = nvc0_software_init;
+	psw->base.base.fini = nvc0_software_fini;
+	psw->base.base.context_new = nvc0_software_context_new;
+	psw->base.base.context_del = nvc0_software_context_del;
+	psw->base.base.object_new = nvc0_software_object_new;
+	nouveau_software_create(&psw->base);
+
+	NVOBJ_ENGINE_ADD(dev, SW, &psw->base.base);
+	NVOBJ_CLASS(dev, 0x906e, SW);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nvd0_display.c b/drivers/gpu/drm/nouveau/nvd0_display.c
index 0247250939e8..c486d3ce3c2c 100644
--- a/drivers/gpu/drm/nouveau/nvd0_display.c
+++ b/drivers/gpu/drm/nouveau/nvd0_display.c
@@ -33,6 +33,7 @@
 #include "nouveau_crtc.h"
 #include "nouveau_dma.h"
 #include "nouveau_fb.h"
+#include "nouveau_software.h"
 #include "nv50_display.h"
 
 #define EVO_DMA_NR 9
@@ -284,8 +285,6 @@ nvd0_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	u32 *push;
 	int ret;
 
-	evo_sync(crtc->dev, EVO_MASTER);
-
 	swap_interval <<= 4;
 	if (swap_interval == 0)
 		swap_interval |= 0x100;
@@ -300,15 +299,16 @@ nvd0_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		if (ret)
 			return ret;
 
-		offset  = chan->dispc_vma[nv_crtc->index].offset;
+
+		offset  = nvc0_software_crtc(chan, nv_crtc->index);
 		offset += evo->sem.offset;
 
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
 		OUT_RING  (chan, upper_32_bits(offset));
 		OUT_RING  (chan, lower_32_bits(offset));
 		OUT_RING  (chan, 0xf00d0000 | evo->sem.value);
 		OUT_RING  (chan, 0x1002);
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
 		OUT_RING  (chan, upper_32_bits(offset));
 		OUT_RING  (chan, lower_32_bits(offset ^ 0x10));
 		OUT_RING  (chan, 0x74b1e000);
@@ -882,7 +882,7 @@ nvd0_crtc_create(struct drm_device *dev, int index)
 	drm_mode_crtc_set_gamma_size(crtc, 256);
 
 	ret = nouveau_bo_new(dev, 64 * 64 * 4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -895,7 +895,7 @@ nvd0_crtc_create(struct drm_device *dev, int index)
 		goto out;
 
 	ret = nouveau_bo_new(dev, 8192, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->lut.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->lut.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->lut.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -2030,7 +2030,7 @@ nvd0_display_create(struct drm_device *dev)
 
 	/* small shared memory area we use for notifiers and semaphores */
 	ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &disp->sync);
+			     0, 0x0000, NULL, &disp->sync);
 	if (!ret) {
 		ret = nouveau_bo_pin(disp->sync, TTM_PL_FLAG_VRAM);
 		if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nve0_fifo.c b/drivers/gpu/drm/nouveau/nve0_fifo.c
new file mode 100644
index 000000000000..1855ecbd843b
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_fifo.c
@@ -0,0 +1,423 @@
+/*
+ * Copyright 2010 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_mm.h"
+#include "nouveau_fifo.h"
+
+#define NVE0_FIFO_ENGINE_NUM 32
+
+static void nve0_fifo_isr(struct drm_device *);
+
+struct nve0_fifo_engine {
+	struct nouveau_gpuobj *playlist[2];
+	int cur_playlist;
+};
+
+struct nve0_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct nve0_fifo_engine engine[NVE0_FIFO_ENGINE_NUM];
+	struct {
+		struct nouveau_gpuobj *mem;
+		struct nouveau_vma bar;
+	} user;
+	int spoon_nr;
+};
+
+struct nve0_fifo_chan {
+	struct nouveau_fifo_chan base;
+	u32 engine;
+};
+
+static void
+nve0_fifo_playlist_update(struct drm_device *dev, u32 engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
+	struct nve0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct nve0_fifo_engine *peng = &priv->engine[engine];
+	struct nouveau_gpuobj *cur;
+	u32 match = (engine << 16) | 0x00000001;
+	int ret, i, p;
+
+	cur = peng->playlist[peng->cur_playlist];
+	if (unlikely(cur == NULL)) {
+		ret = nouveau_gpuobj_new(dev, NULL, 0x8000, 0x1000, 0, &cur);
+		if (ret) {
+			NV_ERROR(dev, "PFIFO: playlist alloc failed\n");
+			return;
+		}
+
+		peng->playlist[peng->cur_playlist] = cur;
+	}
+
+	peng->cur_playlist = !peng->cur_playlist;
+
+	for (i = 0, p = 0; i < priv->base.channels; i++) {
+		u32 ctrl = nv_rd32(dev, 0x800004 + (i * 8)) & 0x001f0001;
+		if (ctrl != match)
+			continue;
+		nv_wo32(cur, p + 0, i);
+		nv_wo32(cur, p + 4, 0x00000000);
+		p += 8;
+	}
+	pinstmem->flush(dev);
+
+	nv_wr32(dev, 0x002270, cur->vinst >> 12);
+	nv_wr32(dev, 0x002274, (engine << 20) | (p >> 3));
+	if (!nv_wait(dev, 0x002284 + (engine * 4), 0x00100000, 0x00000000))
+		NV_ERROR(dev, "PFIFO: playlist %d update timeout\n", engine);
+}
+
+static int
+nve0_fifo_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	struct nve0_fifo_chan *fctx;
+	u64 usermem = priv->user.mem->vinst + chan->id * 512;
+	u64 ib_virt = chan->pushbuf_base + chan->dma.ib_base * 4;
+	int ret = 0, i;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	fctx->engine = 0; /* PGRAPH */
+
+	/* allocate vram for control regs, map into polling area */
+	chan->user = ioremap_wc(pci_resource_start(dev->pdev, 1) +
+				priv->user.bar.offset + (chan->id * 512), 512);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	for (i = 0; i < 0x100; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+	nv_wo32(chan->ramin, 0x08, lower_32_bits(usermem));
+	nv_wo32(chan->ramin, 0x0c, upper_32_bits(usermem));
+	nv_wo32(chan->ramin, 0x10, 0x0000face);
+	nv_wo32(chan->ramin, 0x30, 0xfffff902);
+	nv_wo32(chan->ramin, 0x48, lower_32_bits(ib_virt));
+	nv_wo32(chan->ramin, 0x4c, drm_order(chan->dma.ib_max + 1) << 16 |
+				     upper_32_bits(ib_virt));
+	nv_wo32(chan->ramin, 0x84, 0x20400000);
+	nv_wo32(chan->ramin, 0x94, 0x30000001);
+	nv_wo32(chan->ramin, 0x9c, 0x00000100);
+	nv_wo32(chan->ramin, 0xac, 0x0000001f);
+	nv_wo32(chan->ramin, 0xe4, 0x00000000);
+	nv_wo32(chan->ramin, 0xe8, chan->id);
+	nv_wo32(chan->ramin, 0xf8, 0x10003080); /* 0x002310 */
+	nv_wo32(chan->ramin, 0xfc, 0x10000010); /* 0x002350 */
+	pinstmem->flush(dev);
+
+	nv_wr32(dev, 0x800000 + (chan->id * 8), 0x80000000 |
+						(chan->ramin->vinst >> 12));
+	nv_mask(dev, 0x800004 + (chan->id * 8), 0x00000400, 0x00000400);
+	nve0_fifo_playlist_update(dev, fctx->engine);
+	nv_mask(dev, 0x800004 + (chan->id * 8), 0x00000400, 0x00000400);
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nve0_fifo_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nve0_fifo_chan *fctx = chan->engctx[engine];
+	struct drm_device *dev = chan->dev;
+
+	nv_mask(dev, 0x800004 + (chan->id * 8), 0x00000800, 0x00000800);
+	nv_wr32(dev, 0x002634, chan->id);
+	if (!nv_wait(dev, 0x0002634, 0xffffffff, chan->id))
+		NV_WARN(dev, "0x2634 != chid: 0x%08x\n", nv_rd32(dev, 0x2634));
+	nve0_fifo_playlist_update(dev, fctx->engine);
+	nv_wr32(dev, 0x800000 + (chan->id * 8), 0x00000000);
+
+	if (chan->user) {
+		iounmap(chan->user);
+		chan->user = NULL;
+	}
+
+	chan->engctx[NVOBJ_ENGINE_FIFO] = NULL;
+	kfree(fctx);
+}
+
+static int
+nve0_fifo_init(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	struct nve0_fifo_chan *fctx;
+	int i;
+
+	/* reset PFIFO, enable all available PSUBFIFO areas */
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000100);
+	nv_wr32(dev, 0x000204, 0xffffffff);
+
+	priv->spoon_nr = hweight32(nv_rd32(dev, 0x000204));
+	NV_DEBUG(dev, "PFIFO: %d subfifo(s)\n", priv->spoon_nr);
+
+	/* PSUBFIFO[n] */
+	for (i = 0; i < priv->spoon_nr; i++) {
+		nv_mask(dev, 0x04013c + (i * 0x2000), 0x10000100, 0x00000000);
+		nv_wr32(dev, 0x040108 + (i * 0x2000), 0xffffffff); /* INTR */
+		nv_wr32(dev, 0x04010c + (i * 0x2000), 0xfffffeff); /* INTR_EN */
+	}
+
+	nv_wr32(dev, 0x002254, 0x10000000 | priv->user.bar.offset >> 12);
+
+	nv_wr32(dev, 0x002a00, 0xffffffff);
+	nv_wr32(dev, 0x002100, 0xffffffff);
+	nv_wr32(dev, 0x002140, 0xbfffffff);
+
+	/* restore PFIFO context table */
+	for (i = 0; i < priv->base.channels; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (!chan || !(fctx = chan->engctx[engine]))
+			continue;
+
+		nv_wr32(dev, 0x800000 + (i * 8), 0x80000000 |
+						 (chan->ramin->vinst >> 12));
+		nv_mask(dev, 0x800004 + (i * 8), 0x00000400, 0x00000400);
+		nve0_fifo_playlist_update(dev, fctx->engine);
+		nv_mask(dev, 0x800004 + (i * 8), 0x00000400, 0x00000400);
+	}
+
+	return 0;
+}
+
+static int
+nve0_fifo_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (!(nv_rd32(dev, 0x800004 + (i * 8)) & 1))
+			continue;
+
+		nv_mask(dev, 0x800004 + (i * 8), 0x00000800, 0x00000800);
+		nv_wr32(dev, 0x002634, i);
+		if (!nv_wait(dev, 0x002634, 0xffffffff, i)) {
+			NV_INFO(dev, "PFIFO: kick ch %d failed: 0x%08x\n",
+				i, nv_rd32(dev, 0x002634));
+			return -EBUSY;
+		}
+	}
+
+	nv_wr32(dev, 0x002140, 0x00000000);
+	return 0;
+}
+
+struct nouveau_enum nve0_fifo_fault_unit[] = {
+	{}
+};
+
+struct nouveau_enum nve0_fifo_fault_reason[] = {
+	{ 0x00, "PT_NOT_PRESENT" },
+	{ 0x01, "PT_TOO_SHORT" },
+	{ 0x02, "PAGE_NOT_PRESENT" },
+	{ 0x03, "VM_LIMIT_EXCEEDED" },
+	{ 0x04, "NO_CHANNEL" },
+	{ 0x05, "PAGE_SYSTEM_ONLY" },
+	{ 0x06, "PAGE_READ_ONLY" },
+	{ 0x0a, "COMPRESSED_SYSRAM" },
+	{ 0x0c, "INVALID_STORAGE_TYPE" },
+	{}
+};
+
+struct nouveau_enum nve0_fifo_fault_hubclient[] = {
+	{}
+};
+
+struct nouveau_enum nve0_fifo_fault_gpcclient[] = {
+	{}
+};
+
+struct nouveau_bitfield nve0_fifo_subfifo_intr[] = {
+	{ 0x00200000, "ILLEGAL_MTHD" },
+	{ 0x00800000, "EMPTY_SUBC" },
+	{}
+};
+
+static void
+nve0_fifo_isr_vm_fault(struct drm_device *dev, int unit)
+{
+	u32 inst = nv_rd32(dev, 0x2800 + (unit * 0x10));
+	u32 valo = nv_rd32(dev, 0x2804 + (unit * 0x10));
+	u32 vahi = nv_rd32(dev, 0x2808 + (unit * 0x10));
+	u32 stat = nv_rd32(dev, 0x280c + (unit * 0x10));
+	u32 client = (stat & 0x00001f00) >> 8;
+
+	NV_INFO(dev, "PFIFO: %s fault at 0x%010llx [",
+		(stat & 0x00000080) ? "write" : "read", (u64)vahi << 32 | valo);
+	nouveau_enum_print(nve0_fifo_fault_reason, stat & 0x0000000f);
+	printk("] from ");
+	nouveau_enum_print(nve0_fifo_fault_unit, unit);
+	if (stat & 0x00000040) {
+		printk("/");
+		nouveau_enum_print(nve0_fifo_fault_hubclient, client);
+	} else {
+		printk("/GPC%d/", (stat & 0x1f000000) >> 24);
+		nouveau_enum_print(nve0_fifo_fault_gpcclient, client);
+	}
+	printk(" on channel 0x%010llx\n", (u64)inst << 12);
+}
+
+static void
+nve0_fifo_isr_subfifo_intr(struct drm_device *dev, int unit)
+{
+	u32 stat = nv_rd32(dev, 0x040108 + (unit * 0x2000));
+	u32 addr = nv_rd32(dev, 0x0400c0 + (unit * 0x2000));
+	u32 data = nv_rd32(dev, 0x0400c4 + (unit * 0x2000));
+	u32 chid = nv_rd32(dev, 0x040120 + (unit * 0x2000)) & 0x7f;
+	u32 subc = (addr & 0x00070000);
+	u32 mthd = (addr & 0x00003ffc);
+
+	NV_INFO(dev, "PSUBFIFO %d:", unit);
+	nouveau_bitfield_print(nve0_fifo_subfifo_intr, stat);
+	NV_INFO(dev, "PSUBFIFO %d: ch %d subc %d mthd 0x%04x data 0x%08x\n",
+		unit, chid, subc, mthd, data);
+
+	nv_wr32(dev, 0x0400c0 + (unit * 0x2000), 0x80600008);
+	nv_wr32(dev, 0x040108 + (unit * 0x2000), stat);
+}
+
+static void
+nve0_fifo_isr(struct drm_device *dev)
+{
+	u32 stat = nv_rd32(dev, 0x002100);
+
+	if (stat & 0x00000100) {
+		NV_INFO(dev, "PFIFO: unknown status 0x00000100\n");
+		nv_wr32(dev, 0x002100, 0x00000100);
+		stat &= ~0x00000100;
+	}
+
+	if (stat & 0x10000000) {
+		u32 units = nv_rd32(dev, 0x00259c);
+		u32 u = units;
+
+		while (u) {
+			int i = ffs(u) - 1;
+			nve0_fifo_isr_vm_fault(dev, i);
+			u &= ~(1 << i);
+		}
+
+		nv_wr32(dev, 0x00259c, units);
+		stat &= ~0x10000000;
+	}
+
+	if (stat & 0x20000000) {
+		u32 units = nv_rd32(dev, 0x0025a0);
+		u32 u = units;
+
+		while (u) {
+			int i = ffs(u) - 1;
+			nve0_fifo_isr_subfifo_intr(dev, i);
+			u &= ~(1 << i);
+		}
+
+		nv_wr32(dev, 0x0025a0, units);
+		stat &= ~0x20000000;
+	}
+
+	if (stat & 0x40000000) {
+		NV_INFO(dev, "PFIFO: unknown status 0x40000000\n");
+		nv_mask(dev, 0x002a00, 0x00000000, 0x00000000);
+		stat &= ~0x40000000;
+	}
+
+	if (stat) {
+		NV_INFO(dev, "PFIFO: unhandled status 0x%08x\n", stat);
+		nv_wr32(dev, 0x002100, stat);
+		nv_wr32(dev, 0x002140, 0);
+	}
+}
+
+static void
+nve0_fifo_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	nouveau_vm_put(&priv->user.bar);
+	nouveau_gpuobj_ref(NULL, &priv->user.mem);
+
+	for (i = 0; i < NVE0_FIFO_ENGINE_NUM; i++) {
+		nouveau_gpuobj_ref(NULL, &priv->engine[i].playlist[0]);
+		nouveau_gpuobj_ref(NULL, &priv->engine[i].playlist[1]);
+	}
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nve0_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_fifo_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nve0_fifo_destroy;
+	priv->base.base.init = nve0_fifo_init;
+	priv->base.base.fini = nve0_fifo_fini;
+	priv->base.base.context_new = nve0_fifo_context_new;
+	priv->base.base.context_del = nve0_fifo_context_del;
+	priv->base.channels = 4096;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 512, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->user.mem);
+	if (ret)
+		goto error;
+
+	ret = nouveau_vm_get(dev_priv->bar1_vm, priv->user.mem->size,
+			     12, NV_MEM_ACCESS_RW, &priv->user.bar);
+	if (ret)
+		goto error;
+
+	nouveau_vm_map(&priv->user.bar, *(struct nouveau_mem **)priv->user.mem->node);
+
+	nouveau_irq_register(dev, 8, nve0_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nve0_graph.c b/drivers/gpu/drm/nouveau/nve0_graph.c
new file mode 100644
index 000000000000..8a8051b68f10
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_graph.c
@@ -0,0 +1,831 @@
+/*
+ * Copyright 2010 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <linux/firmware.h>
+#include <linux/module.h>
+
+#include "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_mm.h"
+#include "nouveau_fifo.h"
+
+#include "nve0_graph.h"
+
+static void
+nve0_graph_ctxctl_debug_unit(struct drm_device *dev, u32 base)
+{
+	NV_INFO(dev, "PGRAPH: %06x - done 0x%08x\n", base,
+		nv_rd32(dev, base + 0x400));
+	NV_INFO(dev, "PGRAPH: %06x - stat 0x%08x 0x%08x 0x%08x 0x%08x\n", base,
+		nv_rd32(dev, base + 0x800), nv_rd32(dev, base + 0x804),
+		nv_rd32(dev, base + 0x808), nv_rd32(dev, base + 0x80c));
+	NV_INFO(dev, "PGRAPH: %06x - stat 0x%08x 0x%08x 0x%08x 0x%08x\n", base,
+		nv_rd32(dev, base + 0x810), nv_rd32(dev, base + 0x814),
+		nv_rd32(dev, base + 0x818), nv_rd32(dev, base + 0x81c));
+}
+
+static void
+nve0_graph_ctxctl_debug(struct drm_device *dev)
+{
+	u32 gpcnr = nv_rd32(dev, 0x409604) & 0xffff;
+	u32 gpc;
+
+	nve0_graph_ctxctl_debug_unit(dev, 0x409000);
+	for (gpc = 0; gpc < gpcnr; gpc++)
+		nve0_graph_ctxctl_debug_unit(dev, 0x502000 + (gpc * 0x8000));
+}
+
+static int
+nve0_graph_load_context(struct nouveau_channel *chan)
+{
+	struct drm_device *dev = chan->dev;
+
+	nv_wr32(dev, 0x409840, 0x00000030);
+	nv_wr32(dev, 0x409500, 0x80000000 | chan->ramin->vinst >> 12);
+	nv_wr32(dev, 0x409504, 0x00000003);
+	if (!nv_wait(dev, 0x409800, 0x00000010, 0x00000010))
+		NV_ERROR(dev, "PGRAPH: load_ctx timeout\n");
+
+	return 0;
+}
+
+static int
+nve0_graph_unload_context_to(struct drm_device *dev, u64 chan)
+{
+	nv_wr32(dev, 0x409840, 0x00000003);
+	nv_wr32(dev, 0x409500, 0x80000000 | chan >> 12);
+	nv_wr32(dev, 0x409504, 0x00000009);
+	if (!nv_wait(dev, 0x409800, 0x00000001, 0x00000000)) {
+		NV_ERROR(dev, "PGRAPH: unload_ctx timeout\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+static int
+nve0_graph_construct_context(struct nouveau_channel *chan)
+{
+	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
+	struct nve0_graph_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_GR);
+	struct nve0_graph_chan *grch = chan->engctx[NVOBJ_ENGINE_GR];
+	struct drm_device *dev = chan->dev;
+	int ret, i;
+	u32 *ctx;
+
+	ctx = kmalloc(priv->grctx_size, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	nve0_graph_load_context(chan);
+
+	nv_wo32(grch->grctx, 0x1c, 1);
+	nv_wo32(grch->grctx, 0x20, 0);
+	nv_wo32(grch->grctx, 0x28, 0);
+	nv_wo32(grch->grctx, 0x2c, 0);
+	dev_priv->engine.instmem.flush(dev);
+
+	ret = nve0_grctx_generate(chan);
+	if (ret)
+		goto err;
+
+	ret = nve0_graph_unload_context_to(dev, chan->ramin->vinst);
+	if (ret)
+		goto err;
+
+	for (i = 0; i < priv->grctx_size; i += 4)
+		ctx[i / 4] = nv_ro32(grch->grctx, i);
+
+	priv->grctx_vals = ctx;
+	return 0;
+
+err:
+	kfree(ctx);
+	return ret;
+}
+
+static int
+nve0_graph_create_context_mmio_list(struct nouveau_channel *chan)
+{
+	struct nve0_graph_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_GR);
+	struct nve0_graph_chan *grch = chan->engctx[NVOBJ_ENGINE_GR];
+	struct drm_device *dev = chan->dev;
+	u32 magic[GPC_MAX][2];
+	u16 offset = 0x0000;
+	int gpc;
+	int ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 0x3000, 256, NVOBJ_FLAG_VM,
+				 &grch->unk408004);
+	if (ret)
+		return ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 0x8000, 256, NVOBJ_FLAG_VM,
+				 &grch->unk40800c);
+	if (ret)
+		return ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 384 * 1024, 4096,
+				 NVOBJ_FLAG_VM | NVOBJ_FLAG_VM_USER,
+				 &grch->unk418810);
+	if (ret)
+		return ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 0x1000, 0, NVOBJ_FLAG_VM,
+				 &grch->mmio);
+	if (ret)
+		return ret;
+
+#define mmio(r,v) do {                                                         \
+	nv_wo32(grch->mmio, (grch->mmio_nr * 8) + 0, (r));                     \
+	nv_wo32(grch->mmio, (grch->mmio_nr * 8) + 4, (v));                     \
+	grch->mmio_nr++;                                                       \
+} while (0)
+	mmio(0x40800c, grch->unk40800c->linst >> 8);
+	mmio(0x408010, 0x80000000);
+	mmio(0x419004, grch->unk40800c->linst >> 8);
+	mmio(0x419008, 0x00000000);
+	mmio(0x4064cc, 0x80000000);
+	mmio(0x408004, grch->unk408004->linst >> 8);
+	mmio(0x408008, 0x80000030);
+	mmio(0x418808, grch->unk408004->linst >> 8);
+	mmio(0x41880c, 0x80000030);
+	mmio(0x4064c8, 0x01800600);
+	mmio(0x418810, 0x80000000 | grch->unk418810->linst >> 12);
+	mmio(0x419848, 0x10000000 | grch->unk418810->linst >> 12);
+	mmio(0x405830, 0x02180648);
+	mmio(0x4064c4, 0x0192ffff);
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		u16 magic0 = 0x0218 * priv->tpc_nr[gpc];
+		u16 magic1 = 0x0648 * priv->tpc_nr[gpc];
+		magic[gpc][0]  = 0x10000000 | (magic0 << 16) | offset;
+		magic[gpc][1]  = 0x00000000 | (magic1 << 16);
+		offset += 0x0324 * priv->tpc_nr[gpc];
+	}
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		mmio(GPC_UNIT(gpc, 0x30c0), magic[gpc][0]);
+		mmio(GPC_UNIT(gpc, 0x30e4), magic[gpc][1] | offset);
+		offset += 0x07ff * priv->tpc_nr[gpc];
+	}
+
+	mmio(0x17e91c, 0x06060609);
+	mmio(0x17e920, 0x00090a05);
+#undef mmio
+	return 0;
+}
+
+static int
+nve0_graph_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
+	struct nve0_graph_priv *priv = nv_engine(dev, engine);
+	struct nve0_graph_chan *grch;
+	struct nouveau_gpuobj *grctx;
+	int ret, i;
+
+	grch = kzalloc(sizeof(*grch), GFP_KERNEL);
+	if (!grch)
+		return -ENOMEM;
+	chan->engctx[NVOBJ_ENGINE_GR] = grch;
+
+	ret = nouveau_gpuobj_new(dev, chan, priv->grctx_size, 256,
+				 NVOBJ_FLAG_VM | NVOBJ_FLAG_ZERO_ALLOC,
+				 &grch->grctx);
+	if (ret)
+		goto error;
+	grctx = grch->grctx;
+
+	ret = nve0_graph_create_context_mmio_list(chan);
+	if (ret)
+		goto error;
+
+	nv_wo32(chan->ramin, 0x0210, lower_32_bits(grctx->linst) | 4);
+	nv_wo32(chan->ramin, 0x0214, upper_32_bits(grctx->linst));
+	pinstmem->flush(dev);
+
+	if (!priv->grctx_vals) {
+		ret = nve0_graph_construct_context(chan);
+		if (ret)
+			goto error;
+	}
+
+	for (i = 0; i < priv->grctx_size; i += 4)
+		nv_wo32(grctx, i, priv->grctx_vals[i / 4]);
+	nv_wo32(grctx, 0xf4, 0);
+	nv_wo32(grctx, 0xf8, 0);
+	nv_wo32(grctx, 0x10, grch->mmio_nr);
+	nv_wo32(grctx, 0x14, lower_32_bits(grch->mmio->linst));
+	nv_wo32(grctx, 0x18, upper_32_bits(grch->mmio->linst));
+	nv_wo32(grctx, 0x1c, 1);
+	nv_wo32(grctx, 0x20, 0);
+	nv_wo32(grctx, 0x28, 0);
+	nv_wo32(grctx, 0x2c, 0);
+
+	pinstmem->flush(dev);
+	return 0;
+
+error:
+	priv->base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nve0_graph_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nve0_graph_chan *grch = chan->engctx[engine];
+
+	nouveau_gpuobj_ref(NULL, &grch->mmio);
+	nouveau_gpuobj_ref(NULL, &grch->unk418810);
+	nouveau_gpuobj_ref(NULL, &grch->unk40800c);
+	nouveau_gpuobj_ref(NULL, &grch->unk408004);
+	nouveau_gpuobj_ref(NULL, &grch->grctx);
+	chan->engctx[engine] = NULL;
+}
+
+static int
+nve0_graph_object_new(struct nouveau_channel *chan, int engine,
+		      u32 handle, u16 class)
+{
+	return 0;
+}
+
+static int
+nve0_graph_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nve0_graph_init_obj418880(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	int i;
+
+	nv_wr32(dev, GPC_BCAST(0x0880), 0x00000000);
+	nv_wr32(dev, GPC_BCAST(0x08a4), 0x00000000);
+	for (i = 0; i < 4; i++)
+		nv_wr32(dev, GPC_BCAST(0x0888) + (i * 4), 0x00000000);
+	nv_wr32(dev, GPC_BCAST(0x08b4), priv->unk4188b4->vinst >> 8);
+	nv_wr32(dev, GPC_BCAST(0x08b8), priv->unk4188b8->vinst >> 8);
+}
+
+static void
+nve0_graph_init_regs(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x400080, 0x003083c2);
+	nv_wr32(dev, 0x400088, 0x0001ffe7);
+	nv_wr32(dev, 0x40008c, 0x00000000);
+	nv_wr32(dev, 0x400090, 0x00000030);
+	nv_wr32(dev, 0x40013c, 0x003901f7);
+	nv_wr32(dev, 0x400140, 0x00000100);
+	nv_wr32(dev, 0x400144, 0x00000000);
+	nv_wr32(dev, 0x400148, 0x00000110);
+	nv_wr32(dev, 0x400138, 0x00000000);
+	nv_wr32(dev, 0x400130, 0x00000000);
+	nv_wr32(dev, 0x400134, 0x00000000);
+	nv_wr32(dev, 0x400124, 0x00000002);
+}
+
+static void
+nve0_graph_init_units(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x409ffc, 0x00000000);
+	nv_wr32(dev, 0x409c14, 0x00003e3e);
+	nv_wr32(dev, 0x409c24, 0x000f0000);
+
+	nv_wr32(dev, 0x404000, 0xc0000000);
+	nv_wr32(dev, 0x404600, 0xc0000000);
+	nv_wr32(dev, 0x408030, 0xc0000000);
+	nv_wr32(dev, 0x404490, 0xc0000000);
+	nv_wr32(dev, 0x406018, 0xc0000000);
+	nv_wr32(dev, 0x407020, 0xc0000000);
+	nv_wr32(dev, 0x405840, 0xc0000000);
+	nv_wr32(dev, 0x405844, 0x00ffffff);
+
+	nv_mask(dev, 0x419cc0, 0x00000008, 0x00000008);
+	nv_mask(dev, 0x419eb4, 0x00001000, 0x00001000);
+
+}
+
+static void
+nve0_graph_init_gpc_0(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	const u32 magicgpc918 = DIV_ROUND_UP(0x00800000, priv->tpc_total);
+	u32 data[TPC_MAX / 8];
+	u8  tpcnr[GPC_MAX];
+	int i, gpc, tpc;
+
+	nv_wr32(dev, GPC_UNIT(0, 0x3018), 0x00000001);
+
+	memset(data, 0x00, sizeof(data));
+	memcpy(tpcnr, priv->tpc_nr, sizeof(priv->tpc_nr));
+	for (i = 0, gpc = -1; i < priv->tpc_total; i++) {
+		do {
+			gpc = (gpc + 1) % priv->gpc_nr;
+		} while (!tpcnr[gpc]);
+		tpc = priv->tpc_nr[gpc] - tpcnr[gpc]--;
+
+		data[i / 8] |= tpc << ((i % 8) * 4);
+	}
+
+	nv_wr32(dev, GPC_BCAST(0x0980), data[0]);
+	nv_wr32(dev, GPC_BCAST(0x0984), data[1]);
+	nv_wr32(dev, GPC_BCAST(0x0988), data[2]);
+	nv_wr32(dev, GPC_BCAST(0x098c), data[3]);
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0914), priv->magic_not_rop_nr << 8 |
+						  priv->tpc_nr[gpc]);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0910), 0x00040000 | priv->tpc_total);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0918), magicgpc918);
+	}
+
+	nv_wr32(dev, GPC_BCAST(0x1bd4), magicgpc918);
+	nv_wr32(dev, GPC_BCAST(0x08ac), nv_rd32(dev, 0x100800));
+}
+
+static void
+nve0_graph_init_gpc_1(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	int gpc, tpc;
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		nv_wr32(dev, GPC_UNIT(gpc, 0x3038), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0420), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0900), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x1028), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0824), 0xc0000000);
+		for (tpc = 0; tpc < priv->tpc_nr[gpc]; tpc++) {
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x508), 0xffffffff);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x50c), 0xffffffff);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x224), 0xc0000000);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x48c), 0xc0000000);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x084), 0xc0000000);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x644), 0x001ffffe);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x64c), 0x0000000f);
+		}
+		nv_wr32(dev, GPC_UNIT(gpc, 0x2c90), 0xffffffff);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x2c94), 0xffffffff);
+	}
+}
+
+static void
+nve0_graph_init_rop(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	int rop;
+
+	for (rop = 0; rop < priv->rop_nr; rop++) {
+		nv_wr32(dev, ROP_UNIT(rop, 0x144), 0xc0000000);
+		nv_wr32(dev, ROP_UNIT(rop, 0x070), 0xc0000000);
+		nv_wr32(dev, ROP_UNIT(rop, 0x204), 0xffffffff);
+		nv_wr32(dev, ROP_UNIT(rop, 0x208), 0xffffffff);
+	}
+}
+
+static void
+nve0_graph_init_fuc(struct drm_device *dev, u32 fuc_base,
+		    struct nve0_graph_fuc *code, struct nve0_graph_fuc *data)
+{
+	int i;
+
+	nv_wr32(dev, fuc_base + 0x01c0, 0x01000000);
+	for (i = 0; i < data->size / 4; i++)
+		nv_wr32(dev, fuc_base + 0x01c4, data->data[i]);
+
+	nv_wr32(dev, fuc_base + 0x0180, 0x01000000);
+	for (i = 0; i < code->size / 4; i++) {
+		if ((i & 0x3f) == 0)
+			nv_wr32(dev, fuc_base + 0x0188, i >> 6);
+		nv_wr32(dev, fuc_base + 0x0184, code->data[i]);
+	}
+}
+
+static int
+nve0_graph_init_ctxctl(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	u32 r000260;
+
+	/* load fuc microcode */
+	r000260 = nv_mask(dev, 0x000260, 0x00000001, 0x00000000);
+	nve0_graph_init_fuc(dev, 0x409000, &priv->fuc409c, &priv->fuc409d);
+	nve0_graph_init_fuc(dev, 0x41a000, &priv->fuc41ac, &priv->fuc41ad);
+	nv_wr32(dev, 0x000260, r000260);
+
+	/* start both of them running */
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x41a10c, 0x00000000);
+	nv_wr32(dev, 0x40910c, 0x00000000);
+	nv_wr32(dev, 0x41a100, 0x00000002);
+	nv_wr32(dev, 0x409100, 0x00000002);
+	if (!nv_wait(dev, 0x409800, 0x00000001, 0x00000001))
+		NV_INFO(dev, "0x409800 wait failed\n");
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x7fffffff);
+	nv_wr32(dev, 0x409504, 0x00000021);
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x00000000);
+	nv_wr32(dev, 0x409504, 0x00000010);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x10 timeout\n");
+		return -EBUSY;
+	}
+	priv->grctx_size = nv_rd32(dev, 0x409800);
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x00000000);
+	nv_wr32(dev, 0x409504, 0x00000016);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x16 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x00000000);
+	nv_wr32(dev, 0x409504, 0x00000025);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x25 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409800, 0x00000000);
+	nv_wr32(dev, 0x409500, 0x00000001);
+	nv_wr32(dev, 0x409504, 0x00000030);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x30 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409810, 0xb00095c8);
+	nv_wr32(dev, 0x409800, 0x00000000);
+	nv_wr32(dev, 0x409500, 0x00000001);
+	nv_wr32(dev, 0x409504, 0x00000031);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x31 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409810, 0x00080420);
+	nv_wr32(dev, 0x409800, 0x00000000);
+	nv_wr32(dev, 0x409500, 0x00000001);
+	nv_wr32(dev, 0x409504, 0x00000032);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x32 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409614, 0x00000070);
+	nv_wr32(dev, 0x409614, 0x00000770);
+	nv_wr32(dev, 0x40802c, 0x00000001);
+	return 0;
+}
+
+static int
+nve0_graph_init(struct drm_device *dev, int engine)
+{
+	int ret;
+
+	nv_mask(dev, 0x000200, 0x18001000, 0x00000000);
+	nv_mask(dev, 0x000200, 0x18001000, 0x18001000);
+
+	nve0_graph_init_obj418880(dev);
+	nve0_graph_init_regs(dev);
+	nve0_graph_init_gpc_0(dev);
+
+	nv_wr32(dev, 0x400500, 0x00010001);
+	nv_wr32(dev, 0x400100, 0xffffffff);
+	nv_wr32(dev, 0x40013c, 0xffffffff);
+
+	nve0_graph_init_units(dev);
+	nve0_graph_init_gpc_1(dev);
+	nve0_graph_init_rop(dev);
+
+	nv_wr32(dev, 0x400108, 0xffffffff);
+	nv_wr32(dev, 0x400138, 0xffffffff);
+	nv_wr32(dev, 0x400118, 0xffffffff);
+	nv_wr32(dev, 0x400130, 0xffffffff);
+	nv_wr32(dev, 0x40011c, 0xffffffff);
+	nv_wr32(dev, 0x400134, 0xffffffff);
+	nv_wr32(dev, 0x400054, 0x34ce3464);
+
+	ret = nve0_graph_init_ctxctl(dev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int
+nve0_graph_isr_chid(struct drm_device *dev, u64 inst)
+{
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_channel *chan;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&dev_priv->channels.lock, flags);
+	for (i = 0; i < pfifo->channels; i++) {
+		chan = dev_priv->channels.ptr[i];
+		if (!chan || !chan->ramin)
+			continue;
+
+		if (inst == chan->ramin->vinst)
+			break;
+	}
+	spin_unlock_irqrestore(&dev_priv->channels.lock, flags);
+	return i;
+}
+
+static void
+nve0_graph_ctxctl_isr(struct drm_device *dev)
+{
+	u32 ustat = nv_rd32(dev, 0x409c18);
+
+	if (ustat & 0x00000001)
+		NV_INFO(dev, "PGRAPH: CTXCTRL ucode error\n");
+	if (ustat & 0x00080000)
+		NV_INFO(dev, "PGRAPH: CTXCTRL watchdog timeout\n");
+	if (ustat & ~0x00080001)
+		NV_INFO(dev, "PGRAPH: CTXCTRL 0x%08x\n", ustat);
+
+	nve0_graph_ctxctl_debug(dev);
+	nv_wr32(dev, 0x409c20, ustat);
+}
+
+static void
+nve0_graph_trap_isr(struct drm_device *dev, int chid)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	u32 trap = nv_rd32(dev, 0x400108);
+	int rop;
+
+	if (trap & 0x00000001) {
+		u32 stat = nv_rd32(dev, 0x404000);
+		NV_INFO(dev, "PGRAPH: DISPATCH ch %d 0x%08x\n", chid, stat);
+		nv_wr32(dev, 0x404000, 0xc0000000);
+		nv_wr32(dev, 0x400108, 0x00000001);
+		trap &= ~0x00000001;
+	}
+
+	if (trap & 0x00000010) {
+		u32 stat = nv_rd32(dev, 0x405840);
+		NV_INFO(dev, "PGRAPH: SHADER ch %d 0x%08x\n", chid, stat);
+		nv_wr32(dev, 0x405840, 0xc0000000);
+		nv_wr32(dev, 0x400108, 0x00000010);
+		trap &= ~0x00000010;
+	}
+
+	if (trap & 0x02000000) {
+		for (rop = 0; rop < priv->rop_nr; rop++) {
+			u32 statz = nv_rd32(dev, ROP_UNIT(rop, 0x070));
+			u32 statc = nv_rd32(dev, ROP_UNIT(rop, 0x144));
+			NV_INFO(dev, "PGRAPH: ROP%d ch %d 0x%08x 0x%08x\n",
+				     rop, chid, statz, statc);
+			nv_wr32(dev, ROP_UNIT(rop, 0x070), 0xc0000000);
+			nv_wr32(dev, ROP_UNIT(rop, 0x144), 0xc0000000);
+		}
+		nv_wr32(dev, 0x400108, 0x02000000);
+		trap &= ~0x02000000;
+	}
+
+	if (trap) {
+		NV_INFO(dev, "PGRAPH: TRAP ch %d 0x%08x\n", chid, trap);
+		nv_wr32(dev, 0x400108, trap);
+	}
+}
+
+static void
+nve0_graph_isr(struct drm_device *dev)
+{
+	u64 inst = (u64)(nv_rd32(dev, 0x409b00) & 0x0fffffff) << 12;
+	u32 chid = nve0_graph_isr_chid(dev, inst);
+	u32 stat = nv_rd32(dev, 0x400100);
+	u32 addr = nv_rd32(dev, 0x400704);
+	u32 mthd = (addr & 0x00003ffc);
+	u32 subc = (addr & 0x00070000) >> 16;
+	u32 data = nv_rd32(dev, 0x400708);
+	u32 code = nv_rd32(dev, 0x400110);
+	u32 class = nv_rd32(dev, 0x404200 + (subc * 4));
+
+	if (stat & 0x00000010) {
+		if (nouveau_gpuobj_mthd_call2(dev, chid, class, mthd, data)) {
+			NV_INFO(dev, "PGRAPH: ILLEGAL_MTHD ch %d [0x%010llx] "
+				     "subc %d class 0x%04x mthd 0x%04x "
+				     "data 0x%08x\n",
+				chid, inst, subc, class, mthd, data);
+		}
+		nv_wr32(dev, 0x400100, 0x00000010);
+		stat &= ~0x00000010;
+	}
+
+	if (stat & 0x00000020) {
+		NV_INFO(dev, "PGRAPH: ILLEGAL_CLASS ch %d [0x%010llx] subc %d "
+			     "class 0x%04x mthd 0x%04x data 0x%08x\n",
+			chid, inst, subc, class, mthd, data);
+		nv_wr32(dev, 0x400100, 0x00000020);
+		stat &= ~0x00000020;
+	}
+
+	if (stat & 0x00100000) {
+		NV_INFO(dev, "PGRAPH: DATA_ERROR [");
+		nouveau_enum_print(nv50_data_error_names, code);
+		printk("] ch %d [0x%010llx] subc %d class 0x%04x "
+		       "mthd 0x%04x data 0x%08x\n",
+		       chid, inst, subc, class, mthd, data);
+		nv_wr32(dev, 0x400100, 0x00100000);
+		stat &= ~0x00100000;
+	}
+
+	if (stat & 0x00200000) {
+		nve0_graph_trap_isr(dev, chid);
+		nv_wr32(dev, 0x400100, 0x00200000);
+		stat &= ~0x00200000;
+	}
+
+	if (stat & 0x00080000) {
+		nve0_graph_ctxctl_isr(dev);
+		nv_wr32(dev, 0x400100, 0x00080000);
+		stat &= ~0x00080000;
+	}
+
+	if (stat) {
+		NV_INFO(dev, "PGRAPH: unknown stat 0x%08x\n", stat);
+		nv_wr32(dev, 0x400100, stat);
+	}
+
+	nv_wr32(dev, 0x400500, 0x00010001);
+}
+
+static int
+nve0_graph_create_fw(struct drm_device *dev, const char *fwname,
+		     struct nve0_graph_fuc *fuc)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	const struct firmware *fw;
+	char f[32];
+	int ret;
+
+	snprintf(f, sizeof(f), "nouveau/nv%02x_%s", dev_priv->chipset, fwname);
+	ret = request_firmware(&fw, f, &dev->pdev->dev);
+	if (ret)
+		return ret;
+
+	fuc->size = fw->size;
+	fuc->data = kmemdup(fw->data, fuc->size, GFP_KERNEL);
+	release_firmware(fw);
+	return (fuc->data != NULL) ? 0 : -ENOMEM;
+}
+
+static void
+nve0_graph_destroy_fw(struct nve0_graph_fuc *fuc)
+{
+	if (fuc->data) {
+		kfree(fuc->data);
+		fuc->data = NULL;
+	}
+}
+
+static void
+nve0_graph_destroy(struct drm_device *dev, int engine)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, engine);
+
+	nve0_graph_destroy_fw(&priv->fuc409c);
+	nve0_graph_destroy_fw(&priv->fuc409d);
+	nve0_graph_destroy_fw(&priv->fuc41ac);
+	nve0_graph_destroy_fw(&priv->fuc41ad);
+
+	nouveau_irq_unregister(dev, 12);
+
+	nouveau_gpuobj_ref(NULL, &priv->unk4188b8);
+	nouveau_gpuobj_ref(NULL, &priv->unk4188b4);
+
+	if (priv->grctx_vals)
+		kfree(priv->grctx_vals);
+
+	NVOBJ_ENGINE_DEL(dev, GR);
+	kfree(priv);
+}
+
+int
+nve0_graph_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_graph_priv *priv;
+	int ret, gpc, i;
+	u32 kepler;
+
+	kepler = nve0_graph_class(dev);
+	if (!kepler) {
+		NV_ERROR(dev, "PGRAPH: unsupported chipset, please report!\n");
+		return 0;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.destroy = nve0_graph_destroy;
+	priv->base.init = nve0_graph_init;
+	priv->base.fini = nve0_graph_fini;
+	priv->base.context_new = nve0_graph_context_new;
+	priv->base.context_del = nve0_graph_context_del;
+	priv->base.object_new = nve0_graph_object_new;
+
+	NVOBJ_ENGINE_ADD(dev, GR, &priv->base);
+	nouveau_irq_register(dev, 12, nve0_graph_isr);
+
+	NV_INFO(dev, "PGRAPH: using external firmware\n");
+	if (nve0_graph_create_fw(dev, "fuc409c", &priv->fuc409c) ||
+	    nve0_graph_create_fw(dev, "fuc409d", &priv->fuc409d) ||
+	    nve0_graph_create_fw(dev, "fuc41ac", &priv->fuc41ac) ||
+	    nve0_graph_create_fw(dev, "fuc41ad", &priv->fuc41ad)) {
+		ret = 0;
+		goto error;
+	}
+
+	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 256, 0, &priv->unk4188b4);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 256, 0, &priv->unk4188b8);
+	if (ret)
+		goto error;
+
+	for (i = 0; i < 0x1000; i += 4) {
+		nv_wo32(priv->unk4188b4, i, 0x00000010);
+		nv_wo32(priv->unk4188b8, i, 0x00000010);
+	}
+
+	priv->gpc_nr  =  nv_rd32(dev, 0x409604) & 0x0000001f;
+	priv->rop_nr = (nv_rd32(dev, 0x409604) & 0x001f0000) >> 16;
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		priv->tpc_nr[gpc] = nv_rd32(dev, GPC_UNIT(gpc, 0x2608));
+		priv->tpc_total += priv->tpc_nr[gpc];
+	}
+
+	switch (dev_priv->chipset) {
+	case 0xe4:
+		if (priv->tpc_total == 8)
+			priv->magic_not_rop_nr = 3;
+		else
+		if (priv->tpc_total == 7)
+			priv->magic_not_rop_nr = 1;
+		break;
+	case 0xe7:
+		priv->magic_not_rop_nr = 1;
+		break;
+	default:
+		break;
+	}
+
+	if (!priv->magic_not_rop_nr) {
+		NV_ERROR(dev, "PGRAPH: unknown config: %d/%d/%d/%d, %d\n",
+			 priv->tpc_nr[0], priv->tpc_nr[1], priv->tpc_nr[2],
+			 priv->tpc_nr[3], priv->rop_nr);
+		priv->magic_not_rop_nr = 0x00;
+	}
+
+	NVOBJ_CLASS(dev, 0xa097, GR); /* subc 0: 3D */
+	NVOBJ_CLASS(dev, 0xa0c0, GR); /* subc 1: COMPUTE */
+	NVOBJ_CLASS(dev, 0xa040, GR); /* subc 2: P2MF */
+	NVOBJ_CLASS(dev, 0x902d, GR); /* subc 3: 2D */
+	NVOBJ_CLASS(dev, 0xa0b5, GR); /* subc 4: COPY */
+	return 0;
+
+error:
+	nve0_graph_destroy(dev, NVOBJ_ENGINE_GR);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nve0_graph.h b/drivers/gpu/drm/nouveau/nve0_graph.h
new file mode 100644
index 000000000000..2ba70449ba01
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_graph.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#ifndef __NVE0_GRAPH_H__
+#define __NVE0_GRAPH_H__
+
+#define GPC_MAX 4
+#define TPC_MAX 32
+
+#define ROP_BCAST(r)     (0x408800 + (r))
+#define ROP_UNIT(u, r)   (0x410000 + (u) * 0x400 + (r))
+#define GPC_BCAST(r)     (0x418000 + (r))
+#define GPC_UNIT(t, r)   (0x500000 + (t) * 0x8000 + (r))
+#define TPC_UNIT(t, m, r) (0x504000 + (t) * 0x8000 + (m) * 0x800 + (r))
+
+struct nve0_graph_fuc {
+	u32 *data;
+	u32  size;
+};
+
+struct nve0_graph_priv {
+	struct nouveau_exec_engine base;
+
+	struct nve0_graph_fuc fuc409c;
+	struct nve0_graph_fuc fuc409d;
+	struct nve0_graph_fuc fuc41ac;
+	struct nve0_graph_fuc fuc41ad;
+
+	u8 gpc_nr;
+	u8 rop_nr;
+	u8 tpc_nr[GPC_MAX];
+	u8 tpc_total;
+
+	u32  grctx_size;
+	u32 *grctx_vals;
+	struct nouveau_gpuobj *unk4188b4;
+	struct nouveau_gpuobj *unk4188b8;
+
+	u8 magic_not_rop_nr;
+};
+
+struct nve0_graph_chan {
+	struct nouveau_gpuobj *grctx;
+	struct nouveau_gpuobj *unk408004; /* 0x418810 too */
+	struct nouveau_gpuobj *unk40800c; /* 0x419004 too */
+	struct nouveau_gpuobj *unk418810; /* 0x419848 too */
+	struct nouveau_gpuobj *mmio;
+	int mmio_nr;
+};
+
+int nve0_grctx_generate(struct nouveau_channel *);
+
+/* nve0_graph.c uses this also to determine supported chipsets */
+static inline u32
+nve0_graph_class(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	switch (dev_priv->chipset) {
+	case 0xe4:
+	case 0xe7:
+		return 0xa097;
+	default:
+		return 0;
+	}
+}
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nve0_grctx.c b/drivers/gpu/drm/nouveau/nve0_grctx.c
new file mode 100644
index 000000000000..d8cb360e92c1
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_grctx.c
@@ -0,0 +1,2777 @@
+/*
+ * Copyright 2010 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_mm.h"
+#include "nve0_graph.h"
+
+static void
+nv_icmd(struct drm_device *dev, u32 icmd, u32 data)
+{
+	nv_wr32(dev, 0x400204, data);
+	nv_wr32(dev, 0x400200, icmd);
+	while (nv_rd32(dev, 0x400700) & 0x00000002) {}
+}
+
+static void
+nve0_grctx_generate_icmd(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x400208, 0x80000000);
+	nv_icmd(dev, 0x001000, 0x00000004);
+	nv_icmd(dev, 0x000039, 0x00000000);
+	nv_icmd(dev, 0x00003a, 0x00000000);
+	nv_icmd(dev, 0x00003b, 0x00000000);
+	nv_icmd(dev, 0x0000a9, 0x0000ffff);
+	nv_icmd(dev, 0x000038, 0x0fac6881);
+	nv_icmd(dev, 0x00003d, 0x00000001);
+	nv_icmd(dev, 0x0000e8, 0x00000400);
+	nv_icmd(dev, 0x0000e9, 0x00000400);
+	nv_icmd(dev, 0x0000ea, 0x00000400);
+	nv_icmd(dev, 0x0000eb, 0x00000400);
+	nv_icmd(dev, 0x0000ec, 0x00000400);
+	nv_icmd(dev, 0x0000ed, 0x00000400);
+	nv_icmd(dev, 0x0000ee, 0x00000400);
+	nv_icmd(dev, 0x0000ef, 0x00000400);
+	nv_icmd(dev, 0x000078, 0x00000300);
+	nv_icmd(dev, 0x000079, 0x00000300);
+	nv_icmd(dev, 0x00007a, 0x00000300);
+	nv_icmd(dev, 0x00007b, 0x00000300);
+	nv_icmd(dev, 0x00007c, 0x00000300);
+	nv_icmd(dev, 0x00007d, 0x00000300);
+	nv_icmd(dev, 0x00007e, 0x00000300);
+	nv_icmd(dev, 0x00007f, 0x00000300);
+	nv_icmd(dev, 0x000050, 0x00000011);
+	nv_icmd(dev, 0x000058, 0x00000008);
+	nv_icmd(dev, 0x000059, 0x00000008);
+	nv_icmd(dev, 0x00005a, 0x00000008);
+	nv_icmd(dev, 0x00005b, 0x00000008);
+	nv_icmd(dev, 0x00005c, 0x00000008);
+	nv_icmd(dev, 0x00005d, 0x00000008);
+	nv_icmd(dev, 0x00005e, 0x00000008);
+	nv_icmd(dev, 0x00005f, 0x00000008);
+	nv_icmd(dev, 0x000208, 0x00000001);
+	nv_icmd(dev, 0x000209, 0x00000001);
+	nv_icmd(dev, 0x00020a, 0x00000001);
+	nv_icmd(dev, 0x00020b, 0x00000001);
+	nv_icmd(dev, 0x00020c, 0x00000001);
+	nv_icmd(dev, 0x00020d, 0x00000001);
+	nv_icmd(dev, 0x00020e, 0x00000001);
+	nv_icmd(dev, 0x00020f, 0x00000001);
+	nv_icmd(dev, 0x000081, 0x00000001);
+	nv_icmd(dev, 0x000085, 0x00000004);
+	nv_icmd(dev, 0x000088, 0x00000400);
+	nv_icmd(dev, 0x000090, 0x00000300);
+	nv_icmd(dev, 0x000098, 0x00001001);
+	nv_icmd(dev, 0x0000e3, 0x00000001);
+	nv_icmd(dev, 0x0000da, 0x00000001);
+	nv_icmd(dev, 0x0000f8, 0x00000003);
+	nv_icmd(dev, 0x0000fa, 0x00000001);
+	nv_icmd(dev, 0x00009f, 0x0000ffff);
+	nv_icmd(dev, 0x0000a0, 0x0000ffff);
+	nv_icmd(dev, 0x0000a1, 0x0000ffff);
+	nv_icmd(dev, 0x0000a2, 0x0000ffff);
+	nv_icmd(dev, 0x0000b1, 0x00000001);
+	nv_icmd(dev, 0x0000ad, 0x0000013e);
+	nv_icmd(dev, 0x0000e1, 0x00000010);
+	nv_icmd(dev, 0x000290, 0x00000000);
+	nv_icmd(dev, 0x000291, 0x00000000);
+	nv_icmd(dev, 0x000292, 0x00000000);
+	nv_icmd(dev, 0x000293, 0x00000000);
+	nv_icmd(dev, 0x000294, 0x00000000);
+	nv_icmd(dev, 0x000295, 0x00000000);
+	nv_icmd(dev, 0x000296, 0x00000000);
+	nv_icmd(dev, 0x000297, 0x00000000);
+	nv_icmd(dev, 0x000298, 0x00000000);
+	nv_icmd(dev, 0x000299, 0x00000000);
+	nv_icmd(dev, 0x00029a, 0x00000000);
+	nv_icmd(dev, 0x00029b, 0x00000000);
+	nv_icmd(dev, 0x00029c, 0x00000000);
+	nv_icmd(dev, 0x00029d, 0x00000000);
+	nv_icmd(dev, 0x00029e, 0x00000000);
+	nv_icmd(dev, 0x00029f, 0x00000000);
+	nv_icmd(dev, 0x0003b0, 0x00000000);
+	nv_icmd(dev, 0x0003b1, 0x00000000);
+	nv_icmd(dev, 0x0003b2, 0x00000000);
+	nv_icmd(dev, 0x0003b3, 0x00000000);
+	nv_icmd(dev, 0x0003b4, 0x00000000);
+	nv_icmd(dev, 0x0003b5, 0x00000000);
+	nv_icmd(dev, 0x0003b6, 0x00000000);
+	nv_icmd(dev, 0x0003b7, 0x00000000);
+	nv_icmd(dev, 0x0003b8, 0x00000000);
+	nv_icmd(dev, 0x0003b9, 0x00000000);
+	nv_icmd(dev, 0x0003ba, 0x00000000);
+	nv_icmd(dev, 0x0003bb, 0x00000000);
+	nv_icmd(dev, 0x0003bc, 0x00000000);
+	nv_icmd(dev, 0x0003bd, 0x00000000);
+	nv_icmd(dev, 0x0003be, 0x00000000);
+	nv_icmd(dev, 0x0003bf, 0x00000000);
+	nv_icmd(dev, 0x0002a0, 0x00000000);
+	nv_icmd(dev, 0x0002a1, 0x00000000);
+	nv_icmd(dev, 0x0002a2, 0x00000000);
+	nv_icmd(dev, 0x0002a3, 0x00000000);
+	nv_icmd(dev, 0x0002a4, 0x00000000);
+	nv_icmd(dev, 0x0002a5, 0x00000000);
+	nv_icmd(dev, 0x0002a6, 0x00000000);
+	nv_icmd(dev, 0x0002a7, 0x00000000);
+	nv_icmd(dev, 0x0002a8, 0x00000000);
+	nv_icmd(dev, 0x0002a9, 0x00000000);
+	nv_icmd(dev, 0x0002aa, 0x00000000);
+	nv_icmd(dev, 0x0002ab, 0x00000000);
+	nv_icmd(dev, 0x0002ac, 0x00000000);
+	nv_icmd(dev, 0x0002ad, 0x00000000);
+	nv_icmd(dev, 0x0002ae, 0x00000000);
+	nv_icmd(dev, 0x0002af, 0x00000000);
+	nv_icmd(dev, 0x000420, 0x00000000);
+	nv_icmd(dev, 0x000421, 0x00000000);
+	nv_icmd(dev, 0x000422, 0x00000000);
+	nv_icmd(dev, 0x000423, 0x00000000);
+	nv_icmd(dev, 0x000424, 0x00000000);
+	nv_icmd(dev, 0x000425, 0x00000000);
+	nv_icmd(dev, 0x000426, 0x00000000);
+	nv_icmd(dev, 0x000427, 0x00000000);
+	nv_icmd(dev, 0x000428, 0x00000000);
+	nv_icmd(dev, 0x000429, 0x00000000);
+	nv_icmd(dev, 0x00042a, 0x00000000);
+	nv_icmd(dev, 0x00042b, 0x00000000);
+	nv_icmd(dev, 0x00042c, 0x00000000);
+	nv_icmd(dev, 0x00042d, 0x00000000);
+	nv_icmd(dev, 0x00042e, 0x00000000);
+	nv_icmd(dev, 0x00042f, 0x00000000);
+	nv_icmd(dev, 0x0002b0, 0x00000000);
+	nv_icmd(dev, 0x0002b1, 0x00000000);
+	nv_icmd(dev, 0x0002b2, 0x00000000);
+	nv_icmd(dev, 0x0002b3, 0x00000000);
+	nv_icmd(dev, 0x0002b4, 0x00000000);
+	nv_icmd(dev, 0x0002b5, 0x00000000);
+	nv_icmd(dev, 0x0002b6, 0x00000000);
+	nv_icmd(dev, 0x0002b7, 0x00000000);
+	nv_icmd(dev, 0x0002b8, 0x00000000);
+	nv_icmd(dev, 0x0002b9, 0x00000000);
+	nv_icmd(dev, 0x0002ba, 0x00000000);
+	nv_icmd(dev, 0x0002bb, 0x00000000);
+	nv_icmd(dev, 0x0002bc, 0x00000000);
+	nv_icmd(dev, 0x0002bd, 0x00000000);
+	nv_icmd(dev, 0x0002be, 0x00000000);
+	nv_icmd(dev, 0x0002bf, 0x00000000);
+	nv_icmd(dev, 0x000430, 0x00000000);
+	nv_icmd(dev, 0x000431, 0x00000000);
+	nv_icmd(dev, 0x000432, 0x00000000);
+	nv_icmd(dev, 0x000433, 0x00000000);
+	nv_icmd(dev, 0x000434, 0x00000000);
+	nv_icmd(dev, 0x000435, 0x00000000);
+	nv_icmd(dev, 0x000436, 0x00000000);
+	nv_icmd(dev, 0x000437, 0x00000000);
+	nv_icmd(dev, 0x000438, 0x00000000);
+	nv_icmd(dev, 0x000439, 0x00000000);
+	nv_icmd(dev, 0x00043a, 0x00000000);
+	nv_icmd(dev, 0x00043b, 0x00000000);
+	nv_icmd(dev, 0x00043c, 0x00000000);
+	nv_icmd(dev, 0x00043d, 0x00000000);
+	nv_icmd(dev, 0x00043e, 0x00000000);
+	nv_icmd(dev, 0x00043f, 0x00000000);
+	nv_icmd(dev, 0x0002c0, 0x00000000);
+	nv_icmd(dev, 0x0002c1, 0x00000000);
+	nv_icmd(dev, 0x0002c2, 0x00000000);
+	nv_icmd(dev, 0x0002c3, 0x00000000);
+	nv_icmd(dev, 0x0002c4, 0x00000000);
+	nv_icmd(dev, 0x0002c5, 0x00000000);
+	nv_icmd(dev, 0x0002c6, 0x00000000);
+	nv_icmd(dev, 0x0002c7, 0x00000000);
+	nv_icmd(dev, 0x0002c8, 0x00000000);
+	nv_icmd(dev, 0x0002c9, 0x00000000);
+	nv_icmd(dev, 0x0002ca, 0x00000000);
+	nv_icmd(dev, 0x0002cb, 0x00000000);
+	nv_icmd(dev, 0x0002cc, 0x00000000);
+	nv_icmd(dev, 0x0002cd, 0x00000000);
+	nv_icmd(dev, 0x0002ce, 0x00000000);
+	nv_icmd(dev, 0x0002cf, 0x00000000);
+	nv_icmd(dev, 0x0004d0, 0x00000000);
+	nv_icmd(dev, 0x0004d1, 0x00000000);
+	nv_icmd(dev, 0x0004d2, 0x00000000);
+	nv_icmd(dev, 0x0004d3, 0x00000000);
+	nv_icmd(dev, 0x0004d4, 0x00000000);
+	nv_icmd(dev, 0x0004d5, 0x00000000);
+	nv_icmd(dev, 0x0004d6, 0x00000000);
+	nv_icmd(dev, 0x0004d7, 0x00000000);
+	nv_icmd(dev, 0x0004d8, 0x00000000);
+	nv_icmd(dev, 0x0004d9, 0x00000000);
+	nv_icmd(dev, 0x0004da, 0x00000000);
+	nv_icmd(dev, 0x0004db, 0x00000000);
+	nv_icmd(dev, 0x0004dc, 0x00000000);
+	nv_icmd(dev, 0x0004dd, 0x00000000);
+	nv_icmd(dev, 0x0004de, 0x00000000);
+	nv_icmd(dev, 0x0004df, 0x00000000);
+	nv_icmd(dev, 0x000720, 0x00000000);
+	nv_icmd(dev, 0x000721, 0x00000000);
+	nv_icmd(dev, 0x000722, 0x00000000);
+	nv_icmd(dev, 0x000723, 0x00000000);
+	nv_icmd(dev, 0x000724, 0x00000000);
+	nv_icmd(dev, 0x000725, 0x00000000);
+	nv_icmd(dev, 0x000726, 0x00000000);
+	nv_icmd(dev, 0x000727, 0x00000000);
+	nv_icmd(dev, 0x000728, 0x00000000);
+	nv_icmd(dev, 0x000729, 0x00000000);
+	nv_icmd(dev, 0x00072a, 0x00000000);
+	nv_icmd(dev, 0x00072b, 0x00000000);
+	nv_icmd(dev, 0x00072c, 0x00000000);
+	nv_icmd(dev, 0x00072d, 0x00000000);
+	nv_icmd(dev, 0x00072e, 0x00000000);
+	nv_icmd(dev, 0x00072f, 0x00000000);
+	nv_icmd(dev, 0x0008c0, 0x00000000);
+	nv_icmd(dev, 0x0008c1, 0x00000000);
+	nv_icmd(dev, 0x0008c2, 0x00000000);
+	nv_icmd(dev, 0x0008c3, 0x00000000);
+	nv_icmd(dev, 0x0008c4, 0x00000000);
+	nv_icmd(dev, 0x0008c5, 0x00000000);
+	nv_icmd(dev, 0x0008c6, 0x00000000);
+	nv_icmd(dev, 0x0008c7, 0x00000000);
+	nv_icmd(dev, 0x0008c8, 0x00000000);
+	nv_icmd(dev, 0x0008c9, 0x00000000);
+	nv_icmd(dev, 0x0008ca, 0x00000000);
+	nv_icmd(dev, 0x0008cb, 0x00000000);
+	nv_icmd(dev, 0x0008cc, 0x00000000);
+	nv_icmd(dev, 0x0008cd, 0x00000000);
+	nv_icmd(dev, 0x0008ce, 0x00000000);
+	nv_icmd(dev, 0x0008cf, 0x00000000);
+	nv_icmd(dev, 0x000890, 0x00000000);
+	nv_icmd(dev, 0x000891, 0x00000000);
+	nv_icmd(dev, 0x000892, 0x00000000);
+	nv_icmd(dev, 0x000893, 0x00000000);
+	nv_icmd(dev, 0x000894, 0x00000000);
+	nv_icmd(dev, 0x000895, 0x00000000);
+	nv_icmd(dev, 0x000896, 0x00000000);
+	nv_icmd(dev, 0x000897, 0x00000000);
+	nv_icmd(dev, 0x000898, 0x00000000);
+	nv_icmd(dev, 0x000899, 0x00000000);
+	nv_icmd(dev, 0x00089a, 0x00000000);
+	nv_icmd(dev, 0x00089b, 0x00000000);
+	nv_icmd(dev, 0x00089c, 0x00000000);
+	nv_icmd(dev, 0x00089d, 0x00000000);
+	nv_icmd(dev, 0x00089e, 0x00000000);
+	nv_icmd(dev, 0x00089f, 0x00000000);
+	nv_icmd(dev, 0x0008e0, 0x00000000);
+	nv_icmd(dev, 0x0008e1, 0x00000000);
+	nv_icmd(dev, 0x0008e2, 0x00000000);
+	nv_icmd(dev, 0x0008e3, 0x00000000);
+	nv_icmd(dev, 0x0008e4, 0x00000000);
+	nv_icmd(dev, 0x0008e5, 0x00000000);
+	nv_icmd(dev, 0x0008e6, 0x00000000);
+	nv_icmd(dev, 0x0008e7, 0x00000000);
+	nv_icmd(dev, 0x0008e8, 0x00000000);
+	nv_icmd(dev, 0x0008e9, 0x00000000);
+	nv_icmd(dev, 0x0008ea, 0x00000000);
+	nv_icmd(dev, 0x0008eb, 0x00000000);
+	nv_icmd(dev, 0x0008ec, 0x00000000);
+	nv_icmd(dev, 0x0008ed, 0x00000000);
+	nv_icmd(dev, 0x0008ee, 0x00000000);
+	nv_icmd(dev, 0x0008ef, 0x00000000);
+	nv_icmd(dev, 0x0008a0, 0x00000000);
+	nv_icmd(dev, 0x0008a1, 0x00000000);
+	nv_icmd(dev, 0x0008a2, 0x00000000);
+	nv_icmd(dev, 0x0008a3, 0x00000000);
+	nv_icmd(dev, 0x0008a4, 0x00000000);
+	nv_icmd(dev, 0x0008a5, 0x00000000);
+	nv_icmd(dev, 0x0008a6, 0x00000000);
+	nv_icmd(dev, 0x0008a7, 0x00000000);
+	nv_icmd(dev, 0x0008a8, 0x00000000);
+	nv_icmd(dev, 0x0008a9, 0x00000000);
+	nv_icmd(dev, 0x0008aa, 0x00000000);
+	nv_icmd(dev, 0x0008ab, 0x00000000);
+	nv_icmd(dev, 0x0008ac, 0x00000000);
+	nv_icmd(dev, 0x0008ad, 0x00000000);
+	nv_icmd(dev, 0x0008ae, 0x00000000);
+	nv_icmd(dev, 0x0008af, 0x00000000);
+	nv_icmd(dev, 0x0008f0, 0x00000000);
+	nv_icmd(dev, 0x0008f1, 0x00000000);
+	nv_icmd(dev, 0x0008f2, 0x00000000);
+	nv_icmd(dev, 0x0008f3, 0x00000000);
+	nv_icmd(dev, 0x0008f4, 0x00000000);
+	nv_icmd(dev, 0x0008f5, 0x00000000);
+	nv_icmd(dev, 0x0008f6, 0x00000000);
+	nv_icmd(dev, 0x0008f7, 0x00000000);
+	nv_icmd(dev, 0x0008f8, 0x00000000);
+	nv_icmd(dev, 0x0008f9, 0x00000000);
+	nv_icmd(dev, 0x0008fa, 0x00000000);
+	nv_icmd(dev, 0x0008fb, 0x00000000);
+	nv_icmd(dev, 0x0008fc, 0x00000000);
+	nv_icmd(dev, 0x0008fd, 0x00000000);
+	nv_icmd(dev, 0x0008fe, 0x00000000);
+	nv_icmd(dev, 0x0008ff, 0x00000000);
+	nv_icmd(dev, 0x00094c, 0x000000ff);
+	nv_icmd(dev, 0x00094d, 0xffffffff);
+	nv_icmd(dev, 0x00094e, 0x00000002);
+	nv_icmd(dev, 0x0002ec, 0x00000001);
+	nv_icmd(dev, 0x000303, 0x00000001);
+	nv_icmd(dev, 0x0002e6, 0x00000001);
+	nv_icmd(dev, 0x000466, 0x00000052);
+	nv_icmd(dev, 0x000301, 0x3f800000);
+	nv_icmd(dev, 0x000304, 0x30201000);
+	nv_icmd(dev, 0x000305, 0x70605040);
+	nv_icmd(dev, 0x000306, 0xb8a89888);
+	nv_icmd(dev, 0x000307, 0xf8e8d8c8);
+	nv_icmd(dev, 0x00030a, 0x00ffff00);
+	nv_icmd(dev, 0x00030b, 0x0000001a);
+	nv_icmd(dev, 0x00030c, 0x00000001);
+	nv_icmd(dev, 0x000318, 0x00000001);
+	nv_icmd(dev, 0x000340, 0x00000000);
+	nv_icmd(dev, 0x000375, 0x00000001);
+	nv_icmd(dev, 0x00037d, 0x00000006);
+	nv_icmd(dev, 0x0003a0, 0x00000002);
+	nv_icmd(dev, 0x0003aa, 0x00000001);
+	nv_icmd(dev, 0x0003a9, 0x00000001);
+	nv_icmd(dev, 0x000380, 0x00000001);
+	nv_icmd(dev, 0x000383, 0x00000011);
+	nv_icmd(dev, 0x000360, 0x00000040);
+	nv_icmd(dev, 0x000366, 0x00000000);
+	nv_icmd(dev, 0x000367, 0x00000000);
+	nv_icmd(dev, 0x000368, 0x00000fff);
+	nv_icmd(dev, 0x000370, 0x00000000);
+	nv_icmd(dev, 0x000371, 0x00000000);
+	nv_icmd(dev, 0x000372, 0x000fffff);
+	nv_icmd(dev, 0x00037a, 0x00000012);
+	nv_icmd(dev, 0x000619, 0x00000003);
+	nv_icmd(dev, 0x000811, 0x00000003);
+	nv_icmd(dev, 0x000812, 0x00000004);
+	nv_icmd(dev, 0x000813, 0x00000006);
+	nv_icmd(dev, 0x000814, 0x00000008);
+	nv_icmd(dev, 0x000815, 0x0000000b);
+	nv_icmd(dev, 0x000800, 0x00000001);
+	nv_icmd(dev, 0x000801, 0x00000001);
+	nv_icmd(dev, 0x000802, 0x00000001);
+	nv_icmd(dev, 0x000803, 0x00000001);
+	nv_icmd(dev, 0x000804, 0x00000001);
+	nv_icmd(dev, 0x000805, 0x00000001);
+	nv_icmd(dev, 0x000632, 0x00000001);
+	nv_icmd(dev, 0x000633, 0x00000002);
+	nv_icmd(dev, 0x000634, 0x00000003);
+	nv_icmd(dev, 0x000635, 0x00000004);
+	nv_icmd(dev, 0x000654, 0x3f800000);
+	nv_icmd(dev, 0x000657, 0x3f800000);
+	nv_icmd(dev, 0x000655, 0x3f800000);
+	nv_icmd(dev, 0x000656, 0x3f800000);
+	nv_icmd(dev, 0x0006cd, 0x3f800000);
+	nv_icmd(dev, 0x0007f5, 0x3f800000);
+	nv_icmd(dev, 0x0007dc, 0x39291909);
+	nv_icmd(dev, 0x0007dd, 0x79695949);
+	nv_icmd(dev, 0x0007de, 0xb9a99989);
+	nv_icmd(dev, 0x0007df, 0xf9e9d9c9);
+	nv_icmd(dev, 0x0007e8, 0x00003210);
+	nv_icmd(dev, 0x0007e9, 0x00007654);
+	nv_icmd(dev, 0x0007ea, 0x00000098);
+	nv_icmd(dev, 0x0007ec, 0x39291909);
+	nv_icmd(dev, 0x0007ed, 0x79695949);
+	nv_icmd(dev, 0x0007ee, 0xb9a99989);
+	nv_icmd(dev, 0x0007ef, 0xf9e9d9c9);
+	nv_icmd(dev, 0x0007f0, 0x00003210);
+	nv_icmd(dev, 0x0007f1, 0x00007654);
+	nv_icmd(dev, 0x0007f2, 0x00000098);
+	nv_icmd(dev, 0x0005a5, 0x00000001);
+	nv_icmd(dev, 0x000980, 0x00000000);
+	nv_icmd(dev, 0x000981, 0x00000000);
+	nv_icmd(dev, 0x000982, 0x00000000);
+	nv_icmd(dev, 0x000983, 0x00000000);
+	nv_icmd(dev, 0x000984, 0x00000000);
+	nv_icmd(dev, 0x000985, 0x00000000);
+	nv_icmd(dev, 0x000986, 0x00000000);
+	nv_icmd(dev, 0x000987, 0x00000000);
+	nv_icmd(dev, 0x000988, 0x00000000);
+	nv_icmd(dev, 0x000989, 0x00000000);
+	nv_icmd(dev, 0x00098a, 0x00000000);
+	nv_icmd(dev, 0x00098b, 0x00000000);
+	nv_icmd(dev, 0x00098c, 0x00000000);
+	nv_icmd(dev, 0x00098d, 0x00000000);
+	nv_icmd(dev, 0x00098e, 0x00000000);
+	nv_icmd(dev, 0x00098f, 0x00000000);
+	nv_icmd(dev, 0x000990, 0x00000000);
+	nv_icmd(dev, 0x000991, 0x00000000);
+	nv_icmd(dev, 0x000992, 0x00000000);
+	nv_icmd(dev, 0x000993, 0x00000000);
+	nv_icmd(dev, 0x000994, 0x00000000);
+	nv_icmd(dev, 0x000995, 0x00000000);
+	nv_icmd(dev, 0x000996, 0x00000000);
+	nv_icmd(dev, 0x000997, 0x00000000);
+	nv_icmd(dev, 0x000998, 0x00000000);
+	nv_icmd(dev, 0x000999, 0x00000000);
+	nv_icmd(dev, 0x00099a, 0x00000000);
+	nv_icmd(dev, 0x00099b, 0x00000000);
+	nv_icmd(dev, 0x00099c, 0x00000000);
+	nv_icmd(dev, 0x00099d, 0x00000000);
+	nv_icmd(dev, 0x00099e, 0x00000000);
+	nv_icmd(dev, 0x00099f, 0x00000000);
+	nv_icmd(dev, 0x0009a0, 0x00000000);
+	nv_icmd(dev, 0x0009a1, 0x00000000);
+	nv_icmd(dev, 0x0009a2, 0x00000000);
+	nv_icmd(dev, 0x0009a3, 0x00000000);
+	nv_icmd(dev, 0x0009a4, 0x00000000);
+	nv_icmd(dev, 0x0009a5, 0x00000000);
+	nv_icmd(dev, 0x0009a6, 0x00000000);
+	nv_icmd(dev, 0x0009a7, 0x00000000);
+	nv_icmd(dev, 0x0009a8, 0x00000000);
+	nv_icmd(dev, 0x0009a9, 0x00000000);
+	nv_icmd(dev, 0x0009aa, 0x00000000);
+	nv_icmd(dev, 0x0009ab, 0x00000000);
+	nv_icmd(dev, 0x0009ac, 0x00000000);
+	nv_icmd(dev, 0x0009ad, 0x00000000);
+	nv_icmd(dev, 0x0009ae, 0x00000000);
+	nv_icmd(dev, 0x0009af, 0x00000000);
+	nv_icmd(dev, 0x0009b0, 0x00000000);
+	nv_icmd(dev, 0x0009b1, 0x00000000);
+	nv_icmd(dev, 0x0009b2, 0x00000000);
+	nv_icmd(dev, 0x0009b3, 0x00000000);
+	nv_icmd(dev, 0x0009b4, 0x00000000);
+	nv_icmd(dev, 0x0009b5, 0x00000000);
+	nv_icmd(dev, 0x0009b6, 0x00000000);
+	nv_icmd(dev, 0x0009b7, 0x00000000);
+	nv_icmd(dev, 0x0009b8, 0x00000000);
+	nv_icmd(dev, 0x0009b9, 0x00000000);
+	nv_icmd(dev, 0x0009ba, 0x00000000);
+	nv_icmd(dev, 0x0009bb, 0x00000000);
+	nv_icmd(dev, 0x0009bc, 0x00000000);
+	nv_icmd(dev, 0x0009bd, 0x00000000);
+	nv_icmd(dev, 0x0009be, 0x00000000);
+	nv_icmd(dev, 0x0009bf, 0x00000000);
+	nv_icmd(dev, 0x0009c0, 0x00000000);
+	nv_icmd(dev, 0x0009c1, 0x00000000);
+	nv_icmd(dev, 0x0009c2, 0x00000000);
+	nv_icmd(dev, 0x0009c3, 0x00000000);
+	nv_icmd(dev, 0x0009c4, 0x00000000);
+	nv_icmd(dev, 0x0009c5, 0x00000000);
+	nv_icmd(dev, 0x0009c6, 0x00000000);
+	nv_icmd(dev, 0x0009c7, 0x00000000);
+	nv_icmd(dev, 0x0009c8, 0x00000000);
+	nv_icmd(dev, 0x0009c9, 0x00000000);
+	nv_icmd(dev, 0x0009ca, 0x00000000);
+	nv_icmd(dev, 0x0009cb, 0x00000000);
+	nv_icmd(dev, 0x0009cc, 0x00000000);
+	nv_icmd(dev, 0x0009cd, 0x00000000);
+	nv_icmd(dev, 0x0009ce, 0x00000000);
+	nv_icmd(dev, 0x0009cf, 0x00000000);
+	nv_icmd(dev, 0x0009d0, 0x00000000);
+	nv_icmd(dev, 0x0009d1, 0x00000000);
+	nv_icmd(dev, 0x0009d2, 0x00000000);
+	nv_icmd(dev, 0x0009d3, 0x00000000);
+	nv_icmd(dev, 0x0009d4, 0x00000000);
+	nv_icmd(dev, 0x0009d5, 0x00000000);
+	nv_icmd(dev, 0x0009d6, 0x00000000);
+	nv_icmd(dev, 0x0009d7, 0x00000000);
+	nv_icmd(dev, 0x0009d8, 0x00000000);
+	nv_icmd(dev, 0x0009d9, 0x00000000);
+	nv_icmd(dev, 0x0009da, 0x00000000);
+	nv_icmd(dev, 0x0009db, 0x00000000);
+	nv_icmd(dev, 0x0009dc, 0x00000000);
+	nv_icmd(dev, 0x0009dd, 0x00000000);
+	nv_icmd(dev, 0x0009de, 0x00000000);
+	nv_icmd(dev, 0x0009df, 0x00000000);
+	nv_icmd(dev, 0x0009e0, 0x00000000);
+	nv_icmd(dev, 0x0009e1, 0x00000000);
+	nv_icmd(dev, 0x0009e2, 0x00000000);
+	nv_icmd(dev, 0x0009e3, 0x00000000);
+	nv_icmd(dev, 0x0009e4, 0x00000000);
+	nv_icmd(dev, 0x0009e5, 0x00000000);
+	nv_icmd(dev, 0x0009e6, 0x00000000);
+	nv_icmd(dev, 0x0009e7, 0x00000000);
+	nv_icmd(dev, 0x0009e8, 0x00000000);
+	nv_icmd(dev, 0x0009e9, 0x00000000);
+	nv_icmd(dev, 0x0009ea, 0x00000000);
+	nv_icmd(dev, 0x0009eb, 0x00000000);
+	nv_icmd(dev, 0x0009ec, 0x00000000);
+	nv_icmd(dev, 0x0009ed, 0x00000000);
+	nv_icmd(dev, 0x0009ee, 0x00000000);
+	nv_icmd(dev, 0x0009ef, 0x00000000);
+	nv_icmd(dev, 0x0009f0, 0x00000000);
+	nv_icmd(dev, 0x0009f1, 0x00000000);
+	nv_icmd(dev, 0x0009f2, 0x00000000);
+	nv_icmd(dev, 0x0009f3, 0x00000000);
+	nv_icmd(dev, 0x0009f4, 0x00000000);
+	nv_icmd(dev, 0x0009f5, 0x00000000);
+	nv_icmd(dev, 0x0009f6, 0x00000000);
+	nv_icmd(dev, 0x0009f7, 0x00000000);
+	nv_icmd(dev, 0x0009f8, 0x00000000);
+	nv_icmd(dev, 0x0009f9, 0x00000000);
+	nv_icmd(dev, 0x0009fa, 0x00000000);
+	nv_icmd(dev, 0x0009fb, 0x00000000);
+	nv_icmd(dev, 0x0009fc, 0x00000000);
+	nv_icmd(dev, 0x0009fd, 0x00000000);
+	nv_icmd(dev, 0x0009fe, 0x00000000);
+	nv_icmd(dev, 0x0009ff, 0x00000000);
+	nv_icmd(dev, 0x000468, 0x00000004);
+	nv_icmd(dev, 0x00046c, 0x00000001);
+	nv_icmd(dev, 0x000470, 0x00000000);
+	nv_icmd(dev, 0x000471, 0x00000000);
+	nv_icmd(dev, 0x000472, 0x00000000);
+	nv_icmd(dev, 0x000473, 0x00000000);
+	nv_icmd(dev, 0x000474, 0x00000000);
+	nv_icmd(dev, 0x000475, 0x00000000);
+	nv_icmd(dev, 0x000476, 0x00000000);
+	nv_icmd(dev, 0x000477, 0x00000000);
+	nv_icmd(dev, 0x000478, 0x00000000);
+	nv_icmd(dev, 0x000479, 0x00000000);
+	nv_icmd(dev, 0x00047a, 0x00000000);
+	nv_icmd(dev, 0x00047b, 0x00000000);
+	nv_icmd(dev, 0x00047c, 0x00000000);
+	nv_icmd(dev, 0x00047d, 0x00000000);
+	nv_icmd(dev, 0x00047e, 0x00000000);
+	nv_icmd(dev, 0x00047f, 0x00000000);
+	nv_icmd(dev, 0x000480, 0x00000000);
+	nv_icmd(dev, 0x000481, 0x00000000);
+	nv_icmd(dev, 0x000482, 0x00000000);
+	nv_icmd(dev, 0x000483, 0x00000000);
+	nv_icmd(dev, 0x000484, 0x00000000);
+	nv_icmd(dev, 0x000485, 0x00000000);
+	nv_icmd(dev, 0x000486, 0x00000000);
+	nv_icmd(dev, 0x000487, 0x00000000);
+	nv_icmd(dev, 0x000488, 0x00000000);
+	nv_icmd(dev, 0x000489, 0x00000000);
+	nv_icmd(dev, 0x00048a, 0x00000000);
+	nv_icmd(dev, 0x00048b, 0x00000000);
+	nv_icmd(dev, 0x00048c, 0x00000000);
+	nv_icmd(dev, 0x00048d, 0x00000000);
+	nv_icmd(dev, 0x00048e, 0x00000000);
+	nv_icmd(dev, 0x00048f, 0x00000000);
+	nv_icmd(dev, 0x000490, 0x00000000);
+	nv_icmd(dev, 0x000491, 0x00000000);
+	nv_icmd(dev, 0x000492, 0x00000000);
+	nv_icmd(dev, 0x000493, 0x00000000);
+	nv_icmd(dev, 0x000494, 0x00000000);
+	nv_icmd(dev, 0x000495, 0x00000000);
+	nv_icmd(dev, 0x000496, 0x00000000);
+	nv_icmd(dev, 0x000497, 0x00000000);
+	nv_icmd(dev, 0x000498, 0x00000000);
+	nv_icmd(dev, 0x000499, 0x00000000);
+	nv_icmd(dev, 0x00049a, 0x00000000);
+	nv_icmd(dev, 0x00049b, 0x00000000);
+	nv_icmd(dev, 0x00049c, 0x00000000);
+	nv_icmd(dev, 0x00049d, 0x00000000);
+	nv_icmd(dev, 0x00049e, 0x00000000);
+	nv_icmd(dev, 0x00049f, 0x00000000);
+	nv_icmd(dev, 0x0004a0, 0x00000000);
+	nv_icmd(dev, 0x0004a1, 0x00000000);
+	nv_icmd(dev, 0x0004a2, 0x00000000);
+	nv_icmd(dev, 0x0004a3, 0x00000000);
+	nv_icmd(dev, 0x0004a4, 0x00000000);
+	nv_icmd(dev, 0x0004a5, 0x00000000);
+	nv_icmd(dev, 0x0004a6, 0x00000000);
+	nv_icmd(dev, 0x0004a7, 0x00000000);
+	nv_icmd(dev, 0x0004a8, 0x00000000);
+	nv_icmd(dev, 0x0004a9, 0x00000000);
+	nv_icmd(dev, 0x0004aa, 0x00000000);
+	nv_icmd(dev, 0x0004ab, 0x00000000);
+	nv_icmd(dev, 0x0004ac, 0x00000000);
+	nv_icmd(dev, 0x0004ad, 0x00000000);
+	nv_icmd(dev, 0x0004ae, 0x00000000);
+	nv_icmd(dev, 0x0004af, 0x00000000);
+	nv_icmd(dev, 0x0004b0, 0x00000000);
+	nv_icmd(dev, 0x0004b1, 0x00000000);
+	nv_icmd(dev, 0x0004b2, 0x00000000);
+	nv_icmd(dev, 0x0004b3, 0x00000000);
+	nv_icmd(dev, 0x0004b4, 0x00000000);
+	nv_icmd(dev, 0x0004b5, 0x00000000);
+	nv_icmd(dev, 0x0004b6, 0x00000000);
+	nv_icmd(dev, 0x0004b7, 0x00000000);
+	nv_icmd(dev, 0x0004b8, 0x00000000);
+	nv_icmd(dev, 0x0004b9, 0x00000000);
+	nv_icmd(dev, 0x0004ba, 0x00000000);
+	nv_icmd(dev, 0x0004bb, 0x00000000);
+	nv_icmd(dev, 0x0004bc, 0x00000000);
+	nv_icmd(dev, 0x0004bd, 0x00000000);
+	nv_icmd(dev, 0x0004be, 0x00000000);
+	nv_icmd(dev, 0x0004bf, 0x00000000);
+	nv_icmd(dev, 0x0004c0, 0x00000000);
+	nv_icmd(dev, 0x0004c1, 0x00000000);
+	nv_icmd(dev, 0x0004c2, 0x00000000);
+	nv_icmd(dev, 0x0004c3, 0x00000000);
+	nv_icmd(dev, 0x0004c4, 0x00000000);
+	nv_icmd(dev, 0x0004c5, 0x00000000);
+	nv_icmd(dev, 0x0004c6, 0x00000000);
+	nv_icmd(dev, 0x0004c7, 0x00000000);
+	nv_icmd(dev, 0x0004c8, 0x00000000);
+	nv_icmd(dev, 0x0004c9, 0x00000000);
+	nv_icmd(dev, 0x0004ca, 0x00000000);
+	nv_icmd(dev, 0x0004cb, 0x00000000);
+	nv_icmd(dev, 0x0004cc, 0x00000000);
+	nv_icmd(dev, 0x0004cd, 0x00000000);
+	nv_icmd(dev, 0x0004ce, 0x00000000);
+	nv_icmd(dev, 0x0004cf, 0x00000000);
+	nv_icmd(dev, 0x000510, 0x3f800000);
+	nv_icmd(dev, 0x000511, 0x3f800000);
+	nv_icmd(dev, 0x000512, 0x3f800000);
+	nv_icmd(dev, 0x000513, 0x3f800000);
+	nv_icmd(dev, 0x000514, 0x3f800000);
+	nv_icmd(dev, 0x000515, 0x3f800000);
+	nv_icmd(dev, 0x000516, 0x3f800000);
+	nv_icmd(dev, 0x000517, 0x3f800000);
+	nv_icmd(dev, 0x000518, 0x3f800000);
+	nv_icmd(dev, 0x000519, 0x3f800000);
+	nv_icmd(dev, 0x00051a, 0x3f800000);
+	nv_icmd(dev, 0x00051b, 0x3f800000);
+	nv_icmd(dev, 0x00051c, 0x3f800000);
+	nv_icmd(dev, 0x00051d, 0x3f800000);
+	nv_icmd(dev, 0x00051e, 0x3f800000);
+	nv_icmd(dev, 0x00051f, 0x3f800000);
+	nv_icmd(dev, 0x000520, 0x000002b6);
+	nv_icmd(dev, 0x000529, 0x00000001);
+	nv_icmd(dev, 0x000530, 0xffff0000);
+	nv_icmd(dev, 0x000531, 0xffff0000);
+	nv_icmd(dev, 0x000532, 0xffff0000);
+	nv_icmd(dev, 0x000533, 0xffff0000);
+	nv_icmd(dev, 0x000534, 0xffff0000);
+	nv_icmd(dev, 0x000535, 0xffff0000);
+	nv_icmd(dev, 0x000536, 0xffff0000);
+	nv_icmd(dev, 0x000537, 0xffff0000);
+	nv_icmd(dev, 0x000538, 0xffff0000);
+	nv_icmd(dev, 0x000539, 0xffff0000);
+	nv_icmd(dev, 0x00053a, 0xffff0000);
+	nv_icmd(dev, 0x00053b, 0xffff0000);
+	nv_icmd(dev, 0x00053c, 0xffff0000);
+	nv_icmd(dev, 0x00053d, 0xffff0000);
+	nv_icmd(dev, 0x00053e, 0xffff0000);
+	nv_icmd(dev, 0x00053f, 0xffff0000);
+	nv_icmd(dev, 0x000585, 0x0000003f);
+	nv_icmd(dev, 0x000576, 0x00000003);
+	nv_icmd(dev, 0x00057b, 0x00000059);
+	nv_icmd(dev, 0x000586, 0x00000040);
+	nv_icmd(dev, 0x000582, 0x00000080);
+	nv_icmd(dev, 0x000583, 0x00000080);
+	nv_icmd(dev, 0x0005c2, 0x00000001);
+	nv_icmd(dev, 0x000638, 0x00000001);
+	nv_icmd(dev, 0x000639, 0x00000001);
+	nv_icmd(dev, 0x00063a, 0x00000002);
+	nv_icmd(dev, 0x00063b, 0x00000001);
+	nv_icmd(dev, 0x00063c, 0x00000001);
+	nv_icmd(dev, 0x00063d, 0x00000002);
+	nv_icmd(dev, 0x00063e, 0x00000001);
+	nv_icmd(dev, 0x0008b8, 0x00000001);
+	nv_icmd(dev, 0x0008b9, 0x00000001);
+	nv_icmd(dev, 0x0008ba, 0x00000001);
+	nv_icmd(dev, 0x0008bb, 0x00000001);
+	nv_icmd(dev, 0x0008bc, 0x00000001);
+	nv_icmd(dev, 0x0008bd, 0x00000001);
+	nv_icmd(dev, 0x0008be, 0x00000001);
+	nv_icmd(dev, 0x0008bf, 0x00000001);
+	nv_icmd(dev, 0x000900, 0x00000001);
+	nv_icmd(dev, 0x000901, 0x00000001);
+	nv_icmd(dev, 0x000902, 0x00000001);
+	nv_icmd(dev, 0x000903, 0x00000001);
+	nv_icmd(dev, 0x000904, 0x00000001);
+	nv_icmd(dev, 0x000905, 0x00000001);
+	nv_icmd(dev, 0x000906, 0x00000001);
+	nv_icmd(dev, 0x000907, 0x00000001);
+	nv_icmd(dev, 0x000908, 0x00000002);
+	nv_icmd(dev, 0x000909, 0x00000002);
+	nv_icmd(dev, 0x00090a, 0x00000002);
+	nv_icmd(dev, 0x00090b, 0x00000002);
+	nv_icmd(dev, 0x00090c, 0x00000002);
+	nv_icmd(dev, 0x00090d, 0x00000002);
+	nv_icmd(dev, 0x00090e, 0x00000002);
+	nv_icmd(dev, 0x00090f, 0x00000002);
+	nv_icmd(dev, 0x000910, 0x00000001);
+	nv_icmd(dev, 0x000911, 0x00000001);
+	nv_icmd(dev, 0x000912, 0x00000001);
+	nv_icmd(dev, 0x000913, 0x00000001);
+	nv_icmd(dev, 0x000914, 0x00000001);
+	nv_icmd(dev, 0x000915, 0x00000001);
+	nv_icmd(dev, 0x000916, 0x00000001);
+	nv_icmd(dev, 0x000917, 0x00000001);
+	nv_icmd(dev, 0x000918, 0x00000001);
+	nv_icmd(dev, 0x000919, 0x00000001);
+	nv_icmd(dev, 0x00091a, 0x00000001);
+	nv_icmd(dev, 0x00091b, 0x00000001);
+	nv_icmd(dev, 0x00091c, 0x00000001);
+	nv_icmd(dev, 0x00091d, 0x00000001);
+	nv_icmd(dev, 0x00091e, 0x00000001);
+	nv_icmd(dev, 0x00091f, 0x00000001);
+	nv_icmd(dev, 0x000920, 0x00000002);
+	nv_icmd(dev, 0x000921, 0x00000002);
+	nv_icmd(dev, 0x000922, 0x00000002);
+	nv_icmd(dev, 0x000923, 0x00000002);
+	nv_icmd(dev, 0x000924, 0x00000002);
+	nv_icmd(dev, 0x000925, 0x00000002);
+	nv_icmd(dev, 0x000926, 0x00000002);
+	nv_icmd(dev, 0x000927, 0x00000002);
+	nv_icmd(dev, 0x000928, 0x00000001);
+	nv_icmd(dev, 0x000929, 0x00000001);
+	nv_icmd(dev, 0x00092a, 0x00000001);
+	nv_icmd(dev, 0x00092b, 0x00000001);
+	nv_icmd(dev, 0x00092c, 0x00000001);
+	nv_icmd(dev, 0x00092d, 0x00000001);
+	nv_icmd(dev, 0x00092e, 0x00000001);
+	nv_icmd(dev, 0x00092f, 0x00000001);
+	nv_icmd(dev, 0x000648, 0x00000001);
+	nv_icmd(dev, 0x000649, 0x00000001);
+	nv_icmd(dev, 0x00064a, 0x00000001);
+	nv_icmd(dev, 0x00064b, 0x00000001);
+	nv_icmd(dev, 0x00064c, 0x00000001);
+	nv_icmd(dev, 0x00064d, 0x00000001);
+	nv_icmd(dev, 0x00064e, 0x00000001);
+	nv_icmd(dev, 0x00064f, 0x00000001);
+	nv_icmd(dev, 0x000650, 0x00000001);
+	nv_icmd(dev, 0x000658, 0x0000000f);
+	nv_icmd(dev, 0x0007ff, 0x0000000a);
+	nv_icmd(dev, 0x00066a, 0x40000000);
+	nv_icmd(dev, 0x00066b, 0x10000000);
+	nv_icmd(dev, 0x00066c, 0xffff0000);
+	nv_icmd(dev, 0x00066d, 0xffff0000);
+	nv_icmd(dev, 0x0007af, 0x00000008);
+	nv_icmd(dev, 0x0007b0, 0x00000008);
+	nv_icmd(dev, 0x0007f6, 0x00000001);
+	nv_icmd(dev, 0x0006b2, 0x00000055);
+	nv_icmd(dev, 0x0007ad, 0x00000003);
+	nv_icmd(dev, 0x000937, 0x00000001);
+	nv_icmd(dev, 0x000971, 0x00000008);
+	nv_icmd(dev, 0x000972, 0x00000040);
+	nv_icmd(dev, 0x000973, 0x0000012c);
+	nv_icmd(dev, 0x00097c, 0x00000040);
+	nv_icmd(dev, 0x000979, 0x00000003);
+	nv_icmd(dev, 0x000975, 0x00000020);
+	nv_icmd(dev, 0x000976, 0x00000001);
+	nv_icmd(dev, 0x000977, 0x00000020);
+	nv_icmd(dev, 0x000978, 0x00000001);
+	nv_icmd(dev, 0x000957, 0x00000003);
+	nv_icmd(dev, 0x00095e, 0x20164010);
+	nv_icmd(dev, 0x00095f, 0x00000020);
+	nv_icmd(dev, 0x00097d, 0x00000020);
+	nv_icmd(dev, 0x000683, 0x00000006);
+	nv_icmd(dev, 0x000685, 0x003fffff);
+	nv_icmd(dev, 0x000687, 0x003fffff);
+	nv_icmd(dev, 0x0006a0, 0x00000005);
+	nv_icmd(dev, 0x000840, 0x00400008);
+	nv_icmd(dev, 0x000841, 0x08000080);
+	nv_icmd(dev, 0x000842, 0x00400008);
+	nv_icmd(dev, 0x000843, 0x08000080);
+	nv_icmd(dev, 0x000818, 0x00000000);
+	nv_icmd(dev, 0x000819, 0x00000000);
+	nv_icmd(dev, 0x00081a, 0x00000000);
+	nv_icmd(dev, 0x00081b, 0x00000000);
+	nv_icmd(dev, 0x00081c, 0x00000000);
+	nv_icmd(dev, 0x00081d, 0x00000000);
+	nv_icmd(dev, 0x00081e, 0x00000000);
+	nv_icmd(dev, 0x00081f, 0x00000000);
+	nv_icmd(dev, 0x000848, 0x00000000);
+	nv_icmd(dev, 0x000849, 0x00000000);
+	nv_icmd(dev, 0x00084a, 0x00000000);
+	nv_icmd(dev, 0x00084b, 0x00000000);
+	nv_icmd(dev, 0x00084c, 0x00000000);
+	nv_icmd(dev, 0x00084d, 0x00000000);
+	nv_icmd(dev, 0x00084e, 0x00000000);
+	nv_icmd(dev, 0x00084f, 0x00000000);
+	nv_icmd(dev, 0x000850, 0x00000000);
+	nv_icmd(dev, 0x000851, 0x00000000);
+	nv_icmd(dev, 0x000852, 0x00000000);
+	nv_icmd(dev, 0x000853, 0x00000000);
+	nv_icmd(dev, 0x000854, 0x00000000);
+	nv_icmd(dev, 0x000855, 0x00000000);
+	nv_icmd(dev, 0x000856, 0x00000000);
+	nv_icmd(dev, 0x000857, 0x00000000);
+	nv_icmd(dev, 0x000738, 0x00000000);
+	nv_icmd(dev, 0x0006aa, 0x00000001);
+	nv_icmd(dev, 0x0006ab, 0x00000002);
+	nv_icmd(dev, 0x0006ac, 0x00000080);
+	nv_icmd(dev, 0x0006ad, 0x00000100);
+	nv_icmd(dev, 0x0006ae, 0x00000100);
+	nv_icmd(dev, 0x0006b1, 0x00000011);
+	nv_icmd(dev, 0x0006bb, 0x000000cf);
+	nv_icmd(dev, 0x0006ce, 0x2a712488);
+	nv_icmd(dev, 0x000739, 0x4085c000);
+	nv_icmd(dev, 0x00073a, 0x00000080);
+	nv_icmd(dev, 0x000786, 0x80000100);
+	nv_icmd(dev, 0x00073c, 0x00010100);
+	nv_icmd(dev, 0x00073d, 0x02800000);
+	nv_icmd(dev, 0x000787, 0x000000cf);
+	nv_icmd(dev, 0x00078c, 0x00000008);
+	nv_icmd(dev, 0x000792, 0x00000001);
+	nv_icmd(dev, 0x000794, 0x00000001);
+	nv_icmd(dev, 0x000795, 0x00000001);
+	nv_icmd(dev, 0x000796, 0x00000001);
+	nv_icmd(dev, 0x000797, 0x000000cf);
+	nv_icmd(dev, 0x000836, 0x00000001);
+	nv_icmd(dev, 0x00079a, 0x00000002);
+	nv_icmd(dev, 0x000833, 0x04444480);
+	nv_icmd(dev, 0x0007a1, 0x00000001);
+	nv_icmd(dev, 0x0007a3, 0x00000001);
+	nv_icmd(dev, 0x0007a4, 0x00000001);
+	nv_icmd(dev, 0x0007a5, 0x00000001);
+	nv_icmd(dev, 0x000831, 0x00000004);
+	nv_icmd(dev, 0x000b07, 0x00000002);
+	nv_icmd(dev, 0x000b08, 0x00000100);
+	nv_icmd(dev, 0x000b09, 0x00000100);
+	nv_icmd(dev, 0x000b0a, 0x00000001);
+	nv_icmd(dev, 0x000a04, 0x000000ff);
+	nv_icmd(dev, 0x000a0b, 0x00000040);
+	nv_icmd(dev, 0x00097f, 0x00000100);
+	nv_icmd(dev, 0x000a02, 0x00000001);
+	nv_icmd(dev, 0x000809, 0x00000007);
+	nv_icmd(dev, 0x00c221, 0x00000040);
+	nv_icmd(dev, 0x00c1b0, 0x0000000f);
+	nv_icmd(dev, 0x00c1b1, 0x0000000f);
+	nv_icmd(dev, 0x00c1b2, 0x0000000f);
+	nv_icmd(dev, 0x00c1b3, 0x0000000f);
+	nv_icmd(dev, 0x00c1b4, 0x0000000f);
+	nv_icmd(dev, 0x00c1b5, 0x0000000f);
+	nv_icmd(dev, 0x00c1b6, 0x0000000f);
+	nv_icmd(dev, 0x00c1b7, 0x0000000f);
+	nv_icmd(dev, 0x00c1b8, 0x0fac6881);
+	nv_icmd(dev, 0x00c1b9, 0x00fac688);
+	nv_icmd(dev, 0x00c401, 0x00000001);
+	nv_icmd(dev, 0x00c402, 0x00010001);
+	nv_icmd(dev, 0x00c403, 0x00000001);
+	nv_icmd(dev, 0x00c404, 0x00000001);
+	nv_icmd(dev, 0x00c40e, 0x00000020);
+	nv_icmd(dev, 0x00c500, 0x00000003);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_icmd(dev, 0x001000, 0x00000002);
+	nv_icmd(dev, 0x0006aa, 0x00000001);
+	nv_icmd(dev, 0x0006ad, 0x00000100);
+	nv_icmd(dev, 0x0006ae, 0x00000100);
+	nv_icmd(dev, 0x0006b1, 0x00000011);
+	nv_icmd(dev, 0x00078c, 0x00000008);
+	nv_icmd(dev, 0x000792, 0x00000001);
+	nv_icmd(dev, 0x000794, 0x00000001);
+	nv_icmd(dev, 0x000795, 0x00000001);
+	nv_icmd(dev, 0x000796, 0x00000001);
+	nv_icmd(dev, 0x000797, 0x000000cf);
+	nv_icmd(dev, 0x00079a, 0x00000002);
+	nv_icmd(dev, 0x000833, 0x04444480);
+	nv_icmd(dev, 0x0007a1, 0x00000001);
+	nv_icmd(dev, 0x0007a3, 0x00000001);
+	nv_icmd(dev, 0x0007a4, 0x00000001);
+	nv_icmd(dev, 0x0007a5, 0x00000001);
+	nv_icmd(dev, 0x000831, 0x00000004);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_icmd(dev, 0x001000, 0x00000008);
+	nv_icmd(dev, 0x000039, 0x00000000);
+	nv_icmd(dev, 0x00003a, 0x00000000);
+	nv_icmd(dev, 0x00003b, 0x00000000);
+	nv_icmd(dev, 0x000380, 0x00000001);
+	nv_icmd(dev, 0x000366, 0x00000000);
+	nv_icmd(dev, 0x000367, 0x00000000);
+	nv_icmd(dev, 0x000368, 0x00000fff);
+	nv_icmd(dev, 0x000370, 0x00000000);
+	nv_icmd(dev, 0x000371, 0x00000000);
+	nv_icmd(dev, 0x000372, 0x000fffff);
+	nv_icmd(dev, 0x000813, 0x00000006);
+	nv_icmd(dev, 0x000814, 0x00000008);
+	nv_icmd(dev, 0x000957, 0x00000003);
+	nv_icmd(dev, 0x000818, 0x00000000);
+	nv_icmd(dev, 0x000819, 0x00000000);
+	nv_icmd(dev, 0x00081a, 0x00000000);
+	nv_icmd(dev, 0x00081b, 0x00000000);
+	nv_icmd(dev, 0x00081c, 0x00000000);
+	nv_icmd(dev, 0x00081d, 0x00000000);
+	nv_icmd(dev, 0x00081e, 0x00000000);
+	nv_icmd(dev, 0x00081f, 0x00000000);
+	nv_icmd(dev, 0x000848, 0x00000000);
+	nv_icmd(dev, 0x000849, 0x00000000);
+	nv_icmd(dev, 0x00084a, 0x00000000);
+	nv_icmd(dev, 0x00084b, 0x00000000);
+	nv_icmd(dev, 0x00084c, 0x00000000);
+	nv_icmd(dev, 0x00084d, 0x00000000);
+	nv_icmd(dev, 0x00084e, 0x00000000);
+	nv_icmd(dev, 0x00084f, 0x00000000);
+	nv_icmd(dev, 0x000850, 0x00000000);
+	nv_icmd(dev, 0x000851, 0x00000000);
+	nv_icmd(dev, 0x000852, 0x00000000);
+	nv_icmd(dev, 0x000853, 0x00000000);
+	nv_icmd(dev, 0x000854, 0x00000000);
+	nv_icmd(dev, 0x000855, 0x00000000);
+	nv_icmd(dev, 0x000856, 0x00000000);
+	nv_icmd(dev, 0x000857, 0x00000000);
+	nv_icmd(dev, 0x000738, 0x00000000);
+	nv_icmd(dev, 0x000b07, 0x00000002);
+	nv_icmd(dev, 0x000b08, 0x00000100);
+	nv_icmd(dev, 0x000b09, 0x00000100);
+	nv_icmd(dev, 0x000b0a, 0x00000001);
+	nv_icmd(dev, 0x000a04, 0x000000ff);
+	nv_icmd(dev, 0x00097f, 0x00000100);
+	nv_icmd(dev, 0x000a02, 0x00000001);
+	nv_icmd(dev, 0x000809, 0x00000007);
+	nv_icmd(dev, 0x00c221, 0x00000040);
+	nv_icmd(dev, 0x00c401, 0x00000001);
+	nv_icmd(dev, 0x00c402, 0x00010001);
+	nv_icmd(dev, 0x00c403, 0x00000001);
+	nv_icmd(dev, 0x00c404, 0x00000001);
+	nv_icmd(dev, 0x00c40e, 0x00000020);
+	nv_icmd(dev, 0x00c500, 0x00000003);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_icmd(dev, 0x001000, 0x00000001);
+	nv_icmd(dev, 0x000b07, 0x00000002);
+	nv_icmd(dev, 0x000b08, 0x00000100);
+	nv_icmd(dev, 0x000b09, 0x00000100);
+	nv_icmd(dev, 0x000b0a, 0x00000001);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_wr32(dev, 0x400208, 0x00000000);
+}
+
+static void
+nv_mthd(struct drm_device *dev, u32 class, u32 mthd, u32 data)
+{
+	nv_wr32(dev, 0x40448c, data);
+	nv_wr32(dev, 0x404488, 0x80000000 | (mthd << 14) | class);
+}
+
+static void
+nve0_grctx_generate_a097(struct drm_device *dev)
+{
+	nv_mthd(dev, 0xa097, 0x0800, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0840, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0880, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0900, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0940, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0980, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0804, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0844, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0884, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0904, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0944, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0984, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0808, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0848, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0888, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x08c8, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0908, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0948, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0988, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x09c8, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x080c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x084c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x088c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x08cc, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x090c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x094c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x098c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x09cc, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x0810, 0x000000cf);
+	nv_mthd(dev, 0xa097, 0x0850, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0890, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0910, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0950, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0990, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0814, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0854, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0894, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x08d4, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0914, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0954, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0994, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x09d4, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0818, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0858, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0898, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x08d8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0918, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0958, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0998, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x09d8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x081c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x085c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x089c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x091c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x095c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x099c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0820, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0860, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0920, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0960, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ca0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ce0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cf0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ca4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ce4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cf4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ca8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cb8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ce8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cf8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c3c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cbc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ccc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cdc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cfc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1da0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1db0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1de0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1df0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1da4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1db4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1de4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1df4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1da8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1db8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1de8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1df8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d3c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dbc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ddc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dfc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f3c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fa0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fa8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fb8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fe0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fe8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ff0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ff8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fa4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fbc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fdc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fe4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ff4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ffc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2000, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2040, 0x00000011);
+	nv_mthd(dev, 0xa097, 0x2080, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x20c0, 0x00000030);
+	nv_mthd(dev, 0xa097, 0x2100, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x2140, 0x00000051);
+	nv_mthd(dev, 0xa097, 0x200c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x204c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x208c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x20cc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x210c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x214c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x2010, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2050, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2090, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x20d0, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x2110, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x2150, 0x00000004);
+	nv_mthd(dev, 0xa097, 0x0380, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0384, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0388, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x038c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0700, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0710, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0720, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0730, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0704, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0714, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0724, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0734, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0708, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0718, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0728, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0738, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2800, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2804, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2808, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x280c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2810, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2814, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2818, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x281c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2820, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2824, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2828, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x282c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2830, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2834, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2838, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x283c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2840, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2844, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2848, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x284c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2850, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2854, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2858, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x285c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2860, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2864, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2868, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x286c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2870, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2874, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2878, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x287c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2880, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2884, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2888, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x288c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2890, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2894, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2898, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x289c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2900, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2904, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2908, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x290c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2910, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2914, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2918, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x291c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2920, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2924, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2928, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x292c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2930, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2934, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2938, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x293c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2940, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2944, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2948, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x294c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2950, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2954, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2958, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x295c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2960, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2964, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2968, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x296c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2970, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2974, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2978, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x297c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2980, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2984, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2988, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x298c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2990, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2994, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2998, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x299c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aa0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ac0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ae0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ba0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0be0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aa4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ac4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ae4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ba4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0be4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aa8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ac8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ae8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ba8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0be8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0acc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ab0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ad0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0af0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bf0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ab4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ad4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0af4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bf4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ca0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ce0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cf0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ca4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ce4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cf4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ca8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cb8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ce8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cf8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c0c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c1c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c2c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c3c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c4c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c5c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c6c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c7c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c8c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c9c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cac, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cbc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0ccc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cdc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cec, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cfc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0d00, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d08, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d10, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d18, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d20, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d28, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d30, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d38, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d04, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d0c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d14, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d1c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d24, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d2c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d34, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d3c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ea0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0eb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ec0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ed0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ee0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ef0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e04, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e14, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e24, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e34, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e44, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e54, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e64, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e74, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e84, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e94, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ea4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0eb4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ec4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ed4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ee4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ef4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e08, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e18, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e28, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e38, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e48, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e58, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e68, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e78, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e88, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e98, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ea8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0eb8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ec8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ed8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ee8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ef8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1e00, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e20, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e40, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e60, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e80, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ea0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ec0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ee0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e04, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e24, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e44, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e64, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e84, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ea4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ec4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ee4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e08, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e28, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e48, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e68, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e88, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ea8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ec8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ee8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e0c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e2c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e4c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e6c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e8c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eac, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ecc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eec, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e10, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e30, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e50, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e70, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e90, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eb0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ed0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ef0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e14, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e34, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e54, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e74, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e94, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1eb4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ed4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ef4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e18, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e38, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e58, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e78, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e98, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eb8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ed8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ef8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x3400, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3404, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3408, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x340c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3410, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3414, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3418, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x341c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3420, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3424, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3428, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x342c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3430, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3434, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3438, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x343c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3440, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3444, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3448, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x344c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3450, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3454, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3458, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x345c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3460, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3464, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3468, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x346c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3470, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3474, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3478, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x347c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3480, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3484, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3488, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x348c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3490, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3494, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3498, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x349c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3500, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3504, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3508, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x350c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3510, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3514, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3518, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x351c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3520, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3524, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3528, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x352c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3530, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3534, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3538, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x353c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3540, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3544, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3548, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x354c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3550, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3554, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3558, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x355c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3560, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3564, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3568, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x356c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3570, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3574, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3578, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x357c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3580, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3584, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3588, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x358c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3590, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3594, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3598, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x359c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x030c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1944, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1514, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d68, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x121c, 0x0fac6881);
+	nv_mthd(dev, 0xa097, 0x0fac, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1538, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0fe0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fe4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fe8, 0x00000014);
+	nv_mthd(dev, 0xa097, 0x0fec, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0ff0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x179c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1228, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x122c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x1230, 0x00010001);
+	nv_mthd(dev, 0xa097, 0x07f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15b4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1534, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x153c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x16b4, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x0fbc, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0fc0, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0fc4, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0fc8, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0df8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dfc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1948, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1970, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x161c, 0x000009f0);
+	nv_mthd(dev, 0xa097, 0x0dcc, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x163c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1160, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1164, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1168, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x116c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1170, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1174, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1178, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x117c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1180, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1184, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1188, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x118c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1190, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1194, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1198, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x119c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11a0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11a4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11a8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11ac, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11b0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11b4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11b8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11bc, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11c0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11c4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11c8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11cc, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11d0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11d4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11d8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11dc, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1880, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1884, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1888, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x188c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1890, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1894, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1898, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x189c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17d0, 0x000000ff);
+	nv_mthd(dev, 0xa097, 0x17d4, 0xffffffff);
+	nv_mthd(dev, 0xa097, 0x17d8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x17dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1434, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1438, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dec, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x13a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1318, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1644, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0748, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0de8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1648, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1120, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1124, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1128, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x112c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1118, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x164c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1658, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1910, 0x00000290);
+	nv_mthd(dev, 0xa097, 0x1518, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x165c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1520, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1604, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1570, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x13b0, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x13b4, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x020c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1670, 0x30201000);
+	nv_mthd(dev, 0xa097, 0x1674, 0x70605040);
+	nv_mthd(dev, 0xa097, 0x1678, 0xb8a89888);
+	nv_mthd(dev, 0xa097, 0x167c, 0xf8e8d8c8);
+	nv_mthd(dev, 0xa097, 0x166c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1680, 0x00ffff00);
+	nv_mthd(dev, 0xa097, 0x12d0, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x12d4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1684, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1688, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dac, 0x00001b02);
+	nv_mthd(dev, 0xa097, 0x0db0, 0x00001b02);
+	nv_mthd(dev, 0xa097, 0x0db4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x168c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x156c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x187c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1110, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0dc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1234, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1690, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12ac, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0790, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0794, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0798, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x079c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x077c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1000, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x10fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1290, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0218, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x12d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12dc, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x0d94, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x155c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1560, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1564, 0x00000fff);
+	nv_mthd(dev, 0xa097, 0x1574, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1578, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x157c, 0x000fffff);
+	nv_mthd(dev, 0xa097, 0x1354, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1610, 0x00000012);
+	nv_mthd(dev, 0xa097, 0x1608, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x160c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x260c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x162c, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x0210, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0320, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0324, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0328, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x032c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0330, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0334, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0338, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0750, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0760, 0x39291909);
+	nv_mthd(dev, 0xa097, 0x0764, 0x79695949);
+	nv_mthd(dev, 0xa097, 0x0768, 0xb9a99989);
+	nv_mthd(dev, 0xa097, 0x076c, 0xf9e9d9c9);
+	nv_mthd(dev, 0xa097, 0x0770, 0x30201000);
+	nv_mthd(dev, 0xa097, 0x0774, 0x70605040);
+	nv_mthd(dev, 0xa097, 0x0778, 0x00009080);
+	nv_mthd(dev, 0xa097, 0x0780, 0x39291909);
+	nv_mthd(dev, 0xa097, 0x0784, 0x79695949);
+	nv_mthd(dev, 0xa097, 0x0788, 0xb9a99989);
+	nv_mthd(dev, 0xa097, 0x078c, 0xf9e9d9c9);
+	nv_mthd(dev, 0xa097, 0x07d0, 0x30201000);
+	nv_mthd(dev, 0xa097, 0x07d4, 0x70605040);
+	nv_mthd(dev, 0xa097, 0x07d8, 0x00009080);
+	nv_mthd(dev, 0xa097, 0x037c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0740, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0744, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2600, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1918, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x191c, 0x00000900);
+	nv_mthd(dev, 0xa097, 0x1920, 0x00000405);
+	nv_mthd(dev, 0xa097, 0x1308, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1924, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x13ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x192c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x193c, 0x00002c1c);
+	nv_mthd(dev, 0xa097, 0x0d7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x02c0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1510, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1940, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ff4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ff8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x194c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1950, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1968, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1590, 0x0000003f);
+	nv_mthd(dev, 0xa097, 0x07e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x196c, 0x00000011);
+	nv_mthd(dev, 0xa097, 0x02e4, 0x0000b001);
+	nv_mthd(dev, 0xa097, 0x036c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0370, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x197c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x02d8, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x1980, 0x00000080);
+	nv_mthd(dev, 0xa097, 0x1504, 0x00000080);
+	nv_mthd(dev, 0xa097, 0x1984, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0300, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x13a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1310, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1314, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1380, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1384, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1388, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x138c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1390, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1394, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x139c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1398, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1594, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1598, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x159c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15a0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15a4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0f54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fa0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x130c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1360, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1364, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1368, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x136c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1370, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1374, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1378, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x137c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x133c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1340, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1344, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1348, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x134c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1350, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1358, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x12e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x131c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1320, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1324, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1328, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1140, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19c8, 0x00001500);
+	nv_mthd(dev, 0xa097, 0x135c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19e0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19e4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19e8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19ec, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19f0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19f4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19f8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19fc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19cc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a00, 0x00001111);
+	nv_mthd(dev, 0xa097, 0x1a04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d6c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d70, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x10f8, 0x00001010);
+	nv_mthd(dev, 0xa097, 0x0d80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0da0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1508, 0x80000000);
+	nv_mthd(dev, 0xa097, 0x150c, 0x40000000);
+	nv_mthd(dev, 0xa097, 0x1668, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0318, 0x00000008);
+	nv_mthd(dev, 0xa097, 0x031c, 0x00000008);
+	nv_mthd(dev, 0xa097, 0x0d9c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0374, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0378, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x07dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x074c, 0x00000055);
+	nv_mthd(dev, 0xa097, 0x1420, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x17bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17c4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1008, 0x00000008);
+	nv_mthd(dev, 0xa097, 0x100c, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x1010, 0x0000012c);
+	nv_mthd(dev, 0xa097, 0x0d60, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x075c, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x1018, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x101c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1020, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x1024, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1444, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1448, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x144c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0360, 0x20164010);
+	nv_mthd(dev, 0xa097, 0x0364, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x0368, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0de4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0204, 0x00000006);
+	nv_mthd(dev, 0xa097, 0x0208, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x02cc, 0x003fffff);
+	nv_mthd(dev, 0xa097, 0x02d0, 0x003fffff);
+	nv_mthd(dev, 0xa097, 0x1220, 0x00000005);
+	nv_mthd(dev, 0xa097, 0x0fdc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f98, 0x00400008);
+	nv_mthd(dev, 0xa097, 0x1284, 0x08000080);
+	nv_mthd(dev, 0xa097, 0x1450, 0x00400008);
+	nv_mthd(dev, 0xa097, 0x1454, 0x08000080);
+	nv_mthd(dev, 0xa097, 0x0214, 0x00000000);
+}
+
+static void
+nve0_grctx_generate_902d(struct drm_device *dev)
+{
+	nv_mthd(dev, 0x902d, 0x0200, 0x000000cf);
+	nv_mthd(dev, 0x902d, 0x0204, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0208, 0x00000020);
+	nv_mthd(dev, 0x902d, 0x020c, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0210, 0x00000000);
+	nv_mthd(dev, 0x902d, 0x0214, 0x00000080);
+	nv_mthd(dev, 0x902d, 0x0218, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x021c, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x0220, 0x00000000);
+	nv_mthd(dev, 0x902d, 0x0224, 0x00000000);
+	nv_mthd(dev, 0x902d, 0x0230, 0x000000cf);
+	nv_mthd(dev, 0x902d, 0x0234, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0238, 0x00000020);
+	nv_mthd(dev, 0x902d, 0x023c, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0244, 0x00000080);
+	nv_mthd(dev, 0x902d, 0x0248, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x024c, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x3410, 0x00000000);
+}
+
+static void
+nve0_graph_generate_unk40xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404010, 0x0);
+	nv_wr32(dev, 0x404014, 0x0);
+	nv_wr32(dev, 0x404018, 0x0);
+	nv_wr32(dev, 0x40401c, 0x0);
+	nv_wr32(dev, 0x404020, 0x0);
+	nv_wr32(dev, 0x404024, 0xe000);
+	nv_wr32(dev, 0x404028, 0x0);
+	nv_wr32(dev, 0x4040a8, 0x0);
+	nv_wr32(dev, 0x4040ac, 0x0);
+	nv_wr32(dev, 0x4040b0, 0x0);
+	nv_wr32(dev, 0x4040b4, 0x0);
+	nv_wr32(dev, 0x4040b8, 0x0);
+	nv_wr32(dev, 0x4040bc, 0x0);
+	nv_wr32(dev, 0x4040c0, 0x0);
+	nv_wr32(dev, 0x4040c4, 0x0);
+	nv_wr32(dev, 0x4040c8, 0xf800008f);
+	nv_wr32(dev, 0x4040d0, 0x0);
+	nv_wr32(dev, 0x4040d4, 0x0);
+	nv_wr32(dev, 0x4040d8, 0x0);
+	nv_wr32(dev, 0x4040dc, 0x0);
+	nv_wr32(dev, 0x4040e0, 0x0);
+	nv_wr32(dev, 0x4040e4, 0x0);
+	nv_wr32(dev, 0x4040e8, 0x1000);
+	nv_wr32(dev, 0x4040f8, 0x0);
+	nv_wr32(dev, 0x404130, 0x0);
+	nv_wr32(dev, 0x404134, 0x0);
+	nv_wr32(dev, 0x404138, 0x20000040);
+	nv_wr32(dev, 0x404150, 0x2e);
+	nv_wr32(dev, 0x404154, 0x400);
+	nv_wr32(dev, 0x404158, 0x200);
+	nv_wr32(dev, 0x404164, 0x55);
+	nv_wr32(dev, 0x4041a0, 0x0);
+	nv_wr32(dev, 0x4041a4, 0x0);
+	nv_wr32(dev, 0x4041a8, 0x0);
+	nv_wr32(dev, 0x4041ac, 0x0);
+	nv_wr32(dev, 0x404200, 0x0);
+	nv_wr32(dev, 0x404204, 0x0);
+	nv_wr32(dev, 0x404208, 0x0);
+	nv_wr32(dev, 0x40420c, 0x0);
+}
+
+static void
+nve0_graph_generate_unk44xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404404, 0x0);
+	nv_wr32(dev, 0x404408, 0x0);
+	nv_wr32(dev, 0x40440c, 0x0);
+	nv_wr32(dev, 0x404410, 0x0);
+	nv_wr32(dev, 0x404414, 0x0);
+	nv_wr32(dev, 0x404418, 0x0);
+	nv_wr32(dev, 0x40441c, 0x0);
+	nv_wr32(dev, 0x404420, 0x0);
+	nv_wr32(dev, 0x404424, 0x0);
+	nv_wr32(dev, 0x404428, 0x0);
+	nv_wr32(dev, 0x40442c, 0x0);
+	nv_wr32(dev, 0x404430, 0x0);
+	nv_wr32(dev, 0x404434, 0x0);
+	nv_wr32(dev, 0x404438, 0x0);
+	nv_wr32(dev, 0x404460, 0x0);
+	nv_wr32(dev, 0x404464, 0x0);
+	nv_wr32(dev, 0x404468, 0xffffff);
+	nv_wr32(dev, 0x40446c, 0x0);
+	nv_wr32(dev, 0x404480, 0x1);
+	nv_wr32(dev, 0x404498, 0x1);
+}
+
+static void
+nve0_graph_generate_unk46xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404604, 0x14);
+	nv_wr32(dev, 0x404608, 0x0);
+	nv_wr32(dev, 0x40460c, 0x3fff);
+	nv_wr32(dev, 0x404610, 0x100);
+	nv_wr32(dev, 0x404618, 0x0);
+	nv_wr32(dev, 0x40461c, 0x0);
+	nv_wr32(dev, 0x404620, 0x0);
+	nv_wr32(dev, 0x404624, 0x0);
+	nv_wr32(dev, 0x40462c, 0x0);
+	nv_wr32(dev, 0x404630, 0x0);
+	nv_wr32(dev, 0x404640, 0x0);
+	nv_wr32(dev, 0x404654, 0x0);
+	nv_wr32(dev, 0x404660, 0x0);
+	nv_wr32(dev, 0x404678, 0x0);
+	nv_wr32(dev, 0x40467c, 0x2);
+	nv_wr32(dev, 0x404680, 0x0);
+	nv_wr32(dev, 0x404684, 0x0);
+	nv_wr32(dev, 0x404688, 0x0);
+	nv_wr32(dev, 0x40468c, 0x0);
+	nv_wr32(dev, 0x404690, 0x0);
+	nv_wr32(dev, 0x404694, 0x0);
+	nv_wr32(dev, 0x404698, 0x0);
+	nv_wr32(dev, 0x40469c, 0x0);
+	nv_wr32(dev, 0x4046a0, 0x7f0080);
+	nv_wr32(dev, 0x4046a4, 0x0);
+	nv_wr32(dev, 0x4046a8, 0x0);
+	nv_wr32(dev, 0x4046ac, 0x0);
+	nv_wr32(dev, 0x4046b0, 0x0);
+	nv_wr32(dev, 0x4046b4, 0x0);
+	nv_wr32(dev, 0x4046b8, 0x0);
+	nv_wr32(dev, 0x4046bc, 0x0);
+	nv_wr32(dev, 0x4046c0, 0x0);
+	nv_wr32(dev, 0x4046c8, 0x0);
+	nv_wr32(dev, 0x4046cc, 0x0);
+	nv_wr32(dev, 0x4046d0, 0x0);
+}
+
+static void
+nve0_graph_generate_unk47xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404700, 0x0);
+	nv_wr32(dev, 0x404704, 0x0);
+	nv_wr32(dev, 0x404708, 0x0);
+	nv_wr32(dev, 0x404718, 0x0);
+	nv_wr32(dev, 0x40471c, 0x0);
+	nv_wr32(dev, 0x404720, 0x0);
+	nv_wr32(dev, 0x404724, 0x0);
+	nv_wr32(dev, 0x404728, 0x0);
+	nv_wr32(dev, 0x40472c, 0x0);
+	nv_wr32(dev, 0x404730, 0x0);
+	nv_wr32(dev, 0x404734, 0x100);
+	nv_wr32(dev, 0x404738, 0x0);
+	nv_wr32(dev, 0x40473c, 0x0);
+	nv_wr32(dev, 0x404744, 0x0);
+	nv_wr32(dev, 0x404748, 0x0);
+	nv_wr32(dev, 0x404754, 0x0);
+}
+
+static void
+nve0_graph_generate_unk58xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x405800, 0xf8000bf);
+	nv_wr32(dev, 0x405830, 0x2180648);
+	nv_wr32(dev, 0x405834, 0x8000000);
+	nv_wr32(dev, 0x405838, 0x0);
+	nv_wr32(dev, 0x405854, 0x0);
+	nv_wr32(dev, 0x405870, 0x1);
+	nv_wr32(dev, 0x405874, 0x1);
+	nv_wr32(dev, 0x405878, 0x1);
+	nv_wr32(dev, 0x40587c, 0x1);
+	nv_wr32(dev, 0x405a00, 0x0);
+	nv_wr32(dev, 0x405a04, 0x0);
+	nv_wr32(dev, 0x405a18, 0x0);
+	nv_wr32(dev, 0x405b00, 0x0);
+	nv_wr32(dev, 0x405b10, 0x1000);
+}
+
+static void
+nve0_graph_generate_unk60xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x406020, 0x4103c1);
+	nv_wr32(dev, 0x406028, 0x1);
+	nv_wr32(dev, 0x40602c, 0x1);
+	nv_wr32(dev, 0x406030, 0x1);
+	nv_wr32(dev, 0x406034, 0x1);
+}
+
+static void
+nve0_graph_generate_unk64xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x4064a8, 0x0);
+	nv_wr32(dev, 0x4064ac, 0x3fff);
+	nv_wr32(dev, 0x4064b4, 0x0);
+	nv_wr32(dev, 0x4064b8, 0x0);
+	nv_wr32(dev, 0x4064c0, 0x801a00f0);
+	nv_wr32(dev, 0x4064c4, 0x192ffff);
+	nv_wr32(dev, 0x4064c8, 0x1800600);
+	nv_wr32(dev, 0x4064cc, 0x0);
+	nv_wr32(dev, 0x4064d0, 0x0);
+	nv_wr32(dev, 0x4064d4, 0x0);
+	nv_wr32(dev, 0x4064d8, 0x0);
+	nv_wr32(dev, 0x4064dc, 0x0);
+	nv_wr32(dev, 0x4064e0, 0x0);
+	nv_wr32(dev, 0x4064e4, 0x0);
+	nv_wr32(dev, 0x4064e8, 0x0);
+	nv_wr32(dev, 0x4064ec, 0x0);
+	nv_wr32(dev, 0x4064fc, 0x22a);
+}
+
+static void
+nve0_graph_generate_unk70xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x407040, 0x0);
+}
+
+static void
+nve0_graph_generate_unk78xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x407804, 0x23);
+	nv_wr32(dev, 0x40780c, 0xa418820);
+	nv_wr32(dev, 0x407810, 0x62080e6);
+	nv_wr32(dev, 0x407814, 0x20398a4);
+	nv_wr32(dev, 0x407818, 0xe629062);
+	nv_wr32(dev, 0x40781c, 0xa418820);
+	nv_wr32(dev, 0x407820, 0xe6);
+	nv_wr32(dev, 0x4078bc, 0x103);
+}
+
+static void
+nve0_graph_generate_unk80xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x408000, 0x0);
+	nv_wr32(dev, 0x408004, 0x0);
+	nv_wr32(dev, 0x408008, 0x30);
+	nv_wr32(dev, 0x40800c, 0x0);
+	nv_wr32(dev, 0x408010, 0x0);
+	nv_wr32(dev, 0x408014, 0x69);
+	nv_wr32(dev, 0x408018, 0xe100e100);
+	nv_wr32(dev, 0x408064, 0x0);
+}
+
+static void
+nve0_graph_generate_unk88xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x408800, 0x2802a3c);
+	nv_wr32(dev, 0x408804, 0x40);
+	nv_wr32(dev, 0x408808, 0x1043e005);
+	nv_wr32(dev, 0x408840, 0xb);
+	nv_wr32(dev, 0x408900, 0x3080b801);
+	nv_wr32(dev, 0x408904, 0x62000001);
+	nv_wr32(dev, 0x408908, 0xc8102f);
+	nv_wr32(dev, 0x408980, 0x11d);
+}
+
+static void
+nve0_graph_generate_gpc(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x418380, 0x16);
+	nv_wr32(dev, 0x418400, 0x38004e00);
+	nv_wr32(dev, 0x418404, 0x71e0ffff);
+	nv_wr32(dev, 0x41840c, 0x1008);
+	nv_wr32(dev, 0x418410, 0xfff0fff);
+	nv_wr32(dev, 0x418414, 0x2200fff);
+	nv_wr32(dev, 0x418450, 0x0);
+	nv_wr32(dev, 0x418454, 0x0);
+	nv_wr32(dev, 0x418458, 0x0);
+	nv_wr32(dev, 0x41845c, 0x0);
+	nv_wr32(dev, 0x418460, 0x0);
+	nv_wr32(dev, 0x418464, 0x0);
+	nv_wr32(dev, 0x418468, 0x1);
+	nv_wr32(dev, 0x41846c, 0x0);
+	nv_wr32(dev, 0x418470, 0x0);
+	nv_wr32(dev, 0x418600, 0x1f);
+	nv_wr32(dev, 0x418684, 0xf);
+	nv_wr32(dev, 0x418700, 0x2);
+	nv_wr32(dev, 0x418704, 0x80);
+	nv_wr32(dev, 0x418708, 0x0);
+	nv_wr32(dev, 0x41870c, 0x0);
+	nv_wr32(dev, 0x418710, 0x0);
+	nv_wr32(dev, 0x418800, 0x7006860a);
+	nv_wr32(dev, 0x418808, 0x0);
+	nv_wr32(dev, 0x41880c, 0x0);
+	nv_wr32(dev, 0x418810, 0x0);
+	nv_wr32(dev, 0x418828, 0x44);
+	nv_wr32(dev, 0x418830, 0x10000001);
+	nv_wr32(dev, 0x4188d8, 0x8);
+	nv_wr32(dev, 0x4188e0, 0x1000000);
+	nv_wr32(dev, 0x4188e8, 0x0);
+	nv_wr32(dev, 0x4188ec, 0x0);
+	nv_wr32(dev, 0x4188f0, 0x0);
+	nv_wr32(dev, 0x4188f4, 0x0);
+	nv_wr32(dev, 0x4188f8, 0x0);
+	nv_wr32(dev, 0x4188fc, 0x20100018);
+	nv_wr32(dev, 0x41891c, 0xff00ff);
+	nv_wr32(dev, 0x418924, 0x0);
+	nv_wr32(dev, 0x418928, 0xffff00);
+	nv_wr32(dev, 0x41892c, 0xff00);
+	nv_wr32(dev, 0x418a00, 0x0);
+	nv_wr32(dev, 0x418a04, 0x0);
+	nv_wr32(dev, 0x418a08, 0x0);
+	nv_wr32(dev, 0x418a0c, 0x10000);
+	nv_wr32(dev, 0x418a10, 0x0);
+	nv_wr32(dev, 0x418a14, 0x0);
+	nv_wr32(dev, 0x418a18, 0x0);
+	nv_wr32(dev, 0x418a20, 0x0);
+	nv_wr32(dev, 0x418a24, 0x0);
+	nv_wr32(dev, 0x418a28, 0x0);
+	nv_wr32(dev, 0x418a2c, 0x10000);
+	nv_wr32(dev, 0x418a30, 0x0);
+	nv_wr32(dev, 0x418a34, 0x0);
+	nv_wr32(dev, 0x418a38, 0x0);
+	nv_wr32(dev, 0x418a40, 0x0);
+	nv_wr32(dev, 0x418a44, 0x0);
+	nv_wr32(dev, 0x418a48, 0x0);
+	nv_wr32(dev, 0x418a4c, 0x10000);
+	nv_wr32(dev, 0x418a50, 0x0);
+	nv_wr32(dev, 0x418a54, 0x0);
+	nv_wr32(dev, 0x418a58, 0x0);
+	nv_wr32(dev, 0x418a60, 0x0);
+	nv_wr32(dev, 0x418a64, 0x0);
+	nv_wr32(dev, 0x418a68, 0x0);
+	nv_wr32(dev, 0x418a6c, 0x10000);
+	nv_wr32(dev, 0x418a70, 0x0);
+	nv_wr32(dev, 0x418a74, 0x0);
+	nv_wr32(dev, 0x418a78, 0x0);
+	nv_wr32(dev, 0x418a80, 0x0);
+	nv_wr32(dev, 0x418a84, 0x0);
+	nv_wr32(dev, 0x418a88, 0x0);
+	nv_wr32(dev, 0x418a8c, 0x10000);
+	nv_wr32(dev, 0x418a90, 0x0);
+	nv_wr32(dev, 0x418a94, 0x0);
+	nv_wr32(dev, 0x418a98, 0x0);
+	nv_wr32(dev, 0x418aa0, 0x0);
+	nv_wr32(dev, 0x418aa4, 0x0);
+	nv_wr32(dev, 0x418aa8, 0x0);
+	nv_wr32(dev, 0x418aac, 0x10000);
+	nv_wr32(dev, 0x418ab0, 0x0);
+	nv_wr32(dev, 0x418ab4, 0x0);
+	nv_wr32(dev, 0x418ab8, 0x0);
+	nv_wr32(dev, 0x418ac0, 0x0);
+	nv_wr32(dev, 0x418ac4, 0x0);
+	nv_wr32(dev, 0x418ac8, 0x0);
+	nv_wr32(dev, 0x418acc, 0x10000);
+	nv_wr32(dev, 0x418ad0, 0x0);
+	nv_wr32(dev, 0x418ad4, 0x0);
+	nv_wr32(dev, 0x418ad8, 0x0);
+	nv_wr32(dev, 0x418ae0, 0x0);
+	nv_wr32(dev, 0x418ae4, 0x0);
+	nv_wr32(dev, 0x418ae8, 0x0);
+	nv_wr32(dev, 0x418aec, 0x10000);
+	nv_wr32(dev, 0x418af0, 0x0);
+	nv_wr32(dev, 0x418af4, 0x0);
+	nv_wr32(dev, 0x418af8, 0x0);
+	nv_wr32(dev, 0x418b00, 0x6);
+	nv_wr32(dev, 0x418b08, 0xa418820);
+	nv_wr32(dev, 0x418b0c, 0x62080e6);
+	nv_wr32(dev, 0x418b10, 0x20398a4);
+	nv_wr32(dev, 0x418b14, 0xe629062);
+	nv_wr32(dev, 0x418b18, 0xa418820);
+	nv_wr32(dev, 0x418b1c, 0xe6);
+	nv_wr32(dev, 0x418bb8, 0x103);
+	nv_wr32(dev, 0x418c08, 0x1);
+	nv_wr32(dev, 0x418c10, 0x0);
+	nv_wr32(dev, 0x418c14, 0x0);
+	nv_wr32(dev, 0x418c18, 0x0);
+	nv_wr32(dev, 0x418c1c, 0x0);
+	nv_wr32(dev, 0x418c20, 0x0);
+	nv_wr32(dev, 0x418c24, 0x0);
+	nv_wr32(dev, 0x418c28, 0x0);
+	nv_wr32(dev, 0x418c2c, 0x0);
+	nv_wr32(dev, 0x418c40, 0xffffffff);
+	nv_wr32(dev, 0x418c6c, 0x1);
+	nv_wr32(dev, 0x418c80, 0x20200004);
+	nv_wr32(dev, 0x418c8c, 0x1);
+	nv_wr32(dev, 0x419000, 0x780);
+	nv_wr32(dev, 0x419004, 0x0);
+	nv_wr32(dev, 0x419008, 0x0);
+	nv_wr32(dev, 0x419014, 0x4);
+}
+
+static void
+nve0_graph_generate_tpc(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x419848, 0x0);
+	nv_wr32(dev, 0x419864, 0x129);
+	nv_wr32(dev, 0x419888, 0x0);
+	nv_wr32(dev, 0x419a00, 0xf0);
+	nv_wr32(dev, 0x419a04, 0x1);
+	nv_wr32(dev, 0x419a08, 0x21);
+	nv_wr32(dev, 0x419a0c, 0x20000);
+	nv_wr32(dev, 0x419a10, 0x0);
+	nv_wr32(dev, 0x419a14, 0x200);
+	nv_wr32(dev, 0x419a1c, 0xc000);
+	nv_wr32(dev, 0x419a20, 0x800);
+	nv_wr32(dev, 0x419a30, 0x1);
+	nv_wr32(dev, 0x419ac4, 0x37f440);
+	nv_wr32(dev, 0x419c00, 0xa);
+	nv_wr32(dev, 0x419c04, 0x80000006);
+	nv_wr32(dev, 0x419c08, 0x2);
+	nv_wr32(dev, 0x419c20, 0x0);
+	nv_wr32(dev, 0x419c24, 0x84210);
+	nv_wr32(dev, 0x419c28, 0x3efbefbe);
+	nv_wr32(dev, 0x419ce8, 0x0);
+	nv_wr32(dev, 0x419cf4, 0x3203);
+	nv_wr32(dev, 0x419e04, 0x0);
+	nv_wr32(dev, 0x419e08, 0x0);
+	nv_wr32(dev, 0x419e0c, 0x0);
+	nv_wr32(dev, 0x419e10, 0x402);
+	nv_wr32(dev, 0x419e44, 0x13eff2);
+	nv_wr32(dev, 0x419e48, 0x0);
+	nv_wr32(dev, 0x419e4c, 0x7f);
+	nv_wr32(dev, 0x419e50, 0x0);
+	nv_wr32(dev, 0x419e54, 0x0);
+	nv_wr32(dev, 0x419e58, 0x0);
+	nv_wr32(dev, 0x419e5c, 0x0);
+	nv_wr32(dev, 0x419e60, 0x0);
+	nv_wr32(dev, 0x419e64, 0x0);
+	nv_wr32(dev, 0x419e68, 0x0);
+	nv_wr32(dev, 0x419e6c, 0x0);
+	nv_wr32(dev, 0x419e70, 0x0);
+	nv_wr32(dev, 0x419e74, 0x0);
+	nv_wr32(dev, 0x419e78, 0x0);
+	nv_wr32(dev, 0x419e7c, 0x0);
+	nv_wr32(dev, 0x419e80, 0x0);
+	nv_wr32(dev, 0x419e84, 0x0);
+	nv_wr32(dev, 0x419e88, 0x0);
+	nv_wr32(dev, 0x419e8c, 0x0);
+	nv_wr32(dev, 0x419e90, 0x0);
+	nv_wr32(dev, 0x419e94, 0x0);
+	nv_wr32(dev, 0x419e98, 0x0);
+	nv_wr32(dev, 0x419eac, 0x1fcf);
+	nv_wr32(dev, 0x419eb0, 0xd3f);
+	nv_wr32(dev, 0x419ec8, 0x1304f);
+	nv_wr32(dev, 0x419f30, 0x0);
+	nv_wr32(dev, 0x419f34, 0x0);
+	nv_wr32(dev, 0x419f38, 0x0);
+	nv_wr32(dev, 0x419f3c, 0x0);
+	nv_wr32(dev, 0x419f40, 0x0);
+	nv_wr32(dev, 0x419f44, 0x0);
+	nv_wr32(dev, 0x419f48, 0x0);
+	nv_wr32(dev, 0x419f4c, 0x0);
+	nv_wr32(dev, 0x419f58, 0x0);
+	nv_wr32(dev, 0x419f78, 0xb);
+}
+
+static void
+nve0_graph_generate_tpcunk(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x41be24, 0x6);
+	nv_wr32(dev, 0x41bec0, 0x12180000);
+	nv_wr32(dev, 0x41bec4, 0x37f7f);
+	nv_wr32(dev, 0x41bee4, 0x6480430);
+	nv_wr32(dev, 0x41bf00, 0xa418820);
+	nv_wr32(dev, 0x41bf04, 0x62080e6);
+	nv_wr32(dev, 0x41bf08, 0x20398a4);
+	nv_wr32(dev, 0x41bf0c, 0xe629062);
+	nv_wr32(dev, 0x41bf10, 0xa418820);
+	nv_wr32(dev, 0x41bf14, 0xe6);
+	nv_wr32(dev, 0x41bfd0, 0x900103);
+	nv_wr32(dev, 0x41bfe0, 0x400001);
+	nv_wr32(dev, 0x41bfe4, 0x0);
+}
+
+int
+nve0_grctx_generate(struct nouveau_channel *chan)
+{
+	struct nve0_graph_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_GR);
+	struct nve0_graph_chan *grch = chan->engctx[NVOBJ_ENGINE_GR];
+	struct drm_device *dev = chan->dev;
+	u32 data[6] = {}, data2[2] = {}, tmp;
+	u32 tpc_set = 0, tpc_mask = 0;
+	u8 tpcnr[GPC_MAX], a, b;
+	u8 shift, ntpcv;
+	int i, gpc, tpc, id;
+
+	nv_mask(dev, 0x000260, 0x00000001, 0x00000000);
+	nv_wr32(dev, 0x400204, 0x00000000);
+	nv_wr32(dev, 0x400208, 0x00000000);
+
+	nve0_graph_generate_unk40xx(dev);
+	nve0_graph_generate_unk44xx(dev);
+	nve0_graph_generate_unk46xx(dev);
+	nve0_graph_generate_unk47xx(dev);
+	nve0_graph_generate_unk58xx(dev);
+	nve0_graph_generate_unk60xx(dev);
+	nve0_graph_generate_unk64xx(dev);
+	nve0_graph_generate_unk70xx(dev);
+	nve0_graph_generate_unk78xx(dev);
+	nve0_graph_generate_unk80xx(dev);
+	nve0_graph_generate_unk88xx(dev);
+	nve0_graph_generate_gpc(dev);
+	nve0_graph_generate_tpc(dev);
+	nve0_graph_generate_tpcunk(dev);
+
+	nv_wr32(dev, 0x404154, 0x0);
+
+	for (i = 0; i < grch->mmio_nr * 8; i += 8) {
+		u32 reg = nv_ro32(grch->mmio, i + 0);
+		u32 val = nv_ro32(grch->mmio, i + 4);
+		nv_wr32(dev, reg, val);
+	}
+
+	nv_wr32(dev, 0x418c6c, 0x1);
+	nv_wr32(dev, 0x41980c, 0x10);
+	nv_wr32(dev, 0x41be08, 0x4);
+	nv_wr32(dev, 0x4064c0, 0x801a00f0);
+	nv_wr32(dev, 0x405800, 0xf8000bf);
+	nv_wr32(dev, 0x419c00, 0xa);
+
+	for (tpc = 0, id = 0; tpc < 4; tpc++) {
+		for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+			if (tpc < priv->tpc_nr[gpc]) {
+				nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x0698), id);
+				nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x04e8), id);
+				nv_wr32(dev, GPC_UNIT(gpc, 0x0c10 + tpc * 4), id);
+				nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x0088), id++);
+			}
+
+			nv_wr32(dev, GPC_UNIT(gpc, 0x0c08), priv->tpc_nr[gpc]);
+			nv_wr32(dev, GPC_UNIT(gpc, 0x0c8c), priv->tpc_nr[gpc]);
+		}
+	}
+
+	tmp = 0;
+	for (i = 0; i < priv->gpc_nr; i++)
+		tmp |= priv->tpc_nr[i] << (i * 4);
+	nv_wr32(dev, 0x406028, tmp);
+	nv_wr32(dev, 0x405870, tmp);
+
+	nv_wr32(dev, 0x40602c, 0x0);
+	nv_wr32(dev, 0x405874, 0x0);
+	nv_wr32(dev, 0x406030, 0x0);
+	nv_wr32(dev, 0x405878, 0x0);
+	nv_wr32(dev, 0x406034, 0x0);
+	nv_wr32(dev, 0x40587c, 0x0);
+
+	/* calculate first set of magics */
+	memcpy(tpcnr, priv->tpc_nr, sizeof(priv->tpc_nr));
+
+	gpc = -1;
+	for (tpc = 0; tpc < priv->tpc_total; tpc++) {
+		do {
+			gpc = (gpc + 1) % priv->gpc_nr;
+		} while (!tpcnr[gpc]);
+		tpcnr[gpc]--;
+
+		data[tpc / 6] |= gpc << ((tpc % 6) * 5);
+	}
+
+	for (; tpc < 32; tpc++)
+		data[tpc / 6] |= 7 << ((tpc % 6) * 5);
+
+	/* and the second... */
+	shift = 0;
+	ntpcv = priv->tpc_total;
+	while (!(ntpcv & (1 << 4))) {
+		ntpcv <<= 1;
+		shift++;
+	}
+
+	data2[0]  = ntpcv << 16;
+	data2[0] |= shift << 21;
+	data2[0] |= (((1 << (0 + 5)) % ntpcv) << 24);
+	data2[0] |= priv->tpc_total << 8;
+	data2[0] |= priv->magic_not_rop_nr;
+	for (i = 1; i < 7; i++)
+		data2[1] |= ((1 << (i + 5)) % ntpcv) << ((i - 1) * 5);
+
+	/* and write it all the various parts of PGRAPH */
+	nv_wr32(dev, 0x418bb8, (priv->tpc_total << 8) | priv->magic_not_rop_nr);
+	for (i = 0; i < 6; i++)
+		nv_wr32(dev, 0x418b08 + (i * 4), data[i]);
+
+	nv_wr32(dev, 0x41bfd0, data2[0]);
+	nv_wr32(dev, 0x41bfe4, data2[1]);
+	for (i = 0; i < 6; i++)
+		nv_wr32(dev, 0x41bf00 + (i * 4), data[i]);
+
+	nv_wr32(dev, 0x4078bc, (priv->tpc_total << 8) | priv->magic_not_rop_nr);
+	for (i = 0; i < 6; i++)
+		nv_wr32(dev, 0x40780c + (i * 4), data[i]);
+
+
+	memcpy(tpcnr, priv->tpc_nr, sizeof(priv->tpc_nr));
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++)
+		tpc_mask |= ((1 << priv->tpc_nr[gpc]) - 1) << (gpc * 8);
+
+	for (i = 0, gpc = -1, b = -1; i < 32; i++) {
+		a = (i * (priv->tpc_total - 1)) / 32;
+		if (a != b) {
+			b = a;
+			do {
+				gpc = (gpc + 1) % priv->gpc_nr;
+			} while (!tpcnr[gpc]);
+			tpc = priv->tpc_nr[gpc] - tpcnr[gpc]--;
+
+			tpc_set |= 1 << ((gpc * 8) + tpc);
+		}
+
+		nv_wr32(dev, 0x406800 + (i * 0x20), tpc_set);
+		nv_wr32(dev, 0x406c00 + (i * 0x20), tpc_set ^ tpc_mask);
+	}
+
+	for (i = 0; i < 8; i++)
+		nv_wr32(dev, 0x4064d0 + (i * 0x04), 0x00000000);
+
+	nv_wr32(dev, 0x405b00, 0x201);
+	nv_wr32(dev, 0x408850, 0x2);
+	nv_wr32(dev, 0x408958, 0x2);
+	nv_wr32(dev, 0x419f78, 0xa);
+
+	nve0_grctx_generate_icmd(dev);
+	nve0_grctx_generate_a097(dev);
+	nve0_grctx_generate_902d(dev);
+
+	nv_mask(dev, 0x000260, 0x00000001, 0x00000001);
+	nv_wr32(dev, 0x418800, 0x7026860a); //XXX
+	nv_wr32(dev, 0x41be10, 0x00bb8bc7); //XXX
+	return 0;
+}
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index 9d83729956ff..a6598fd66423 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -70,8 +70,9 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	r200.o radeon_legacy_tv.o r600_cs.o r600_blit.o r600_blit_shaders.o \
 	r600_blit_kms.o radeon_pm.o atombios_dp.o r600_audio.o r600_hdmi.o \
 	evergreen.o evergreen_cs.o evergreen_blit_shaders.o evergreen_blit_kms.o \
-	radeon_trace_points.o ni.o cayman_blit_shaders.o atombios_encoders.o \
-	radeon_semaphore.o radeon_sa.o atombios_i2c.o si.o si_blit_shaders.o
+	evergreen_hdmi.o radeon_trace_points.o ni.o cayman_blit_shaders.o \
+	atombios_encoders.o radeon_semaphore.o radeon_sa.o atombios_i2c.o si.o \
+	si_blit_shaders.o radeon_prime.o
 
 radeon-$(CONFIG_COMPAT) += radeon_ioc32.o
 radeon-$(CONFIG_VGA_SWITCHEROO) += radeon_atpx_handler.o
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index af1054f8202a..01d77d1554f4 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -591,8 +591,7 @@ static u32 atombios_adjust_pll(struct drm_crtc *crtc,
 		if (encoder->crtc == crtc) {
 			radeon_encoder = to_radeon_encoder(encoder);
 			connector = radeon_get_connector_for_encoder(encoder);
-			/* if (connector && connector->display_info.bpc)
-				bpc = connector->display_info.bpc; */
+			bpc = radeon_get_monitor_bpc(connector);
 			encoder_mode = atombios_get_encoder_mode(encoder);
 			is_duallink = radeon_dig_monitor_is_duallink(encoder, mode->clock);
 			if ((radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT | ATOM_DEVICE_DFP_SUPPORT)) ||
@@ -968,9 +967,7 @@ static void atombios_crtc_set_pll(struct drm_crtc *crtc, struct drm_display_mode
 		struct radeon_connector_atom_dig *dig_connector =
 			radeon_connector->con_priv;
 		int dp_clock;
-
-		/* if (connector->display_info.bpc)
-			bpc = connector->display_info.bpc; */
+		bpc = radeon_get_monitor_bpc(connector);
 
 		switch (encoder_mode) {
 		case ATOM_ENCODER_MODE_DP_MST:
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index c57d85664e77..5131b3b0f7d2 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -405,13 +405,10 @@ static void dp_get_adjust_train(u8 link_status[DP_LINK_STATUS_SIZE],
 /* get bpc from the EDID */
 static int convert_bpc_to_bpp(int bpc)
 {
-#if 0
 	if (bpc == 0)
 		return 24;
 	else
 		return bpc * 3;
-#endif
-	return 24;
 }
 
 /* get the max pix clock supported by the link rate and lane num */
@@ -463,7 +460,7 @@ static int radeon_dp_get_dp_lane_number(struct drm_connector *connector,
 					u8 dpcd[DP_DPCD_SIZE],
 					int pix_clock)
 {
-	int bpp = convert_bpc_to_bpp(connector->display_info.bpc);
+	int bpp = convert_bpc_to_bpp(radeon_get_monitor_bpc(connector));
 	int max_link_rate = dp_get_max_link_rate(dpcd);
 	int max_lane_num = dp_get_max_lane_number(dpcd);
 	int lane_num;
@@ -482,7 +479,7 @@ static int radeon_dp_get_dp_link_clock(struct drm_connector *connector,
 				       u8 dpcd[DP_DPCD_SIZE],
 				       int pix_clock)
 {
-	int bpp = convert_bpc_to_bpp(connector->display_info.bpc);
+	int bpp = convert_bpc_to_bpp(radeon_get_monitor_bpc(connector));
 	int lane_num, max_pix_clock;
 
 	if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) ==
@@ -533,6 +530,23 @@ u8 radeon_dp_getsinktype(struct radeon_connector *radeon_connector)
 					 dig_connector->dp_i2c_bus->rec.i2c_id, 0);
 }
 
+static void radeon_dp_probe_oui(struct radeon_connector *radeon_connector)
+{
+	struct radeon_connector_atom_dig *dig_connector = radeon_connector->con_priv;
+	u8 buf[3];
+
+	if (!(dig_connector->dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
+		return;
+
+	if (radeon_dp_aux_native_read(radeon_connector, DP_SINK_OUI, buf, 3, 0))
+		DRM_DEBUG_KMS("Sink OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+
+	if (radeon_dp_aux_native_read(radeon_connector, DP_BRANCH_OUI, buf, 3, 0))
+		DRM_DEBUG_KMS("Branch OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+}
+
 bool radeon_dp_getdpcd(struct radeon_connector *radeon_connector)
 {
 	struct radeon_connector_atom_dig *dig_connector = radeon_connector->con_priv;
@@ -546,6 +560,9 @@ bool radeon_dp_getdpcd(struct radeon_connector *radeon_connector)
 		for (i = 0; i < 8; i++)
 			DRM_DEBUG_KMS("%02x ", msg[i]);
 		DRM_DEBUG_KMS("\n");
+
+		radeon_dp_probe_oui(radeon_connector);
+
 		return true;
 	}
 	dig_connector->dpcd[0] = 0;
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index 2d39f9977e00..e7b1ec5ae8c6 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -545,7 +545,7 @@ atombios_dig_encoder_setup(struct drm_encoder *encoder, int action, int panel_mo
 		dp_clock = dig_connector->dp_clock;
 		dp_lane_count = dig_connector->dp_lane_count;
 		hpd_id = radeon_connector->hpd.hpd;
-		/* bpc = connector->display_info.bpc; */
+		bpc = radeon_get_monitor_bpc(connector);
 	}
 
 	/* no dig encoder assigned */
@@ -1163,7 +1163,7 @@ atombios_external_encoder_setup(struct drm_encoder *encoder,
 		dp_lane_count = dig_connector->dp_lane_count;
 		connector_object_id =
 			(radeon_connector->connector_object_id & OBJECT_ID_MASK) >> OBJECT_ID_SHIFT;
-		/* bpc = connector->display_info.bpc; */
+		bpc = radeon_get_monitor_bpc(connector);
 	}
 
 	memset(&args, 0, sizeof(args));
@@ -1926,7 +1926,10 @@ radeon_atom_encoder_mode_set(struct drm_encoder *encoder,
 
 	if (atombios_get_encoder_mode(encoder) == ATOM_ENCODER_MODE_HDMI) {
 		r600_hdmi_enable(encoder);
-		r600_hdmi_setmode(encoder, adjusted_mode);
+		if (ASIC_IS_DCE4(rdev))
+			evergreen_hdmi_setmode(encoder, adjusted_mode);
+		else
+			r600_hdmi_setmode(encoder, adjusted_mode);
 	}
 }
 
@@ -2081,6 +2084,7 @@ radeon_atom_ext_encoder_setup_ddc(struct drm_encoder *encoder)
 
 static void radeon_atom_encoder_prepare(struct drm_encoder *encoder)
 {
+	struct radeon_device *rdev = encoder->dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
 
@@ -2089,8 +2093,16 @@ static void radeon_atom_encoder_prepare(struct drm_encoder *encoder)
 	    (radeon_encoder_get_dp_bridge_encoder_id(encoder) !=
 	     ENCODER_OBJECT_ID_NONE)) {
 		struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-		if (dig)
+		if (dig) {
 			dig->dig_encoder = radeon_atom_pick_dig_encoder(encoder);
+			if (radeon_encoder->active_device & ATOM_DEVICE_DFP_SUPPORT) {
+				if (rdev->family >= CHIP_R600)
+					dig->afmt = rdev->mode_info.afmt[dig->dig_encoder];
+				else
+					/* RS600/690/740 have only 1 afmt block */
+					dig->afmt = rdev->mode_info.afmt[0];
+			}
+		}
 	}
 
 	radeon_atom_output_lock(encoder, true);
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index cfa372cb1cb3..58991af90502 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -2424,27 +2424,18 @@ bool evergreen_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *rin
 	u32 srbm_status;
 	u32 grbm_status;
 	u32 grbm_status_se0, grbm_status_se1;
-	struct r100_gpu_lockup *lockup = &rdev->config.evergreen.lockup;
-	int r;
 
 	srbm_status = RREG32(SRBM_STATUS);
 	grbm_status = RREG32(GRBM_STATUS);
 	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
 	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
 	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 static int evergreen_gpu_soft_reset(struct radeon_device *rdev)
@@ -2594,6 +2585,7 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	u32 hpd1, hpd2, hpd3, hpd4, hpd5, hpd6;
 	u32 grbm_int_cntl = 0;
 	u32 grph1 = 0, grph2 = 0, grph3 = 0, grph4 = 0, grph5 = 0, grph6 = 0;
+	u32 afmt1 = 0, afmt2 = 0, afmt3 = 0, afmt4 = 0, afmt5 = 0, afmt6 = 0;
 
 	if (!rdev->irq.installed) {
 		WARN(1, "Can't enable IRQ/MSI because no handler is installed\n");
@@ -2614,6 +2606,13 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	hpd5 = RREG32(DC_HPD5_INT_CONTROL) & ~DC_HPDx_INT_EN;
 	hpd6 = RREG32(DC_HPD6_INT_CONTROL) & ~DC_HPDx_INT_EN;
 
+	afmt1 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt2 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt3 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt4 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt5 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt6 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+
 	if (rdev->family >= CHIP_CAYMAN) {
 		/* enable CP interrupts on all rings */
 		if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
@@ -2690,6 +2689,30 @@ int evergreen_irq_set(struct radeon_device *rdev)
 		DRM_DEBUG("evergreen_irq_set: hpd 6\n");
 		hpd6 |= DC_HPDx_INT_EN;
 	}
+	if (rdev->irq.afmt[0]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 0\n");
+		afmt1 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[1]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 1\n");
+		afmt2 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[2]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 2\n");
+		afmt3 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[3]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 3\n");
+		afmt4 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[4]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 4\n");
+		afmt5 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[5]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 5\n");
+		afmt6 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
 	if (rdev->irq.gui_idle) {
 		DRM_DEBUG("gui idle\n");
 		grbm_int_cntl |= GUI_IDLE_INT_ENABLE;
@@ -2732,6 +2755,13 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	WREG32(DC_HPD5_INT_CONTROL, hpd5);
 	WREG32(DC_HPD6_INT_CONTROL, hpd6);
 
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET, afmt1);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET, afmt2);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET, afmt3);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET, afmt4);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, afmt5);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, afmt6);
+
 	return 0;
 }
 
@@ -2756,6 +2786,13 @@ static void evergreen_irq_ack(struct radeon_device *rdev)
 		rdev->irq.stat_regs.evergreen.d6grph_int = RREG32(GRPH_INT_STATUS + EVERGREEN_CRTC5_REGISTER_OFFSET);
 	}
 
+	rdev->irq.stat_regs.evergreen.afmt_status1 = RREG32(AFMT_STATUS + EVERGREEN_CRTC0_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status2 = RREG32(AFMT_STATUS + EVERGREEN_CRTC1_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status3 = RREG32(AFMT_STATUS + EVERGREEN_CRTC2_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status4 = RREG32(AFMT_STATUS + EVERGREEN_CRTC3_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status5 = RREG32(AFMT_STATUS + EVERGREEN_CRTC4_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status6 = RREG32(AFMT_STATUS + EVERGREEN_CRTC5_REGISTER_OFFSET);
+
 	if (rdev->irq.stat_regs.evergreen.d1grph_int & GRPH_PFLIP_INT_OCCURRED)
 		WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC0_REGISTER_OFFSET, GRPH_PFLIP_INT_CLEAR);
 	if (rdev->irq.stat_regs.evergreen.d2grph_int & GRPH_PFLIP_INT_OCCURRED)
@@ -2829,6 +2866,36 @@ static void evergreen_irq_ack(struct radeon_device *rdev)
 		tmp |= DC_HPDx_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status1 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status2 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status3 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status4 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status5 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status6 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, tmp);
+	}
 }
 
 void evergreen_irq_disable(struct radeon_device *rdev)
@@ -2878,6 +2945,7 @@ int evergreen_irq_process(struct radeon_device *rdev)
 	u32 ring_index;
 	unsigned long flags;
 	bool queue_hotplug = false;
+	bool queue_hdmi = false;
 
 	if (!rdev->ih.enabled || rdev->shutdown)
 		return IRQ_NONE;
@@ -3111,6 +3179,55 @@ restart_ih:
 				break;
 			}
 			break;
+		case 44: /* hdmi */
+			switch (src_data) {
+			case 0:
+				if (rdev->irq.stat_regs.evergreen.afmt_status1 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status1 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI0\n");
+				}
+				break;
+			case 1:
+				if (rdev->irq.stat_regs.evergreen.afmt_status2 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status2 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI1\n");
+				}
+				break;
+			case 2:
+				if (rdev->irq.stat_regs.evergreen.afmt_status3 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status3 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI2\n");
+				}
+				break;
+			case 3:
+				if (rdev->irq.stat_regs.evergreen.afmt_status4 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status4 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI3\n");
+				}
+				break;
+			case 4:
+				if (rdev->irq.stat_regs.evergreen.afmt_status5 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status5 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI4\n");
+				}
+				break;
+			case 5:
+				if (rdev->irq.stat_regs.evergreen.afmt_status6 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status6 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI5\n");
+				}
+				break;
+			default:
+				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
+				break;
+			}
+			break;
 		case 176: /* CP_INT in ring buffer */
 		case 177: /* CP_INT in IB1 */
 		case 178: /* CP_INT in IB2 */
@@ -3154,6 +3271,8 @@ restart_ih:
 		goto restart_ih;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
+	if (queue_hdmi)
+		schedule_work(&rdev->audio_work);
 	rdev->ih.rptr = rptr;
 	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	spin_unlock_irqrestore(&rdev->ih.lock, flags);
@@ -3248,12 +3367,9 @@ static int evergreen_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -3319,10 +3435,6 @@ int evergreen_init(struct radeon_device *rdev)
 {
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -3434,7 +3546,6 @@ void evergreen_fini(struct radeon_device *rdev)
 	evergreen_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_bo_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/evergreen_blit_kms.c b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
index 222acd2d33df..1e96bd458cfd 100644
--- a/drivers/gpu/drm/radeon/evergreen_blit_kms.c
+++ b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
@@ -637,7 +637,6 @@ int evergreen_blit_init(struct radeon_device *rdev)
 	if (rdev->r600_blit.shader_obj)
 		goto done;
 
-	mutex_init(&rdev->r600_blit.mutex);
 	rdev->r600_blit.state_offset = 0;
 
 	if (rdev->family < CHIP_CAYMAN)
@@ -669,7 +668,7 @@ int evergreen_blit_init(struct radeon_device *rdev)
 	obj_size = ALIGN(obj_size, 256);
 
 	r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				&rdev->r600_blit.shader_obj);
+			     NULL, &rdev->r600_blit.shader_obj);
 	if (r) {
 		DRM_ERROR("evergreen failed to allocate shader\n");
 		return r;
diff --git a/drivers/gpu/drm/radeon/evergreen_cs.c b/drivers/gpu/drm/radeon/evergreen_cs.c
index 70089d32b80f..4e7dd2b4843d 100644
--- a/drivers/gpu/drm/radeon/evergreen_cs.c
+++ b/drivers/gpu/drm/radeon/evergreen_cs.c
@@ -1057,7 +1057,7 @@ static int evergreen_cs_packet_parse_vline(struct radeon_cs_parser *p)
 	uint32_t header, h_idx, reg, wait_reg_mem_info;
 	volatile uint32_t *ib;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 
 	/* parse the WAIT_REG_MEM */
 	r = evergreen_cs_packet_parse(p, &wait_reg_mem, p->idx);
@@ -1215,7 +1215,7 @@ static int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 		if (!(evergreen_reg_safe_bm[i] & m))
 			return 0;
 	}
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	switch (reg) {
 	/* force following reg to 0 in an attempt to disable out buffer
 	 * which will need us to better understand how it works to perform
@@ -1896,7 +1896,7 @@ static int evergreen_packet3_check(struct radeon_cs_parser *p,
 	u32 idx_value;
 
 	track = (struct evergreen_cs_track *)p->track;
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	idx_value = radeon_get_ib_value(p, idx);
 
@@ -2610,8 +2610,8 @@ int evergreen_cs_parse(struct radeon_cs_parser *p)
 		}
 	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
 #if 0
-	for (r = 0; r < p->ib->length_dw; r++) {
-		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib->ptr[r]);
+	for (r = 0; r < p->ib.length_dw; r++) {
+		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib.ptr[r]);
 		mdelay(1);
 	}
 #endif
diff --git a/drivers/gpu/drm/radeon/evergreen_hdmi.c b/drivers/gpu/drm/radeon/evergreen_hdmi.c
new file mode 100644
index 000000000000..a51f880985f8
--- /dev/null
+++ b/drivers/gpu/drm/radeon/evergreen_hdmi.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2008 Advanced Micro Devices, Inc.
+ * Copyright 2008 Red Hat Inc.
+ * Copyright 2009 Christian König.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Christian König
+ *          Rafał Miłecki
+ */
+#include "drmP.h"
+#include "radeon_drm.h"
+#include "radeon.h"
+#include "radeon_asic.h"
+#include "evergreend.h"
+#include "atom.h"
+
+/*
+ * update the N and CTS parameters for a given pixel clock rate
+ */
+static void evergreen_hdmi_update_ACR(struct drm_encoder *encoder, uint32_t clock)
+{
+	struct drm_device *dev = encoder->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_hdmi_acr acr = r600_hdmi_acr(clock);
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
+
+	WREG32(HDMI_ACR_32_0 + offset, HDMI_ACR_CTS_32(acr.cts_32khz));
+	WREG32(HDMI_ACR_32_1 + offset, acr.n_32khz);
+
+	WREG32(HDMI_ACR_44_0 + offset, HDMI_ACR_CTS_44(acr.cts_44_1khz));
+	WREG32(HDMI_ACR_44_1 + offset, acr.n_44_1khz);
+
+	WREG32(HDMI_ACR_48_0 + offset, HDMI_ACR_CTS_48(acr.cts_48khz));
+	WREG32(HDMI_ACR_48_1 + offset, acr.n_48khz);
+}
+
+/*
+ * calculate the crc for a given info frame
+ */
+static void evergreen_hdmi_infoframe_checksum(uint8_t packetType,
+					 uint8_t versionNumber,
+					 uint8_t length,
+					 uint8_t *frame)
+{
+	int i;
+	frame[0] = packetType + versionNumber + length;
+	for (i = 1; i <= length; i++)
+		frame[0] += frame[i];
+	frame[0] = 0x100 - frame[0];
+}
+
+/*
+ * build a HDMI Video Info Frame
+ */
+static void evergreen_hdmi_videoinfoframe(
+	struct drm_encoder *encoder,
+	uint8_t color_format,
+	int active_information_present,
+	uint8_t active_format_aspect_ratio,
+	uint8_t scan_information,
+	uint8_t colorimetry,
+	uint8_t ex_colorimetry,
+	uint8_t quantization,
+	int ITC,
+	uint8_t picture_aspect_ratio,
+	uint8_t video_format_identification,
+	uint8_t pixel_repetition,
+	uint8_t non_uniform_picture_scaling,
+	uint8_t bar_info_data_valid,
+	uint16_t top_bar,
+	uint16_t bottom_bar,
+	uint16_t left_bar,
+	uint16_t right_bar
+)
+{
+	struct drm_device *dev = encoder->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
+
+	uint8_t frame[14];
+
+	frame[0x0] = 0;
+	frame[0x1] =
+		(scan_information & 0x3) |
+		((bar_info_data_valid & 0x3) << 2) |
+		((active_information_present & 0x1) << 4) |
+		((color_format & 0x3) << 5);
+	frame[0x2] =
+		(active_format_aspect_ratio & 0xF) |
+		((picture_aspect_ratio & 0x3) << 4) |
+		((colorimetry & 0x3) << 6);
+	frame[0x3] =
+		(non_uniform_picture_scaling & 0x3) |
+		((quantization & 0x3) << 2) |
+		((ex_colorimetry & 0x7) << 4) |
+		((ITC & 0x1) << 7);
+	frame[0x4] = (video_format_identification & 0x7F);
+	frame[0x5] = (pixel_repetition & 0xF);
+	frame[0x6] = (top_bar & 0xFF);
+	frame[0x7] = (top_bar >> 8);
+	frame[0x8] = (bottom_bar & 0xFF);
+	frame[0x9] = (bottom_bar >> 8);
+	frame[0xA] = (left_bar & 0xFF);
+	frame[0xB] = (left_bar >> 8);
+	frame[0xC] = (right_bar & 0xFF);
+	frame[0xD] = (right_bar >> 8);
+
+	evergreen_hdmi_infoframe_checksum(0x82, 0x02, 0x0D, frame);
+	/* Our header values (type, version, length) should be alright, Intel
+	 * is using the same. Checksum function also seems to be OK, it works
+	 * fine for audio infoframe. However calculated value is always lower
+	 * by 2 in comparison to fglrx. It breaks displaying anything in case
+	 * of TVs that strictly check the checksum. Hack it manually here to
+	 * workaround this issue. */
+	frame[0x0] += 2;
+
+	WREG32(AFMT_AVI_INFO0 + offset,
+		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
+	WREG32(AFMT_AVI_INFO1 + offset,
+		frame[0x4] | (frame[0x5] << 8) | (frame[0x6] << 16) | (frame[0x7] << 24));
+	WREG32(AFMT_AVI_INFO2 + offset,
+		frame[0x8] | (frame[0x9] << 8) | (frame[0xA] << 16) | (frame[0xB] << 24));
+	WREG32(AFMT_AVI_INFO3 + offset,
+		frame[0xC] | (frame[0xD] << 8));
+}
+
+/*
+ * update the info frames with the data from the current display mode
+ */
+void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset;
+
+	if (ASIC_IS_DCE5(rdev))
+		return;
+
+	/* Silent, r600_hdmi_enable will raise WARN for us */
+	if (!dig->afmt->enabled)
+		return;
+	offset = dig->afmt->offset;
+
+	r600_audio_set_clock(encoder, mode->clock);
+
+	WREG32(HDMI_VBI_PACKET_CONTROL + offset,
+	       HDMI_NULL_SEND); /* send null packets when required */
+
+	WREG32(AFMT_AUDIO_CRC_CONTROL + offset, 0x1000);
+
+	WREG32(HDMI_AUDIO_PACKET_CONTROL + offset,
+	       HDMI_AUDIO_DELAY_EN(1) | /* set the default audio delay */
+	       HDMI_AUDIO_PACKETS_PER_LINE(3)); /* should be suffient for all audio modes and small enough for all hblanks */
+
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + offset,
+	       AFMT_AUDIO_SAMPLE_SEND | /* send audio packets */
+	       AFMT_60958_CS_UPDATE); /* allow 60958 channel status fields to be updated */
+
+	WREG32(HDMI_ACR_PACKET_CONTROL + offset,
+	       HDMI_ACR_AUTO_SEND | /* allow hw to sent ACR packets when required */
+	       HDMI_ACR_SOURCE); /* select SW CTS value */
+
+	WREG32(HDMI_VBI_PACKET_CONTROL + offset,
+	       HDMI_NULL_SEND | /* send null packets when required */
+	       HDMI_GC_SEND | /* send general control packets */
+	       HDMI_GC_CONT); /* send general control packets every frame */
+
+	WREG32(HDMI_INFOFRAME_CONTROL0 + offset,
+	       HDMI_AVI_INFO_SEND | /* enable AVI info frames */
+	       HDMI_AVI_INFO_CONT | /* send AVI info frames every frame/field */
+	       HDMI_AUDIO_INFO_SEND | /* enable audio info frames (frames won't be set until audio is enabled) */
+	       HDMI_AUDIO_INFO_CONT); /* required for audio info values to be updated */
+
+	WREG32(AFMT_INFOFRAME_CONTROL0 + offset,
+	       AFMT_AUDIO_INFO_UPDATE); /* required for audio info values to be updated */
+
+	WREG32(HDMI_INFOFRAME_CONTROL1 + offset,
+	       HDMI_AVI_INFO_LINE(2) | /* anything other than 0 */
+	       HDMI_AUDIO_INFO_LINE(2)); /* anything other than 0 */
+
+	WREG32(HDMI_GC + offset, 0); /* unset HDMI_GC_AVMUTE */
+
+	evergreen_hdmi_videoinfoframe(encoder, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+				      0, 0, 0, 0, 0, 0);
+
+	evergreen_hdmi_update_ACR(encoder, mode->clock);
+
+	/* it's unknown what these bits do excatly, but it's indeed quite useful for debugging */
+	WREG32(AFMT_RAMP_CONTROL0 + offset, 0x00FFFFFF);
+	WREG32(AFMT_RAMP_CONTROL1 + offset, 0x007FFFFF);
+	WREG32(AFMT_RAMP_CONTROL2 + offset, 0x00000001);
+	WREG32(AFMT_RAMP_CONTROL3 + offset, 0x00000001);
+}
diff --git a/drivers/gpu/drm/radeon/evergreen_reg.h b/drivers/gpu/drm/radeon/evergreen_reg.h
index 96c10b3991aa..8beac1065025 100644
--- a/drivers/gpu/drm/radeon/evergreen_reg.h
+++ b/drivers/gpu/drm/radeon/evergreen_reg.h
@@ -232,6 +232,4 @@
 /* HDMI blocks at 0x7030, 0x7c30, 0x10830, 0x11430, 0x12030, 0x12c30 */
 #define EVERGREEN_HDMI_BASE				0x7030
 
-#define EVERGREEN_HDMI_CONFIG_OFFSET			0xf0
-
 #endif
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index b4eefc355f16..79130bfd1d6f 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -112,6 +112,226 @@
 #define	CP_SEM_INCOMPLETE_TIMER_CNTL			0x85C8
 #define	CP_DEBUG					0xC1FC
 
+/* Audio clocks */
+#define DCCG_AUDIO_DTO_SOURCE             0x05ac
+#       define DCCG_AUDIO_DTO0_SOURCE_SEL(x) ((x) << 0) /* crtc0 - crtc5 */
+#       define DCCG_AUDIO_DTO_SEL         (1 << 4) /* 0=dto0 1=dto1 */
+
+#define DCCG_AUDIO_DTO0_PHASE             0x05b0
+#define DCCG_AUDIO_DTO0_MODULE            0x05b4
+#define DCCG_AUDIO_DTO0_LOAD              0x05b8
+#define DCCG_AUDIO_DTO0_CNTL              0x05bc
+
+#define DCCG_AUDIO_DTO1_PHASE             0x05c0
+#define DCCG_AUDIO_DTO1_MODULE            0x05c4
+#define DCCG_AUDIO_DTO1_LOAD              0x05c8
+#define DCCG_AUDIO_DTO1_CNTL              0x05cc
+
+/* DCE 4.0 AFMT */
+#define HDMI_CONTROL                         0x7030
+#       define HDMI_KEEPOUT_MODE             (1 << 0)
+#       define HDMI_PACKET_GEN_VERSION       (1 << 4) /* 0 = r6xx compat */
+#       define HDMI_ERROR_ACK                (1 << 8)
+#       define HDMI_ERROR_MASK               (1 << 9)
+#       define HDMI_DEEP_COLOR_ENABLE        (1 << 24)
+#       define HDMI_DEEP_COLOR_DEPTH         (((x) & 3) << 28)
+#       define HDMI_24BIT_DEEP_COLOR         0
+#       define HDMI_30BIT_DEEP_COLOR         1
+#       define HDMI_36BIT_DEEP_COLOR         2
+#define HDMI_STATUS                          0x7034
+#       define HDMI_ACTIVE_AVMUTE            (1 << 0)
+#       define HDMI_AUDIO_PACKET_ERROR       (1 << 16)
+#       define HDMI_VBI_PACKET_ERROR         (1 << 20)
+#define HDMI_AUDIO_PACKET_CONTROL            0x7038
+#       define HDMI_AUDIO_DELAY_EN(x)        (((x) & 3) << 4)
+#       define HDMI_AUDIO_PACKETS_PER_LINE(x)  (((x) & 0x1f) << 16)
+#define HDMI_ACR_PACKET_CONTROL              0x703c
+#       define HDMI_ACR_SEND                 (1 << 0)
+#       define HDMI_ACR_CONT                 (1 << 1)
+#       define HDMI_ACR_SELECT(x)            (((x) & 3) << 4)
+#       define HDMI_ACR_HW                   0
+#       define HDMI_ACR_32                   1
+#       define HDMI_ACR_44                   2
+#       define HDMI_ACR_48                   3
+#       define HDMI_ACR_SOURCE               (1 << 8) /* 0 - hw; 1 - cts value */
+#       define HDMI_ACR_AUTO_SEND            (1 << 12)
+#       define HDMI_ACR_N_MULTIPLE(x)        (((x) & 7) << 16)
+#       define HDMI_ACR_X1                   1
+#       define HDMI_ACR_X2                   2
+#       define HDMI_ACR_X4                   4
+#       define HDMI_ACR_AUDIO_PRIORITY       (1 << 31)
+#define HDMI_VBI_PACKET_CONTROL              0x7040
+#       define HDMI_NULL_SEND                (1 << 0)
+#       define HDMI_GC_SEND                  (1 << 4)
+#       define HDMI_GC_CONT                  (1 << 5) /* 0 - once; 1 - every frame */
+#define HDMI_INFOFRAME_CONTROL0              0x7044
+#       define HDMI_AVI_INFO_SEND            (1 << 0)
+#       define HDMI_AVI_INFO_CONT            (1 << 1)
+#       define HDMI_AUDIO_INFO_SEND          (1 << 4)
+#       define HDMI_AUDIO_INFO_CONT          (1 << 5)
+#       define HDMI_MPEG_INFO_SEND           (1 << 8)
+#       define HDMI_MPEG_INFO_CONT           (1 << 9)
+#define HDMI_INFOFRAME_CONTROL1              0x7048
+#       define HDMI_AVI_INFO_LINE(x)         (((x) & 0x3f) << 0)
+#       define HDMI_AUDIO_INFO_LINE(x)       (((x) & 0x3f) << 8)
+#       define HDMI_MPEG_INFO_LINE(x)        (((x) & 0x3f) << 16)
+#define HDMI_GENERIC_PACKET_CONTROL          0x704c
+#       define HDMI_GENERIC0_SEND            (1 << 0)
+#       define HDMI_GENERIC0_CONT            (1 << 1)
+#       define HDMI_GENERIC1_SEND            (1 << 4)
+#       define HDMI_GENERIC1_CONT            (1 << 5)
+#       define HDMI_GENERIC0_LINE(x)         (((x) & 0x3f) << 16)
+#       define HDMI_GENERIC1_LINE(x)         (((x) & 0x3f) << 24)
+#define HDMI_GC                              0x7058
+#       define HDMI_GC_AVMUTE                (1 << 0)
+#       define HDMI_GC_AVMUTE_CONT           (1 << 2)
+#define AFMT_AUDIO_PACKET_CONTROL2           0x705c
+#       define AFMT_AUDIO_LAYOUT_OVRD        (1 << 0)
+#       define AFMT_AUDIO_LAYOUT_SELECT      (1 << 1)
+#       define AFMT_60958_CS_SOURCE          (1 << 4)
+#       define AFMT_AUDIO_CHANNEL_ENABLE(x)  (((x) & 0xff) << 8)
+#       define AFMT_DP_AUDIO_STREAM_ID(x)    (((x) & 0xff) << 16)
+#define AFMT_AVI_INFO0                       0x7084
+#       define AFMT_AVI_INFO_CHECKSUM(x)     (((x) & 0xff) << 0)
+#       define AFMT_AVI_INFO_S(x)            (((x) & 3) << 8)
+#       define AFMT_AVI_INFO_B(x)            (((x) & 3) << 10)
+#       define AFMT_AVI_INFO_A(x)            (((x) & 1) << 12)
+#       define AFMT_AVI_INFO_Y(x)            (((x) & 3) << 13)
+#       define AFMT_AVI_INFO_Y_RGB           0
+#       define AFMT_AVI_INFO_Y_YCBCR422      1
+#       define AFMT_AVI_INFO_Y_YCBCR444      2
+#       define AFMT_AVI_INFO_Y_A_B_S(x)      (((x) & 0xff) << 8)
+#       define AFMT_AVI_INFO_R(x)            (((x) & 0xf) << 16)
+#       define AFMT_AVI_INFO_M(x)            (((x) & 0x3) << 20)
+#       define AFMT_AVI_INFO_C(x)            (((x) & 0x3) << 22)
+#       define AFMT_AVI_INFO_C_M_R(x)        (((x) & 0xff) << 16)
+#       define AFMT_AVI_INFO_SC(x)           (((x) & 0x3) << 24)
+#       define AFMT_AVI_INFO_Q(x)            (((x) & 0x3) << 26)
+#       define AFMT_AVI_INFO_EC(x)           (((x) & 0x3) << 28)
+#       define AFMT_AVI_INFO_ITC(x)          (((x) & 0x1) << 31)
+#       define AFMT_AVI_INFO_ITC_EC_Q_SC(x)  (((x) & 0xff) << 24)
+#define AFMT_AVI_INFO1                       0x7088
+#       define AFMT_AVI_INFO_VIC(x)          (((x) & 0x7f) << 0) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_PR(x)           (((x) & 0xf) << 8) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_CN(x)           (((x) & 0x3) << 12)
+#       define AFMT_AVI_INFO_YQ(x)           (((x) & 0x3) << 14)
+#       define AFMT_AVI_INFO_TOP(x)          (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO2                       0x708c
+#       define AFMT_AVI_INFO_BOTTOM(x)       (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_LEFT(x)         (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO3                       0x7090
+#       define AFMT_AVI_INFO_RIGHT(x)        (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_VERSION(x)      (((x) & 3) << 24)
+#define AFMT_MPEG_INFO0                      0x7094
+#       define AFMT_MPEG_INFO_CHECKSUM(x)    (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MB0(x)         (((x) & 0xff) << 8)
+#       define AFMT_MPEG_INFO_MB1(x)         (((x) & 0xff) << 16)
+#       define AFMT_MPEG_INFO_MB2(x)         (((x) & 0xff) << 24)
+#define AFMT_MPEG_INFO1                      0x7098
+#       define AFMT_MPEG_INFO_MB3(x)         (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MF(x)          (((x) & 3) << 8)
+#       define AFMT_MPEG_INFO_FR(x)          (((x) & 1) << 12)
+#define AFMT_GENERIC0_HDR                    0x709c
+#define AFMT_GENERIC0_0                      0x70a0
+#define AFMT_GENERIC0_1                      0x70a4
+#define AFMT_GENERIC0_2                      0x70a8
+#define AFMT_GENERIC0_3                      0x70ac
+#define AFMT_GENERIC0_4                      0x70b0
+#define AFMT_GENERIC0_5                      0x70b4
+#define AFMT_GENERIC0_6                      0x70b8
+#define AFMT_GENERIC1_HDR                    0x70bc
+#define AFMT_GENERIC1_0                      0x70c0
+#define AFMT_GENERIC1_1                      0x70c4
+#define AFMT_GENERIC1_2                      0x70c8
+#define AFMT_GENERIC1_3                      0x70cc
+#define AFMT_GENERIC1_4                      0x70d0
+#define AFMT_GENERIC1_5                      0x70d4
+#define AFMT_GENERIC1_6                      0x70d8
+#define HDMI_ACR_32_0                        0x70dc
+#       define HDMI_ACR_CTS_32(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_32_1                        0x70e0
+#       define HDMI_ACR_N_32(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_44_0                        0x70e4
+#       define HDMI_ACR_CTS_44(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_44_1                        0x70e8
+#       define HDMI_ACR_N_44(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_48_0                        0x70ec
+#       define HDMI_ACR_CTS_48(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_48_1                        0x70f0
+#       define HDMI_ACR_N_48(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_STATUS_0                    0x70f4
+#define HDMI_ACR_STATUS_1                    0x70f8
+#define AFMT_AUDIO_INFO0                     0x70fc
+#       define AFMT_AUDIO_INFO_CHECKSUM(x)   (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_CC(x)         (((x) & 7) << 8)
+#       define AFMT_AUDIO_INFO_CT(x)         (((x) & 0xf) << 11)
+#       define AFMT_AUDIO_INFO_CHECKSUM_OFFSET(x)   (((x) & 0xff) << 16)
+#       define AFMT_AUDIO_INFO_CXT(x)        (((x) & 0x1f) << 24)
+#define AFMT_AUDIO_INFO1                     0x7100
+#       define AFMT_AUDIO_INFO_CA(x)         (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_LSV(x)        (((x) & 0xf) << 11)
+#       define AFMT_AUDIO_INFO_DM_INH(x)     (((x) & 1) << 15)
+#       define AFMT_AUDIO_INFO_DM_INH_LSV(x) (((x) & 0xff) << 8)
+#       define AFMT_AUDIO_INFO_LFEBPL(x)     (((x) & 3) << 16)
+#define AFMT_60958_0                         0x7104
+#       define AFMT_60958_CS_A(x)            (((x) & 1) << 0)
+#       define AFMT_60958_CS_B(x)            (((x) & 1) << 1)
+#       define AFMT_60958_CS_C(x)            (((x) & 1) << 2)
+#       define AFMT_60958_CS_D(x)            (((x) & 3) << 3)
+#       define AFMT_60958_CS_MODE(x)         (((x) & 3) << 6)
+#       define AFMT_60958_CS_CATEGORY_CODE(x)      (((x) & 0xff) << 8)
+#       define AFMT_60958_CS_SOURCE_NUMBER(x)      (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_L(x)   (((x) & 0xf) << 20)
+#       define AFMT_60958_CS_SAMPLING_FREQUENCY(x) (((x) & 0xf) << 24)
+#       define AFMT_60958_CS_CLOCK_ACCURACY(x)     (((x) & 3) << 28)
+#define AFMT_60958_1                         0x7108
+#       define AFMT_60958_CS_WORD_LENGTH(x)  (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_ORIGINAL_SAMPLING_FREQUENCY(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_VALID_L(x)      (((x) & 1) << 16)
+#       define AFMT_60958_CS_VALID_R(x)      (((x) & 1) << 18)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_R(x)   (((x) & 0xf) << 20)
+#define AFMT_AUDIO_CRC_CONTROL               0x710c
+#       define AFMT_AUDIO_CRC_EN             (1 << 0)
+#define AFMT_RAMP_CONTROL0                   0x7110
+#       define AFMT_RAMP_MAX_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_RAMP_DATA_SIGN           (1 << 31)
+#define AFMT_RAMP_CONTROL1                   0x7114
+#       define AFMT_RAMP_MIN_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_AUDIO_TEST_CH_DISABLE(x) (((x) & 0xff) << 24)
+#define AFMT_RAMP_CONTROL2                   0x7118
+#       define AFMT_RAMP_INC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_RAMP_CONTROL3                   0x711c
+#       define AFMT_RAMP_DEC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_60958_2                         0x7120
+#       define AFMT_60958_CS_CHANNEL_NUMBER_2(x)   (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_3(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_4(x)   (((x) & 0xf) << 8)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_5(x)   (((x) & 0xf) << 12)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_6(x)   (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_7(x)   (((x) & 0xf) << 20)
+#define AFMT_STATUS                          0x7128
+#       define AFMT_AUDIO_ENABLE             (1 << 4)
+#       define AFMT_AUDIO_HBR_ENABLE         (1 << 8)
+#       define AFMT_AZ_FORMAT_WTRIG          (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_INT      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG      (1 << 30)
+#define AFMT_AUDIO_PACKET_CONTROL            0x712c
+#       define AFMT_AUDIO_SAMPLE_SEND        (1 << 0)
+#       define AFMT_RESET_FIFO_WHEN_AUDIO_DIS (1 << 11) /* set to 1 */
+#       define AFMT_AUDIO_TEST_EN            (1 << 12)
+#       define AFMT_AUDIO_CHANNEL_SWAP       (1 << 24)
+#       define AFMT_60958_CS_UPDATE          (1 << 26)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_MASK (1 << 27)
+#       define AFMT_AZ_FORMAT_WTRIG_MASK     (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_ACK      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_ACK  (1 << 30)
+#define AFMT_VBI_PACKET_CONTROL              0x7130
+#       define AFMT_GENERIC0_UPDATE          (1 << 2)
+#define AFMT_INFOFRAME_CONTROL0              0x7134
+#       define AFMT_AUDIO_INFO_SOURCE        (1 << 6) /* 0 - sound block; 1 - afmt regs */
+#       define AFMT_AUDIO_INFO_UPDATE        (1 << 7)
+#       define AFMT_MPEG_INFO_UPDATE         (1 << 10)
+#define AFMT_GENERIC0_7                      0x7138
 
 #define	GC_USER_SHADER_PIPE_CONFIG			0x8954
 #define		INACTIVE_QD_PIPES(x)				((x) << 8)
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index a48ca53fcd6a..b01c2dd627b0 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -1392,35 +1392,6 @@ int cayman_cp_resume(struct radeon_device *rdev)
 	return 0;
 }
 
-bool cayman_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
-{
-	u32 srbm_status;
-	u32 grbm_status;
-	u32 grbm_status_se0, grbm_status_se1;
-	struct r100_gpu_lockup *lockup = &rdev->config.cayman.lockup;
-	int r;
-
-	srbm_status = RREG32(SRBM_STATUS);
-	grbm_status = RREG32(GRBM_STATUS);
-	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
-	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
-	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, ring);
-		return false;
-	}
-	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	/* XXX deal with CP0,1,2 */
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
-}
-
 static int cayman_gpu_soft_reset(struct radeon_device *rdev)
 {
 	struct evergreen_mc_save save;
@@ -1601,12 +1572,9 @@ static int cayman_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	r = radeon_vm_manager_start(rdev);
 	if (r)
@@ -1661,10 +1629,6 @@ int cayman_init(struct radeon_device *rdev)
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -1776,7 +1740,6 @@ void cayman_fini(struct radeon_device *rdev)
 	cayman_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index fe33d35dae8c..fb44e7e49083 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -139,9 +139,9 @@ int r100_reloc_pitch_offset(struct radeon_cs_parser *p,
 		}
 
 		tmp |= tile_flags;
-		p->ib->ptr[idx] = (value & 0x3fc00000) | tmp;
+		p->ib.ptr[idx] = (value & 0x3fc00000) | tmp;
 	} else
-		p->ib->ptr[idx] = (value & 0xffc00000) | tmp;
+		p->ib.ptr[idx] = (value & 0xffc00000) | tmp;
 	return 0;
 }
 
@@ -156,7 +156,7 @@ int r100_packet3_load_vbpntr(struct radeon_cs_parser *p,
 	volatile uint32_t *ib;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 	c = radeon_get_ib_value(p, idx++) & 0x1F;
 	if (c > 16) {
@@ -660,7 +660,7 @@ int r100_pci_gart_enable(struct radeon_device *rdev)
 	tmp = RREG32(RADEON_AIC_CNTL) | RADEON_PCIGART_TRANSLATE_EN;
 	WREG32(RADEON_AIC_CNTL, tmp);
 	r100_pci_gart_tlb_flush(rdev);
-	DRM_INFO("PCIE GART of %uM enabled (table at 0x%016llX).\n",
+	DRM_INFO("PCI GART of %uM enabled (table at 0x%016llX).\n",
 		 (unsigned)(rdev->mc.gtt_size >> 20),
 		 (unsigned long long)rdev->gart.table_addr);
 	rdev->gart.ready = true;
@@ -1180,6 +1180,10 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	WREG32(RADEON_CP_RB_WPTR_DELAY, 0);
 	WREG32(RADEON_CP_CSQ_MODE, 0x00004D4D);
 	WREG32(RADEON_CP_CSQ_CNTL, RADEON_CSQ_PRIBM_INDBM);
+
+	/* at this point everything should be setup correctly to enable master */
+	pci_set_master(rdev->pdev);
+
 	radeon_ring_start(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 	r = radeon_ring_test(rdev, RADEON_RING_TYPE_GFX_INDEX, ring);
 	if (r) {
@@ -1271,7 +1275,7 @@ void r100_cs_dump_packet(struct radeon_cs_parser *p,
 	unsigned i;
 	unsigned idx;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx;
 	for (i = 0; i <= (pkt->count + 1); i++, idx++) {
 		DRM_INFO("ib[%d]=0x%08X\n", idx, ib[idx]);
@@ -1350,7 +1354,7 @@ int r100_cs_packet_parse_vline(struct radeon_cs_parser *p)
 	uint32_t header, h_idx, reg;
 	volatile uint32_t *ib;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 
 	/* parse the wait until */
 	r = r100_cs_packet_parse(p, &waitreloc, p->idx);
@@ -1529,7 +1533,7 @@ static int r100_packet0_check(struct radeon_cs_parser *p,
 	u32 tile_flags = 0;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 
 	idx_value = radeon_get_ib_value(p, idx);
@@ -1885,7 +1889,7 @@ static int r100_packet3_check(struct radeon_cs_parser *p,
 	volatile uint32_t *ib;
 	int r;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	track = (struct r100_cs_track *)p->track;
 	switch (pkt->opcode) {
@@ -2004,6 +2008,8 @@ int r100_cs_parse(struct radeon_cs_parser *p)
 	int r;
 
 	track = kzalloc(sizeof(*track), GFP_KERNEL);
+	if (!track)
+		return -ENOMEM;
 	r100_cs_track_clear(p->rdev, track);
 	p->track = track;
 	do {
@@ -2155,79 +2161,18 @@ int r100_mc_wait_for_idle(struct radeon_device *rdev)
 	return -1;
 }
 
-void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup, struct radeon_ring *ring)
-{
-	lockup->last_cp_rptr = ring->rptr;
-	lockup->last_jiffies = jiffies;
-}
-
-/**
- * r100_gpu_cp_is_lockup() - check if CP is lockup by recording information
- * @rdev:	radeon device structure
- * @lockup:	r100_gpu_lockup structure holding CP lockup tracking informations
- * @cp:		radeon_cp structure holding CP information
- *
- * We don't need to initialize the lockup tracking information as we will either
- * have CP rptr to a different value of jiffies wrap around which will force
- * initialization of the lockup tracking informations.
- *
- * A possible false positivie is if we get call after while and last_cp_rptr ==
- * the current CP rptr, even if it's unlikely it might happen. To avoid this
- * if the elapsed time since last call is bigger than 2 second than we return
- * false and update the tracking information. Due to this the caller must call
- * r100_gpu_cp_is_lockup several time in less than 2sec for lockup to be reported
- * the fencing code should be cautious about that.
- *
- * Caller should write to the ring to force CP to do something so we don't get
- * false positive when CP is just gived nothing to do.
- *
- **/
-bool r100_gpu_cp_is_lockup(struct radeon_device *rdev, struct r100_gpu_lockup *lockup, struct radeon_ring *ring)
-{
-	unsigned long cjiffies, elapsed;
-
-	cjiffies = jiffies;
-	if (!time_after(cjiffies, lockup->last_jiffies)) {
-		/* likely a wrap around */
-		lockup->last_cp_rptr = ring->rptr;
-		lockup->last_jiffies = jiffies;
-		return false;
-	}
-	if (ring->rptr != lockup->last_cp_rptr) {
-		/* CP is still working no lockup */
-		lockup->last_cp_rptr = ring->rptr;
-		lockup->last_jiffies = jiffies;
-		return false;
-	}
-	elapsed = jiffies_to_msecs(cjiffies - lockup->last_jiffies);
-	if (elapsed >= 10000) {
-		dev_err(rdev->dev, "GPU lockup CP stall for more than %lumsec\n", elapsed);
-		return true;
-	}
-	/* give a chance to the GPU ... */
-	return false;
-}
-
 bool r100_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 rbbm_status;
-	int r;
 
 	rbbm_status = RREG32(R_000E40_RBBM_STATUS);
 	if (!G_000E40_GUI_ACTIVE(rbbm_status)) {
-		r100_gpu_lockup_update(&rdev->config.r100.lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r100.lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 void r100_bm_disable(struct radeon_device *rdev)
@@ -2296,7 +2241,6 @@ int r100_asic_reset(struct radeon_device *rdev)
 	if (G_000E40_SE_BUSY(status) || G_000E40_RE_BUSY(status) ||
 		G_000E40_TAM_BUSY(status) || G_000E40_PB_BUSY(status)) {
 		dev_err(rdev->dev, "failed to reset GPU\n");
-		rdev->gpu_lockup = true;
 		ret = -1;
 	} else
 		dev_info(rdev->dev, "GPU reset succeed\n");
@@ -3742,7 +3686,7 @@ void r100_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 
 int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	struct radeon_ib *ib;
+	struct radeon_ib ib;
 	uint32_t scratch;
 	uint32_t tmp = 0;
 	unsigned i;
@@ -3758,22 +3702,22 @@ int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	if (r) {
 		return r;
 	}
-	ib->ptr[0] = PACKET0(scratch, 0);
-	ib->ptr[1] = 0xDEADBEEF;
-	ib->ptr[2] = PACKET2(0);
-	ib->ptr[3] = PACKET2(0);
-	ib->ptr[4] = PACKET2(0);
-	ib->ptr[5] = PACKET2(0);
-	ib->ptr[6] = PACKET2(0);
-	ib->ptr[7] = PACKET2(0);
-	ib->length_dw = 8;
-	r = radeon_ib_schedule(rdev, ib);
+	ib.ptr[0] = PACKET0(scratch, 0);
+	ib.ptr[1] = 0xDEADBEEF;
+	ib.ptr[2] = PACKET2(0);
+	ib.ptr[3] = PACKET2(0);
+	ib.ptr[4] = PACKET2(0);
+	ib.ptr[5] = PACKET2(0);
+	ib.ptr[6] = PACKET2(0);
+	ib.ptr[7] = PACKET2(0);
+	ib.length_dw = 8;
+	r = radeon_ib_schedule(rdev, &ib);
 	if (r) {
 		radeon_scratch_free(rdev, scratch);
 		radeon_ib_free(rdev, &ib);
 		return r;
 	}
-	r = radeon_fence_wait(ib->fence, false);
+	r = radeon_fence_wait(ib.fence, false);
 	if (r) {
 		return r;
 	}
@@ -3965,12 +3909,9 @@ static int r100_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/r200.c b/drivers/gpu/drm/radeon/r200.c
index a59cc474d537..a26144d01207 100644
--- a/drivers/gpu/drm/radeon/r200.c
+++ b/drivers/gpu/drm/radeon/r200.c
@@ -154,7 +154,7 @@ int r200_packet0_check(struct radeon_cs_parser *p,
 	u32 tile_flags = 0;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 	idx_value = radeon_get_ib_value(p, idx);
 	switch (reg) {
diff --git a/drivers/gpu/drm/radeon/r300.c b/drivers/gpu/drm/radeon/r300.c
index fa14383f9ca0..97722a33e513 100644
--- a/drivers/gpu/drm/radeon/r300.c
+++ b/drivers/gpu/drm/radeon/r300.c
@@ -377,28 +377,6 @@ void r300_gpu_init(struct radeon_device *rdev)
 		 rdev->num_gb_pipes, rdev->num_z_pipes);
 }
 
-bool r300_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
-{
-	u32 rbbm_status;
-	int r;
-
-	rbbm_status = RREG32(R_000E40_RBBM_STATUS);
-	if (!G_000E40_GUI_ACTIVE(rbbm_status)) {
-		r100_gpu_lockup_update(&rdev->config.r300.lockup, ring);
-		return false;
-	}
-	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(RADEON_CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r300.lockup, ring);
-}
-
 int r300_asic_reset(struct radeon_device *rdev)
 {
 	struct r100_mc_save save;
@@ -449,7 +427,6 @@ int r300_asic_reset(struct radeon_device *rdev)
 	/* Check if GPU is idle */
 	if (G_000E40_GA_BUSY(status) || G_000E40_VAP_BUSY(status)) {
 		dev_err(rdev->dev, "failed to reset GPU\n");
-		rdev->gpu_lockup = true;
 		ret = -1;
 	} else
 		dev_info(rdev->dev, "GPU reset succeed\n");
@@ -627,7 +604,7 @@ static int r300_packet0_check(struct radeon_cs_parser *p,
 	int r;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 	idx_value = radeon_get_ib_value(p, idx);
 
@@ -1169,7 +1146,7 @@ static int r300_packet3_check(struct radeon_cs_parser *p,
 	unsigned idx;
 	int r;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	track = (struct r100_cs_track *)p->track;
 	switch(pkt->opcode) {
@@ -1418,12 +1395,9 @@ static int r300_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/r420.c b/drivers/gpu/drm/radeon/r420.c
index f3fcaacfea01..99137be7a300 100644
--- a/drivers/gpu/drm/radeon/r420.c
+++ b/drivers/gpu/drm/radeon/r420.c
@@ -279,12 +279,9 @@ static int r420_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/r520.c b/drivers/gpu/drm/radeon/r520.c
index ebcc15b03c9f..b5cf8375cd25 100644
--- a/drivers/gpu/drm/radeon/r520.c
+++ b/drivers/gpu/drm/radeon/r520.c
@@ -207,12 +207,10 @@ static int r520_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index c8187c4b6ae8..f388a1d73b63 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -713,6 +713,14 @@ void r600_hpd_init(struct radeon_device *rdev)
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 
+		if (connector->connector_type == DRM_MODE_CONNECTOR_eDP ||
+		    connector->connector_type == DRM_MODE_CONNECTOR_LVDS) {
+			/* don't try to enable hpd on eDP or LVDS avoid breaking the
+			 * aux dp channel on imac and help (but not completely fix)
+			 * https://bugzilla.redhat.com/show_bug.cgi?id=726143
+			 */
+			continue;
+		}
 		if (ASIC_IS_DCE3(rdev)) {
 			u32 tmp = DC_HPDx_CONNECTION_TIMER(0x9c4) | DC_HPDx_RX_INT_TIMER(0xfa);
 			if (ASIC_IS_DCE32(rdev))
@@ -1223,7 +1231,7 @@ int r600_vram_scratch_init(struct radeon_device *rdev)
 	if (rdev->vram_scratch.robj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				     &rdev->vram_scratch.robj);
+				     NULL, &rdev->vram_scratch.robj);
 		if (r) {
 			return r;
 		}
@@ -1350,31 +1358,17 @@ bool r600_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 	u32 srbm_status;
 	u32 grbm_status;
 	u32 grbm_status2;
-	struct r100_gpu_lockup *lockup;
-	int r;
-
-	if (rdev->family >= CHIP_RV770)
-		lockup = &rdev->config.rv770.lockup;
-	else
-		lockup = &rdev->config.r600.lockup;
 
 	srbm_status = RREG32(R_000E50_SRBM_STATUS);
 	grbm_status = RREG32(R_008010_GRBM_STATUS);
 	grbm_status2 = RREG32(R_008014_GRBM_STATUS2);
 	if (!G_008010_GUI_ACTIVE(grbm_status)) {
-		r100_gpu_lockup_update(lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 int r600_asic_reset(struct radeon_device *rdev)
@@ -2377,20 +2371,15 @@ int r600_copy_blit(struct radeon_device *rdev,
 		   unsigned num_gpu_pages,
 		   struct radeon_fence *fence)
 {
+	struct radeon_sa_bo *vb = NULL;
 	int r;
 
-	mutex_lock(&rdev->r600_blit.mutex);
-	rdev->r600_blit.vb_ib = NULL;
-	r = r600_blit_prepare_copy(rdev, num_gpu_pages);
+	r = r600_blit_prepare_copy(rdev, num_gpu_pages, &vb);
 	if (r) {
-		if (rdev->r600_blit.vb_ib)
-			radeon_ib_free(rdev, &rdev->r600_blit.vb_ib);
-		mutex_unlock(&rdev->r600_blit.mutex);
 		return r;
 	}
-	r600_kms_blit_copy(rdev, src_offset, dst_offset, num_gpu_pages);
-	r600_blit_done_copy(rdev, fence);
-	mutex_unlock(&rdev->r600_blit.mutex);
+	r600_kms_blit_copy(rdev, src_offset, dst_offset, num_gpu_pages, vb);
+	r600_blit_done_copy(rdev, fence, vb);
 	return 0;
 }
 
@@ -2494,12 +2483,9 @@ int r600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
@@ -2574,10 +2560,6 @@ int r600_init(struct radeon_device *rdev)
 	if (r600_debugfs_mc_info_init(rdev)) {
 		DRM_ERROR("Failed to register debugfs file for mc !\n");
 	}
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -2675,7 +2657,6 @@ void r600_fini(struct radeon_device *rdev)
 	r600_vram_scratch_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
@@ -2704,7 +2685,7 @@ void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 
 int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	struct radeon_ib *ib;
+	struct radeon_ib ib;
 	uint32_t scratch;
 	uint32_t tmp = 0;
 	unsigned i;
@@ -2722,18 +2703,18 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 		DRM_ERROR("radeon: failed to get ib (%d).\n", r);
 		return r;
 	}
-	ib->ptr[0] = PACKET3(PACKET3_SET_CONFIG_REG, 1);
-	ib->ptr[1] = ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
-	ib->ptr[2] = 0xDEADBEEF;
-	ib->length_dw = 3;
-	r = radeon_ib_schedule(rdev, ib);
+	ib.ptr[0] = PACKET3(PACKET3_SET_CONFIG_REG, 1);
+	ib.ptr[1] = ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
+	ib.ptr[2] = 0xDEADBEEF;
+	ib.length_dw = 3;
+	r = radeon_ib_schedule(rdev, &ib);
 	if (r) {
 		radeon_scratch_free(rdev, scratch);
 		radeon_ib_free(rdev, &ib);
 		DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
 		return r;
 	}
-	r = radeon_fence_wait(ib->fence, false);
+	r = radeon_fence_wait(ib.fence, false);
 	if (r) {
 		DRM_ERROR("radeon: fence wait failed (%d).\n", r);
 		return r;
@@ -2745,7 +2726,7 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 		DRM_UDELAY(1);
 	}
 	if (i < rdev->usec_timeout) {
-		DRM_INFO("ib test on ring %d succeeded in %u usecs\n", ib->fence->ring, i);
+		DRM_INFO("ib test on ring %d succeeded in %u usecs\n", ib.fence->ring, i);
 	} else {
 		DRM_ERROR("radeon: ib test failed (scratch(0x%04X)=0x%08X)\n",
 			  scratch, tmp);
@@ -2788,7 +2769,7 @@ int r600_ih_ring_alloc(struct radeon_device *rdev)
 		r = radeon_bo_create(rdev, rdev->ih.ring_size,
 				     PAGE_SIZE, true,
 				     RADEON_GEM_DOMAIN_GTT,
-				     &rdev->ih.ring_obj);
+				     NULL, &rdev->ih.ring_obj);
 		if (r) {
 			DRM_ERROR("radeon: failed to create ih ring buffer (%d).\n", r);
 			return r;
@@ -2968,6 +2949,15 @@ static void r600_disable_interrupt_state(struct radeon_device *rdev)
 			WREG32(DC_HPD5_INT_CONTROL, tmp);
 			tmp = RREG32(DC_HPD6_INT_CONTROL) & DC_HPDx_INT_POLARITY;
 			WREG32(DC_HPD6_INT_CONTROL, tmp);
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0, tmp);
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1, tmp);
+		} else {
+			tmp = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(HDMI0_AUDIO_PACKET_CONTROL, tmp);
+			tmp = RREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL, tmp);
 		}
 	} else {
 		WREG32(DACA_AUTODETECT_INT_CONTROL, 0);
@@ -2978,6 +2968,10 @@ static void r600_disable_interrupt_state(struct radeon_device *rdev)
 		WREG32(DC_HOT_PLUG_DETECT2_INT_CONTROL, tmp);
 		tmp = RREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL) & DC_HOT_PLUG_DETECTx_INT_POLARITY;
 		WREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL, tmp);
+		tmp = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL, tmp);
+		tmp = RREG32(HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+		WREG32(HDMI1_AUDIO_PACKET_CONTROL, tmp);
 	}
 }
 
@@ -3047,6 +3041,9 @@ int r600_irq_init(struct radeon_device *rdev)
 	else
 		r600_disable_interrupt_state(rdev);
 
+	/* at this point everything should be setup correctly to enable master */
+	pci_set_master(rdev->pdev);
+
 	/* enable irqs */
 	r600_enable_interrupts(rdev);
 
@@ -3071,7 +3068,7 @@ int r600_irq_set(struct radeon_device *rdev)
 	u32 mode_int = 0;
 	u32 hpd1, hpd2, hpd3, hpd4 = 0, hpd5 = 0, hpd6 = 0;
 	u32 grbm_int_cntl = 0;
-	u32 hdmi1, hdmi2;
+	u32 hdmi0, hdmi1;
 	u32 d1grph = 0, d2grph = 0;
 
 	if (!rdev->irq.installed) {
@@ -3086,9 +3083,7 @@ int r600_irq_set(struct radeon_device *rdev)
 		return 0;
 	}
 
-	hdmi1 = RREG32(R600_HDMI_BLOCK1 + R600_HDMI_CNTL) & ~R600_HDMI_INT_EN;
 	if (ASIC_IS_DCE3(rdev)) {
-		hdmi2 = RREG32(R600_HDMI_BLOCK3 + R600_HDMI_CNTL) & ~R600_HDMI_INT_EN;
 		hpd1 = RREG32(DC_HPD1_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd2 = RREG32(DC_HPD2_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd3 = RREG32(DC_HPD3_INT_CONTROL) & ~DC_HPDx_INT_EN;
@@ -3096,12 +3091,18 @@ int r600_irq_set(struct radeon_device *rdev)
 		if (ASIC_IS_DCE32(rdev)) {
 			hpd5 = RREG32(DC_HPD5_INT_CONTROL) & ~DC_HPDx_INT_EN;
 			hpd6 = RREG32(DC_HPD6_INT_CONTROL) & ~DC_HPDx_INT_EN;
+			hdmi0 = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+			hdmi1 = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+		} else {
+			hdmi0 = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			hdmi1 = RREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
 		}
 	} else {
-		hdmi2 = RREG32(R600_HDMI_BLOCK2 + R600_HDMI_CNTL) & ~R600_HDMI_INT_EN;
 		hpd1 = RREG32(DC_HOT_PLUG_DETECT1_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd2 = RREG32(DC_HOT_PLUG_DETECT2_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd3 = RREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL) & ~DC_HPDx_INT_EN;
+		hdmi0 = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+		hdmi1 = RREG32(HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
 	}
 
 	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
@@ -3143,13 +3144,13 @@ int r600_irq_set(struct radeon_device *rdev)
 		DRM_DEBUG("r600_irq_set: hpd 6\n");
 		hpd6 |= DC_HPDx_INT_EN;
 	}
-	if (rdev->irq.hdmi[0]) {
-		DRM_DEBUG("r600_irq_set: hdmi 1\n");
-		hdmi1 |= R600_HDMI_INT_EN;
+	if (rdev->irq.afmt[0]) {
+		DRM_DEBUG("r600_irq_set: hdmi 0\n");
+		hdmi0 |= HDMI0_AZ_FORMAT_WTRIG_MASK;
 	}
-	if (rdev->irq.hdmi[1]) {
-		DRM_DEBUG("r600_irq_set: hdmi 2\n");
-		hdmi2 |= R600_HDMI_INT_EN;
+	if (rdev->irq.afmt[1]) {
+		DRM_DEBUG("r600_irq_set: hdmi 0\n");
+		hdmi1 |= HDMI0_AZ_FORMAT_WTRIG_MASK;
 	}
 	if (rdev->irq.gui_idle) {
 		DRM_DEBUG("gui idle\n");
@@ -3161,9 +3162,7 @@ int r600_irq_set(struct radeon_device *rdev)
 	WREG32(D1GRPH_INTERRUPT_CONTROL, d1grph);
 	WREG32(D2GRPH_INTERRUPT_CONTROL, d2grph);
 	WREG32(GRBM_INT_CNTL, grbm_int_cntl);
-	WREG32(R600_HDMI_BLOCK1 + R600_HDMI_CNTL, hdmi1);
 	if (ASIC_IS_DCE3(rdev)) {
-		WREG32(R600_HDMI_BLOCK3 + R600_HDMI_CNTL, hdmi2);
 		WREG32(DC_HPD1_INT_CONTROL, hpd1);
 		WREG32(DC_HPD2_INT_CONTROL, hpd2);
 		WREG32(DC_HPD3_INT_CONTROL, hpd3);
@@ -3171,12 +3170,18 @@ int r600_irq_set(struct radeon_device *rdev)
 		if (ASIC_IS_DCE32(rdev)) {
 			WREG32(DC_HPD5_INT_CONTROL, hpd5);
 			WREG32(DC_HPD6_INT_CONTROL, hpd6);
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0, hdmi0);
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1, hdmi1);
+		} else {
+			WREG32(HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
+			WREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL, hdmi1);
 		}
 	} else {
-		WREG32(R600_HDMI_BLOCK2 + R600_HDMI_CNTL, hdmi2);
 		WREG32(DC_HOT_PLUG_DETECT1_INT_CONTROL, hpd1);
 		WREG32(DC_HOT_PLUG_DETECT2_INT_CONTROL, hpd2);
 		WREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL, hpd3);
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
+		WREG32(HDMI1_AUDIO_PACKET_CONTROL, hdmi1);
 	}
 
 	return 0;
@@ -3190,10 +3195,19 @@ static void r600_irq_ack(struct radeon_device *rdev)
 		rdev->irq.stat_regs.r600.disp_int = RREG32(DCE3_DISP_INTERRUPT_STATUS);
 		rdev->irq.stat_regs.r600.disp_int_cont = RREG32(DCE3_DISP_INTERRUPT_STATUS_CONTINUE);
 		rdev->irq.stat_regs.r600.disp_int_cont2 = RREG32(DCE3_DISP_INTERRUPT_STATUS_CONTINUE2);
+		if (ASIC_IS_DCE32(rdev)) {
+			rdev->irq.stat_regs.r600.hdmi0_status = RREG32(AFMT_STATUS + DCE3_HDMI_OFFSET0);
+			rdev->irq.stat_regs.r600.hdmi1_status = RREG32(AFMT_STATUS + DCE3_HDMI_OFFSET1);
+		} else {
+			rdev->irq.stat_regs.r600.hdmi0_status = RREG32(HDMI0_STATUS);
+			rdev->irq.stat_regs.r600.hdmi1_status = RREG32(DCE3_HDMI1_STATUS);
+		}
 	} else {
 		rdev->irq.stat_regs.r600.disp_int = RREG32(DISP_INTERRUPT_STATUS);
 		rdev->irq.stat_regs.r600.disp_int_cont = RREG32(DISP_INTERRUPT_STATUS_CONTINUE);
 		rdev->irq.stat_regs.r600.disp_int_cont2 = 0;
+		rdev->irq.stat_regs.r600.hdmi0_status = RREG32(HDMI0_STATUS);
+		rdev->irq.stat_regs.r600.hdmi1_status = RREG32(HDMI1_STATUS);
 	}
 	rdev->irq.stat_regs.r600.d1grph_int = RREG32(D1GRPH_INTERRUPT_STATUS);
 	rdev->irq.stat_regs.r600.d2grph_int = RREG32(D2GRPH_INTERRUPT_STATUS);
@@ -3259,17 +3273,32 @@ static void r600_irq_ack(struct radeon_device *rdev)
 			tmp |= DC_HPDx_INT_ACK;
 			WREG32(DC_HPD6_INT_CONTROL, tmp);
 		}
-	}
-	if (RREG32(R600_HDMI_BLOCK1 + R600_HDMI_STATUS) & R600_HDMI_INT_PENDING) {
-		WREG32_P(R600_HDMI_BLOCK1 + R600_HDMI_CNTL, R600_HDMI_INT_ACK, ~R600_HDMI_INT_ACK);
-	}
-	if (ASIC_IS_DCE3(rdev)) {
-		if (RREG32(R600_HDMI_BLOCK3 + R600_HDMI_STATUS) & R600_HDMI_INT_PENDING) {
-			WREG32_P(R600_HDMI_BLOCK3 + R600_HDMI_CNTL, R600_HDMI_INT_ACK, ~R600_HDMI_INT_ACK);
+		if (rdev->irq.stat_regs.r600.hdmi0_status & AFMT_AZ_FORMAT_WTRIG) {
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0);
+			tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0, tmp);
+		}
+		if (rdev->irq.stat_regs.r600.hdmi1_status & AFMT_AZ_FORMAT_WTRIG) {
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1);
+			tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1, tmp);
 		}
 	} else {
-		if (RREG32(R600_HDMI_BLOCK2 + R600_HDMI_STATUS) & R600_HDMI_INT_PENDING) {
-			WREG32_P(R600_HDMI_BLOCK2 + R600_HDMI_CNTL, R600_HDMI_INT_ACK, ~R600_HDMI_INT_ACK);
+		if (rdev->irq.stat_regs.r600.hdmi0_status & HDMI0_AZ_FORMAT_WTRIG) {
+			tmp = RREG32(HDMI0_AUDIO_PACKET_CONTROL);
+			tmp |= HDMI0_AZ_FORMAT_WTRIG_ACK;
+			WREG32(HDMI0_AUDIO_PACKET_CONTROL, tmp);
+		}
+		if (rdev->irq.stat_regs.r600.hdmi1_status & HDMI0_AZ_FORMAT_WTRIG) {
+			if (ASIC_IS_DCE3(rdev)) {
+				tmp = RREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL);
+				tmp |= HDMI0_AZ_FORMAT_WTRIG_ACK;
+				WREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL, tmp);
+			} else {
+				tmp = RREG32(HDMI1_AUDIO_PACKET_CONTROL);
+				tmp |= HDMI0_AZ_FORMAT_WTRIG_ACK;
+				WREG32(HDMI1_AUDIO_PACKET_CONTROL, tmp);
+			}
 		}
 	}
 }
@@ -3345,6 +3374,7 @@ int r600_irq_process(struct radeon_device *rdev)
 	u32 ring_index;
 	unsigned long flags;
 	bool queue_hotplug = false;
+	bool queue_hdmi = false;
 
 	if (!rdev->ih.enabled || rdev->shutdown)
 		return IRQ_NONE;
@@ -3480,9 +3510,26 @@ restart_ih:
 				break;
 			}
 			break;
-		case 21: /* HDMI */
-			DRM_DEBUG("IH: HDMI: 0x%x\n", src_data);
-			r600_audio_schedule_polling(rdev);
+		case 21: /* hdmi */
+			switch (src_data) {
+			case 4:
+				if (rdev->irq.stat_regs.r600.hdmi0_status & HDMI0_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.r600.hdmi0_status &= ~HDMI0_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI0\n");
+				}
+				break;
+			case 5:
+				if (rdev->irq.stat_regs.r600.hdmi1_status & HDMI0_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.r600.hdmi1_status &= ~HDMI0_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI1\n");
+				}
+				break;
+			default:
+				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
+				break;
+			}
 			break;
 		case 176: /* CP_INT in ring buffer */
 		case 177: /* CP_INT in IB1 */
@@ -3514,6 +3561,8 @@ restart_ih:
 		goto restart_ih;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
+	if (queue_hdmi)
+		schedule_work(&rdev->audio_work);
 	rdev->ih.rptr = rptr;
 	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	spin_unlock_irqrestore(&rdev->ih.lock, flags);
diff --git a/drivers/gpu/drm/radeon/r600_audio.c b/drivers/gpu/drm/radeon/r600_audio.c
index ba66f3093d46..7c4fa77f018f 100644
--- a/drivers/gpu/drm/radeon/r600_audio.c
+++ b/drivers/gpu/drm/radeon/r600_audio.c
@@ -29,7 +29,28 @@
 #include "radeon_asic.h"
 #include "atom.h"
 
-#define AUDIO_TIMER_INTERVALL 100 /* 1/10 sekund should be enough */
+/*
+ * check if enc_priv stores radeon_encoder_atom_dig
+ */
+static bool radeon_dig_encoder(struct drm_encoder *encoder)
+{
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	switch (radeon_encoder->encoder_id) {
+	case ENCODER_OBJECT_ID_INTERNAL_LVDS:
+	case ENCODER_OBJECT_ID_INTERNAL_TMDS1:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
+	case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
+	case ENCODER_OBJECT_ID_INTERNAL_DVO1:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
+	case ENCODER_OBJECT_ID_INTERNAL_DDI:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_LVTMA:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY1:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY2:
+		return true;
+	}
+	return false;
+}
 
 /*
  * check if the chipset is supported
@@ -42,118 +63,85 @@ static int r600_audio_chipset_supported(struct radeon_device *rdev)
 		|| rdev->family == CHIP_RS740;
 }
 
-/*
- * current number of channels
- */
-int r600_audio_channels(struct radeon_device *rdev)
+struct r600_audio r600_audio_status(struct radeon_device *rdev)
 {
-	return (RREG32(R600_AUDIO_RATE_BPS_CHANNEL) & 0x7) + 1;
-}
+	struct r600_audio status;
+	uint32_t value;
 
-/*
- * current bits per sample
- */
-int r600_audio_bits_per_sample(struct radeon_device *rdev)
-{
-	uint32_t value = (RREG32(R600_AUDIO_RATE_BPS_CHANNEL) & 0xF0) >> 4;
-	switch (value) {
-	case 0x0: return  8;
-	case 0x1: return 16;
-	case 0x2: return 20;
-	case 0x3: return 24;
-	case 0x4: return 32;
-	}
+	value = RREG32(R600_AUDIO_RATE_BPS_CHANNEL);
 
-	dev_err(rdev->dev, "Unknown bits per sample 0x%x using 16 instead\n",
-		(int)value);
+	/* number of channels */
+	status.channels = (value & 0x7) + 1;
 
-	return 16;
-}
-
-/*
- * current sampling rate in HZ
- */
-int r600_audio_rate(struct radeon_device *rdev)
-{
-	uint32_t value = RREG32(R600_AUDIO_RATE_BPS_CHANNEL);
-	uint32_t result;
+	/* bits per sample */
+	switch ((value & 0xF0) >> 4) {
+	case 0x0:
+		status.bits_per_sample = 8;
+		break;
+	case 0x1:
+		status.bits_per_sample = 16;
+		break;
+	case 0x2:
+		status.bits_per_sample = 20;
+		break;
+	case 0x3:
+		status.bits_per_sample = 24;
+		break;
+	case 0x4:
+		status.bits_per_sample = 32;
+		break;
+	default:
+		dev_err(rdev->dev, "Unknown bits per sample 0x%x, using 16\n",
+			(int)value);
+		status.bits_per_sample = 16;
+	}
 
+	/* current sampling rate in HZ */
 	if (value & 0x4000)
-		result = 44100;
+		status.rate = 44100;
 	else
-		result = 48000;
+		status.rate = 48000;
+	status.rate *= ((value >> 11) & 0x7) + 1;
+	status.rate /= ((value >> 8) & 0x7) + 1;
 
-	result *= ((value >> 11) & 0x7) + 1;
-	result /= ((value >> 8) & 0x7) + 1;
+	value = RREG32(R600_AUDIO_STATUS_BITS);
 
-	return result;
-}
+	/* iec 60958 status bits */
+	status.status_bits = value & 0xff;
 
-/*
- * iec 60958 status bits
- */
-uint8_t r600_audio_status_bits(struct radeon_device *rdev)
-{
-	return RREG32(R600_AUDIO_STATUS_BITS) & 0xff;
-}
+	/* iec 60958 category code */
+	status.category_code = (value >> 8) & 0xff;
 
-/*
- * iec 60958 category code
- */
-uint8_t r600_audio_category_code(struct radeon_device *rdev)
-{
-	return (RREG32(R600_AUDIO_STATUS_BITS) >> 8) & 0xff;
-}
-
-/*
- * schedule next audio update event
- */
-void r600_audio_schedule_polling(struct radeon_device *rdev)
-{
-	mod_timer(&rdev->audio_timer,
-		jiffies + msecs_to_jiffies(AUDIO_TIMER_INTERVALL));
+	return status;
 }
 
 /*
  * update all hdmi interfaces with current audio parameters
  */
-static void r600_audio_update_hdmi(unsigned long param)
+void r600_audio_update_hdmi(struct work_struct *work)
 {
-	struct radeon_device *rdev = (struct radeon_device *)param;
+	struct radeon_device *rdev = container_of(work, struct radeon_device,
+						  audio_work);
 	struct drm_device *dev = rdev->ddev;
-
-	int channels = r600_audio_channels(rdev);
-	int rate = r600_audio_rate(rdev);
-	int bps = r600_audio_bits_per_sample(rdev);
-	uint8_t status_bits = r600_audio_status_bits(rdev);
-	uint8_t category_code = r600_audio_category_code(rdev);
-
+	struct r600_audio audio_status = r600_audio_status(rdev);
 	struct drm_encoder *encoder;
-	int changes = 0, still_going = 0;
-
-	changes |= channels != rdev->audio_channels;
-	changes |= rate != rdev->audio_rate;
-	changes |= bps != rdev->audio_bits_per_sample;
-	changes |= status_bits != rdev->audio_status_bits;
-	changes |= category_code != rdev->audio_category_code;
-
-	if (changes) {
-		rdev->audio_channels = channels;
-		rdev->audio_rate = rate;
-		rdev->audio_bits_per_sample = bps;
-		rdev->audio_status_bits = status_bits;
-		rdev->audio_category_code = category_code;
+	bool changed = false;
+
+	if (rdev->audio_status.channels != audio_status.channels ||
+	    rdev->audio_status.rate != audio_status.rate ||
+	    rdev->audio_status.bits_per_sample != audio_status.bits_per_sample ||
+	    rdev->audio_status.status_bits != audio_status.status_bits ||
+	    rdev->audio_status.category_code != audio_status.category_code) {
+		rdev->audio_status = audio_status;
+		changed = true;
 	}
 
 	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-		still_going |= radeon_encoder->audio_polling_active;
-		if (changes || r600_hdmi_buffer_status_changed(encoder))
+		if (!radeon_dig_encoder(encoder))
+			continue;
+		if (changed || r600_hdmi_buffer_status_changed(encoder))
 			r600_hdmi_update_audio_settings(encoder);
 	}
-
-	if (still_going)
-		r600_audio_schedule_polling(rdev);
 }
 
 /*
@@ -177,7 +165,7 @@ static void r600_audio_engine_enable(struct radeon_device *rdev, bool enable)
 }
 
 /*
- * initialize the audio vars and register the update timer
+ * initialize the audio vars
  */
 int r600_audio_init(struct radeon_device *rdev)
 {
@@ -186,51 +174,16 @@ int r600_audio_init(struct radeon_device *rdev)
 
 	r600_audio_engine_enable(rdev, true);
 
-	rdev->audio_channels = -1;
-	rdev->audio_rate = -1;
-	rdev->audio_bits_per_sample = -1;
-	rdev->audio_status_bits = 0;
-	rdev->audio_category_code = 0;
-
-	setup_timer(
-		&rdev->audio_timer,
-		r600_audio_update_hdmi,
-		(unsigned long)rdev);
+	rdev->audio_status.channels = -1;
+	rdev->audio_status.rate = -1;
+	rdev->audio_status.bits_per_sample = -1;
+	rdev->audio_status.status_bits = 0;
+	rdev->audio_status.category_code = 0;
 
 	return 0;
 }
 
 /*
- * enable the polling timer, to check for status changes
- */
-void r600_audio_enable_polling(struct drm_encoder *encoder)
-{
-	struct drm_device *dev = encoder->dev;
-	struct radeon_device *rdev = dev->dev_private;
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-
-	DRM_DEBUG("r600_audio_enable_polling: %d\n",
-		radeon_encoder->audio_polling_active);
-	if (radeon_encoder->audio_polling_active)
-		return;
-
-	radeon_encoder->audio_polling_active = 1;
-	if (rdev->audio_enabled)
-		mod_timer(&rdev->audio_timer, jiffies + 1);
-}
-
-/*
- * disable the polling timer, so we get no more status updates
- */
-void r600_audio_disable_polling(struct drm_encoder *encoder)
-{
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	DRM_DEBUG("r600_audio_disable_polling: %d\n",
-		radeon_encoder->audio_polling_active);
-	radeon_encoder->audio_polling_active = 0;
-}
-
-/*
  * atach the audio codec to the clock source of the encoder
  */
 void r600_audio_set_clock(struct drm_encoder *encoder, int clock)
@@ -297,7 +250,5 @@ void r600_audio_fini(struct radeon_device *rdev)
 	if (!rdev->audio_enabled)
 		return;
 
-	del_timer(&rdev->audio_timer);
-
 	r600_audio_engine_enable(rdev, false);
 }
diff --git a/drivers/gpu/drm/radeon/r600_blit_kms.c b/drivers/gpu/drm/radeon/r600_blit_kms.c
index db38f587f27a..03b6e0d3d503 100644
--- a/drivers/gpu/drm/radeon/r600_blit_kms.c
+++ b/drivers/gpu/drm/radeon/r600_blit_kms.c
@@ -513,7 +513,6 @@ int r600_blit_init(struct radeon_device *rdev)
 	rdev->r600_blit.primitives.set_default_state = set_default_state;
 
 	rdev->r600_blit.ring_size_common = 40; /* shaders + def state */
-	rdev->r600_blit.ring_size_common += 16; /* fence emit for VB IB */
 	rdev->r600_blit.ring_size_common += 5; /* done copy */
 	rdev->r600_blit.ring_size_common += 16; /* fence emit for done copy */
 
@@ -528,7 +527,6 @@ int r600_blit_init(struct radeon_device *rdev)
 	if (rdev->r600_blit.shader_obj)
 		goto done;
 
-	mutex_init(&rdev->r600_blit.mutex);
 	rdev->r600_blit.state_offset = 0;
 
 	if (rdev->family >= CHIP_RV770)
@@ -554,7 +552,7 @@ int r600_blit_init(struct radeon_device *rdev)
 	obj_size = ALIGN(obj_size, 256);
 
 	r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				&rdev->r600_blit.shader_obj);
+			     NULL, &rdev->r600_blit.shader_obj);
 	if (r) {
 		DRM_ERROR("r600 failed to allocate shader\n");
 		return r;
@@ -621,27 +619,6 @@ void r600_blit_fini(struct radeon_device *rdev)
 	radeon_bo_unref(&rdev->r600_blit.shader_obj);
 }
 
-static int r600_vb_ib_get(struct radeon_device *rdev, unsigned size)
-{
-	int r;
-	r = radeon_ib_get(rdev, RADEON_RING_TYPE_GFX_INDEX,
-			  &rdev->r600_blit.vb_ib, size);
-	if (r) {
-		DRM_ERROR("failed to get IB for vertex buffer\n");
-		return r;
-	}
-
-	rdev->r600_blit.vb_total = size;
-	rdev->r600_blit.vb_used = 0;
-	return 0;
-}
-
-static void r600_vb_ib_put(struct radeon_device *rdev)
-{
-	radeon_fence_emit(rdev, rdev->r600_blit.vb_ib->fence);
-	radeon_ib_free(rdev, &rdev->r600_blit.vb_ib);
-}
-
 static unsigned r600_blit_create_rect(unsigned num_gpu_pages,
 				      int *width, int *height, int max_dim)
 {
@@ -688,7 +665,8 @@ static unsigned r600_blit_create_rect(unsigned num_gpu_pages,
 }
 
 
-int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages)
+int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages,
+			   struct radeon_sa_bo **vb)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
@@ -705,46 +683,54 @@ int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages)
 	}
 
 	/* 48 bytes for vertex per loop */
-	r = r600_vb_ib_get(rdev, (num_loops*48)+256);
-	if (r)
+	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo, vb,
+			     (num_loops*48)+256, 256, true);
+	if (r) {
 		return r;
+	}
 
 	/* calculate number of loops correctly */
 	ring_size = num_loops * dwords_per_loop;
 	ring_size += rdev->r600_blit.ring_size_common;
 	r = radeon_ring_lock(rdev, ring, ring_size);
-	if (r)
+	if (r) {
+		radeon_sa_bo_free(rdev, vb, NULL);
 		return r;
+	}
 
 	rdev->r600_blit.primitives.set_default_state(rdev);
 	rdev->r600_blit.primitives.set_shaders(rdev);
 	return 0;
 }
 
-void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence)
+void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence,
+			 struct radeon_sa_bo *vb)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	if (rdev->r600_blit.vb_ib)
-		r600_vb_ib_put(rdev);
-
-	if (fence)
-		r = radeon_fence_emit(rdev, fence);
+	r = radeon_fence_emit(rdev, fence);
+	if (r) {
+		radeon_ring_unlock_undo(rdev, ring);
+		return;
+	}
 
-	radeon_ring_unlock_commit(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
+	radeon_ring_unlock_commit(rdev, ring);
+	radeon_sa_bo_free(rdev, &vb, fence);
 }
 
 void r600_kms_blit_copy(struct radeon_device *rdev,
 			u64 src_gpu_addr, u64 dst_gpu_addr,
-			unsigned num_gpu_pages)
+			unsigned num_gpu_pages,
+			struct radeon_sa_bo *vb)
 {
 	u64 vb_gpu_addr;
-	u32 *vb;
+	u32 *vb_cpu_addr;
 
-	DRM_DEBUG("emitting copy %16llx %16llx %d %d\n",
-		  src_gpu_addr, dst_gpu_addr,
-		  num_gpu_pages, rdev->r600_blit.vb_used);
-	vb = (u32 *)(rdev->r600_blit.vb_ib->ptr + rdev->r600_blit.vb_used);
+	DRM_DEBUG("emitting copy %16llx %16llx %d\n",
+		  src_gpu_addr, dst_gpu_addr, num_gpu_pages);
+	vb_cpu_addr = (u32 *)radeon_sa_bo_cpu_addr(vb);
+	vb_gpu_addr = radeon_sa_bo_gpu_addr(vb);
 
 	while (num_gpu_pages) {
 		int w, h;
@@ -756,39 +742,34 @@ void r600_kms_blit_copy(struct radeon_device *rdev,
 		size_in_bytes = pages_per_loop * RADEON_GPU_PAGE_SIZE;
 		DRM_DEBUG("rectangle w=%d h=%d\n", w, h);
 
-		if ((rdev->r600_blit.vb_used + 48) > rdev->r600_blit.vb_total) {
-			WARN_ON(1);
-		}
-
-		vb[0] = 0;
-		vb[1] = 0;
-		vb[2] = 0;
-		vb[3] = 0;
+		vb_cpu_addr[0] = 0;
+		vb_cpu_addr[1] = 0;
+		vb_cpu_addr[2] = 0;
+		vb_cpu_addr[3] = 0;
 
-		vb[4] = 0;
-		vb[5] = i2f(h);
-		vb[6] = 0;
-		vb[7] = i2f(h);
+		vb_cpu_addr[4] = 0;
+		vb_cpu_addr[5] = i2f(h);
+		vb_cpu_addr[6] = 0;
+		vb_cpu_addr[7] = i2f(h);
 
-		vb[8] = i2f(w);
-		vb[9] = i2f(h);
-		vb[10] = i2f(w);
-		vb[11] = i2f(h);
+		vb_cpu_addr[8] = i2f(w);
+		vb_cpu_addr[9] = i2f(h);
+		vb_cpu_addr[10] = i2f(w);
+		vb_cpu_addr[11] = i2f(h);
 
 		rdev->r600_blit.primitives.set_tex_resource(rdev, FMT_8_8_8_8,
 							    w, h, w, src_gpu_addr, size_in_bytes);
 		rdev->r600_blit.primitives.set_render_target(rdev, COLOR_8_8_8_8,
 							     w, h, dst_gpu_addr);
 		rdev->r600_blit.primitives.set_scissors(rdev, 0, 0, w, h);
-		vb_gpu_addr = rdev->r600_blit.vb_ib->gpu_addr + rdev->r600_blit.vb_used;
 		rdev->r600_blit.primitives.set_vtx_resource(rdev, vb_gpu_addr);
 		rdev->r600_blit.primitives.draw_auto(rdev);
 		rdev->r600_blit.primitives.cp_set_surface_sync(rdev,
 				    PACKET3_CB_ACTION_ENA | PACKET3_CB0_DEST_BASE_ENA,
 				    size_in_bytes, dst_gpu_addr);
 
-		vb += 12;
-		rdev->r600_blit.vb_used += 4*12;
+		vb_cpu_addr += 12;
+		vb_gpu_addr += 4*12;
 		src_gpu_addr += size_in_bytes;
 		dst_gpu_addr += size_in_bytes;
 		num_gpu_pages -= pages_per_loop;
diff --git a/drivers/gpu/drm/radeon/r600_cs.c b/drivers/gpu/drm/radeon/r600_cs.c
index b8e12af304a9..0133f5f09bd6 100644
--- a/drivers/gpu/drm/radeon/r600_cs.c
+++ b/drivers/gpu/drm/radeon/r600_cs.c
@@ -345,7 +345,7 @@ static int r600_cs_track_validate_cb(struct radeon_cs_parser *p, int i)
 	u32 height, height_align, pitch, pitch_align, depth_align;
 	u64 base_offset, base_align;
 	struct array_mode_checker array_check;
-	volatile u32 *ib = p->ib->ptr;
+	volatile u32 *ib = p->ib.ptr;
 	unsigned array_mode;
 	u32 format;
 
@@ -471,7 +471,7 @@ static int r600_cs_track_validate_db(struct radeon_cs_parser *p)
 	u64 base_offset, base_align;
 	struct array_mode_checker array_check;
 	int array_mode;
-	volatile u32 *ib = p->ib->ptr;
+	volatile u32 *ib = p->ib.ptr;
 
 
 	if (track->db_bo == NULL) {
@@ -961,7 +961,7 @@ static int r600_cs_packet_parse_vline(struct radeon_cs_parser *p)
 	uint32_t header, h_idx, reg, wait_reg_mem_info;
 	volatile uint32_t *ib;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 
 	/* parse the WAIT_REG_MEM */
 	r = r600_cs_packet_parse(p, &wait_reg_mem, p->idx);
@@ -1110,7 +1110,7 @@ static int r600_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 	m = 1 << ((reg >> 2) & 31);
 	if (!(r600_reg_safe_bm[i] & m))
 		return 0;
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	switch (reg) {
 	/* force following reg to 0 in an attempt to disable out buffer
 	 * which will need us to better understand how it works to perform
@@ -1714,7 +1714,7 @@ static int r600_packet3_check(struct radeon_cs_parser *p,
 	u32 idx_value;
 
 	track = (struct r600_cs_track *)p->track;
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	idx_value = radeon_get_ib_value(p, idx);
 
@@ -2249,8 +2249,8 @@ int r600_cs_parse(struct radeon_cs_parser *p)
 		}
 	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
 #if 0
-	for (r = 0; r < p->ib->length_dw; r++) {
-		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib->ptr[r]);
+	for (r = 0; r < p->ib.length_dw; r++) {
+		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib.ptr[r]);
 		mdelay(1);
 	}
 #endif
@@ -2298,7 +2298,6 @@ int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
 {
 	struct radeon_cs_parser parser;
 	struct radeon_cs_chunk *ib_chunk;
-	struct radeon_ib fake_ib;
 	struct r600_cs_track *track;
 	int r;
 
@@ -2314,9 +2313,8 @@ int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
 	parser.dev = &dev->pdev->dev;
 	parser.rdev = NULL;
 	parser.family = family;
-	parser.ib = &fake_ib;
 	parser.track = track;
-	fake_ib.ptr = ib;
+	parser.ib.ptr = ib;
 	r = radeon_cs_parser_init(&parser, data);
 	if (r) {
 		DRM_ERROR("Failed to initialize parser !\n");
@@ -2333,8 +2331,8 @@ int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
 	 * input memory (cached) and write to the IB (which can be
 	 * uncached). */
 	ib_chunk = &parser.chunks[parser.chunk_ib_idx];
-	parser.ib->length_dw = ib_chunk->length_dw;
-	*l = parser.ib->length_dw;
+	parser.ib.length_dw = ib_chunk->length_dw;
+	*l = parser.ib.length_dw;
 	r = r600_cs_parse(&parser);
 	if (r) {
 		DRM_ERROR("Invalid command stream !\n");
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index 0b5920671450..226379e00ac1 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -27,6 +27,7 @@
 #include "radeon_drm.h"
 #include "radeon.h"
 #include "radeon_asic.h"
+#include "r600d.h"
 #include "atom.h"
 
 /*
@@ -52,19 +53,7 @@ enum r600_hdmi_iec_status_bits {
 	AUDIO_STATUS_LEVEL        = 0x80
 };
 
-struct {
-	uint32_t Clock;
-
-	int N_32kHz;
-	int CTS_32kHz;
-
-	int N_44_1kHz;
-	int CTS_44_1kHz;
-
-	int N_48kHz;
-	int CTS_48kHz;
-
-} r600_hdmi_ACR[] = {
+struct radeon_hdmi_acr r600_hdmi_predefined_acr[] = {
     /*	     32kHz	  44.1kHz	48kHz    */
     /* Clock      N     CTS      N     CTS      N     CTS */
     {  25174,  4576,  28125,  7007,  31250,  6864,  28125 }, /*  25,20/1.001 MHz */
@@ -83,7 +72,7 @@ struct {
 /*
  * calculate CTS value if it's not found in the table
  */
-static void r600_hdmi_calc_CTS(uint32_t clock, int *CTS, int N, int freq)
+static void r600_hdmi_calc_cts(uint32_t clock, int *CTS, int N, int freq)
 {
 	if (*CTS == 0)
 		*CTS = clock * N / (128 * freq) * 1000;
@@ -91,6 +80,24 @@ static void r600_hdmi_calc_CTS(uint32_t clock, int *CTS, int N, int freq)
 		  N, *CTS, freq);
 }
 
+struct radeon_hdmi_acr r600_hdmi_acr(uint32_t clock)
+{
+	struct radeon_hdmi_acr res;
+	u8 i;
+
+	for (i = 0; r600_hdmi_predefined_acr[i].clock != clock &&
+	     r600_hdmi_predefined_acr[i].clock != 0; i++)
+		;
+	res = r600_hdmi_predefined_acr[i];
+
+	/* In case some CTS are missing */
+	r600_hdmi_calc_cts(clock, &res.cts_32khz, res.n_32khz, 32000);
+	r600_hdmi_calc_cts(clock, &res.cts_44_1khz, res.n_44_1khz, 44100);
+	r600_hdmi_calc_cts(clock, &res.cts_48khz, res.n_48khz, 48000);
+
+	return res;
+}
+
 /*
  * update the N and CTS parameters for a given pixel clock rate
  */
@@ -98,30 +105,19 @@ static void r600_hdmi_update_ACR(struct drm_encoder *encoder, uint32_t clock)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
-	int CTS;
-	int N;
-	int i;
+	struct radeon_hdmi_acr acr = r600_hdmi_acr(clock);
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
+
+	WREG32(HDMI0_ACR_32_0 + offset, HDMI0_ACR_CTS_32(acr.cts_32khz));
+	WREG32(HDMI0_ACR_32_1 + offset, acr.n_32khz);
 
-	for (i = 0; r600_hdmi_ACR[i].Clock != clock && r600_hdmi_ACR[i].Clock != 0; i++);
-
-	CTS = r600_hdmi_ACR[i].CTS_32kHz;
-	N = r600_hdmi_ACR[i].N_32kHz;
-	r600_hdmi_calc_CTS(clock, &CTS, N, 32000);
-	WREG32(offset+R600_HDMI_32kHz_CTS, CTS << 12);
-	WREG32(offset+R600_HDMI_32kHz_N, N);
-
-	CTS = r600_hdmi_ACR[i].CTS_44_1kHz;
-	N = r600_hdmi_ACR[i].N_44_1kHz;
-	r600_hdmi_calc_CTS(clock, &CTS, N, 44100);
-	WREG32(offset+R600_HDMI_44_1kHz_CTS, CTS << 12);
-	WREG32(offset+R600_HDMI_44_1kHz_N, N);
-
-	CTS = r600_hdmi_ACR[i].CTS_48kHz;
-	N = r600_hdmi_ACR[i].N_48kHz;
-	r600_hdmi_calc_CTS(clock, &CTS, N, 48000);
-	WREG32(offset+R600_HDMI_48kHz_CTS, CTS << 12);
-	WREG32(offset+R600_HDMI_48kHz_N, N);
+	WREG32(HDMI0_ACR_44_0 + offset, HDMI0_ACR_CTS_44(acr.cts_44_1khz));
+	WREG32(HDMI0_ACR_44_1 + offset, acr.n_44_1khz);
+
+	WREG32(HDMI0_ACR_48_0 + offset, HDMI0_ACR_CTS_48(acr.cts_48khz));
+	WREG32(HDMI0_ACR_48_1 + offset, acr.n_48khz);
 }
 
 /*
@@ -165,7 +161,9 @@ static void r600_hdmi_videoinfoframe(
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
 
 	uint8_t frame[14];
 
@@ -204,13 +202,13 @@ static void r600_hdmi_videoinfoframe(
 	 * workaround this issue. */
 	frame[0x0] += 2;
 
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_0,
+	WREG32(HDMI0_AVI_INFO0 + offset,
 		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_1,
+	WREG32(HDMI0_AVI_INFO1 + offset,
 		frame[0x4] | (frame[0x5] << 8) | (frame[0x6] << 16) | (frame[0x7] << 24));
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_2,
+	WREG32(HDMI0_AVI_INFO2 + offset,
 		frame[0x8] | (frame[0x9] << 8) | (frame[0xA] << 16) | (frame[0xB] << 24));
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_3,
+	WREG32(HDMI0_AVI_INFO3 + offset,
 		frame[0xC] | (frame[0xD] << 8));
 }
 
@@ -231,7 +229,9 @@ static void r600_hdmi_audioinfoframe(
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
 
 	uint8_t frame[11];
 
@@ -249,22 +249,24 @@ static void r600_hdmi_audioinfoframe(
 
 	r600_hdmi_infoframe_checksum(0x84, 0x01, 0x0A, frame);
 
-	WREG32(offset+R600_HDMI_AUDIOINFOFRAME_0,
+	WREG32(HDMI0_AUDIO_INFO0 + offset,
 		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
-	WREG32(offset+R600_HDMI_AUDIOINFOFRAME_1,
+	WREG32(HDMI0_AUDIO_INFO1 + offset,
 		frame[0x4] | (frame[0x5] << 8) | (frame[0x6] << 16) | (frame[0x8] << 24));
 }
 
 /*
  * test if audio buffer is filled enough to start playing
  */
-static int r600_hdmi_is_audio_buffer_filled(struct drm_encoder *encoder)
+static bool r600_hdmi_is_audio_buffer_filled(struct drm_encoder *encoder)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
 
-	return (RREG32(offset+R600_HDMI_STATUS) & 0x10) != 0;
+	return (RREG32(HDMI0_STATUS + offset) & 0x10) != 0;
 }
 
 /*
@@ -273,14 +275,15 @@ static int r600_hdmi_is_audio_buffer_filled(struct drm_encoder *encoder)
 int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder)
 {
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 	int status, result;
 
-	if (!radeon_encoder->hdmi_offset)
+	if (!dig->afmt || !dig->afmt->enabled)
 		return 0;
 
 	status = r600_hdmi_is_audio_buffer_filled(encoder);
-	result = radeon_encoder->hdmi_buffer_status != status;
-	radeon_encoder->hdmi_buffer_status = status;
+	result = dig->afmt->last_buffer_filled_status != status;
+	dig->afmt->last_buffer_filled_status = status;
 
 	return result;
 }
@@ -288,26 +291,23 @@ int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder)
 /*
  * write the audio workaround status to the hardware
  */
-void r600_hdmi_audio_workaround(struct drm_encoder *encoder)
+static void r600_hdmi_audio_workaround(struct drm_encoder *encoder)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	uint32_t offset = radeon_encoder->hdmi_offset;
-
-	if (!offset)
-		return;
-
-	if (!radeon_encoder->hdmi_audio_workaround ||
-		r600_hdmi_is_audio_buffer_filled(encoder)) {
-
-		/* disable audio workaround */
-		WREG32_P(offset+R600_HDMI_CNTL, 0x00000001, ~0x00001001);
-
-	} else {
-		/* enable audio workaround */
-		WREG32_P(offset+R600_HDMI_CNTL, 0x00001001, ~0x00001001);
-	}
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
+	bool hdmi_audio_workaround = false; /* FIXME */
+	u32 value;
+
+	if (!hdmi_audio_workaround ||
+	    r600_hdmi_is_audio_buffer_filled(encoder))
+		value = 0; /* disable workaround */
+	else
+		value = HDMI0_AUDIO_TEST_EN; /* enable workaround */
+	WREG32_P(HDMI0_AUDIO_PACKET_CONTROL + offset,
+		 value, ~HDMI0_AUDIO_TEST_EN);
 }
 
 
@@ -318,39 +318,75 @@ void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mod
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset;
 
 	if (ASIC_IS_DCE5(rdev))
 		return;
 
-	if (!offset)
+	/* Silent, r600_hdmi_enable will raise WARN for us */
+	if (!dig->afmt->enabled)
 		return;
+	offset = dig->afmt->offset;
 
 	r600_audio_set_clock(encoder, mode->clock);
 
-	WREG32(offset+R600_HDMI_UNKNOWN_0, 0x1000);
-	WREG32(offset+R600_HDMI_UNKNOWN_1, 0x0);
-	WREG32(offset+R600_HDMI_UNKNOWN_2, 0x1000);
+	WREG32(HDMI0_VBI_PACKET_CONTROL + offset,
+	       HDMI0_NULL_SEND); /* send null packets when required */
 
-	r600_hdmi_update_ACR(encoder, mode->clock);
+	WREG32(HDMI0_AUDIO_CRC_CONTROL + offset, 0x1000);
+
+	if (ASIC_IS_DCE32(rdev)) {
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL + offset,
+		       HDMI0_AUDIO_DELAY_EN(1) | /* default audio delay */
+		       HDMI0_AUDIO_PACKETS_PER_LINE(3)); /* should be suffient for all audio modes and small enough for all hblanks */
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + offset,
+		       AFMT_AUDIO_SAMPLE_SEND | /* send audio packets */
+		       AFMT_60958_CS_UPDATE); /* allow 60958 channel status fields to be updated */
+	} else {
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL + offset,
+		       HDMI0_AUDIO_SAMPLE_SEND | /* send audio packets */
+		       HDMI0_AUDIO_DELAY_EN(1) | /* default audio delay */
+		       HDMI0_AUDIO_SEND_MAX_PACKETS | /* send NULL packets if no audio is available */
+		       HDMI0_AUDIO_PACKETS_PER_LINE(3) | /* should be suffient for all audio modes and small enough for all hblanks */
+		       HDMI0_60958_CS_UPDATE); /* allow 60958 channel status fields to be updated */
+	}
+
+	WREG32(HDMI0_ACR_PACKET_CONTROL + offset,
+	       HDMI0_ACR_AUTO_SEND | /* allow hw to sent ACR packets when required */
+	       HDMI0_ACR_SOURCE); /* select SW CTS value */
+
+	WREG32(HDMI0_VBI_PACKET_CONTROL + offset,
+	       HDMI0_NULL_SEND | /* send null packets when required */
+	       HDMI0_GC_SEND | /* send general control packets */
+	       HDMI0_GC_CONT); /* send general control packets every frame */
 
-	WREG32(offset+R600_HDMI_VIDEOCNTL, 0x13);
+	/* TODO: HDMI0_AUDIO_INFO_UPDATE */
+	WREG32(HDMI0_INFOFRAME_CONTROL0 + offset,
+	       HDMI0_AVI_INFO_SEND | /* enable AVI info frames */
+	       HDMI0_AVI_INFO_CONT | /* send AVI info frames every frame/field */
+	       HDMI0_AUDIO_INFO_SEND | /* enable audio info frames (frames won't be set until audio is enabled) */
+	       HDMI0_AUDIO_INFO_CONT); /* send audio info frames every frame/field */
 
-	WREG32(offset+R600_HDMI_VERSION, 0x202);
+	WREG32(HDMI0_INFOFRAME_CONTROL1 + offset,
+	       HDMI0_AVI_INFO_LINE(2) | /* anything other than 0 */
+	       HDMI0_AUDIO_INFO_LINE(2)); /* anything other than 0 */
+
+	WREG32(HDMI0_GC + offset, 0); /* unset HDMI0_GC_AVMUTE */
 
 	r600_hdmi_videoinfoframe(encoder, RGB, 0, 0, 0, 0,
 		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
 
+	r600_hdmi_update_ACR(encoder, mode->clock);
+
 	/* it's unknown what these bits do excatly, but it's indeed quite useful for debugging */
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_0, 0x00FFFFFF);
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_1, 0x007FFFFF);
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_2, 0x00000001);
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_3, 0x00000001);
+	WREG32(HDMI0_RAMP_CONTROL0 + offset, 0x00FFFFFF);
+	WREG32(HDMI0_RAMP_CONTROL1 + offset, 0x007FFFFF);
+	WREG32(HDMI0_RAMP_CONTROL2 + offset, 0x00000001);
+	WREG32(HDMI0_RAMP_CONTROL3 + offset, 0x00000001);
 
 	r600_hdmi_audio_workaround(encoder);
-
-	/* audio packets per line, does anyone know how to calc this ? */
-	WREG32_P(offset+R600_HDMI_CNTL, 0x00040000, ~0x001F0000);
 }
 
 /*
@@ -360,145 +396,82 @@ void r600_hdmi_update_audio_settings(struct drm_encoder *encoder)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
-
-	int channels = r600_audio_channels(rdev);
-	int rate = r600_audio_rate(rdev);
-	int bps = r600_audio_bits_per_sample(rdev);
-	uint8_t status_bits = r600_audio_status_bits(rdev);
-	uint8_t category_code = r600_audio_category_code(rdev);
-
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	struct r600_audio audio = r600_audio_status(rdev);
+	uint32_t offset;
 	uint32_t iec;
 
-	if (!offset)
+	if (!dig->afmt || !dig->afmt->enabled)
 		return;
+	offset = dig->afmt->offset;
 
 	DRM_DEBUG("%s with %d channels, %d Hz sampling rate, %d bits per sample,\n",
 		 r600_hdmi_is_audio_buffer_filled(encoder) ? "playing" : "stopped",
-		channels, rate, bps);
+		  audio.channels, audio.rate, audio.bits_per_sample);
 	DRM_DEBUG("0x%02X IEC60958 status bits and 0x%02X category code\n",
-		  (int)status_bits, (int)category_code);
+		  (int)audio.status_bits, (int)audio.category_code);
 
 	iec = 0;
-	if (status_bits & AUDIO_STATUS_PROFESSIONAL)
+	if (audio.status_bits & AUDIO_STATUS_PROFESSIONAL)
 		iec |= 1 << 0;
-	if (status_bits & AUDIO_STATUS_NONAUDIO)
+	if (audio.status_bits & AUDIO_STATUS_NONAUDIO)
 		iec |= 1 << 1;
-	if (status_bits & AUDIO_STATUS_COPYRIGHT)
+	if (audio.status_bits & AUDIO_STATUS_COPYRIGHT)
 		iec |= 1 << 2;
-	if (status_bits & AUDIO_STATUS_EMPHASIS)
+	if (audio.status_bits & AUDIO_STATUS_EMPHASIS)
 		iec |= 1 << 3;
 
-	iec |= category_code << 8;
-
-	switch (rate) {
-	case  32000: iec |= 0x3 << 24; break;
-	case  44100: iec |= 0x0 << 24; break;
-	case  88200: iec |= 0x8 << 24; break;
-	case 176400: iec |= 0xc << 24; break;
-	case  48000: iec |= 0x2 << 24; break;
-	case  96000: iec |= 0xa << 24; break;
-	case 192000: iec |= 0xe << 24; break;
+	iec |= HDMI0_60958_CS_CATEGORY_CODE(audio.category_code);
+
+	switch (audio.rate) {
+	case 32000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x3);
+		break;
+	case 44100:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x0);
+		break;
+	case 48000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x2);
+		break;
+	case 88200:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x8);
+		break;
+	case 96000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0xa);
+		break;
+	case 176400:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0xc);
+		break;
+	case 192000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0xe);
+		break;
 	}
 
-	WREG32(offset+R600_HDMI_IEC60958_1, iec);
+	WREG32(HDMI0_60958_0 + offset, iec);
 
 	iec = 0;
-	switch (bps) {
-	case 16: iec |= 0x2; break;
-	case 20: iec |= 0x3; break;
-	case 24: iec |= 0xb; break;
+	switch (audio.bits_per_sample) {
+	case 16:
+		iec |= HDMI0_60958_CS_WORD_LENGTH(0x2);
+		break;
+	case 20:
+		iec |= HDMI0_60958_CS_WORD_LENGTH(0x3);
+		break;
+	case 24:
+		iec |= HDMI0_60958_CS_WORD_LENGTH(0xb);
+		break;
 	}
-	if (status_bits & AUDIO_STATUS_V)
+	if (audio.status_bits & AUDIO_STATUS_V)
 		iec |= 0x5 << 16;
+	WREG32_P(HDMI0_60958_1 + offset, iec, ~0x5000f);
 
-	WREG32_P(offset+R600_HDMI_IEC60958_2, iec, ~0x5000f);
-
-	/* 0x021 or 0x031 sets the audio frame length */
-	WREG32(offset+R600_HDMI_AUDIOCNTL, 0x31);
-	r600_hdmi_audioinfoframe(encoder, channels-1, 0, 0, 0, 0, 0, 0, 0);
+	r600_hdmi_audioinfoframe(encoder, audio.channels - 1, 0, 0, 0, 0, 0, 0,
+				 0);
 
 	r600_hdmi_audio_workaround(encoder);
 }
 
-static int r600_hdmi_find_free_block(struct drm_device *dev)
-{
-	struct radeon_device *rdev = dev->dev_private;
-	struct drm_encoder *encoder;
-	struct radeon_encoder *radeon_encoder;
-	bool free_blocks[3] = { true, true, true };
-
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		radeon_encoder = to_radeon_encoder(encoder);
-		switch (radeon_encoder->hdmi_offset) {
-		case R600_HDMI_BLOCK1:
-			free_blocks[0] = false;
-			break;
-		case R600_HDMI_BLOCK2:
-			free_blocks[1] = false;
-			break;
-		case R600_HDMI_BLOCK3:
-			free_blocks[2] = false;
-			break;
-		}
-	}
-
-	if (rdev->family == CHIP_RS600 || rdev->family == CHIP_RS690 ||
-	    rdev->family == CHIP_RS740) {
-		return free_blocks[0] ? R600_HDMI_BLOCK1 : 0;
-	} else if (rdev->family >= CHIP_R600) {
-		if (free_blocks[0])
-			return R600_HDMI_BLOCK1;
-		else if (free_blocks[1])
-			return R600_HDMI_BLOCK2;
-	}
-	return 0;
-}
-
-static void r600_hdmi_assign_block(struct drm_encoder *encoder)
-{
-	struct drm_device *dev = encoder->dev;
-	struct radeon_device *rdev = dev->dev_private;
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-
-	u16 eg_offsets[] = {
-		EVERGREEN_CRTC0_REGISTER_OFFSET,
-		EVERGREEN_CRTC1_REGISTER_OFFSET,
-		EVERGREEN_CRTC2_REGISTER_OFFSET,
-		EVERGREEN_CRTC3_REGISTER_OFFSET,
-		EVERGREEN_CRTC4_REGISTER_OFFSET,
-		EVERGREEN_CRTC5_REGISTER_OFFSET,
-	};
-
-	if (!dig) {
-		dev_err(rdev->dev, "Enabling HDMI on non-dig encoder\n");
-		return;
-	}
-
-	if (ASIC_IS_DCE5(rdev)) {
-		/* TODO */
-	} else if (ASIC_IS_DCE4(rdev)) {
-		if (dig->dig_encoder >= ARRAY_SIZE(eg_offsets)) {
-			dev_err(rdev->dev, "Enabling HDMI on unknown dig\n");
-			return;
-		}
-		radeon_encoder->hdmi_offset = EVERGREEN_HDMI_BASE +
-						eg_offsets[dig->dig_encoder];
-		radeon_encoder->hdmi_config_offset = radeon_encoder->hdmi_offset
-						+ EVERGREEN_HDMI_CONFIG_OFFSET;
-	} else if (ASIC_IS_DCE3(rdev)) {
-		radeon_encoder->hdmi_offset = dig->dig_encoder ?
-			R600_HDMI_BLOCK3 : R600_HDMI_BLOCK1;
-		if (ASIC_IS_DCE32(rdev))
-			radeon_encoder->hdmi_config_offset = dig->dig_encoder ?
-				R600_HDMI_CONFIG2 : R600_HDMI_CONFIG1;
-	} else if (rdev->family >= CHIP_R600 || rdev->family == CHIP_RS600 ||
-		   rdev->family == CHIP_RS690 || rdev->family == CHIP_RS740) {
-		radeon_encoder->hdmi_offset = r600_hdmi_find_free_block(dev);
-	}
-}
-
 /*
  * enable the HDMI engine
  */
@@ -507,64 +480,57 @@ void r600_hdmi_enable(struct drm_encoder *encoder)
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 	uint32_t offset;
+	u32 hdmi;
 
 	if (ASIC_IS_DCE5(rdev))
 		return;
 
-	if (!radeon_encoder->hdmi_offset) {
-		r600_hdmi_assign_block(encoder);
-		if (!radeon_encoder->hdmi_offset) {
-			dev_warn(rdev->dev, "Could not find HDMI block for "
-				"0x%x encoder\n", radeon_encoder->encoder_id);
-			return;
-		}
-	}
+	/* Silent, r600_hdmi_enable will raise WARN for us */
+	if (dig->afmt->enabled)
+		return;
+	offset = dig->afmt->offset;
 
-	offset = radeon_encoder->hdmi_offset;
-	if (ASIC_IS_DCE5(rdev)) {
-		/* TODO */
-	} else if (ASIC_IS_DCE4(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0xc, 0x1, ~0x1);
-	} else if (ASIC_IS_DCE32(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0x4, 0x1, ~0x1);
-	} else if (ASIC_IS_DCE3(rdev)) {
-		/* TODO */
-	} else if (rdev->family >= CHIP_R600) {
+	/* Older chipsets require setting HDMI and routing manually */
+	if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
+		hdmi = HDMI0_ERROR_ACK | HDMI0_ENABLE;
 		switch (radeon_encoder->encoder_id) {
 		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
 			WREG32_P(AVIVO_TMDSA_CNTL, AVIVO_TMDSA_CNTL_HDMI_EN,
 				 ~AVIVO_TMDSA_CNTL_HDMI_EN);
-			WREG32(offset + R600_HDMI_ENABLE, 0x101);
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_TMDSA);
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
 			WREG32_P(AVIVO_LVTMA_CNTL, AVIVO_LVTMA_CNTL_HDMI_EN,
 				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
-			WREG32(offset + R600_HDMI_ENABLE, 0x105);
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_LVTMA);
+			break;
+		case ENCODER_OBJECT_ID_INTERNAL_DDI:
+			WREG32_P(DDIA_CNTL, DDIA_HDMI_EN, ~DDIA_HDMI_EN);
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_DDIA);
+			break;
+		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_DVOA);
 			break;
 		default:
-			dev_err(rdev->dev, "Unknown HDMI output type\n");
+			dev_err(rdev->dev, "Invalid encoder for HDMI: 0x%X\n",
+				radeon_encoder->encoder_id);
 			break;
 		}
+		WREG32(HDMI0_CONTROL + offset, hdmi);
 	}
 
-	if (rdev->irq.installed
-	    && rdev->family != CHIP_RS600
-	    && rdev->family != CHIP_RS690
-	    && rdev->family != CHIP_RS740
-	    && !ASIC_IS_DCE4(rdev)) {
+	if (rdev->irq.installed) {
 		/* if irq is available use it */
-		rdev->irq.hdmi[offset == R600_HDMI_BLOCK1 ? 0 : 1] = true;
+		rdev->irq.afmt[dig->afmt->id] = true;
 		radeon_irq_set(rdev);
-
-		r600_audio_disable_polling(encoder);
-	} else {
-		/* if not fallback to polling */
-		r600_audio_enable_polling(encoder);
 	}
 
+	dig->afmt->enabled = true;
+
 	DRM_DEBUG("Enabling HDMI interface @ 0x%04X for encoder 0x%x\n",
-		radeon_encoder->hdmi_offset, radeon_encoder->encoder_id);
+		  offset, radeon_encoder->encoder_id);
 }
 
 /*
@@ -575,51 +541,51 @@ void r600_hdmi_disable(struct drm_encoder *encoder)
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 	uint32_t offset;
 
 	if (ASIC_IS_DCE5(rdev))
 		return;
 
-	offset = radeon_encoder->hdmi_offset;
-	if (!offset) {
-		dev_err(rdev->dev, "Disabling not enabled HDMI\n");
+	/* Called for ATOM_ENCODER_MODE_HDMI only */
+	if (!dig || !dig->afmt) {
+		WARN_ON(1);
 		return;
 	}
+	if (!dig->afmt->enabled)
+		return;
+	offset = dig->afmt->offset;
 
 	DRM_DEBUG("Disabling HDMI interface @ 0x%04X for encoder 0x%x\n",
-		offset, radeon_encoder->encoder_id);
+		  offset, radeon_encoder->encoder_id);
 
 	/* disable irq */
-	rdev->irq.hdmi[offset == R600_HDMI_BLOCK1 ? 0 : 1] = false;
+	rdev->irq.afmt[dig->afmt->id] = false;
 	radeon_irq_set(rdev);
 
-	/* disable polling */
-	r600_audio_disable_polling(encoder);
-
-	if (ASIC_IS_DCE5(rdev)) {
-		/* TODO */
-	} else if (ASIC_IS_DCE4(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0xc, 0, ~0x1);
-	} else if (ASIC_IS_DCE32(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0x4, 0, ~0x1);
-	} else if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
+	/* Older chipsets not handled by AtomBIOS */
+	if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
 		switch (radeon_encoder->encoder_id) {
 		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
 			WREG32_P(AVIVO_TMDSA_CNTL, 0,
 				 ~AVIVO_TMDSA_CNTL_HDMI_EN);
-			WREG32(offset + R600_HDMI_ENABLE, 0);
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
 			WREG32_P(AVIVO_LVTMA_CNTL, 0,
 				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
-			WREG32(offset + R600_HDMI_ENABLE, 0);
+			break;
+		case ENCODER_OBJECT_ID_INTERNAL_DDI:
+			WREG32_P(DDIA_CNTL, 0, ~DDIA_HDMI_EN);
+			break;
+		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
 			break;
 		default:
-			dev_err(rdev->dev, "Unknown HDMI output type\n");
+			dev_err(rdev->dev, "Invalid encoder for HDMI: 0x%X\n",
+				radeon_encoder->encoder_id);
 			break;
 		}
+		WREG32(HDMI0_CONTROL + offset, HDMI0_ERROR_ACK);
 	}
 
-	radeon_encoder->hdmi_offset = 0;
-	radeon_encoder->hdmi_config_offset = 0;
+	dig->afmt->enabled = false;
 }
diff --git a/drivers/gpu/drm/radeon/r600_reg.h b/drivers/gpu/drm/radeon/r600_reg.h
index f869897c7456..2b960cb5c18a 100644
--- a/drivers/gpu/drm/radeon/r600_reg.h
+++ b/drivers/gpu/drm/radeon/r600_reg.h
@@ -156,45 +156,10 @@
 #define R600_AUDIO_PIN_WIDGET_CNTL        0x73d4
 #define R600_AUDIO_STATUS_BITS            0x73d8
 
-/* HDMI base register addresses */
-#define R600_HDMI_BLOCK1                  0x7400
-#define R600_HDMI_BLOCK2                  0x7700
-#define R600_HDMI_BLOCK3                  0x7800
-
-/* HDMI registers */
-#define R600_HDMI_ENABLE                0x00
-#define R600_HDMI_STATUS                0x04
-#       define R600_HDMI_INT_PENDING    (1 << 29)
-#define R600_HDMI_CNTL                  0x08
-#       define R600_HDMI_INT_EN         (1 << 28)
-#       define R600_HDMI_INT_ACK        (1 << 29)
-#define R600_HDMI_UNKNOWN_0             0x0C
-#define R600_HDMI_AUDIOCNTL             0x10
-#define R600_HDMI_VIDEOCNTL             0x14
-#define R600_HDMI_VERSION               0x18
-#define R600_HDMI_UNKNOWN_1             0x28
-#define R600_HDMI_VIDEOINFOFRAME_0      0x54
-#define R600_HDMI_VIDEOINFOFRAME_1      0x58
-#define R600_HDMI_VIDEOINFOFRAME_2      0x5c
-#define R600_HDMI_VIDEOINFOFRAME_3      0x60
-#define R600_HDMI_32kHz_CTS             0xac
-#define R600_HDMI_32kHz_N               0xb0
-#define R600_HDMI_44_1kHz_CTS           0xb4
-#define R600_HDMI_44_1kHz_N             0xb8
-#define R600_HDMI_48kHz_CTS             0xbc
-#define R600_HDMI_48kHz_N               0xc0
-#define R600_HDMI_AUDIOINFOFRAME_0      0xcc
-#define R600_HDMI_AUDIOINFOFRAME_1      0xd0
-#define R600_HDMI_IEC60958_1            0xd4
-#define R600_HDMI_IEC60958_2            0xd8
-#define R600_HDMI_UNKNOWN_2             0xdc
-#define R600_HDMI_AUDIO_DEBUG_0         0xe0
-#define R600_HDMI_AUDIO_DEBUG_1         0xe4
-#define R600_HDMI_AUDIO_DEBUG_2         0xe8
-#define R600_HDMI_AUDIO_DEBUG_3         0xec
-
-/* HDMI additional config base register addresses */
-#define R600_HDMI_CONFIG1                 0x7600
-#define R600_HDMI_CONFIG2                 0x7a00
+#define DCE2_HDMI_OFFSET0		(0x7400 - 0x7400)
+#define DCE2_HDMI_OFFSET1		(0x7700 - 0x7400)
+/* DCE3.2 second instance starts at 0x7800 */
+#define DCE3_HDMI_OFFSET0		(0x7400 - 0x7400)
+#define DCE3_HDMI_OFFSET1		(0x7800 - 0x7400)
 
 #endif
diff --git a/drivers/gpu/drm/radeon/r600d.h b/drivers/gpu/drm/radeon/r600d.h
index 59f9c993cc31..15bd3b216243 100644
--- a/drivers/gpu/drm/radeon/r600d.h
+++ b/drivers/gpu/drm/radeon/r600d.h
@@ -824,6 +824,239 @@
 #       define TARGET_LINK_SPEED_MASK                     (0xf << 0)
 #       define SELECTABLE_DEEMPHASIS                      (1 << 6)
 
+/* Audio clocks */
+#define DCCG_AUDIO_DTO0_PHASE             0x0514
+#define DCCG_AUDIO_DTO0_MODULE            0x0518
+#define DCCG_AUDIO_DTO0_LOAD              0x051c
+#       define DTO_LOAD                   (1 << 31)
+#define DCCG_AUDIO_DTO0_CNTL              0x0520
+
+#define DCCG_AUDIO_DTO1_PHASE             0x0524
+#define DCCG_AUDIO_DTO1_MODULE            0x0528
+#define DCCG_AUDIO_DTO1_LOAD              0x052c
+#define DCCG_AUDIO_DTO1_CNTL              0x0530
+
+#define DCCG_AUDIO_DTO_SELECT             0x0534
+
+/* digital blocks */
+#define TMDSA_CNTL                       0x7880
+#       define TMDSA_HDMI_EN             (1 << 2)
+#define LVTMA_CNTL                       0x7a80
+#       define LVTMA_HDMI_EN             (1 << 2)
+#define DDIA_CNTL                        0x7200
+#       define DDIA_HDMI_EN              (1 << 2)
+#define DIG0_CNTL                        0x75a0
+#       define DIG_MODE(x)               (((x) & 7) << 8)
+#       define DIG_MODE_DP               0
+#       define DIG_MODE_LVDS             1
+#       define DIG_MODE_TMDS_DVI         2
+#       define DIG_MODE_TMDS_HDMI        3
+#       define DIG_MODE_SDVO             4
+#define DIG1_CNTL                        0x79a0
+
+/* rs6xx/rs740 and r6xx share the same HDMI blocks, however, rs6xx has only one
+ * instance of the blocks while r6xx has 2.  DCE 3.0 cards are slightly
+ * different due to the new DIG blocks, but also have 2 instances.
+ * DCE 3.0 HDMI blocks are part of each DIG encoder.
+ */
+
+/* rs6xx/rs740/r6xx/dce3 */
+#define HDMI0_CONTROL                0x7400
+/* rs6xx/rs740/r6xx */
+#       define HDMI0_ENABLE          (1 << 0)
+#       define HDMI0_STREAM(x)       (((x) & 3) << 2)
+#       define HDMI0_STREAM_TMDSA    0
+#       define HDMI0_STREAM_LVTMA    1
+#       define HDMI0_STREAM_DVOA     2
+#       define HDMI0_STREAM_DDIA     3
+/* rs6xx/r6xx/dce3 */
+#       define HDMI0_ERROR_ACK       (1 << 8)
+#       define HDMI0_ERROR_MASK      (1 << 9)
+#define HDMI0_STATUS                 0x7404
+#       define HDMI0_ACTIVE_AVMUTE   (1 << 0)
+#       define HDMI0_AUDIO_ENABLE    (1 << 4)
+#       define HDMI0_AZ_FORMAT_WTRIG     (1 << 28)
+#       define HDMI0_AZ_FORMAT_WTRIG_INT (1 << 29)
+#define HDMI0_AUDIO_PACKET_CONTROL   0x7408
+#       define HDMI0_AUDIO_SAMPLE_SEND  (1 << 0)
+#       define HDMI0_AUDIO_DELAY_EN(x)  (((x) & 3) << 4)
+#       define HDMI0_AUDIO_SEND_MAX_PACKETS  (1 << 8)
+#       define HDMI0_AUDIO_TEST_EN         (1 << 12)
+#       define HDMI0_AUDIO_PACKETS_PER_LINE(x)  (((x) & 0x1f) << 16)
+#       define HDMI0_AUDIO_CHANNEL_SWAP    (1 << 24)
+#       define HDMI0_60958_CS_UPDATE       (1 << 26)
+#       define HDMI0_AZ_FORMAT_WTRIG_MASK  (1 << 28)
+#       define HDMI0_AZ_FORMAT_WTRIG_ACK   (1 << 29)
+#define HDMI0_AUDIO_CRC_CONTROL      0x740c
+#       define HDMI0_AUDIO_CRC_EN    (1 << 0)
+#define HDMI0_VBI_PACKET_CONTROL     0x7410
+#       define HDMI0_NULL_SEND       (1 << 0)
+#       define HDMI0_GC_SEND         (1 << 4)
+#       define HDMI0_GC_CONT         (1 << 5) /* 0 - once; 1 - every frame */
+#define HDMI0_INFOFRAME_CONTROL0     0x7414
+#       define HDMI0_AVI_INFO_SEND   (1 << 0)
+#       define HDMI0_AVI_INFO_CONT   (1 << 1)
+#       define HDMI0_AUDIO_INFO_SEND (1 << 4)
+#       define HDMI0_AUDIO_INFO_CONT (1 << 5)
+#       define HDMI0_AUDIO_INFO_SOURCE (1 << 6) /* 0 - sound block; 1 - hmdi regs */
+#       define HDMI0_AUDIO_INFO_UPDATE (1 << 7)
+#       define HDMI0_MPEG_INFO_SEND  (1 << 8)
+#       define HDMI0_MPEG_INFO_CONT  (1 << 9)
+#       define HDMI0_MPEG_INFO_UPDATE  (1 << 10)
+#define HDMI0_INFOFRAME_CONTROL1     0x7418
+#       define HDMI0_AVI_INFO_LINE(x)  (((x) & 0x3f) << 0)
+#       define HDMI0_AUDIO_INFO_LINE(x)  (((x) & 0x3f) << 8)
+#       define HDMI0_MPEG_INFO_LINE(x)  (((x) & 0x3f) << 16)
+#define HDMI0_GENERIC_PACKET_CONTROL 0x741c
+#       define HDMI0_GENERIC0_SEND   (1 << 0)
+#       define HDMI0_GENERIC0_CONT   (1 << 1)
+#       define HDMI0_GENERIC0_UPDATE (1 << 2)
+#       define HDMI0_GENERIC1_SEND   (1 << 4)
+#       define HDMI0_GENERIC1_CONT   (1 << 5)
+#       define HDMI0_GENERIC0_LINE(x)  (((x) & 0x3f) << 16)
+#       define HDMI0_GENERIC1_LINE(x)  (((x) & 0x3f) << 24)
+#define HDMI0_GC                     0x7428
+#       define HDMI0_GC_AVMUTE       (1 << 0)
+#define HDMI0_AVI_INFO0              0x7454
+#       define HDMI0_AVI_INFO_CHECKSUM(x)  (((x) & 0xff) << 0)
+#       define HDMI0_AVI_INFO_S(x)   (((x) & 3) << 8)
+#       define HDMI0_AVI_INFO_B(x)   (((x) & 3) << 10)
+#       define HDMI0_AVI_INFO_A(x)   (((x) & 1) << 12)
+#       define HDMI0_AVI_INFO_Y(x)   (((x) & 3) << 13)
+#       define HDMI0_AVI_INFO_Y_RGB       0
+#       define HDMI0_AVI_INFO_Y_YCBCR422  1
+#       define HDMI0_AVI_INFO_Y_YCBCR444  2
+#       define HDMI0_AVI_INFO_Y_A_B_S(x)   (((x) & 0xff) << 8)
+#       define HDMI0_AVI_INFO_R(x)   (((x) & 0xf) << 16)
+#       define HDMI0_AVI_INFO_M(x)   (((x) & 0x3) << 20)
+#       define HDMI0_AVI_INFO_C(x)   (((x) & 0x3) << 22)
+#       define HDMI0_AVI_INFO_C_M_R(x)   (((x) & 0xff) << 16)
+#       define HDMI0_AVI_INFO_SC(x)  (((x) & 0x3) << 24)
+#       define HDMI0_AVI_INFO_ITC_EC_Q_SC(x)  (((x) & 0xff) << 24)
+#define HDMI0_AVI_INFO1              0x7458
+#       define HDMI0_AVI_INFO_VIC(x) (((x) & 0x7f) << 0) /* don't use avi infoframe v1 */
+#       define HDMI0_AVI_INFO_PR(x)  (((x) & 0xf) << 8) /* don't use avi infoframe v1 */
+#       define HDMI0_AVI_INFO_TOP(x) (((x) & 0xffff) << 16)
+#define HDMI0_AVI_INFO2              0x745c
+#       define HDMI0_AVI_INFO_BOTTOM(x)  (((x) & 0xffff) << 0)
+#       define HDMI0_AVI_INFO_LEFT(x)    (((x) & 0xffff) << 16)
+#define HDMI0_AVI_INFO3              0x7460
+#       define HDMI0_AVI_INFO_RIGHT(x)    (((x) & 0xffff) << 0)
+#       define HDMI0_AVI_INFO_VERSION(x)  (((x) & 3) << 24)
+#define HDMI0_MPEG_INFO0             0x7464
+#       define HDMI0_MPEG_INFO_CHECKSUM(x)  (((x) & 0xff) << 0)
+#       define HDMI0_MPEG_INFO_MB0(x)  (((x) & 0xff) << 8)
+#       define HDMI0_MPEG_INFO_MB1(x)  (((x) & 0xff) << 16)
+#       define HDMI0_MPEG_INFO_MB2(x)  (((x) & 0xff) << 24)
+#define HDMI0_MPEG_INFO1             0x7468
+#       define HDMI0_MPEG_INFO_MB3(x)  (((x) & 0xff) << 0)
+#       define HDMI0_MPEG_INFO_MF(x)   (((x) & 3) << 8)
+#       define HDMI0_MPEG_INFO_FR(x)   (((x) & 1) << 12)
+#define HDMI0_GENERIC0_HDR           0x746c
+#define HDMI0_GENERIC0_0             0x7470
+#define HDMI0_GENERIC0_1             0x7474
+#define HDMI0_GENERIC0_2             0x7478
+#define HDMI0_GENERIC0_3             0x747c
+#define HDMI0_GENERIC0_4             0x7480
+#define HDMI0_GENERIC0_5             0x7484
+#define HDMI0_GENERIC0_6             0x7488
+#define HDMI0_GENERIC1_HDR           0x748c
+#define HDMI0_GENERIC1_0             0x7490
+#define HDMI0_GENERIC1_1             0x7494
+#define HDMI0_GENERIC1_2             0x7498
+#define HDMI0_GENERIC1_3             0x749c
+#define HDMI0_GENERIC1_4             0x74a0
+#define HDMI0_GENERIC1_5             0x74a4
+#define HDMI0_GENERIC1_6             0x74a8
+#define HDMI0_ACR_32_0               0x74ac
+#       define HDMI0_ACR_CTS_32(x)   (((x) & 0xfffff) << 12)
+#define HDMI0_ACR_32_1               0x74b0
+#       define HDMI0_ACR_N_32(x)   (((x) & 0xfffff) << 0)
+#define HDMI0_ACR_44_0               0x74b4
+#       define HDMI0_ACR_CTS_44(x)   (((x) & 0xfffff) << 12)
+#define HDMI0_ACR_44_1               0x74b8
+#       define HDMI0_ACR_N_44(x)   (((x) & 0xfffff) << 0)
+#define HDMI0_ACR_48_0               0x74bc
+#       define HDMI0_ACR_CTS_48(x)   (((x) & 0xfffff) << 12)
+#define HDMI0_ACR_48_1               0x74c0
+#       define HDMI0_ACR_N_48(x)   (((x) & 0xfffff) << 0)
+#define HDMI0_ACR_STATUS_0           0x74c4
+#define HDMI0_ACR_STATUS_1           0x74c8
+#define HDMI0_AUDIO_INFO0            0x74cc
+#       define HDMI0_AUDIO_INFO_CHECKSUM(x)  (((x) & 0xff) << 0)
+#       define HDMI0_AUDIO_INFO_CC(x)  (((x) & 7) << 8)
+#define HDMI0_AUDIO_INFO1            0x74d0
+#       define HDMI0_AUDIO_INFO_CA(x)  (((x) & 0xff) << 0)
+#       define HDMI0_AUDIO_INFO_LSV(x)  (((x) & 0xf) << 11)
+#       define HDMI0_AUDIO_INFO_DM_INH(x)  (((x) & 1) << 15)
+#       define HDMI0_AUDIO_INFO_DM_INH_LSV(x)  (((x) & 0xff) << 8)
+#define HDMI0_60958_0                0x74d4
+#       define HDMI0_60958_CS_A(x)   (((x) & 1) << 0)
+#       define HDMI0_60958_CS_B(x)   (((x) & 1) << 1)
+#       define HDMI0_60958_CS_C(x)   (((x) & 1) << 2)
+#       define HDMI0_60958_CS_D(x)   (((x) & 3) << 3)
+#       define HDMI0_60958_CS_MODE(x)   (((x) & 3) << 6)
+#       define HDMI0_60958_CS_CATEGORY_CODE(x)      (((x) & 0xff) << 8)
+#       define HDMI0_60958_CS_SOURCE_NUMBER(x)      (((x) & 0xf) << 16)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_L(x)   (((x) & 0xf) << 20)
+#       define HDMI0_60958_CS_SAMPLING_FREQUENCY(x) (((x) & 0xf) << 24)
+#       define HDMI0_60958_CS_CLOCK_ACCURACY(x)     (((x) & 3) << 28)
+#define HDMI0_60958_1                0x74d8
+#       define HDMI0_60958_CS_WORD_LENGTH(x)        (((x) & 0xf) << 0)
+#       define HDMI0_60958_CS_ORIGINAL_SAMPLING_FREQUENCY(x)   (((x) & 0xf) << 4)
+#       define HDMI0_60958_CS_VALID_L(x)   (((x) & 1) << 16)
+#       define HDMI0_60958_CS_VALID_R(x)   (((x) & 1) << 18)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_R(x)   (((x) & 0xf) << 20)
+#define HDMI0_ACR_PACKET_CONTROL     0x74dc
+#       define HDMI0_ACR_SEND        (1 << 0)
+#       define HDMI0_ACR_CONT        (1 << 1)
+#       define HDMI0_ACR_SELECT(x)   (((x) & 3) << 4)
+#       define HDMI0_ACR_HW          0
+#       define HDMI0_ACR_32          1
+#       define HDMI0_ACR_44          2
+#       define HDMI0_ACR_48          3
+#       define HDMI0_ACR_SOURCE      (1 << 8) /* 0 - hw; 1 - cts value */
+#       define HDMI0_ACR_AUTO_SEND   (1 << 12)
+#define HDMI0_RAMP_CONTROL0          0x74e0
+#       define HDMI0_RAMP_MAX_COUNT(x)   (((x) & 0xffffff) << 0)
+#define HDMI0_RAMP_CONTROL1          0x74e4
+#       define HDMI0_RAMP_MIN_COUNT(x)   (((x) & 0xffffff) << 0)
+#define HDMI0_RAMP_CONTROL2          0x74e8
+#       define HDMI0_RAMP_INC_COUNT(x)   (((x) & 0xffffff) << 0)
+#define HDMI0_RAMP_CONTROL3          0x74ec
+#       define HDMI0_RAMP_DEC_COUNT(x)   (((x) & 0xffffff) << 0)
+/* HDMI0_60958_2 is r7xx only */
+#define HDMI0_60958_2                0x74f0
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_2(x)   (((x) & 0xf) << 0)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_3(x)   (((x) & 0xf) << 4)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_4(x)   (((x) & 0xf) << 8)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_5(x)   (((x) & 0xf) << 12)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_6(x)   (((x) & 0xf) << 16)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_7(x)   (((x) & 0xf) << 20)
+/* r6xx only; second instance starts at 0x7700 */
+#define HDMI1_CONTROL                0x7700
+#define HDMI1_STATUS                 0x7704
+#define HDMI1_AUDIO_PACKET_CONTROL   0x7708
+/* DCE3; second instance starts at 0x7800 NOT 0x7700 */
+#define DCE3_HDMI1_CONTROL                0x7800
+#define DCE3_HDMI1_STATUS                 0x7804
+#define DCE3_HDMI1_AUDIO_PACKET_CONTROL   0x7808
+/* DCE3.2 (for interrupts) */
+#define AFMT_STATUS                          0x7600
+#       define AFMT_AUDIO_ENABLE             (1 << 4)
+#       define AFMT_AZ_FORMAT_WTRIG          (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_INT      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG      (1 << 30)
+#define AFMT_AUDIO_PACKET_CONTROL            0x7604
+#       define AFMT_AUDIO_SAMPLE_SEND        (1 << 0)
+#       define AFMT_AUDIO_TEST_EN            (1 << 12)
+#       define AFMT_AUDIO_CHANNEL_SWAP       (1 << 24)
+#       define AFMT_60958_CS_UPDATE          (1 << 26)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_MASK (1 << 27)
+#       define AFMT_AZ_FORMAT_WTRIG_MASK     (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_ACK      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_ACK  (1 << 30)
+
 /*
  * PM4
  */
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 138b95216d8d..1dc3a4aba020 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -94,33 +94,38 @@ extern int radeon_disp_priority;
 extern int radeon_hw_i2c;
 extern int radeon_pcie_gen2;
 extern int radeon_msi;
+extern int radeon_lockup_timeout;
 
 /*
  * Copy from radeon_drv.h so we don't have to include both and have conflicting
  * symbol;
  */
-#define RADEON_MAX_USEC_TIMEOUT		100000	/* 100 ms */
-#define RADEON_FENCE_JIFFIES_TIMEOUT	(HZ / 2)
+#define RADEON_MAX_USEC_TIMEOUT			100000	/* 100 ms */
+#define RADEON_FENCE_JIFFIES_TIMEOUT		(HZ / 2)
 /* RADEON_IB_POOL_SIZE must be a power of 2 */
-#define RADEON_IB_POOL_SIZE		16
-#define RADEON_DEBUGFS_MAX_COMPONENTS	32
-#define RADEONFB_CONN_LIMIT		4
-#define RADEON_BIOS_NUM_SCRATCH		8
+#define RADEON_IB_POOL_SIZE			16
+#define RADEON_DEBUGFS_MAX_COMPONENTS		32
+#define RADEONFB_CONN_LIMIT			4
+#define RADEON_BIOS_NUM_SCRATCH			8
 
 /* max number of rings */
-#define RADEON_NUM_RINGS 3
+#define RADEON_NUM_RINGS			3
+
+/* fence seq are set to this number when signaled */
+#define RADEON_FENCE_SIGNALED_SEQ		0LL
+#define RADEON_FENCE_NOTEMITED_SEQ		(~0LL)
 
 /* internal ring indices */
 /* r1xx+ has gfx CP ring */
-#define RADEON_RING_TYPE_GFX_INDEX  0
+#define RADEON_RING_TYPE_GFX_INDEX		0
 
 /* cayman has 2 compute CP rings */
-#define CAYMAN_RING_TYPE_CP1_INDEX 1
-#define CAYMAN_RING_TYPE_CP2_INDEX 2
+#define CAYMAN_RING_TYPE_CP1_INDEX		1
+#define CAYMAN_RING_TYPE_CP2_INDEX		2
 
 /* hardcode those limit for now */
-#define RADEON_VA_RESERVED_SIZE		(8 << 20)
-#define RADEON_IB_VM_MAX_SIZE		(64 << 10)
+#define RADEON_VA_RESERVED_SIZE			(8 << 20)
+#define RADEON_IB_VM_MAX_SIZE			(64 << 10)
 
 /*
  * Errata workarounds.
@@ -253,28 +258,20 @@ struct radeon_fence_driver {
 	uint32_t			scratch_reg;
 	uint64_t			gpu_addr;
 	volatile uint32_t		*cpu_addr;
-	atomic_t			seq;
-	uint32_t			last_seq;
-	unsigned long			last_jiffies;
-	unsigned long			last_timeout;
-	wait_queue_head_t		queue;
-	struct list_head		created;
-	struct list_head		emitted;
-	struct list_head		signaled;
+	/* seq is protected by ring emission lock */
+	uint64_t			seq;
+	atomic64_t			last_seq;
+	unsigned long			last_activity;
 	bool				initialized;
 };
 
 struct radeon_fence {
 	struct radeon_device		*rdev;
 	struct kref			kref;
-	struct list_head		list;
 	/* protected by radeon_fence.lock */
-	uint32_t			seq;
-	bool				emitted;
-	bool				signaled;
+	uint64_t			seq;
 	/* RB, DMA, etc. */
-	int				ring;
-	struct radeon_semaphore		*semaphore;
+	unsigned			ring;
 };
 
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring);
@@ -285,11 +282,14 @@ int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence);
 void radeon_fence_process(struct radeon_device *rdev, int ring);
 bool radeon_fence_signaled(struct radeon_fence *fence);
 int radeon_fence_wait(struct radeon_fence *fence, bool interruptible);
-int radeon_fence_wait_next(struct radeon_device *rdev, int ring);
-int radeon_fence_wait_last(struct radeon_device *rdev, int ring);
+int radeon_fence_wait_next_locked(struct radeon_device *rdev, int ring);
+int radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring);
+int radeon_fence_wait_any(struct radeon_device *rdev,
+			  struct radeon_fence **fences,
+			  bool intr);
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence);
 void radeon_fence_unref(struct radeon_fence **fence);
-int radeon_fence_count_emitted(struct radeon_device *rdev, int ring);
+unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring);
 
 /*
  * Tiling registers
@@ -382,8 +382,11 @@ struct radeon_bo_list {
  * alignment).
  */
 struct radeon_sa_manager {
+	spinlock_t		lock;
 	struct radeon_bo	*bo;
-	struct list_head	sa_bo;
+	struct list_head	*hole;
+	struct list_head	flist[RADEON_NUM_RINGS];
+	struct list_head	olist;
 	unsigned		size;
 	uint64_t		gpu_addr;
 	void			*cpu_ptr;
@@ -394,10 +397,12 @@ struct radeon_sa_bo;
 
 /* sub-allocation buffer */
 struct radeon_sa_bo {
-	struct list_head		list;
+	struct list_head		olist;
+	struct list_head		flist;
 	struct radeon_sa_manager	*manager;
-	unsigned			offset;
-	unsigned			size;
+	unsigned			soffset;
+	unsigned			eoffset;
+	struct radeon_fence		*fence;
 };
 
 /*
@@ -428,42 +433,26 @@ int radeon_mode_dumb_destroy(struct drm_file *file_priv,
 /*
  * Semaphores.
  */
-struct radeon_ring;
-
-#define	RADEON_SEMAPHORE_BO_SIZE	256
-
-struct radeon_semaphore_driver {
-	rwlock_t			lock;
-	struct list_head		bo;
-};
-
-struct radeon_semaphore_bo;
-
 /* everything here is constant */
 struct radeon_semaphore {
-	struct list_head		list;
+	struct radeon_sa_bo		*sa_bo;
+	signed				waiters;
 	uint64_t			gpu_addr;
-	uint32_t			*cpu_ptr;
-	struct radeon_semaphore_bo	*bo;
 };
 
-struct radeon_semaphore_bo {
-	struct list_head		list;
-	struct radeon_ib		*ib;
-	struct list_head		free;
-	struct radeon_semaphore		semaphores[RADEON_SEMAPHORE_BO_SIZE/8];
-	unsigned			nused;
-};
-
-void radeon_semaphore_driver_fini(struct radeon_device *rdev);
 int radeon_semaphore_create(struct radeon_device *rdev,
 			    struct radeon_semaphore **semaphore);
 void radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
 				  struct radeon_semaphore *semaphore);
 void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 				struct radeon_semaphore *semaphore);
+int radeon_semaphore_sync_rings(struct radeon_device *rdev,
+				struct radeon_semaphore *semaphore,
+				bool sync_to[RADEON_NUM_RINGS],
+				int dst_ring);
 void radeon_semaphore_free(struct radeon_device *rdev,
-			   struct radeon_semaphore *semaphore);
+			   struct radeon_semaphore *semaphore,
+			   struct radeon_fence *fence);
 
 /*
  * GART structures, functions & helpers
@@ -560,6 +549,7 @@ struct radeon_unpin_work {
 
 struct r500_irq_stat_regs {
 	u32 disp_int;
+	u32 hdmi0_status;
 };
 
 struct r600_irq_stat_regs {
@@ -568,6 +558,8 @@ struct r600_irq_stat_regs {
 	u32 disp_int_cont2;
 	u32 d1grph_int;
 	u32 d2grph_int;
+	u32 hdmi0_status;
+	u32 hdmi1_status;
 };
 
 struct evergreen_irq_stat_regs {
@@ -583,6 +575,12 @@ struct evergreen_irq_stat_regs {
 	u32 d4grph_int;
 	u32 d5grph_int;
 	u32 d6grph_int;
+	u32 afmt_status1;
+	u32 afmt_status2;
+	u32 afmt_status3;
+	u32 afmt_status4;
+	u32 afmt_status5;
+	u32 afmt_status6;
 };
 
 union radeon_irq_stat_regs {
@@ -593,7 +591,7 @@ union radeon_irq_stat_regs {
 
 #define RADEON_MAX_HPD_PINS 6
 #define RADEON_MAX_CRTCS 6
-#define RADEON_MAX_HDMI_BLOCKS 2
+#define RADEON_MAX_AFMT_BLOCKS 6
 
 struct radeon_irq {
 	bool		installed;
@@ -605,7 +603,7 @@ struct radeon_irq {
 	bool            gui_idle;
 	bool            gui_idle_acked;
 	wait_queue_head_t	idle_queue;
-	bool		hdmi[RADEON_MAX_HDMI_BLOCKS];
+	bool		afmt[RADEON_MAX_AFMT_BLOCKS];
 	spinlock_t sw_lock;
 	int sw_refcount[RADEON_NUM_RINGS];
 	union radeon_irq_stat_regs stat_regs;
@@ -625,26 +623,14 @@ void radeon_irq_kms_pflip_irq_put(struct radeon_device *rdev, int crtc);
  */
 
 struct radeon_ib {
-	struct radeon_sa_bo	sa_bo;
-	unsigned		idx;
-	uint32_t		length_dw;
-	uint64_t		gpu_addr;
-	uint32_t		*ptr;
-	struct radeon_fence	*fence;
-	unsigned		vm_id;
-	bool			is_const_ib;
-};
-
-/*
- * locking -
- * mutex protects scheduled_ibs, ready, alloc_bm
- */
-struct radeon_ib_pool {
-	struct radeon_mutex		mutex;
-	struct radeon_sa_manager	sa_manager;
-	struct radeon_ib		ibs[RADEON_IB_POOL_SIZE];
-	bool				ready;
-	unsigned			head_id;
+	struct radeon_sa_bo		*sa_bo;
+	uint32_t			length_dw;
+	uint64_t			gpu_addr;
+	uint32_t			*ptr;
+	struct radeon_fence		*fence;
+	unsigned			vm_id;
+	bool				is_const_ib;
+	struct radeon_semaphore		*semaphore;
 };
 
 struct radeon_ring {
@@ -659,10 +645,11 @@ struct radeon_ring {
 	unsigned		ring_size;
 	unsigned		ring_free_dw;
 	int			count_dw;
+	unsigned long		last_activity;
+	unsigned		last_rptr;
 	uint64_t		gpu_addr;
 	uint32_t		align_mask;
 	uint32_t		ptr_mask;
-	struct mutex		mutex;
 	bool			ready;
 	u32			ptr_reg_shift;
 	u32			ptr_reg_mask;
@@ -679,7 +666,7 @@ struct radeon_vm {
 	unsigned			last_pfn;
 	u64				pt_gpu_addr;
 	u64				*pt;
-	struct radeon_sa_bo		sa_bo;
+	struct radeon_sa_bo		*sa_bo;
 	struct mutex			mutex;
 	/* last fence for cs using this vm */
 	struct radeon_fence		*fence;
@@ -756,7 +743,6 @@ struct r600_blit_cp_primitives {
 };
 
 struct r600_blit {
-	struct mutex		mutex;
 	struct radeon_bo	*shader_obj;
 	struct r600_blit_cp_primitives primitives;
 	int max_dim;
@@ -766,8 +752,6 @@ struct r600_blit {
 	u32 vs_offset, ps_offset;
 	u32 state_offset;
 	u32 state_len;
-	u32 vb_used, vb_total;
-	struct radeon_ib *vb_ib;
 };
 
 void r600_blit_suspend(struct radeon_device *rdev);
@@ -785,14 +769,14 @@ struct si_rlc {
 };
 
 int radeon_ib_get(struct radeon_device *rdev, int ring,
-		  struct radeon_ib **ib, unsigned size);
-void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib **ib);
-bool radeon_ib_try_free(struct radeon_device *rdev, struct radeon_ib *ib);
+		  struct radeon_ib *ib, unsigned size);
+void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib);
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib);
 int radeon_ib_pool_init(struct radeon_device *rdev);
 void radeon_ib_pool_fini(struct radeon_device *rdev);
 int radeon_ib_pool_start(struct radeon_device *rdev);
 int radeon_ib_pool_suspend(struct radeon_device *rdev);
+int radeon_ib_ring_tests(struct radeon_device *rdev);
 /* Ring access between begin & end cannot sleep */
 int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *cp);
 void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *cp);
@@ -800,8 +784,12 @@ int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *cp, unsign
 int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
 void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *cp);
 void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_undo(struct radeon_ring *ring);
 void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *cp);
 int radeon_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_force_activity(struct radeon_device *rdev, struct radeon_ring *ring);
+void radeon_ring_lockup_update(struct radeon_ring *ring);
+bool radeon_ring_test_lockup(struct radeon_device *rdev, struct radeon_ring *ring);
 int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ring_size,
 		     unsigned rptr_offs, unsigned rptr_reg, unsigned wptr_reg,
 		     u32 ptr_reg_shift, u32 ptr_reg_mask, u32 nop);
@@ -850,8 +838,8 @@ struct radeon_cs_parser {
 	int			chunk_relocs_idx;
 	int			chunk_flags_idx;
 	int			chunk_const_ib_idx;
-	struct radeon_ib	*ib;
-	struct radeon_ib	*const_ib;
+	struct radeon_ib	ib;
+	struct radeon_ib	const_ib;
 	void			*track;
 	unsigned		family;
 	int			parser_error;
@@ -1105,6 +1093,14 @@ int radeon_pm_get_type_index(struct radeon_device *rdev,
 			     enum radeon_pm_state_type ps_type,
 			     int instance);
 
+struct r600_audio {
+	int			channels;
+	int			rate;
+	int			bits_per_sample;
+	u8			status_bits;
+	u8			category_code;
+};
+
 /*
  * Benchmarking
  */
@@ -1144,7 +1140,6 @@ struct radeon_asic {
 	int (*resume)(struct radeon_device *rdev);
 	int (*suspend)(struct radeon_device *rdev);
 	void (*vga_set_state)(struct radeon_device *rdev, bool state);
-	bool (*gpu_is_lockup)(struct radeon_device *rdev, struct radeon_ring *cp);
 	int (*asic_reset)(struct radeon_device *rdev);
 	/* ioctl hw specific callback. Some hw might want to perform special
 	 * operation on specific ioctl. For instance on wait idle some hw
@@ -1173,6 +1168,7 @@ struct radeon_asic {
 		void (*ring_start)(struct radeon_device *rdev, struct radeon_ring *cp);
 		int (*ring_test)(struct radeon_device *rdev, struct radeon_ring *cp);
 		int (*ib_test)(struct radeon_device *rdev, struct radeon_ring *cp);
+		bool (*is_lockup)(struct radeon_device *rdev, struct radeon_ring *cp);
 	} ring[RADEON_NUM_RINGS];
 	/* irqs */
 	struct {
@@ -1251,16 +1247,10 @@ struct radeon_asic {
 /*
  * Asic structures
  */
-struct r100_gpu_lockup {
-	unsigned long	last_jiffies;
-	u32		last_cp_rptr;
-};
-
 struct r100_asic {
 	const unsigned		*reg_safe_bm;
 	unsigned		reg_safe_bm_size;
 	u32			hdp_cntl;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct r300_asic {
@@ -1268,7 +1258,6 @@ struct r300_asic {
 	unsigned		reg_safe_bm_size;
 	u32			resync_scratch;
 	u32			hdp_cntl;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct r600_asic {
@@ -1290,7 +1279,6 @@ struct r600_asic {
 	unsigned		tiling_group_size;
 	unsigned		tile_config;
 	unsigned		backend_map;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct rv770_asic {
@@ -1316,7 +1304,6 @@ struct rv770_asic {
 	unsigned		tiling_group_size;
 	unsigned		tile_config;
 	unsigned		backend_map;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct evergreen_asic {
@@ -1343,7 +1330,6 @@ struct evergreen_asic {
 	unsigned tiling_group_size;
 	unsigned tile_config;
 	unsigned backend_map;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct cayman_asic {
@@ -1382,7 +1368,6 @@ struct cayman_asic {
 	unsigned multi_gpu_tile_size;
 
 	unsigned tile_config;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct si_asic {
@@ -1413,7 +1398,6 @@ struct si_asic {
 	unsigned multi_gpu_tile_size;
 
 	unsigned tile_config;
-	struct r100_gpu_lockup	lockup;
 };
 
 union radeon_asic_config {
@@ -1516,11 +1500,12 @@ struct radeon_device {
 	struct radeon_mode_info		mode_info;
 	struct radeon_scratch		scratch;
 	struct radeon_mman		mman;
-	rwlock_t			fence_lock;
 	struct radeon_fence_driver	fence_drv[RADEON_NUM_RINGS];
-	struct radeon_semaphore_driver	semaphore_drv;
+	wait_queue_head_t		fence_queue;
+	struct mutex			ring_lock;
 	struct radeon_ring		ring[RADEON_NUM_RINGS];
-	struct radeon_ib_pool		ib_pool;
+	bool				ib_pool_ready;
+	struct radeon_sa_manager	ring_tmp_bo;
 	struct radeon_irq		irq;
 	struct radeon_asic		*asic;
 	struct radeon_gem		gem;
@@ -1529,7 +1514,6 @@ struct radeon_device {
 	struct radeon_mutex		cs_mutex;
 	struct radeon_wb		wb;
 	struct radeon_dummy_page	dummy_page;
-	bool				gpu_lockup;
 	bool				shutdown;
 	bool				suspend;
 	bool				need_dma32;
@@ -1546,19 +1530,12 @@ struct radeon_device {
 	struct r600_ih ih; /* r6/700 interrupt ring */
 	struct si_rlc rlc;
 	struct work_struct hotplug_work;
+	struct work_struct audio_work;
 	int num_crtc; /* number of crtcs */
 	struct mutex dc_hw_i2c_mutex; /* display controller hw i2c mutex */
 	struct mutex vram_mutex;
-
-	/* audio stuff */
-	bool			audio_enabled;
-	struct timer_list	audio_timer;
-	int			audio_channels;
-	int			audio_rate;
-	int			audio_bits_per_sample;
-	uint8_t			audio_status_bits;
-	uint8_t			audio_category_code;
-
+	bool audio_enabled;
+	struct r600_audio audio_status; /* audio stuff */
 	struct notifier_block acpi_nb;
 	/* only one userspace can use Hyperz features or CMASK at a time */
 	struct drm_file *hyperz_filp;
@@ -1730,7 +1707,6 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #define radeon_suspend(rdev) (rdev)->asic->suspend((rdev))
 #define radeon_cs_parse(rdev, r, p) (rdev)->asic->ring[(r)].cs_parse((p))
 #define radeon_vga_set_state(rdev, state) (rdev)->asic->vga_set_state((rdev), (state))
-#define radeon_gpu_is_lockup(rdev, cp) (rdev)->asic->gpu_is_lockup((rdev), (cp))
 #define radeon_asic_reset(rdev) (rdev)->asic->asic_reset((rdev))
 #define radeon_gart_tlb_flush(rdev) (rdev)->asic->gart.tlb_flush((rdev))
 #define radeon_gart_set_page(rdev, i, p) (rdev)->asic->gart.set_page((rdev), (i), (p))
@@ -1739,6 +1715,7 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #define radeon_ib_test(rdev, r, cp) (rdev)->asic->ring[(r)].ib_test((rdev), (cp))
 #define radeon_ring_ib_execute(rdev, r, ib) (rdev)->asic->ring[(r)].ib_execute((rdev), (ib))
 #define radeon_ring_ib_parse(rdev, r, ib) (rdev)->asic->ring[(r)].ib_parse((rdev), (ib))
+#define radeon_ring_is_lockup(rdev, r, cp) (rdev)->asic->ring[(r)].is_lockup((rdev), (cp))
 #define radeon_irq_set(rdev) (rdev)->asic->irq.set((rdev))
 #define radeon_irq_process(rdev) (rdev)->asic->irq.process((rdev))
 #define radeon_get_vblank_counter(rdev, crtc) (rdev)->asic->display.get_vblank_counter((rdev), (crtc))
@@ -1828,6 +1805,8 @@ int radeon_vm_bo_rmv(struct radeon_device *rdev,
 		     struct radeon_vm *vm,
 		     struct radeon_bo *bo);
 
+/* audio */
+void r600_audio_update_hdmi(struct work_struct *work);
 
 /*
  * R600 vram scratch functions
@@ -1848,10 +1827,32 @@ int r600_fmt_get_nblocksy(u32 format, u32 h);
 /*
  * r600 functions used by radeon_encoder.c
  */
+struct radeon_hdmi_acr {
+	u32 clock;
+
+	int n_32khz;
+	int cts_32khz;
+
+	int n_44_1khz;
+	int cts_44_1khz;
+
+	int n_48khz;
+	int cts_48khz;
+
+};
+
+extern struct radeon_hdmi_acr r600_hdmi_acr(uint32_t clock);
+
 extern void r600_hdmi_enable(struct drm_encoder *encoder);
 extern void r600_hdmi_disable(struct drm_encoder *encoder);
 extern void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
 
+/*
+ * evergreen functions used by radeon_encoder.c
+ */
+
+extern void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
+
 extern int ni_init_microcode(struct radeon_device *rdev);
 extern int ni_mc_load_microcode(struct radeon_device *rdev);
 
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index be4dc2ff0e40..f533df5f7d50 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -134,7 +134,6 @@ static struct radeon_asic r100_asic = {
 	.suspend = &r100_suspend,
 	.resume = &r100_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r100_gpu_is_lockup,
 	.asic_reset = &r100_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -152,6 +151,7 @@ static struct radeon_asic r100_asic = {
 			.ring_start = &r100_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -208,7 +208,6 @@ static struct radeon_asic r200_asic = {
 	.suspend = &r100_suspend,
 	.resume = &r100_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r100_gpu_is_lockup,
 	.asic_reset = &r100_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -226,6 +225,7 @@ static struct radeon_asic r200_asic = {
 			.ring_start = &r100_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -282,7 +282,6 @@ static struct radeon_asic r300_asic = {
 	.suspend = &r300_suspend,
 	.resume = &r300_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -300,6 +299,7 @@ static struct radeon_asic r300_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -356,7 +356,6 @@ static struct radeon_asic r300_asic_pcie = {
 	.suspend = &r300_suspend,
 	.resume = &r300_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -374,6 +373,7 @@ static struct radeon_asic r300_asic_pcie = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -430,7 +430,6 @@ static struct radeon_asic r420_asic = {
 	.suspend = &r420_suspend,
 	.resume = &r420_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -448,6 +447,7 @@ static struct radeon_asic r420_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -504,7 +504,6 @@ static struct radeon_asic rs400_asic = {
 	.suspend = &rs400_suspend,
 	.resume = &rs400_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -522,6 +521,7 @@ static struct radeon_asic rs400_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -578,7 +578,6 @@ static struct radeon_asic rs600_asic = {
 	.suspend = &rs600_suspend,
 	.resume = &rs600_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -596,6 +595,7 @@ static struct radeon_asic rs600_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -652,7 +652,6 @@ static struct radeon_asic rs690_asic = {
 	.suspend = &rs690_suspend,
 	.resume = &rs690_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -670,6 +669,7 @@ static struct radeon_asic rs690_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -726,7 +726,6 @@ static struct radeon_asic rv515_asic = {
 	.suspend = &rv515_suspend,
 	.resume = &rv515_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -744,6 +743,7 @@ static struct radeon_asic rv515_asic = {
 			.ring_start = &rv515_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -800,7 +800,6 @@ static struct radeon_asic r520_asic = {
 	.suspend = &rv515_suspend,
 	.resume = &r520_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -818,6 +817,7 @@ static struct radeon_asic r520_asic = {
 			.ring_start = &rv515_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -874,7 +874,6 @@ static struct radeon_asic r600_asic = {
 	.suspend = &r600_suspend,
 	.resume = &r600_resume,
 	.vga_set_state = &r600_vga_set_state,
-	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.asic_reset = &r600_asic_reset,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
 	.gui_idle = &r600_gui_idle,
@@ -891,6 +890,7 @@ static struct radeon_asic r600_asic = {
 			.cs_parse = &r600_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &r600_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -946,7 +946,6 @@ static struct radeon_asic rs780_asic = {
 	.fini = &r600_fini,
 	.suspend = &r600_suspend,
 	.resume = &r600_resume,
-	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.vga_set_state = &r600_vga_set_state,
 	.asic_reset = &r600_asic_reset,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -964,6 +963,7 @@ static struct radeon_asic rs780_asic = {
 			.cs_parse = &r600_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &r600_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1020,7 +1020,6 @@ static struct radeon_asic rv770_asic = {
 	.suspend = &rv770_suspend,
 	.resume = &rv770_resume,
 	.asic_reset = &r600_asic_reset,
-	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
 	.gui_idle = &r600_gui_idle,
@@ -1037,6 +1036,7 @@ static struct radeon_asic rv770_asic = {
 			.cs_parse = &r600_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &r600_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1092,7 +1092,6 @@ static struct radeon_asic evergreen_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1110,6 +1109,7 @@ static struct radeon_asic evergreen_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1165,7 +1165,6 @@ static struct radeon_asic sumo_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1183,6 +1182,7 @@ static struct radeon_asic sumo_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 	},
 	.irq = {
@@ -1238,7 +1238,6 @@ static struct radeon_asic btc_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1256,6 +1255,7 @@ static struct radeon_asic btc_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1321,7 +1321,6 @@ static struct radeon_asic cayman_asic = {
 	.fini = &cayman_fini,
 	.suspend = &cayman_suspend,
 	.resume = &cayman_resume,
-	.gpu_is_lockup = &cayman_gpu_is_lockup,
 	.asic_reset = &cayman_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1340,6 +1339,7 @@ static struct radeon_asic cayman_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP1_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1349,6 +1349,7 @@ static struct radeon_asic cayman_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP2_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1358,6 +1359,7 @@ static struct radeon_asic cayman_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1413,7 +1415,6 @@ static struct radeon_asic trinity_asic = {
 	.fini = &cayman_fini,
 	.suspend = &cayman_suspend,
 	.resume = &cayman_resume,
-	.gpu_is_lockup = &cayman_gpu_is_lockup,
 	.asic_reset = &cayman_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1432,6 +1433,7 @@ static struct radeon_asic trinity_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP1_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1441,6 +1443,7 @@ static struct radeon_asic trinity_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP2_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1450,6 +1453,7 @@ static struct radeon_asic trinity_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1515,7 +1519,6 @@ static struct radeon_asic si_asic = {
 	.fini = &si_fini,
 	.suspend = &si_suspend,
 	.resume = &si_resume,
-	.gpu_is_lockup = &si_gpu_is_lockup,
 	.asic_reset = &si_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1534,6 +1537,7 @@ static struct radeon_asic si_asic = {
 			.cs_parse = NULL,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &si_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP1_INDEX] = {
 			.ib_execute = &si_ring_ib_execute,
@@ -1543,6 +1547,7 @@ static struct radeon_asic si_asic = {
 			.cs_parse = NULL,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &si_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP2_INDEX] = {
 			.ib_execute = &si_ring_ib_execute,
@@ -1552,6 +1557,7 @@ static struct radeon_asic si_asic = {
 			.cs_parse = NULL,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &si_gpu_is_lockup,
 		}
 	},
 	.irq = {
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index 3d9f9f1d8f90..e76a941ef14e 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -103,11 +103,6 @@ int r100_pci_gart_enable(struct radeon_device *rdev);
 void r100_pci_gart_disable(struct radeon_device *rdev);
 int r100_debugfs_mc_info_init(struct radeon_device *rdev);
 int r100_gui_wait_for_idle(struct radeon_device *rdev);
-void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup,
-			    struct radeon_ring *cp);
-bool r100_gpu_cp_is_lockup(struct radeon_device *rdev,
-			   struct r100_gpu_lockup *lockup,
-			   struct radeon_ring *cp);
 void r100_ib_fini(struct radeon_device *rdev);
 int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
 void r100_irq_disable(struct radeon_device *rdev);
@@ -159,7 +154,6 @@ extern int r300_init(struct radeon_device *rdev);
 extern void r300_fini(struct radeon_device *rdev);
 extern int r300_suspend(struct radeon_device *rdev);
 extern int r300_resume(struct radeon_device *rdev);
-extern bool r300_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 extern int r300_asic_reset(struct radeon_device *rdev);
 extern void r300_ring_start(struct radeon_device *rdev, struct radeon_ring *ring);
 extern void r300_fence_ring_emit(struct radeon_device *rdev,
@@ -362,26 +356,20 @@ void r600_disable_interrupts(struct radeon_device *rdev);
 void r600_rlc_stop(struct radeon_device *rdev);
 /* r600 audio */
 int r600_audio_init(struct radeon_device *rdev);
-int r600_audio_tmds_index(struct drm_encoder *encoder);
 void r600_audio_set_clock(struct drm_encoder *encoder, int clock);
-int r600_audio_channels(struct radeon_device *rdev);
-int r600_audio_bits_per_sample(struct radeon_device *rdev);
-int r600_audio_rate(struct radeon_device *rdev);
-uint8_t r600_audio_status_bits(struct radeon_device *rdev);
-uint8_t r600_audio_category_code(struct radeon_device *rdev);
-void r600_audio_schedule_polling(struct radeon_device *rdev);
-void r600_audio_enable_polling(struct drm_encoder *encoder);
-void r600_audio_disable_polling(struct drm_encoder *encoder);
+struct r600_audio r600_audio_status(struct radeon_device *rdev);
 void r600_audio_fini(struct radeon_device *rdev);
-void r600_hdmi_init(struct drm_encoder *encoder);
 int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder);
 void r600_hdmi_update_audio_settings(struct drm_encoder *encoder);
 /* r600 blit */
-int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages);
-void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence);
+int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages,
+			   struct radeon_sa_bo **vb);
+void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence,
+			 struct radeon_sa_bo *vb);
 void r600_kms_blit_copy(struct radeon_device *rdev,
 			u64 src_gpu_addr, u64 dst_gpu_addr,
-			unsigned num_gpu_pages);
+			unsigned num_gpu_pages,
+			struct radeon_sa_bo *vb);
 int r600_mc_wait_for_idle(struct radeon_device *rdev);
 
 /*
@@ -446,7 +434,6 @@ int cayman_init(struct radeon_device *rdev);
 void cayman_fini(struct radeon_device *rdev);
 int cayman_suspend(struct radeon_device *rdev);
 int cayman_resume(struct radeon_device *rdev);
-bool cayman_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 int cayman_asic_reset(struct radeon_device *rdev);
 void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
 int cayman_vm_init(struct radeon_device *rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_benchmark.c b/drivers/gpu/drm/radeon/radeon_benchmark.c
index fef7b722b05d..364f5b1a04b9 100644
--- a/drivers/gpu/drm/radeon/radeon_benchmark.c
+++ b/drivers/gpu/drm/radeon/radeon_benchmark.c
@@ -103,7 +103,7 @@ static void radeon_benchmark_move(struct radeon_device *rdev, unsigned size,
 	int time;
 
 	n = RADEON_BENCHMARK_ITERATIONS;
-	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, sdomain, &sobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, sdomain, NULL, &sobj);
 	if (r) {
 		goto out_cleanup;
 	}
@@ -115,7 +115,7 @@ static void radeon_benchmark_move(struct radeon_device *rdev, unsigned size,
 	if (r) {
 		goto out_cleanup;
 	}
-	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, ddomain, &dobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, ddomain, NULL, &dobj);
 	if (r) {
 		goto out_cleanup;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_combios.c b/drivers/gpu/drm/radeon/radeon_combios.c
index 2cad9fde92fc..576f4f6919f2 100644
--- a/drivers/gpu/drm/radeon/radeon_combios.c
+++ b/drivers/gpu/drm/radeon/radeon_combios.c
@@ -1561,6 +1561,11 @@ bool radeon_get_legacy_connector_info_from_table(struct drm_device *dev)
 			   (rdev->pdev->subsystem_device == 0x4150)) {
 			/* Mac G5 tower 9600 */
 			rdev->mode_info.connector_table = CT_MAC_G5_9600;
+		} else if ((rdev->pdev->device == 0x4c66) &&
+			   (rdev->pdev->subsystem_vendor == 0x1002) &&
+			   (rdev->pdev->subsystem_device == 0x4c66)) {
+			/* SAM440ep RV250 embedded board */
+			rdev->mode_info.connector_table = CT_SAM440EP;
 		} else
 #endif /* CONFIG_PPC_PMAC */
 #ifdef CONFIG_PPC64
@@ -2134,6 +2139,67 @@ bool radeon_get_legacy_connector_info_from_table(struct drm_device *dev)
 					    CONNECTOR_OBJECT_ID_SVIDEO,
 					    &hpd);
 		break;
+	case CT_SAM440EP:
+		DRM_INFO("Connector Table: %d (SAM440ep embedded board)\n",
+			 rdev->mode_info.connector_table);
+		/* LVDS */
+		ddc_i2c = combios_setup_i2c_bus(rdev, DDC_NONE_DETECTED, 0, 0);
+		hpd.hpd = RADEON_HPD_NONE;
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_LCD1_SUPPORT,
+								0),
+					  ATOM_DEVICE_LCD1_SUPPORT);
+		radeon_add_legacy_connector(dev, 0, ATOM_DEVICE_LCD1_SUPPORT,
+					    DRM_MODE_CONNECTOR_LVDS, &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_LVDS,
+					    &hpd);
+		/* DVI-I - secondary dac, int tmds */
+		ddc_i2c = combios_setup_i2c_bus(rdev, DDC_DVI, 0, 0);
+		hpd.hpd = RADEON_HPD_1; /* ??? */
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_DFP1_SUPPORT,
+								0),
+					  ATOM_DEVICE_DFP1_SUPPORT);
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_CRT2_SUPPORT,
+								2),
+					  ATOM_DEVICE_CRT2_SUPPORT);
+		radeon_add_legacy_connector(dev, 1,
+					    ATOM_DEVICE_DFP1_SUPPORT |
+					    ATOM_DEVICE_CRT2_SUPPORT,
+					    DRM_MODE_CONNECTOR_DVII, &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_SINGLE_LINK_DVI_I,
+					    &hpd);
+		/* VGA - primary dac */
+		ddc_i2c = combios_setup_i2c_bus(rdev, DDC_VGA, 0, 0);
+		hpd.hpd = RADEON_HPD_NONE;
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_CRT1_SUPPORT,
+								1),
+					  ATOM_DEVICE_CRT1_SUPPORT);
+		radeon_add_legacy_connector(dev, 2,
+					    ATOM_DEVICE_CRT1_SUPPORT,
+					    DRM_MODE_CONNECTOR_VGA, &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_VGA,
+					    &hpd);
+		/* TV - TV DAC */
+		ddc_i2c.valid = false;
+		hpd.hpd = RADEON_HPD_NONE;
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_TV1_SUPPORT,
+								2),
+					  ATOM_DEVICE_TV1_SUPPORT);
+		radeon_add_legacy_connector(dev, 3, ATOM_DEVICE_TV1_SUPPORT,
+					    DRM_MODE_CONNECTOR_SVIDEO,
+					    &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_SVIDEO,
+					    &hpd);
+		break;
 	default:
 		DRM_INFO("Connector table: %d (invalid)\n",
 			 rdev->mode_info.connector_table);
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index 3c2e7a000a2a..2914c5761cfc 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -84,6 +84,62 @@ static void radeon_property_change_mode(struct drm_encoder *encoder)
 					 crtc->x, crtc->y, crtc->fb);
 	}
 }
+
+int radeon_get_monitor_bpc(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+	struct radeon_connector_atom_dig *dig_connector;
+	int bpc = 8;
+
+	switch (connector->connector_type) {
+	case DRM_MODE_CONNECTOR_DVII:
+	case DRM_MODE_CONNECTOR_HDMIB:
+		if (radeon_connector->use_digital) {
+			if (drm_detect_hdmi_monitor(radeon_connector->edid)) {
+				if (connector->display_info.bpc)
+					bpc = connector->display_info.bpc;
+			}
+		}
+		break;
+	case DRM_MODE_CONNECTOR_DVID:
+	case DRM_MODE_CONNECTOR_HDMIA:
+		if (drm_detect_hdmi_monitor(radeon_connector->edid)) {
+			if (connector->display_info.bpc)
+				bpc = connector->display_info.bpc;
+		}
+		break;
+	case DRM_MODE_CONNECTOR_DisplayPort:
+		dig_connector = radeon_connector->con_priv;
+		if ((dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT) ||
+		    (dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_eDP) ||
+		    drm_detect_hdmi_monitor(radeon_connector->edid)) {
+			if (connector->display_info.bpc)
+				bpc = connector->display_info.bpc;
+		}
+		break;
+	case DRM_MODE_CONNECTOR_eDP:
+	case DRM_MODE_CONNECTOR_LVDS:
+		if (connector->display_info.bpc)
+			bpc = connector->display_info.bpc;
+		else if (ASIC_IS_DCE41(rdev) || ASIC_IS_DCE5(rdev)) {
+			struct drm_connector_helper_funcs *connector_funcs =
+				connector->helper_private;
+			struct drm_encoder *encoder = connector_funcs->best_encoder(connector);
+			struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+			struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+
+			if (dig->lcd_misc & ATOM_PANEL_MISC_V13_6BIT_PER_COLOR)
+				bpc = 6;
+			else if (dig->lcd_misc & ATOM_PANEL_MISC_V13_8BIT_PER_COLOR)
+				bpc = 8;
+		}
+		break;
+	}
+	return bpc;
+}
+
 static void
 radeon_connector_update_scratch_regs(struct drm_connector *connector, enum drm_connector_status status)
 {
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 5cac83278338..c7d64a739033 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -118,44 +118,33 @@ static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority
 static int radeon_cs_sync_rings(struct radeon_cs_parser *p)
 {
 	bool sync_to_ring[RADEON_NUM_RINGS] = { };
+	bool need_sync = false;
 	int i, r;
 
 	for (i = 0; i < p->nrelocs; i++) {
+		struct radeon_fence *fence;
+
 		if (!p->relocs[i].robj || !p->relocs[i].robj->tbo.sync_obj)
 			continue;
 
-		if (!(p->relocs[i].flags & RADEON_RELOC_DONT_SYNC)) {
-			struct radeon_fence *fence = p->relocs[i].robj->tbo.sync_obj;
-			if (!radeon_fence_signaled(fence)) {
-				sync_to_ring[fence->ring] = true;
-			}
+		fence = p->relocs[i].robj->tbo.sync_obj;
+		if (fence->ring != p->ring && !radeon_fence_signaled(fence)) {
+			sync_to_ring[fence->ring] = true;
+			need_sync = true;
 		}
 	}
 
-	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		/* no need to sync to our own or unused rings */
-		if (i == p->ring || !sync_to_ring[i] || !p->rdev->ring[i].ready)
-			continue;
-
-		if (!p->ib->fence->semaphore) {
-			r = radeon_semaphore_create(p->rdev, &p->ib->fence->semaphore);
-			if (r)
-				return r;
-		}
-
-		r = radeon_ring_lock(p->rdev, &p->rdev->ring[i], 3);
-		if (r)
-			return r;
-		radeon_semaphore_emit_signal(p->rdev, i, p->ib->fence->semaphore);
-		radeon_ring_unlock_commit(p->rdev, &p->rdev->ring[i]);
+	if (!need_sync) {
+		return 0;
+	}
 
-		r = radeon_ring_lock(p->rdev, &p->rdev->ring[p->ring], 3);
-		if (r)
-			return r;
-		radeon_semaphore_emit_wait(p->rdev, p->ring, p->ib->fence->semaphore);
-		radeon_ring_unlock_commit(p->rdev, &p->rdev->ring[p->ring]);
+	r = radeon_semaphore_create(p->rdev, &p->ib.semaphore);
+	if (r) {
+		return r;
 	}
-	return 0;
+
+	return radeon_semaphore_sync_rings(p->rdev, p->ib.semaphore,
+					   sync_to_ring, p->ring);
 }
 
 int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
@@ -172,6 +161,10 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 	/* get chunks */
 	INIT_LIST_HEAD(&p->validated);
 	p->idx = 0;
+	p->ib.sa_bo = NULL;
+	p->ib.semaphore = NULL;
+	p->const_ib.sa_bo = NULL;
+	p->const_ib.semaphore = NULL;
 	p->chunk_ib_idx = -1;
 	p->chunk_relocs_idx = -1;
 	p->chunk_flags_idx = -1;
@@ -278,11 +271,16 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 				  p->chunks[p->chunk_ib_idx].length_dw);
 			return -EINVAL;
 		}
-		p->chunks[p->chunk_ib_idx].kpage[0] = kmalloc(PAGE_SIZE, GFP_KERNEL);
-		p->chunks[p->chunk_ib_idx].kpage[1] = kmalloc(PAGE_SIZE, GFP_KERNEL);
-		if (p->chunks[p->chunk_ib_idx].kpage[0] == NULL ||
-		    p->chunks[p->chunk_ib_idx].kpage[1] == NULL)
-			return -ENOMEM;
+		if ((p->rdev->flags & RADEON_IS_AGP)) {
+			p->chunks[p->chunk_ib_idx].kpage[0] = kmalloc(PAGE_SIZE, GFP_KERNEL);
+			p->chunks[p->chunk_ib_idx].kpage[1] = kmalloc(PAGE_SIZE, GFP_KERNEL);
+			if (p->chunks[p->chunk_ib_idx].kpage[0] == NULL ||
+			    p->chunks[p->chunk_ib_idx].kpage[1] == NULL) {
+				kfree(p->chunks[i].kpage[0]);
+				kfree(p->chunks[i].kpage[1]);
+				return -ENOMEM;
+			}
+		}
 		p->chunks[p->chunk_ib_idx].kpage_idx[0] = -1;
 		p->chunks[p->chunk_ib_idx].kpage_idx[1] = -1;
 		p->chunks[p->chunk_ib_idx].last_copied_page = -1;
@@ -305,10 +303,9 @@ static void radeon_cs_parser_fini(struct radeon_cs_parser *parser, int error)
 {
 	unsigned i;
 
-
-	if (!error && parser->ib)
+	if (!error)
 		ttm_eu_fence_buffer_objects(&parser->validated,
-					    parser->ib->fence);
+					    parser->ib.fence);
 	else
 		ttm_eu_backoff_reservation(&parser->validated);
 
@@ -323,12 +320,15 @@ static void radeon_cs_parser_fini(struct radeon_cs_parser *parser, int error)
 	kfree(parser->relocs_ptr);
 	for (i = 0; i < parser->nchunks; i++) {
 		kfree(parser->chunks[i].kdata);
-		kfree(parser->chunks[i].kpage[0]);
-		kfree(parser->chunks[i].kpage[1]);
+		if ((parser->rdev->flags & RADEON_IS_AGP)) {
+			kfree(parser->chunks[i].kpage[0]);
+			kfree(parser->chunks[i].kpage[1]);
+		}
 	}
 	kfree(parser->chunks);
 	kfree(parser->chunks_array);
 	radeon_ib_free(parser->rdev, &parser->ib);
+	radeon_ib_free(parser->rdev, &parser->const_ib);
 }
 
 static int radeon_cs_ib_chunk(struct radeon_device *rdev,
@@ -354,7 +354,7 @@ static int radeon_cs_ib_chunk(struct radeon_device *rdev,
 		DRM_ERROR("Failed to get ib !\n");
 		return r;
 	}
-	parser->ib->length_dw = ib_chunk->length_dw;
+	parser->ib.length_dw = ib_chunk->length_dw;
 	r = radeon_cs_parse(rdev, parser->ring, parser);
 	if (r || parser->parser_error) {
 		DRM_ERROR("Invalid command stream !\n");
@@ -369,8 +369,8 @@ static int radeon_cs_ib_chunk(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("Failed to synchronize rings !\n");
 	}
-	parser->ib->vm_id = 0;
-	r = radeon_ib_schedule(rdev, parser->ib);
+	parser->ib.vm_id = 0;
+	r = radeon_ib_schedule(rdev, &parser->ib);
 	if (r) {
 		DRM_ERROR("Failed to schedule IB !\n");
 	}
@@ -421,14 +421,14 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 			DRM_ERROR("Failed to get const ib !\n");
 			return r;
 		}
-		parser->const_ib->is_const_ib = true;
-		parser->const_ib->length_dw = ib_chunk->length_dw;
+		parser->const_ib.is_const_ib = true;
+		parser->const_ib.length_dw = ib_chunk->length_dw;
 		/* Copy the packet into the IB */
-		if (DRM_COPY_FROM_USER(parser->const_ib->ptr, ib_chunk->user_ptr,
+		if (DRM_COPY_FROM_USER(parser->const_ib.ptr, ib_chunk->user_ptr,
 				       ib_chunk->length_dw * 4)) {
 			return -EFAULT;
 		}
-		r = radeon_ring_ib_parse(rdev, parser->ring, parser->const_ib);
+		r = radeon_ring_ib_parse(rdev, parser->ring, &parser->const_ib);
 		if (r) {
 			return r;
 		}
@@ -445,13 +445,13 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 		DRM_ERROR("Failed to get ib !\n");
 		return r;
 	}
-	parser->ib->length_dw = ib_chunk->length_dw;
+	parser->ib.length_dw = ib_chunk->length_dw;
 	/* Copy the packet into the IB */
-	if (DRM_COPY_FROM_USER(parser->ib->ptr, ib_chunk->user_ptr,
+	if (DRM_COPY_FROM_USER(parser->ib.ptr, ib_chunk->user_ptr,
 			       ib_chunk->length_dw * 4)) {
 		return -EFAULT;
 	}
-	r = radeon_ring_ib_parse(rdev, parser->ring, parser->ib);
+	r = radeon_ring_ib_parse(rdev, parser->ring, &parser->ib);
 	if (r) {
 		return r;
 	}
@@ -472,34 +472,44 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 
 	if ((rdev->family >= CHIP_TAHITI) &&
 	    (parser->chunk_const_ib_idx != -1)) {
-		parser->const_ib->vm_id = vm->id;
+		parser->const_ib.vm_id = vm->id;
 		/* ib pool is bind at 0 in virtual address space to gpu_addr is the
 		 * offset inside the pool bo
 		 */
-		parser->const_ib->gpu_addr = parser->const_ib->sa_bo.offset;
-		r = radeon_ib_schedule(rdev, parser->const_ib);
+		parser->const_ib.gpu_addr = parser->const_ib.sa_bo->soffset;
+		r = radeon_ib_schedule(rdev, &parser->const_ib);
 		if (r)
 			goto out;
 	}
 
-	parser->ib->vm_id = vm->id;
+	parser->ib.vm_id = vm->id;
 	/* ib pool is bind at 0 in virtual address space to gpu_addr is the
 	 * offset inside the pool bo
 	 */
-	parser->ib->gpu_addr = parser->ib->sa_bo.offset;
-	parser->ib->is_const_ib = false;
-	r = radeon_ib_schedule(rdev, parser->ib);
+	parser->ib.gpu_addr = parser->ib.sa_bo->soffset;
+	parser->ib.is_const_ib = false;
+	r = radeon_ib_schedule(rdev, &parser->ib);
 out:
 	if (!r) {
 		if (vm->fence) {
 			radeon_fence_unref(&vm->fence);
 		}
-		vm->fence = radeon_fence_ref(parser->ib->fence);
+		vm->fence = radeon_fence_ref(parser->ib.fence);
 	}
 	mutex_unlock(&fpriv->vm.mutex);
 	return r;
 }
 
+static int radeon_cs_handle_lockup(struct radeon_device *rdev, int r)
+{
+	if (r == -EDEADLK) {
+		r = radeon_gpu_reset(rdev);
+		if (!r)
+			r = -EAGAIN;
+	}
+	return r;
+}
+
 int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 {
 	struct radeon_device *rdev = dev->dev_private;
@@ -521,6 +531,7 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	if (r) {
 		DRM_ERROR("Failed to initialize parser !\n");
 		radeon_cs_parser_fini(&parser, r);
+		r = radeon_cs_handle_lockup(rdev, r);
 		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
@@ -529,6 +540,7 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		if (r != -ERESTARTSYS)
 			DRM_ERROR("Failed to parse relocation %d!\n", r);
 		radeon_cs_parser_fini(&parser, r);
+		r = radeon_cs_handle_lockup(rdev, r);
 		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
@@ -542,6 +554,7 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	}
 out:
 	radeon_cs_parser_fini(&parser, r);
+	r = radeon_cs_handle_lockup(rdev, r);
 	radeon_mutex_unlock(&rdev->cs_mutex);
 	return r;
 }
@@ -559,7 +572,7 @@ int radeon_cs_finish_pages(struct radeon_cs_parser *p)
 				size = PAGE_SIZE;
 		}
 		
-		if (DRM_COPY_FROM_USER(p->ib->ptr + (i * (PAGE_SIZE/4)),
+		if (DRM_COPY_FROM_USER(p->ib.ptr + (i * (PAGE_SIZE/4)),
 				       ibc->user_ptr + (i * PAGE_SIZE),
 				       size))
 			return -EFAULT;
@@ -573,9 +586,10 @@ int radeon_cs_update_pages(struct radeon_cs_parser *p, int pg_idx)
 	struct radeon_cs_chunk *ibc = &p->chunks[p->chunk_ib_idx];
 	int i;
 	int size = PAGE_SIZE;
+	bool copy1 = (p->rdev->flags & RADEON_IS_AGP) ? false : true;
 
 	for (i = ibc->last_copied_page + 1; i < pg_idx; i++) {
-		if (DRM_COPY_FROM_USER(p->ib->ptr + (i * (PAGE_SIZE/4)),
+		if (DRM_COPY_FROM_USER(p->ib.ptr + (i * (PAGE_SIZE/4)),
 				       ibc->user_ptr + (i * PAGE_SIZE),
 				       PAGE_SIZE)) {
 			p->parser_error = -EFAULT;
@@ -583,14 +597,16 @@ int radeon_cs_update_pages(struct radeon_cs_parser *p, int pg_idx)
 		}
 	}
 
-	new_page = ibc->kpage_idx[0] < ibc->kpage_idx[1] ? 0 : 1;
-
 	if (pg_idx == ibc->last_page_index) {
 		size = (ibc->length_dw * 4) % PAGE_SIZE;
-			if (size == 0)
-				size = PAGE_SIZE;
+		if (size == 0)
+			size = PAGE_SIZE;
 	}
 
+	new_page = ibc->kpage_idx[0] < ibc->kpage_idx[1] ? 0 : 1;
+	if (copy1)
+		ibc->kpage[new_page] = p->ib.ptr + (pg_idx * (PAGE_SIZE / 4));
+
 	if (DRM_COPY_FROM_USER(ibc->kpage[new_page],
 			       ibc->user_ptr + (pg_idx * PAGE_SIZE),
 			       size)) {
@@ -598,8 +614,9 @@ int radeon_cs_update_pages(struct radeon_cs_parser *p, int pg_idx)
 		return 0;
 	}
 
-	/* copy to IB here */
-	memcpy((void *)(p->ib->ptr+(pg_idx*(PAGE_SIZE/4))), ibc->kpage[new_page], size);
+	/* copy to IB for non single case */
+	if (!copy1)
+		memcpy((void *)(p->ib.ptr+(pg_idx*(PAGE_SIZE/4))), ibc->kpage[new_page], size);
 
 	ibc->last_copied_page = pg_idx;
 	ibc->kpage_idx[new_page] = pg_idx;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 5992502a3448..066c98b888a5 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -193,7 +193,7 @@ int radeon_wb_init(struct radeon_device *rdev)
 
 	if (rdev->wb.wb_obj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_GTT, &rdev->wb.wb_obj);
+				     RADEON_GEM_DOMAIN_GTT, NULL, &rdev->wb.wb_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create WB bo failed\n", r);
 			return r;
@@ -225,9 +225,9 @@ int radeon_wb_init(struct radeon_device *rdev)
 	/* disable event_write fences */
 	rdev->wb.use_event = false;
 	/* disabled via module param */
-	if (radeon_no_wb == 1)
+	if (radeon_no_wb == 1) {
 		rdev->wb.enabled = false;
-	else {
+	} else {
 		if (rdev->flags & RADEON_IS_AGP) {
 			/* often unreliable on AGP */
 			rdev->wb.enabled = false;
@@ -237,8 +237,9 @@ int radeon_wb_init(struct radeon_device *rdev)
 		} else {
 			rdev->wb.enabled = true;
 			/* event_write fences are only available on r600+ */
-			if (rdev->family >= CHIP_R600)
+			if (rdev->family >= CHIP_R600) {
 				rdev->wb.use_event = true;
+			}
 		}
 	}
 	/* always use writeback/events on NI, APUs */
@@ -696,6 +697,11 @@ static bool radeon_switcheroo_can_switch(struct pci_dev *pdev)
 	return can_switch;
 }
 
+static const struct vga_switcheroo_client_ops radeon_switcheroo_ops = {
+	.set_gpu_state = radeon_switcheroo_set_state,
+	.reprobe = NULL,
+	.can_switch = radeon_switcheroo_can_switch,
+};
 
 int radeon_device_init(struct radeon_device *rdev,
 		       struct drm_device *ddev,
@@ -714,7 +720,6 @@ int radeon_device_init(struct radeon_device *rdev,
 	rdev->is_atom_bios = false;
 	rdev->usec_timeout = RADEON_MAX_USEC_TIMEOUT;
 	rdev->mc.gtt_size = radeon_gart_size * 1024 * 1024;
-	rdev->gpu_lockup = false;
 	rdev->accel_working = false;
 
 	DRM_INFO("initializing kernel modesetting (%s 0x%04X:0x%04X 0x%04X:0x%04X).\n",
@@ -724,21 +729,18 @@ int radeon_device_init(struct radeon_device *rdev,
 	/* mutex initialization are all done here so we
 	 * can recall function without having locking issues */
 	radeon_mutex_init(&rdev->cs_mutex);
-	radeon_mutex_init(&rdev->ib_pool.mutex);
-	for (i = 0; i < RADEON_NUM_RINGS; ++i)
-		mutex_init(&rdev->ring[i].mutex);
+	mutex_init(&rdev->ring_lock);
 	mutex_init(&rdev->dc_hw_i2c_mutex);
 	if (rdev->family >= CHIP_R600)
 		spin_lock_init(&rdev->ih.lock);
 	mutex_init(&rdev->gem.mutex);
 	mutex_init(&rdev->pm.mutex);
 	mutex_init(&rdev->vram_mutex);
-	rwlock_init(&rdev->fence_lock);
-	rwlock_init(&rdev->semaphore_drv.lock);
-	INIT_LIST_HEAD(&rdev->gem.objects);
 	init_waitqueue_head(&rdev->irq.vblank_queue);
 	init_waitqueue_head(&rdev->irq.idle_queue);
-	INIT_LIST_HEAD(&rdev->semaphore_drv.bo);
+	r = radeon_gem_init(rdev);
+	if (r)
+		return r;
 	/* initialize vm here */
 	rdev->vm_manager.use_bitmap = 1;
 	rdev->vm_manager.max_pfn = 1 << 20;
@@ -814,10 +816,7 @@ int radeon_device_init(struct radeon_device *rdev,
 	/* this will fail for cards that aren't VGA class devices, just
 	 * ignore it */
 	vga_client_register(rdev->pdev, rdev, NULL, radeon_vga_set_decode);
-	vga_switcheroo_register_client(rdev->pdev,
-				       radeon_switcheroo_set_state,
-				       NULL,
-				       radeon_switcheroo_can_switch);
+	vga_switcheroo_register_client(rdev->pdev, &radeon_switcheroo_ops);
 
 	r = radeon_init(rdev);
 	if (r)
@@ -914,9 +913,12 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 	}
 	/* evict vram memory */
 	radeon_bo_evict_vram(rdev);
+
+	mutex_lock(&rdev->ring_lock);
 	/* wait for gpu to finish processing current batch */
 	for (i = 0; i < RADEON_NUM_RINGS; i++)
-		radeon_fence_wait_last(rdev, i);
+		radeon_fence_wait_empty_locked(rdev, i);
+	mutex_unlock(&rdev->ring_lock);
 
 	radeon_save_bios_scratch_regs(rdev);
 
@@ -955,7 +957,6 @@ int radeon_resume_kms(struct drm_device *dev)
 		console_unlock();
 		return -1;
 	}
-	pci_set_master(dev->pdev);
 	/* resume AGP if in use */
 	radeon_agp_resume(rdev);
 	radeon_resume(rdev);
@@ -988,9 +989,6 @@ int radeon_gpu_reset(struct radeon_device *rdev)
 	int r;
 	int resched;
 
-	/* Prevent CS ioctl from interfering */
-	radeon_mutex_lock(&rdev->cs_mutex);
-
 	radeon_save_bios_scratch_regs(rdev);
 	/* block TTM */
 	resched = ttm_bo_lock_delayed_workqueue(&rdev->mman.bdev);
@@ -1005,8 +1003,6 @@ int radeon_gpu_reset(struct radeon_device *rdev)
 		ttm_bo_unlock_delayed_workqueue(&rdev->mman.bdev, resched);
 	}
 
-	radeon_mutex_unlock(&rdev->cs_mutex);
-
 	if (r) {
 		/* bad news, how to tell it to userspace ? */
 		dev_info(rdev->dev, "GPU reset failed\n");
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 0a1d4bd65edc..64a008d14493 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -573,24 +573,6 @@ static const char *encoder_names[37] = {
 	"INTERNAL_VCE"
 };
 
-static const char *connector_names[15] = {
-	"Unknown",
-	"VGA",
-	"DVI-I",
-	"DVI-D",
-	"DVI-A",
-	"Composite",
-	"S-video",
-	"LVDS",
-	"Component",
-	"DIN",
-	"DisplayPort",
-	"HDMI-A",
-	"HDMI-B",
-	"TV",
-	"eDP",
-};
-
 static const char *hpd_names[6] = {
 	"HPD1",
 	"HPD2",
@@ -613,7 +595,7 @@ static void radeon_print_display_setup(struct drm_device *dev)
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		radeon_connector = to_radeon_connector(connector);
 		DRM_INFO("Connector %d:\n", i);
-		DRM_INFO("  %s\n", connector_names[connector->connector_type]);
+		DRM_INFO("  %s\n", drm_get_connector_name(connector));
 		if (radeon_connector->hpd.hpd != RADEON_HPD_NONE)
 			DRM_INFO("  %s\n", hpd_names[radeon_connector->hpd.hpd]);
 		if (radeon_connector->ddc_bus) {
@@ -1243,6 +1225,93 @@ void radeon_update_display_priority(struct radeon_device *rdev)
 
 }
 
+/*
+ * Allocate hdmi structs and determine register offsets
+ */
+static void radeon_afmt_init(struct radeon_device *rdev)
+{
+	int i;
+
+	for (i = 0; i < RADEON_MAX_AFMT_BLOCKS; i++)
+		rdev->mode_info.afmt[i] = NULL;
+
+	if (ASIC_IS_DCE6(rdev)) {
+		/* todo */
+	} else if (ASIC_IS_DCE4(rdev)) {
+		/* DCE4/5 has 6 audio blocks tied to DIG encoders */
+		/* DCE4.1 has 2 audio blocks tied to DIG encoders */
+		rdev->mode_info.afmt[0] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[0]) {
+			rdev->mode_info.afmt[0]->offset = EVERGREEN_CRTC0_REGISTER_OFFSET;
+			rdev->mode_info.afmt[0]->id = 0;
+		}
+		rdev->mode_info.afmt[1] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[1]) {
+			rdev->mode_info.afmt[1]->offset = EVERGREEN_CRTC1_REGISTER_OFFSET;
+			rdev->mode_info.afmt[1]->id = 1;
+		}
+		if (!ASIC_IS_DCE41(rdev)) {
+			rdev->mode_info.afmt[2] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[2]) {
+				rdev->mode_info.afmt[2]->offset = EVERGREEN_CRTC2_REGISTER_OFFSET;
+				rdev->mode_info.afmt[2]->id = 2;
+			}
+			rdev->mode_info.afmt[3] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[3]) {
+				rdev->mode_info.afmt[3]->offset = EVERGREEN_CRTC3_REGISTER_OFFSET;
+				rdev->mode_info.afmt[3]->id = 3;
+			}
+			rdev->mode_info.afmt[4] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[4]) {
+				rdev->mode_info.afmt[4]->offset = EVERGREEN_CRTC4_REGISTER_OFFSET;
+				rdev->mode_info.afmt[4]->id = 4;
+			}
+			rdev->mode_info.afmt[5] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[5]) {
+				rdev->mode_info.afmt[5]->offset = EVERGREEN_CRTC5_REGISTER_OFFSET;
+				rdev->mode_info.afmt[5]->id = 5;
+			}
+		}
+	} else if (ASIC_IS_DCE3(rdev)) {
+		/* DCE3.x has 2 audio blocks tied to DIG encoders */
+		rdev->mode_info.afmt[0] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[0]) {
+			rdev->mode_info.afmt[0]->offset = DCE3_HDMI_OFFSET0;
+			rdev->mode_info.afmt[0]->id = 0;
+		}
+		rdev->mode_info.afmt[1] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[1]) {
+			rdev->mode_info.afmt[1]->offset = DCE3_HDMI_OFFSET1;
+			rdev->mode_info.afmt[1]->id = 1;
+		}
+	} else if (ASIC_IS_DCE2(rdev)) {
+		/* DCE2 has at least 1 routable audio block */
+		rdev->mode_info.afmt[0] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[0]) {
+			rdev->mode_info.afmt[0]->offset = DCE2_HDMI_OFFSET0;
+			rdev->mode_info.afmt[0]->id = 0;
+		}
+		/* r6xx has 2 routable audio blocks */
+		if (rdev->family >= CHIP_R600) {
+			rdev->mode_info.afmt[1] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[1]) {
+				rdev->mode_info.afmt[1]->offset = DCE2_HDMI_OFFSET1;
+				rdev->mode_info.afmt[1]->id = 1;
+			}
+		}
+	}
+}
+
+static void radeon_afmt_fini(struct radeon_device *rdev)
+{
+	int i;
+
+	for (i = 0; i < RADEON_MAX_AFMT_BLOCKS; i++) {
+		kfree(rdev->mode_info.afmt[i]);
+		rdev->mode_info.afmt[i] = NULL;
+	}
+}
+
 int radeon_modeset_init(struct radeon_device *rdev)
 {
 	int i;
@@ -1251,7 +1320,7 @@ int radeon_modeset_init(struct radeon_device *rdev)
 	drm_mode_config_init(rdev->ddev);
 	rdev->mode_info.mode_config_initialized = true;
 
-	rdev->ddev->mode_config.funcs = (void *)&radeon_mode_funcs;
+	rdev->ddev->mode_config.funcs = &radeon_mode_funcs;
 
 	if (ASIC_IS_DCE5(rdev)) {
 		rdev->ddev->mode_config.max_width = 16384;
@@ -1303,6 +1372,9 @@ int radeon_modeset_init(struct radeon_device *rdev)
 	/* initialize hpd */
 	radeon_hpd_init(rdev);
 
+	/* setup afmt */
+	radeon_afmt_init(rdev);
+
 	/* Initialize power management */
 	radeon_pm_init(rdev);
 
@@ -1319,6 +1391,7 @@ void radeon_modeset_fini(struct radeon_device *rdev)
 	radeon_pm_fini(rdev);
 
 	if (rdev->mode_info.mode_config_initialized) {
+		radeon_afmt_fini(rdev);
 		drm_kms_helper_poll_fini(rdev->ddev);
 		radeon_hpd_fini(rdev);
 		drm_mode_config_cleanup(rdev->ddev);
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index ef7bb3f6ecae..f0bb2b543b13 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -105,6 +105,11 @@ int radeon_mode_dumb_create(struct drm_file *file_priv,
 int radeon_mode_dumb_destroy(struct drm_file *file_priv,
 			     struct drm_device *dev,
 			     uint32_t handle);
+struct dma_buf *radeon_gem_prime_export(struct drm_device *dev,
+					struct drm_gem_object *obj,
+					int flags);
+struct drm_gem_object *radeon_gem_prime_import(struct drm_device *dev,
+					       struct dma_buf *dma_buf);
 
 #if defined(CONFIG_DEBUG_FS)
 int radeon_debugfs_init(struct drm_minor *minor);
@@ -128,6 +133,7 @@ int radeon_disp_priority = 0;
 int radeon_hw_i2c = 0;
 int radeon_pcie_gen2 = 0;
 int radeon_msi = -1;
+int radeon_lockup_timeout = 10000;
 
 MODULE_PARM_DESC(no_wb, "Disable AGP writeback for scratch registers");
 module_param_named(no_wb, radeon_no_wb, int, 0444);
@@ -177,6 +183,9 @@ module_param_named(pcie_gen2, radeon_pcie_gen2, int, 0444);
 MODULE_PARM_DESC(msi, "MSI support (1 = enable, 0 = disable, -1 = auto)");
 module_param_named(msi, radeon_msi, int, 0444);
 
+MODULE_PARM_DESC(lockup_timeout, "GPU lockup timeout in ms (defaul 10000 = 10 seconds, 0 = disable)");
+module_param_named(lockup_timeout, radeon_lockup_timeout, int, 0444);
+
 static int radeon_suspend(struct drm_device *dev, pm_message_t state)
 {
 	drm_radeon_private_t *dev_priv = dev->dev_private;
@@ -329,7 +338,8 @@ static const struct file_operations radeon_driver_kms_fops = {
 static struct drm_driver kms_driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_PCI_DMA | DRIVER_SG |
-	    DRIVER_HAVE_IRQ | DRIVER_HAVE_DMA | DRIVER_IRQ_SHARED | DRIVER_GEM,
+	    DRIVER_HAVE_IRQ | DRIVER_HAVE_DMA | DRIVER_IRQ_SHARED | DRIVER_GEM |
+	    DRIVER_PRIME,
 	.dev_priv_size = 0,
 	.load = radeon_driver_load_kms,
 	.firstopen = radeon_driver_firstopen_kms,
@@ -364,6 +374,12 @@ static struct drm_driver kms_driver = {
 	.dumb_map_offset = radeon_mode_dumb_mmap,
 	.dumb_destroy = radeon_mode_dumb_destroy,
 	.fops = &radeon_driver_kms_fops,
+
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export = radeon_gem_prime_export,
+	.gem_prime_import = radeon_gem_prime_import,
+
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index 4bd36a354fbe..11f5f402d22c 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -63,98 +63,82 @@ static u32 radeon_fence_read(struct radeon_device *rdev, int ring)
 
 int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence)
 {
-	unsigned long irq_flags;
-
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (fence->emitted) {
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
+	/* we are protected by the ring emission mutex */
+	if (fence->seq && fence->seq < RADEON_FENCE_NOTEMITED_SEQ) {
 		return 0;
 	}
-	fence->seq = atomic_add_return(1, &rdev->fence_drv[fence->ring].seq);
-	if (!rdev->ring[fence->ring].ready)
-		/* FIXME: cp is not running assume everythings is done right
-		 * away
-		 */
-		radeon_fence_write(rdev, fence->seq, fence->ring);
-	else
-		radeon_fence_ring_emit(rdev, fence->ring, fence);
-
+	fence->seq = ++rdev->fence_drv[fence->ring].seq;
+	radeon_fence_ring_emit(rdev, fence->ring, fence);
 	trace_radeon_fence_emit(rdev->ddev, fence->seq);
-	fence->emitted = true;
-	list_move_tail(&fence->list, &rdev->fence_drv[fence->ring].emitted);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
-static bool radeon_fence_poll_locked(struct radeon_device *rdev, int ring)
+void radeon_fence_process(struct radeon_device *rdev, int ring)
 {
-	struct radeon_fence *fence;
-	struct list_head *i, *n;
-	uint32_t seq;
+	uint64_t seq, last_seq;
+	unsigned count_loop = 0;
 	bool wake = false;
-	unsigned long cjiffies;
 
-	seq = radeon_fence_read(rdev, ring);
-	if (seq != rdev->fence_drv[ring].last_seq) {
-		rdev->fence_drv[ring].last_seq = seq;
-		rdev->fence_drv[ring].last_jiffies = jiffies;
-		rdev->fence_drv[ring].last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-	} else {
-		cjiffies = jiffies;
-		if (time_after(cjiffies, rdev->fence_drv[ring].last_jiffies)) {
-			cjiffies -= rdev->fence_drv[ring].last_jiffies;
-			if (time_after(rdev->fence_drv[ring].last_timeout, cjiffies)) {
-				/* update the timeout */
-				rdev->fence_drv[ring].last_timeout -= cjiffies;
-			} else {
-				/* the 500ms timeout is elapsed we should test
-				 * for GPU lockup
-				 */
-				rdev->fence_drv[ring].last_timeout = 1;
-			}
-		} else {
-			/* wrap around update last jiffies, we will just wait
-			 * a little longer
-			 */
-			rdev->fence_drv[ring].last_jiffies = cjiffies;
+	/* Note there is a scenario here for an infinite loop but it's
+	 * very unlikely to happen. For it to happen, the current polling
+	 * process need to be interrupted by another process and another
+	 * process needs to update the last_seq btw the atomic read and
+	 * xchg of the current process.
+	 *
+	 * More over for this to go in infinite loop there need to be
+	 * continuously new fence signaled ie radeon_fence_read needs
+	 * to return a different value each time for both the currently
+	 * polling process and the other process that xchg the last_seq
+	 * btw atomic read and xchg of the current process. And the
+	 * value the other process set as last seq must be higher than
+	 * the seq value we just read. Which means that current process
+	 * need to be interrupted after radeon_fence_read and before
+	 * atomic xchg.
+	 *
+	 * To be even more safe we count the number of time we loop and
+	 * we bail after 10 loop just accepting the fact that we might
+	 * have temporarly set the last_seq not to the true real last
+	 * seq but to an older one.
+	 */
+	last_seq = atomic64_read(&rdev->fence_drv[ring].last_seq);
+	do {
+		seq = radeon_fence_read(rdev, ring);
+		seq |= last_seq & 0xffffffff00000000LL;
+		if (seq < last_seq) {
+			seq += 0x100000000LL;
 		}
-		return false;
-	}
-	n = NULL;
-	list_for_each(i, &rdev->fence_drv[ring].emitted) {
-		fence = list_entry(i, struct radeon_fence, list);
-		if (fence->seq == seq) {
-			n = i;
+
+		if (seq == last_seq) {
 			break;
 		}
-	}
-	/* all fence previous to this one are considered as signaled */
-	if (n) {
-		i = n;
-		do {
-			n = i->prev;
-			list_move_tail(i, &rdev->fence_drv[ring].signaled);
-			fence = list_entry(i, struct radeon_fence, list);
-			fence->signaled = true;
-			i = n;
-		} while (i != &rdev->fence_drv[ring].emitted);
+		/* If we loop over we don't want to return without
+		 * checking if a fence is signaled as it means that the
+		 * seq we just read is different from the previous on.
+		 */
 		wake = true;
+		last_seq = seq;
+		if ((count_loop++) > 10) {
+			/* We looped over too many time leave with the
+			 * fact that we might have set an older fence
+			 * seq then the current real last seq as signaled
+			 * by the hw.
+			 */
+			break;
+		}
+	} while (atomic64_xchg(&rdev->fence_drv[ring].last_seq, seq) > seq);
+
+	if (wake) {
+		rdev->fence_drv[ring].last_activity = jiffies;
+		wake_up_all(&rdev->fence_queue);
 	}
-	return wake;
 }
 
 static void radeon_fence_destroy(struct kref *kref)
 {
-	unsigned long irq_flags;
-        struct radeon_fence *fence;
+	struct radeon_fence *fence;
 
 	fence = container_of(kref, struct radeon_fence, kref);
-	write_lock_irqsave(&fence->rdev->fence_lock, irq_flags);
-	list_del(&fence->list);
-	fence->emitted = false;
-	write_unlock_irqrestore(&fence->rdev->fence_lock, irq_flags);
-	if (fence->semaphore)
-		radeon_semaphore_free(fence->rdev, fence->semaphore);
+	fence->seq = RADEON_FENCE_NOTEMITED_SEQ;
 	kfree(fence);
 }
 
@@ -162,171 +146,342 @@ int radeon_fence_create(struct radeon_device *rdev,
 			struct radeon_fence **fence,
 			int ring)
 {
-	unsigned long irq_flags;
-
 	*fence = kmalloc(sizeof(struct radeon_fence), GFP_KERNEL);
 	if ((*fence) == NULL) {
 		return -ENOMEM;
 	}
 	kref_init(&((*fence)->kref));
 	(*fence)->rdev = rdev;
-	(*fence)->emitted = false;
-	(*fence)->signaled = false;
-	(*fence)->seq = 0;
+	(*fence)->seq = RADEON_FENCE_NOTEMITED_SEQ;
 	(*fence)->ring = ring;
-	(*fence)->semaphore = NULL;
-	INIT_LIST_HEAD(&(*fence)->list);
-
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	list_add_tail(&(*fence)->list, &rdev->fence_drv[ring].created);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
-bool radeon_fence_signaled(struct radeon_fence *fence)
+static bool radeon_fence_seq_signaled(struct radeon_device *rdev,
+				      u64 seq, unsigned ring)
 {
-	unsigned long irq_flags;
-	bool signaled = false;
-
-	if (!fence)
+	if (atomic64_read(&rdev->fence_drv[ring].last_seq) >= seq) {
 		return true;
-
-	if (fence->rdev->gpu_lockup)
+	}
+	/* poll new last sequence at least once */
+	radeon_fence_process(rdev, ring);
+	if (atomic64_read(&rdev->fence_drv[ring].last_seq) >= seq) {
 		return true;
+	}
+	return false;
+}
 
-	write_lock_irqsave(&fence->rdev->fence_lock, irq_flags);
-	signaled = fence->signaled;
-	/* if we are shuting down report all fence as signaled */
-	if (fence->rdev->shutdown) {
-		signaled = true;
+bool radeon_fence_signaled(struct radeon_fence *fence)
+{
+	if (!fence) {
+		return true;
 	}
-	if (!fence->emitted) {
+	if (fence->seq == RADEON_FENCE_NOTEMITED_SEQ) {
 		WARN(1, "Querying an unemitted fence : %p !\n", fence);
-		signaled = true;
+		return true;
 	}
-	if (!signaled) {
-		radeon_fence_poll_locked(fence->rdev, fence->ring);
-		signaled = fence->signaled;
+	if (fence->seq == RADEON_FENCE_SIGNALED_SEQ) {
+		return true;
+	}
+	if (radeon_fence_seq_signaled(fence->rdev, fence->seq, fence->ring)) {
+		fence->seq = RADEON_FENCE_SIGNALED_SEQ;
+		return true;
 	}
-	write_unlock_irqrestore(&fence->rdev->fence_lock, irq_flags);
-	return signaled;
+	return false;
+}
+
+static int radeon_fence_wait_seq(struct radeon_device *rdev, u64 target_seq,
+				 unsigned ring, bool intr, bool lock_ring)
+{
+	unsigned long timeout, last_activity;
+	uint64_t seq;
+	unsigned i;
+	bool signaled;
+	int r;
+
+	while (target_seq > atomic64_read(&rdev->fence_drv[ring].last_seq)) {
+		if (!rdev->ring[ring].ready) {
+			return -EBUSY;
+		}
+
+		timeout = jiffies - RADEON_FENCE_JIFFIES_TIMEOUT;
+		if (time_after(rdev->fence_drv[ring].last_activity, timeout)) {
+			/* the normal case, timeout is somewhere before last_activity */
+			timeout = rdev->fence_drv[ring].last_activity - timeout;
+		} else {
+			/* either jiffies wrapped around, or no fence was signaled in the last 500ms
+			 * anyway we will just wait for the minimum amount and then check for a lockup
+			 */
+			timeout = 1;
+		}
+		seq = atomic64_read(&rdev->fence_drv[ring].last_seq);
+		/* Save current last activity valuee, used to check for GPU lockups */
+		last_activity = rdev->fence_drv[ring].last_activity;
+
+		trace_radeon_fence_wait_begin(rdev->ddev, seq);
+		radeon_irq_kms_sw_irq_get(rdev, ring);
+		if (intr) {
+			r = wait_event_interruptible_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_seq_signaled(rdev, target_seq, ring)),
+				timeout);
+                } else {
+			r = wait_event_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_seq_signaled(rdev, target_seq, ring)),
+				timeout);
+		}
+		radeon_irq_kms_sw_irq_put(rdev, ring);
+		if (unlikely(r < 0)) {
+			return r;
+		}
+		trace_radeon_fence_wait_end(rdev->ddev, seq);
+
+		if (unlikely(!signaled)) {
+			/* we were interrupted for some reason and fence
+			 * isn't signaled yet, resume waiting */
+			if (r) {
+				continue;
+			}
+
+			/* check if sequence value has changed since last_activity */
+			if (seq != atomic64_read(&rdev->fence_drv[ring].last_seq)) {
+				continue;
+			}
+
+			if (lock_ring) {
+				mutex_lock(&rdev->ring_lock);
+			}
+
+			/* test if somebody else has already decided that this is a lockup */
+			if (last_activity != rdev->fence_drv[ring].last_activity) {
+				if (lock_ring) {
+					mutex_unlock(&rdev->ring_lock);
+				}
+				continue;
+			}
+
+			if (radeon_ring_is_lockup(rdev, ring, &rdev->ring[ring])) {
+				/* good news we believe it's a lockup */
+				dev_warn(rdev->dev, "GPU lockup (waiting for 0x%016llx last fence id 0x%016llx)\n",
+					 target_seq, seq);
+
+				/* change last activity so nobody else think there is a lockup */
+				for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+					rdev->fence_drv[i].last_activity = jiffies;
+				}
+
+				/* mark the ring as not ready any more */
+				rdev->ring[ring].ready = false;
+				if (lock_ring) {
+					mutex_unlock(&rdev->ring_lock);
+				}
+				return -EDEADLK;
+			}
+
+			if (lock_ring) {
+				mutex_unlock(&rdev->ring_lock);
+			}
+		}
+	}
+	return 0;
 }
 
 int radeon_fence_wait(struct radeon_fence *fence, bool intr)
 {
-	struct radeon_device *rdev;
-	unsigned long irq_flags, timeout;
-	u32 seq;
 	int r;
 
 	if (fence == NULL) {
 		WARN(1, "Querying an invalid fence : %p !\n", fence);
-		return 0;
+		return -EINVAL;
 	}
-	rdev = fence->rdev;
-	if (radeon_fence_signaled(fence)) {
-		return 0;
+
+	r = radeon_fence_wait_seq(fence->rdev, fence->seq,
+				  fence->ring, intr, true);
+	if (r) {
+		return r;
 	}
-	timeout = rdev->fence_drv[fence->ring].last_timeout;
-retry:
-	/* save current sequence used to check for GPU lockup */
-	seq = rdev->fence_drv[fence->ring].last_seq;
-	trace_radeon_fence_wait_begin(rdev->ddev, seq);
-	if (intr) {
-		radeon_irq_kms_sw_irq_get(rdev, fence->ring);
-		r = wait_event_interruptible_timeout(rdev->fence_drv[fence->ring].queue,
-				radeon_fence_signaled(fence), timeout);
-		radeon_irq_kms_sw_irq_put(rdev, fence->ring);
-		if (unlikely(r < 0)) {
-			return r;
+	fence->seq = RADEON_FENCE_SIGNALED_SEQ;
+	return 0;
+}
+
+bool radeon_fence_any_seq_signaled(struct radeon_device *rdev, u64 *seq)
+{
+	unsigned i;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (seq[i] && radeon_fence_seq_signaled(rdev, seq[i], i)) {
+			return true;
 		}
-	} else {
-		radeon_irq_kms_sw_irq_get(rdev, fence->ring);
-		r = wait_event_timeout(rdev->fence_drv[fence->ring].queue,
-			 radeon_fence_signaled(fence), timeout);
-		radeon_irq_kms_sw_irq_put(rdev, fence->ring);
 	}
-	trace_radeon_fence_wait_end(rdev->ddev, seq);
-	if (unlikely(!radeon_fence_signaled(fence))) {
-		/* we were interrupted for some reason and fence isn't
-		 * isn't signaled yet, resume wait
-		 */
-		if (r) {
-			timeout = r;
-			goto retry;
+	return false;
+}
+
+static int radeon_fence_wait_any_seq(struct radeon_device *rdev,
+				     u64 *target_seq, bool intr)
+{
+	unsigned long timeout, last_activity, tmp;
+	unsigned i, ring = RADEON_NUM_RINGS;
+	bool signaled;
+	int r;
+
+	for (i = 0, last_activity = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (!target_seq[i]) {
+			continue;
+		}
+
+		/* use the most recent one as indicator */
+		if (time_after(rdev->fence_drv[i].last_activity, last_activity)) {
+			last_activity = rdev->fence_drv[i].last_activity;
 		}
-		/* don't protect read access to rdev->fence_drv[t].last_seq
-		 * if we experiencing a lockup the value doesn't change
+
+		/* For lockup detection just pick the lowest ring we are
+		 * actively waiting for
 		 */
-		if (seq == rdev->fence_drv[fence->ring].last_seq &&
-		    radeon_gpu_is_lockup(rdev, &rdev->ring[fence->ring])) {
-			/* good news we believe it's a lockup */
-			printk(KERN_WARNING "GPU lockup (waiting for 0x%08X last fence id 0x%08X)\n",
-			     fence->seq, seq);
-			/* FIXME: what should we do ? marking everyone
-			 * as signaled for now
+		if (i < ring) {
+			ring = i;
+		}
+	}
+
+	/* nothing to wait for ? */
+	if (ring == RADEON_NUM_RINGS) {
+		return 0;
+	}
+
+	while (!radeon_fence_any_seq_signaled(rdev, target_seq)) {
+		timeout = jiffies - RADEON_FENCE_JIFFIES_TIMEOUT;
+		if (time_after(last_activity, timeout)) {
+			/* the normal case, timeout is somewhere before last_activity */
+			timeout = last_activity - timeout;
+		} else {
+			/* either jiffies wrapped around, or no fence was signaled in the last 500ms
+			 * anyway we will just wait for the minimum amount and then check for a lockup
 			 */
-			rdev->gpu_lockup = true;
-			r = radeon_gpu_reset(rdev);
-			if (r)
-				return r;
-			radeon_fence_write(rdev, fence->seq, fence->ring);
-			rdev->gpu_lockup = false;
+			timeout = 1;
+		}
+
+		trace_radeon_fence_wait_begin(rdev->ddev, target_seq[ring]);
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			if (target_seq[i]) {
+				radeon_irq_kms_sw_irq_get(rdev, i);
+			}
+		}
+		if (intr) {
+			r = wait_event_interruptible_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_any_seq_signaled(rdev, target_seq)),
+				timeout);
+		} else {
+			r = wait_event_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_any_seq_signaled(rdev, target_seq)),
+				timeout);
+		}
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			if (target_seq[i]) {
+				radeon_irq_kms_sw_irq_put(rdev, i);
+			}
+		}
+		if (unlikely(r < 0)) {
+			return r;
+		}
+		trace_radeon_fence_wait_end(rdev->ddev, target_seq[ring]);
+
+		if (unlikely(!signaled)) {
+			/* we were interrupted for some reason and fence
+			 * isn't signaled yet, resume waiting */
+			if (r) {
+				continue;
+			}
+
+			mutex_lock(&rdev->ring_lock);
+			for (i = 0, tmp = 0; i < RADEON_NUM_RINGS; ++i) {
+				if (time_after(rdev->fence_drv[i].last_activity, tmp)) {
+					tmp = rdev->fence_drv[i].last_activity;
+				}
+			}
+			/* test if somebody else has already decided that this is a lockup */
+			if (last_activity != tmp) {
+				last_activity = tmp;
+				mutex_unlock(&rdev->ring_lock);
+				continue;
+			}
+
+			if (radeon_ring_is_lockup(rdev, ring, &rdev->ring[ring])) {
+				/* good news we believe it's a lockup */
+				dev_warn(rdev->dev, "GPU lockup (waiting for 0x%016llx)\n",
+					 target_seq[ring]);
+
+				/* change last activity so nobody else think there is a lockup */
+				for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+					rdev->fence_drv[i].last_activity = jiffies;
+				}
+
+				/* mark the ring as not ready any more */
+				rdev->ring[ring].ready = false;
+				mutex_unlock(&rdev->ring_lock);
+				return -EDEADLK;
+			}
+			mutex_unlock(&rdev->ring_lock);
 		}
-		timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-		write_lock_irqsave(&rdev->fence_lock, irq_flags);
-		rdev->fence_drv[fence->ring].last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-		rdev->fence_drv[fence->ring].last_jiffies = jiffies;
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		goto retry;
 	}
 	return 0;
 }
 
-int radeon_fence_wait_next(struct radeon_device *rdev, int ring)
+int radeon_fence_wait_any(struct radeon_device *rdev,
+			  struct radeon_fence **fences,
+			  bool intr)
 {
-	unsigned long irq_flags;
-	struct radeon_fence *fence;
+	uint64_t seq[RADEON_NUM_RINGS];
+	unsigned i;
 	int r;
 
-	if (rdev->gpu_lockup) {
-		return 0;
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		seq[i] = 0;
+
+		if (!fences[i]) {
+			continue;
+		}
+
+		if (fences[i]->seq == RADEON_FENCE_SIGNALED_SEQ) {
+			/* something was allready signaled */
+			return 0;
+		}
+
+		if (fences[i]->seq < RADEON_FENCE_NOTEMITED_SEQ) {
+			seq[i] = fences[i]->seq;
+		}
 	}
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (list_empty(&rdev->fence_drv[ring].emitted)) {
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		return 0;
+
+	r = radeon_fence_wait_any_seq(rdev, seq, intr);
+	if (r) {
+		return r;
 	}
-	fence = list_entry(rdev->fence_drv[ring].emitted.next,
-			   struct radeon_fence, list);
-	radeon_fence_ref(fence);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	r = radeon_fence_wait(fence, false);
-	radeon_fence_unref(&fence);
-	return r;
+	return 0;
 }
 
-int radeon_fence_wait_last(struct radeon_device *rdev, int ring)
+int radeon_fence_wait_next_locked(struct radeon_device *rdev, int ring)
 {
-	unsigned long irq_flags;
-	struct radeon_fence *fence;
-	int r;
-
-	if (rdev->gpu_lockup) {
-		return 0;
+	uint64_t seq;
+
+	/* We are not protected by ring lock when reading current seq but
+	 * it's ok as worst case is we return to early while we could have
+	 * wait.
+	 */
+	seq = atomic64_read(&rdev->fence_drv[ring].last_seq) + 1ULL;
+	if (seq >= rdev->fence_drv[ring].seq) {
+		/* nothing to wait for, last_seq is
+		   already the last emited fence */
+		return -ENOENT;
 	}
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (list_empty(&rdev->fence_drv[ring].emitted)) {
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		return 0;
-	}
-	fence = list_entry(rdev->fence_drv[ring].emitted.prev,
-			   struct radeon_fence, list);
-	radeon_fence_ref(fence);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	r = radeon_fence_wait(fence, false);
-	radeon_fence_unref(&fence);
-	return r;
+	return radeon_fence_wait_seq(rdev, seq, ring, false, false);
+}
+
+int radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring)
+{
+	/* We are not protected by ring lock when reading current seq
+	 * but it's ok as wait empty is call from place where no more
+	 * activity can be scheduled so there won't be concurrent access
+	 * to seq value.
+	 */
+	return radeon_fence_wait_seq(rdev, rdev->fence_drv[ring].seq,
+				     ring, false, false);
 }
 
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence)
@@ -345,49 +500,27 @@ void radeon_fence_unref(struct radeon_fence **fence)
 	}
 }
 
-void radeon_fence_process(struct radeon_device *rdev, int ring)
-{
-	unsigned long irq_flags;
-	bool wake;
-
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	wake = radeon_fence_poll_locked(rdev, ring);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	if (wake) {
-		wake_up_all(&rdev->fence_drv[ring].queue);
-	}
-}
-
-int radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
+unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
 {
-	unsigned long irq_flags;
-	int not_processed = 0;
-
-	read_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (!rdev->fence_drv[ring].initialized) {
-		read_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		return 0;
+	uint64_t emitted;
+
+	/* We are not protected by ring lock when reading the last sequence
+	 * but it's ok to report slightly wrong fence count here.
+	 */
+	radeon_fence_process(rdev, ring);
+	emitted = rdev->fence_drv[ring].seq - atomic64_read(&rdev->fence_drv[ring].last_seq);
+	/* to avoid 32bits warp around */
+	if (emitted > 0x10000000) {
+		emitted = 0x10000000;
 	}
-
-	if (!list_empty(&rdev->fence_drv[ring].emitted)) {
-		struct list_head *ptr;
-		list_for_each(ptr, &rdev->fence_drv[ring].emitted) {
-			/* count up to 3, that's enought info */
-			if (++not_processed >= 3)
-				break;
-		}
-	}
-	read_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	return not_processed;
+	return (unsigned)emitted;
 }
 
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 {
-	unsigned long irq_flags;
 	uint64_t index;
 	int r;
 
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
 	radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
 	if (rdev->wb.use_event) {
 		rdev->fence_drv[ring].scratch_reg = 0;
@@ -396,7 +529,6 @@ int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 		r = radeon_scratch_get(rdev, &rdev->fence_drv[ring].scratch_reg);
 		if (r) {
 			dev_err(rdev->dev, "fence failed to get scratch register\n");
-			write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 			return r;
 		}
 		index = RADEON_WB_SCRATCH_OFFSET +
@@ -405,11 +537,10 @@ int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 	}
 	rdev->fence_drv[ring].cpu_addr = &rdev->wb.wb[index/4];
 	rdev->fence_drv[ring].gpu_addr = rdev->wb.gpu_addr + index;
-	radeon_fence_write(rdev, atomic_read(&rdev->fence_drv[ring].seq), ring);
+	radeon_fence_write(rdev, rdev->fence_drv[ring].seq, ring);
 	rdev->fence_drv[ring].initialized = true;
-	DRM_INFO("fence driver on ring %d use gpu addr 0x%08Lx and cpu addr 0x%p\n",
+	dev_info(rdev->dev, "fence driver on ring %d use gpu addr 0x%016llx and cpu addr 0x%p\n",
 		 ring, rdev->fence_drv[ring].gpu_addr, rdev->fence_drv[ring].cpu_addr);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
@@ -418,24 +549,20 @@ static void radeon_fence_driver_init_ring(struct radeon_device *rdev, int ring)
 	rdev->fence_drv[ring].scratch_reg = -1;
 	rdev->fence_drv[ring].cpu_addr = NULL;
 	rdev->fence_drv[ring].gpu_addr = 0;
-	atomic_set(&rdev->fence_drv[ring].seq, 0);
-	INIT_LIST_HEAD(&rdev->fence_drv[ring].created);
-	INIT_LIST_HEAD(&rdev->fence_drv[ring].emitted);
-	INIT_LIST_HEAD(&rdev->fence_drv[ring].signaled);
-	init_waitqueue_head(&rdev->fence_drv[ring].queue);
+	rdev->fence_drv[ring].seq = 0;
+	atomic64_set(&rdev->fence_drv[ring].last_seq, 0);
+	rdev->fence_drv[ring].last_activity = jiffies;
 	rdev->fence_drv[ring].initialized = false;
 }
 
 int radeon_fence_driver_init(struct radeon_device *rdev)
 {
-	unsigned long irq_flags;
 	int ring;
 
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	init_waitqueue_head(&rdev->fence_queue);
 	for (ring = 0; ring < RADEON_NUM_RINGS; ring++) {
 		radeon_fence_driver_init_ring(rdev, ring);
 	}
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	if (radeon_debugfs_fence_init(rdev)) {
 		dev_err(rdev->dev, "fence debugfs file creation failed\n");
 	}
@@ -444,19 +571,18 @@ int radeon_fence_driver_init(struct radeon_device *rdev)
 
 void radeon_fence_driver_fini(struct radeon_device *rdev)
 {
-	unsigned long irq_flags;
 	int ring;
 
+	mutex_lock(&rdev->ring_lock);
 	for (ring = 0; ring < RADEON_NUM_RINGS; ring++) {
 		if (!rdev->fence_drv[ring].initialized)
 			continue;
-		radeon_fence_wait_last(rdev, ring);
-		wake_up_all(&rdev->fence_drv[ring].queue);
-		write_lock_irqsave(&rdev->fence_lock, irq_flags);
+		radeon_fence_wait_empty_locked(rdev, ring);
+		wake_up_all(&rdev->fence_queue);
 		radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 		rdev->fence_drv[ring].initialized = false;
 	}
+	mutex_unlock(&rdev->ring_lock);
 }
 
 
@@ -469,7 +595,6 @@ static int radeon_debugfs_fence_info(struct seq_file *m, void *data)
 	struct drm_info_node *node = (struct drm_info_node *)m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	struct radeon_fence *fence;
 	int i;
 
 	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
@@ -477,14 +602,10 @@ static int radeon_debugfs_fence_info(struct seq_file *m, void *data)
 			continue;
 
 		seq_printf(m, "--- ring %d ---\n", i);
-		seq_printf(m, "Last signaled fence 0x%08X\n",
-			   radeon_fence_read(rdev, i));
-		if (!list_empty(&rdev->fence_drv[i].emitted)) {
-			fence = list_entry(rdev->fence_drv[i].emitted.prev,
-					   struct radeon_fence, list);
-			seq_printf(m, "Last emitted fence %p with 0x%08X\n",
-				   fence,  fence->seq);
-		}
+		seq_printf(m, "Last signaled fence 0x%016llx\n",
+			   (unsigned long long)atomic64_read(&rdev->fence_drv[i].last_seq));
+		seq_printf(m, "Last emitted  0x%016llx\n",
+			   rdev->fence_drv[i].seq);
 	}
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_gart.c b/drivers/gpu/drm/radeon/radeon_gart.c
index 456a77cf4b7f..79db56e6c2ac 100644
--- a/drivers/gpu/drm/radeon/radeon_gart.c
+++ b/drivers/gpu/drm/radeon/radeon_gart.c
@@ -80,7 +80,7 @@ int radeon_gart_table_vram_alloc(struct radeon_device *rdev)
 	if (rdev->gart.robj == NULL) {
 		r = radeon_bo_create(rdev, rdev->gart.table_size,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				     &rdev->gart.robj);
+				     NULL, &rdev->gart.robj);
 		if (r) {
 			return r;
 		}
@@ -326,7 +326,7 @@ static void radeon_vm_unbind_locked(struct radeon_device *rdev,
 	rdev->vm_manager.use_bitmap &= ~(1 << vm->id);
 	list_del_init(&vm->list);
 	vm->id = -1;
-	radeon_sa_bo_free(rdev, &vm->sa_bo);
+	radeon_sa_bo_free(rdev, &vm->sa_bo, NULL);
 	vm->pt = NULL;
 
 	list_for_each_entry(bo_va, &vm->va, vm_list) {
@@ -395,7 +395,7 @@ int radeon_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm)
 retry:
 	r = radeon_sa_bo_new(rdev, &rdev->vm_manager.sa_manager, &vm->sa_bo,
 			     RADEON_GPU_PAGE_ALIGN(vm->last_pfn * 8),
-			     RADEON_GPU_PAGE_SIZE);
+			     RADEON_GPU_PAGE_SIZE, false);
 	if (r) {
 		if (list_empty(&rdev->vm_manager.lru_vm)) {
 			return r;
@@ -404,10 +404,8 @@ retry:
 		radeon_vm_unbind(rdev, vm_evict);
 		goto retry;
 	}
-	vm->pt = rdev->vm_manager.sa_manager.cpu_ptr;
-	vm->pt += (vm->sa_bo.offset >> 3);
-	vm->pt_gpu_addr = rdev->vm_manager.sa_manager.gpu_addr;
-	vm->pt_gpu_addr += vm->sa_bo.offset;
+	vm->pt = radeon_sa_bo_cpu_addr(vm->sa_bo);
+	vm->pt_gpu_addr = radeon_sa_bo_gpu_addr(vm->sa_bo);
 	memset(vm->pt, 0, RADEON_GPU_PAGE_ALIGN(vm->last_pfn * 8));
 
 retry_id:
@@ -428,14 +426,14 @@ retry_id:
 	/* do hw bind */
 	r = rdev->vm_manager.funcs->bind(rdev, vm, id);
 	if (r) {
-		radeon_sa_bo_free(rdev, &vm->sa_bo);
+		radeon_sa_bo_free(rdev, &vm->sa_bo, NULL);
 		return r;
 	}
 	rdev->vm_manager.use_bitmap |= 1 << id;
 	vm->id = id;
 	list_add_tail(&vm->list, &rdev->vm_manager.lru_vm);
-	return radeon_vm_bo_update_pte(rdev, vm, rdev->ib_pool.sa_manager.bo,
-				       &rdev->ib_pool.sa_manager.bo->tbo.mem);
+	return radeon_vm_bo_update_pte(rdev, vm, rdev->ring_tmp_bo.bo,
+				       &rdev->ring_tmp_bo.bo->tbo.mem);
 }
 
 /* object have to be reserved */
@@ -633,7 +631,7 @@ int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
 	/* map the ib pool buffer at 0 in virtual address space, set
 	 * read only
 	 */
-	r = radeon_vm_bo_add(rdev, vm, rdev->ib_pool.sa_manager.bo, 0,
+	r = radeon_vm_bo_add(rdev, vm, rdev->ring_tmp_bo.bo, 0,
 			     RADEON_VM_PAGE_READABLE | RADEON_VM_PAGE_SNOOPED);
 	return r;
 }
@@ -650,12 +648,12 @@ void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm)
 	radeon_mutex_unlock(&rdev->cs_mutex);
 
 	/* remove all bo */
-	r = radeon_bo_reserve(rdev->ib_pool.sa_manager.bo, false);
+	r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
 	if (!r) {
-		bo_va = radeon_bo_va(rdev->ib_pool.sa_manager.bo, vm);
+		bo_va = radeon_bo_va(rdev->ring_tmp_bo.bo, vm);
 		list_del_init(&bo_va->bo_list);
 		list_del_init(&bo_va->vm_list);
-		radeon_bo_unreserve(rdev->ib_pool.sa_manager.bo);
+		radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
 		kfree(bo_va);
 	}
 	if (!list_empty(&vm->va)) {
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index 0519b05968b5..f28bd4b7ef98 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -42,6 +42,8 @@ void radeon_gem_object_free(struct drm_gem_object *gobj)
 	struct radeon_bo *robj = gem_to_radeon_bo(gobj);
 
 	if (robj) {
+		if (robj->gem_base.import_attach)
+			drm_prime_gem_destroy(&robj->gem_base, robj->tbo.sg);
 		radeon_bo_unref(&robj);
 	}
 }
@@ -59,7 +61,7 @@ int radeon_gem_object_create(struct radeon_device *rdev, int size,
 	if (alignment < PAGE_SIZE) {
 		alignment = PAGE_SIZE;
 	}
-	r = radeon_bo_create(rdev, size, alignment, kernel, initial_domain, &robj);
+	r = radeon_bo_create(rdev, size, alignment, kernel, initial_domain, NULL, &robj);
 	if (r) {
 		if (r != -ERESTARTSYS)
 			DRM_ERROR("Failed to allocate GEM object (%d, %d, %u, %d)\n",
@@ -154,6 +156,17 @@ void radeon_gem_object_close(struct drm_gem_object *obj,
 	radeon_bo_unreserve(rbo);
 }
 
+static int radeon_gem_handle_lockup(struct radeon_device *rdev, int r)
+{
+	if (r == -EDEADLK) {
+		radeon_mutex_lock(&rdev->cs_mutex);
+		r = radeon_gpu_reset(rdev);
+		if (!r)
+			r = -EAGAIN;
+		radeon_mutex_unlock(&rdev->cs_mutex);
+	}
+	return r;
+}
 
 /*
  * GEM ioctls.
@@ -210,12 +223,14 @@ int radeon_gem_create_ioctl(struct drm_device *dev, void *data,
 					args->initial_domain, false,
 					false, &gobj);
 	if (r) {
+		r = radeon_gem_handle_lockup(rdev, r);
 		return r;
 	}
 	r = drm_gem_handle_create(filp, gobj, &handle);
 	/* drop reference from allocate - handle holds it now */
 	drm_gem_object_unreference_unlocked(gobj);
 	if (r) {
+		r = radeon_gem_handle_lockup(rdev, r);
 		return r;
 	}
 	args->handle = handle;
@@ -245,6 +260,7 @@ int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	r = radeon_gem_set_domain(gobj, args->read_domains, args->write_domain);
 
 	drm_gem_object_unreference_unlocked(gobj);
+	r = radeon_gem_handle_lockup(robj->rdev, r);
 	return r;
 }
 
@@ -301,6 +317,7 @@ int radeon_gem_busy_ioctl(struct drm_device *dev, void *data,
 		break;
 	}
 	drm_gem_object_unreference_unlocked(gobj);
+	r = radeon_gem_handle_lockup(robj->rdev, r);
 	return r;
 }
 
@@ -322,6 +339,7 @@ int radeon_gem_wait_idle_ioctl(struct drm_device *dev, void *data,
 	if (robj->rdev->asic->ioctl_wait_idle)
 		robj->rdev->asic->ioctl_wait_idle(robj->rdev, robj);
 	drm_gem_object_unreference_unlocked(gobj);
+	r = radeon_gem_handle_lockup(robj->rdev, r);
 	return r;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_irq_kms.c b/drivers/gpu/drm/radeon/radeon_irq_kms.c
index 65060b77c805..5df58d1aba06 100644
--- a/drivers/gpu/drm/radeon/radeon_irq_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_irq_kms.c
@@ -73,6 +73,7 @@ void radeon_driver_irq_preinstall_kms(struct drm_device *dev)
 	for (i = 0; i < RADEON_MAX_CRTCS; i++) {
 		rdev->irq.crtc_vblank_int[i] = false;
 		rdev->irq.pflip[i] = false;
+		rdev->irq.afmt[i] = false;
 	}
 	radeon_irq_set(rdev);
 	/* Clear bits */
@@ -108,6 +109,7 @@ void radeon_driver_irq_uninstall_kms(struct drm_device *dev)
 	for (i = 0; i < RADEON_MAX_CRTCS; i++) {
 		rdev->irq.crtc_vblank_int[i] = false;
 		rdev->irq.pflip[i] = false;
+		rdev->irq.afmt[i] = false;
 	}
 	radeon_irq_set(rdev);
 }
@@ -170,6 +172,7 @@ int radeon_irq_kms_init(struct radeon_device *rdev)
 	int r = 0;
 
 	INIT_WORK(&rdev->hotplug_work, radeon_hotplug_work_func);
+	INIT_WORK(&rdev->audio_work, r600_audio_update_hdmi);
 
 	spin_lock_init(&rdev->irq.sw_lock);
 	for (i = 0; i < rdev->num_crtc; i++)
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 3c2628b14d56..f1016a5820d1 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -57,8 +57,6 @@ int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 	}
 	dev->dev_private = (void *)rdev;
 
-	pci_set_master(dev->pdev);
-
 	/* update BUS flag */
 	if (drm_pci_device_is_agp(dev)) {
 		flags |= RADEON_IS_AGP;
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
index 42db254f6bb0..a0c82229e8f0 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
@@ -369,6 +369,7 @@ void radeon_legacy_backlight_init(struct radeon_encoder *radeon_encoder,
 		goto error;
 	}
 
+	memset(&props, 0, sizeof(props));
 	props.max_brightness = MAX_RADEON_LEVEL;
 	props.type = BACKLIGHT_RAW;
 	bd = backlight_device_register("radeon_bl", &drm_connector->kdev,
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index f7eb5d8b9fd3..5b10ffd7bb2f 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -210,6 +210,7 @@ enum radeon_connector_table {
 	CT_RN50_POWER,
 	CT_MAC_X800,
 	CT_MAC_G5_9600,
+	CT_SAM440EP
 };
 
 enum radeon_dvo_chip {
@@ -219,12 +220,20 @@ enum radeon_dvo_chip {
 
 struct radeon_fbdev;
 
+struct radeon_afmt {
+	bool enabled;
+	int offset;
+	bool last_buffer_filled_status;
+	int id;
+};
+
 struct radeon_mode_info {
 	struct atom_context *atom_context;
 	struct card_info *atom_card_info;
 	enum radeon_connector_table connector_table;
 	bool mode_config_initialized;
 	struct radeon_crtc *crtcs[6];
+	struct radeon_afmt *afmt[6];
 	/* DVI-I properties */
 	struct drm_property *coherent_mode_property;
 	/* DAC enable load detect */
@@ -363,6 +372,7 @@ struct radeon_encoder_atom_dig {
 	int dpms_mode;
 	uint8_t backlight_level;
 	int panel_mode;
+	struct radeon_afmt *afmt;
 };
 
 struct radeon_encoder_atom_dac {
@@ -384,10 +394,6 @@ struct radeon_encoder {
 	struct drm_display_mode native_mode;
 	void *enc_priv;
 	int audio_polling_active;
-	int hdmi_offset;
-	int hdmi_config_offset;
-	int hdmi_audio_workaround;
-	int hdmi_buffer_status;
 	bool is_ext_encoder;
 	u16 caps;
 };
@@ -476,6 +482,7 @@ extern u16 radeon_encoder_get_dp_bridge_encoder_id(struct drm_encoder *encoder);
 extern u16 radeon_connector_encoder_get_dp_bridge_encoder_id(struct drm_connector *connector);
 extern bool radeon_connector_encoder_is_hbr2(struct drm_connector *connector);
 extern bool radeon_connector_is_dp12_capable(struct drm_connector *connector);
+extern int radeon_get_monitor_bpc(struct drm_connector *connector);
 
 extern void radeon_connector_hotplug(struct drm_connector *connector);
 extern int radeon_dp_mode_valid_helper(struct drm_connector *connector,
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index df6a4dbd93f8..830f1a7b486f 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -104,7 +104,7 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 
 int radeon_bo_create(struct radeon_device *rdev,
 		     unsigned long size, int byte_align, bool kernel, u32 domain,
-		     struct radeon_bo **bo_ptr)
+		     struct sg_table *sg, struct radeon_bo **bo_ptr)
 {
 	struct radeon_bo *bo;
 	enum ttm_bo_type type;
@@ -120,6 +120,8 @@ int radeon_bo_create(struct radeon_device *rdev,
 	}
 	if (kernel) {
 		type = ttm_bo_type_kernel;
+	} else if (sg) {
+		type = ttm_bo_type_sg;
 	} else {
 		type = ttm_bo_type_device;
 	}
@@ -155,7 +157,7 @@ retry:
 	mutex_lock(&rdev->vram_mutex);
 	r = ttm_bo_init(&rdev->mman.bdev, &bo->tbo, size, type,
 			&bo->placement, page_align, 0, !kernel, NULL,
-			acc_size, &radeon_ttm_bo_destroy);
+			acc_size, sg, &radeon_ttm_bo_destroy);
 	mutex_unlock(&rdev->vram_mutex);
 	if (unlikely(r != 0)) {
 		if (r != -ERESTARTSYS) {
diff --git a/drivers/gpu/drm/radeon/radeon_object.h b/drivers/gpu/drm/radeon/radeon_object.h
index f9104be88d7c..17fb99f177cf 100644
--- a/drivers/gpu/drm/radeon/radeon_object.h
+++ b/drivers/gpu/drm/radeon/radeon_object.h
@@ -111,9 +111,10 @@ extern int radeon_bo_wait(struct radeon_bo *bo, u32 *mem_type,
 			  bool no_wait);
 
 extern int radeon_bo_create(struct radeon_device *rdev,
-				unsigned long size, int byte_align,
-				bool kernel, u32 domain,
-				struct radeon_bo **bo_ptr);
+			    unsigned long size, int byte_align,
+			    bool kernel, u32 domain,
+			    struct sg_table *sg,
+			    struct radeon_bo **bo_ptr);
 extern int radeon_bo_kmap(struct radeon_bo *bo, void **ptr);
 extern void radeon_bo_kunmap(struct radeon_bo *bo);
 extern void radeon_bo_unref(struct radeon_bo **bo);
@@ -146,6 +147,17 @@ extern struct radeon_bo_va *radeon_bo_va(struct radeon_bo *rbo,
 /*
  * sub allocation
  */
+
+static inline uint64_t radeon_sa_bo_gpu_addr(struct radeon_sa_bo *sa_bo)
+{
+	return sa_bo->manager->gpu_addr + sa_bo->soffset;
+}
+
+static inline void * radeon_sa_bo_cpu_addr(struct radeon_sa_bo *sa_bo)
+{
+	return sa_bo->manager->cpu_ptr + sa_bo->soffset;
+}
+
 extern int radeon_sa_bo_manager_init(struct radeon_device *rdev,
 				     struct radeon_sa_manager *sa_manager,
 				     unsigned size, u32 domain);
@@ -157,9 +169,15 @@ extern int radeon_sa_bo_manager_suspend(struct radeon_device *rdev,
 					struct radeon_sa_manager *sa_manager);
 extern int radeon_sa_bo_new(struct radeon_device *rdev,
 			    struct radeon_sa_manager *sa_manager,
-			    struct radeon_sa_bo *sa_bo,
-			    unsigned size, unsigned align);
+			    struct radeon_sa_bo **sa_bo,
+			    unsigned size, unsigned align, bool block);
 extern void radeon_sa_bo_free(struct radeon_device *rdev,
-			      struct radeon_sa_bo *sa_bo);
+			      struct radeon_sa_bo **sa_bo,
+			      struct radeon_fence *fence);
+#if defined(CONFIG_DEBUG_FS)
+extern void radeon_sa_bo_dump_debug_info(struct radeon_sa_manager *sa_manager,
+					 struct seq_file *m);
+#endif
+
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index caa55d68f319..08825548ee69 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -252,10 +252,7 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 
 	mutex_lock(&rdev->ddev->struct_mutex);
 	mutex_lock(&rdev->vram_mutex);
-	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		if (rdev->ring[i].ring_obj)
-			mutex_lock(&rdev->ring[i].mutex);
-	}
+	mutex_lock(&rdev->ring_lock);
 
 	/* gui idle int has issues on older chips it seems */
 	if (rdev->family >= CHIP_R600) {
@@ -273,13 +270,7 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 	} else {
 		struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 		if (ring->ready) {
-			struct radeon_fence *fence;
-			radeon_ring_alloc(rdev, ring, 64);
-			radeon_fence_create(rdev, &fence, radeon_ring_index(rdev, ring));
-			radeon_fence_emit(rdev, fence);
-			radeon_ring_commit(rdev, ring);
-			radeon_fence_wait(fence, false);
-			radeon_fence_unref(&fence);
+			radeon_fence_wait_empty_locked(rdev, RADEON_RING_TYPE_GFX_INDEX);
 		}
 	}
 	radeon_unmap_vram_bos(rdev);
@@ -311,10 +302,7 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 
 	rdev->pm.dynpm_planned_action = DYNPM_ACTION_NONE;
 
-	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		if (rdev->ring[i].ring_obj)
-			mutex_unlock(&rdev->ring[i].mutex);
-	}
+	mutex_unlock(&rdev->ring_lock);
 	mutex_unlock(&rdev->vram_mutex);
 	mutex_unlock(&rdev->ddev->struct_mutex);
 }
diff --git a/drivers/gpu/drm/radeon/radeon_prime.c b/drivers/gpu/drm/radeon/radeon_prime.c
new file mode 100644
index 000000000000..b8f835d8ecb4
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_prime.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2012 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * based on nouveau_prime.c
+ *
+ * Authors: Alex Deucher
+ */
+#include "drmP.h"
+#include "drm.h"
+
+#include "radeon.h"
+#include "radeon_drm.h"
+
+#include <linux/dma-buf.h>
+
+static struct sg_table *radeon_gem_map_dma_buf(struct dma_buf_attachment *attachment,
+					       enum dma_data_direction dir)
+{
+	struct radeon_bo *bo = attachment->dmabuf->priv;
+	struct drm_device *dev = bo->rdev->ddev;
+	int npages = bo->tbo.num_pages;
+	struct sg_table *sg;
+	int nents;
+
+	mutex_lock(&dev->struct_mutex);
+	sg = drm_prime_pages_to_sg(bo->tbo.ttm->pages, npages);
+	nents = dma_map_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	mutex_unlock(&dev->struct_mutex);
+	return sg;
+}
+
+static void radeon_gem_unmap_dma_buf(struct dma_buf_attachment *attachment,
+				     struct sg_table *sg, enum dma_data_direction dir)
+{
+	dma_unmap_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	sg_free_table(sg);
+	kfree(sg);
+}
+
+static void radeon_gem_dmabuf_release(struct dma_buf *dma_buf)
+{
+	struct radeon_bo *bo = dma_buf->priv;
+
+	if (bo->gem_base.export_dma_buf == dma_buf) {
+		DRM_ERROR("unreference dmabuf %p\n", &bo->gem_base);
+		bo->gem_base.export_dma_buf = NULL;
+		drm_gem_object_unreference_unlocked(&bo->gem_base);
+	}
+}
+
+static void *radeon_gem_kmap_atomic(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void radeon_gem_kunmap_atomic(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+static void *radeon_gem_kmap(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void radeon_gem_kunmap(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+
+const static struct dma_buf_ops radeon_dmabuf_ops =  {
+	.map_dma_buf = radeon_gem_map_dma_buf,
+	.unmap_dma_buf = radeon_gem_unmap_dma_buf,
+	.release = radeon_gem_dmabuf_release,
+	.kmap = radeon_gem_kmap,
+	.kmap_atomic = radeon_gem_kmap_atomic,
+	.kunmap = radeon_gem_kunmap,
+	.kunmap_atomic = radeon_gem_kunmap_atomic,
+};
+
+static int radeon_prime_create(struct drm_device *dev,
+			       size_t size,
+			       struct sg_table *sg,
+			       struct radeon_bo **pbo)
+{
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_bo *bo;
+	int ret;
+
+	ret = radeon_bo_create(rdev, size, PAGE_SIZE, false,
+			       RADEON_GEM_DOMAIN_GTT, sg, pbo);
+	if (ret)
+		return ret;
+	bo = *pbo;
+	bo->gem_base.driver_private = bo;
+
+	mutex_lock(&rdev->gem.mutex);
+	list_add_tail(&bo->list, &rdev->gem.objects);
+	mutex_unlock(&rdev->gem.mutex);
+
+	return 0;
+}
+
+struct dma_buf *radeon_gem_prime_export(struct drm_device *dev,
+					struct drm_gem_object *obj,
+					int flags)
+{
+	struct radeon_bo *bo = gem_to_radeon_bo(obj);
+	int ret = 0;
+
+	/* pin buffer into GTT */
+	ret = radeon_bo_pin(bo, RADEON_GEM_DOMAIN_GTT, NULL);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return dma_buf_export(bo, &radeon_dmabuf_ops, obj->size, flags);
+}
+
+struct drm_gem_object *radeon_gem_prime_import(struct drm_device *dev,
+					       struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct radeon_bo *bo;
+	int ret;
+
+	if (dma_buf->ops == &radeon_dmabuf_ops) {
+		bo = dma_buf->priv;
+		if (bo->gem_base.dev == dev) {
+			drm_gem_object_reference(&bo->gem_base);
+			return &bo->gem_base;
+		}
+	}
+
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_CAST(attach);
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	ret = radeon_prime_create(dev, dma_buf->size, sg, &bo);
+	if (ret)
+		goto fail_unmap;
+
+	bo->gem_base.import_attach = attach;
+
+	return &bo->gem_base;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index cc33b3d7c33b..493a7be75306 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -24,6 +24,7 @@
  * Authors: Dave Airlie
  *          Alex Deucher
  *          Jerome Glisse
+ *          Christian König
  */
 #include <linux/seq_file.h>
 #include <linux/slab.h>
@@ -33,8 +34,10 @@
 #include "radeon.h"
 #include "atom.h"
 
-int radeon_debugfs_ib_init(struct radeon_device *rdev);
-int radeon_debugfs_ring_init(struct radeon_device *rdev);
+/*
+ * IB.
+ */
+int radeon_debugfs_sa_init(struct radeon_device *rdev);
 
 u32 radeon_get_ib_value(struct radeon_cs_parser *p, int idx)
 {
@@ -61,123 +64,37 @@ u32 radeon_get_ib_value(struct radeon_cs_parser *p, int idx)
 	return idx_value;
 }
 
-void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
-{
-#if DRM_DEBUG_CODE
-	if (ring->count_dw <= 0) {
-		DRM_ERROR("radeon: writting more dword to ring than expected !\n");
-	}
-#endif
-	ring->ring[ring->wptr++] = v;
-	ring->wptr &= ring->ptr_mask;
-	ring->count_dw--;
-	ring->ring_free_dw--;
-}
-
-/*
- * IB.
- */
-bool radeon_ib_try_free(struct radeon_device *rdev, struct radeon_ib *ib)
-{
-	bool done = false;
-
-	/* only free ib which have been emited */
-	if (ib->fence && ib->fence->emitted) {
-		if (radeon_fence_signaled(ib->fence)) {
-			radeon_fence_unref(&ib->fence);
-			radeon_sa_bo_free(rdev, &ib->sa_bo);
-			done = true;
-		}
-	}
-	return done;
-}
-
 int radeon_ib_get(struct radeon_device *rdev, int ring,
-		  struct radeon_ib **ib, unsigned size)
+		  struct radeon_ib *ib, unsigned size)
 {
-	struct radeon_fence *fence;
-	unsigned cretry = 0;
-	int r = 0, i, idx;
-
-	*ib = NULL;
-	/* align size on 256 bytes */
-	size = ALIGN(size, 256);
+	int r;
 
-	r = radeon_fence_create(rdev, &fence, ring);
+	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo, &ib->sa_bo, size, 256, true);
 	if (r) {
-		dev_err(rdev->dev, "failed to create fence for new IB\n");
+		dev_err(rdev->dev, "failed to get a new IB (%d)\n", r);
 		return r;
 	}
-
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	idx = rdev->ib_pool.head_id;
-retry:
-	if (cretry > 5) {
-		dev_err(rdev->dev, "failed to get an ib after 5 retry\n");
-		radeon_mutex_unlock(&rdev->ib_pool.mutex);
-		radeon_fence_unref(&fence);
-		return -ENOMEM;
-	}
-	cretry++;
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		radeon_ib_try_free(rdev, &rdev->ib_pool.ibs[idx]);
-		if (rdev->ib_pool.ibs[idx].fence == NULL) {
-			r = radeon_sa_bo_new(rdev, &rdev->ib_pool.sa_manager,
-					     &rdev->ib_pool.ibs[idx].sa_bo,
-					     size, 256);
-			if (!r) {
-				*ib = &rdev->ib_pool.ibs[idx];
-				(*ib)->ptr = rdev->ib_pool.sa_manager.cpu_ptr;
-				(*ib)->ptr += ((*ib)->sa_bo.offset >> 2);
-				(*ib)->gpu_addr = rdev->ib_pool.sa_manager.gpu_addr;
-				(*ib)->gpu_addr += (*ib)->sa_bo.offset;
-				(*ib)->fence = fence;
-				(*ib)->vm_id = 0;
-				(*ib)->is_const_ib = false;
-				/* ib are most likely to be allocated in a ring fashion
-				 * thus rdev->ib_pool.head_id should be the id of the
-				 * oldest ib
-				 */
-				rdev->ib_pool.head_id = (1 + idx);
-				rdev->ib_pool.head_id &= (RADEON_IB_POOL_SIZE - 1);
-				radeon_mutex_unlock(&rdev->ib_pool.mutex);
-				return 0;
-			}
-		}
-		idx = (idx + 1) & (RADEON_IB_POOL_SIZE - 1);
-	}
-	/* this should be rare event, ie all ib scheduled none signaled yet.
-	 */
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		if (rdev->ib_pool.ibs[idx].fence && rdev->ib_pool.ibs[idx].fence->emitted) {
-			r = radeon_fence_wait(rdev->ib_pool.ibs[idx].fence, false);
-			if (!r) {
-				goto retry;
-			}
-			/* an error happened */
-			break;
-		}
-		idx = (idx + 1) & (RADEON_IB_POOL_SIZE - 1);
+	r = radeon_fence_create(rdev, &ib->fence, ring);
+	if (r) {
+		dev_err(rdev->dev, "failed to create fence for new IB (%d)\n", r);
+		radeon_sa_bo_free(rdev, &ib->sa_bo, NULL);
+		return r;
 	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
-	radeon_fence_unref(&fence);
-	return r;
+
+	ib->ptr = radeon_sa_bo_cpu_addr(ib->sa_bo);
+	ib->gpu_addr = radeon_sa_bo_gpu_addr(ib->sa_bo);
+	ib->vm_id = 0;
+	ib->is_const_ib = false;
+	ib->semaphore = NULL;
+
+	return 0;
 }
 
-void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib **ib)
+void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	struct radeon_ib *tmp = *ib;
-
-	*ib = NULL;
-	if (tmp == NULL) {
-		return;
-	}
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	if (tmp->fence && !tmp->fence->emitted) {
-		radeon_sa_bo_free(rdev, &tmp->sa_bo);
-		radeon_fence_unref(&tmp->fence);
-	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
+	radeon_semaphore_free(rdev, ib->semaphore, ib->fence);
+	radeon_sa_bo_free(rdev, &ib->sa_bo, ib->fence);
+	radeon_fence_unref(&ib->fence);
 }
 
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
@@ -187,14 +104,14 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
 
 	if (!ib->length_dw || !ring->ready) {
 		/* TODO: Nothings in the ib we should report. */
-		DRM_ERROR("radeon: couldn't schedule IB(%u).\n", ib->idx);
+		dev_err(rdev->dev, "couldn't schedule ib\n");
 		return -EINVAL;
 	}
 
 	/* 64 dwords should be enough for fence too */
 	r = radeon_ring_lock(rdev, ring, 64);
 	if (r) {
-		DRM_ERROR("radeon: scheduling IB failed (%d).\n", r);
+		dev_err(rdev->dev, "scheduling IB failed (%d).\n", r);
 		return r;
 	}
 	radeon_ring_ib_execute(rdev, ib->fence->ring, ib);
@@ -205,74 +122,90 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
 
 int radeon_ib_pool_init(struct radeon_device *rdev)
 {
-	struct radeon_sa_manager tmp;
-	int i, r;
+	int r;
 
-	r = radeon_sa_bo_manager_init(rdev, &tmp,
+	if (rdev->ib_pool_ready) {
+		return 0;
+	}
+	r = radeon_sa_bo_manager_init(rdev, &rdev->ring_tmp_bo,
 				      RADEON_IB_POOL_SIZE*64*1024,
 				      RADEON_GEM_DOMAIN_GTT);
 	if (r) {
 		return r;
 	}
-
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	if (rdev->ib_pool.ready) {
-		radeon_mutex_unlock(&rdev->ib_pool.mutex);
-		radeon_sa_bo_manager_fini(rdev, &tmp);
-		return 0;
-	}
-
-	rdev->ib_pool.sa_manager = tmp;
-	INIT_LIST_HEAD(&rdev->ib_pool.sa_manager.sa_bo);
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		rdev->ib_pool.ibs[i].fence = NULL;
-		rdev->ib_pool.ibs[i].idx = i;
-		rdev->ib_pool.ibs[i].length_dw = 0;
-		INIT_LIST_HEAD(&rdev->ib_pool.ibs[i].sa_bo.list);
-	}
-	rdev->ib_pool.head_id = 0;
-	rdev->ib_pool.ready = true;
-	DRM_INFO("radeon: ib pool ready.\n");
-
-	if (radeon_debugfs_ib_init(rdev)) {
-		DRM_ERROR("Failed to register debugfs file for IB !\n");
-	}
-	if (radeon_debugfs_ring_init(rdev)) {
-		DRM_ERROR("Failed to register debugfs file for rings !\n");
+	rdev->ib_pool_ready = true;
+	if (radeon_debugfs_sa_init(rdev)) {
+		dev_err(rdev->dev, "failed to register debugfs file for SA\n");
 	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
 	return 0;
 }
 
 void radeon_ib_pool_fini(struct radeon_device *rdev)
 {
-	unsigned i;
-
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	if (rdev->ib_pool.ready) {
-		for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-			radeon_sa_bo_free(rdev, &rdev->ib_pool.ibs[i].sa_bo);
-			radeon_fence_unref(&rdev->ib_pool.ibs[i].fence);
-		}
-		radeon_sa_bo_manager_fini(rdev, &rdev->ib_pool.sa_manager);
-		rdev->ib_pool.ready = false;
+	if (rdev->ib_pool_ready) {
+		radeon_sa_bo_manager_fini(rdev, &rdev->ring_tmp_bo);
+		rdev->ib_pool_ready = false;
 	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
 }
 
 int radeon_ib_pool_start(struct radeon_device *rdev)
 {
-	return radeon_sa_bo_manager_start(rdev, &rdev->ib_pool.sa_manager);
+	return radeon_sa_bo_manager_start(rdev, &rdev->ring_tmp_bo);
 }
 
 int radeon_ib_pool_suspend(struct radeon_device *rdev)
 {
-	return radeon_sa_bo_manager_suspend(rdev, &rdev->ib_pool.sa_manager);
+	return radeon_sa_bo_manager_suspend(rdev, &rdev->ring_tmp_bo);
+}
+
+int radeon_ib_ring_tests(struct radeon_device *rdev)
+{
+	unsigned i;
+	int r;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		struct radeon_ring *ring = &rdev->ring[i];
+
+		if (!ring->ready)
+			continue;
+
+		r = radeon_ib_test(rdev, i, ring);
+		if (r) {
+			ring->ready = false;
+
+			if (i == RADEON_RING_TYPE_GFX_INDEX) {
+				/* oh, oh, that's really bad */
+				DRM_ERROR("radeon: failed testing IB on GFX ring (%d).\n", r);
+		                rdev->accel_working = false;
+				return r;
+
+			} else {
+				/* still not good, but we can live with it */
+				DRM_ERROR("radeon: failed testing IB on ring %d (%d).\n", i, r);
+			}
+		}
+	}
+	return 0;
 }
 
 /*
  * Ring.
  */
+int radeon_debugfs_ring_init(struct radeon_device *rdev, struct radeon_ring *ring);
+
+void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
+{
+#if DRM_DEBUG_CODE
+	if (ring->count_dw <= 0) {
+		DRM_ERROR("radeon: writting more dword to ring than expected !\n");
+	}
+#endif
+	ring->ring[ring->wptr++] = v;
+	ring->wptr &= ring->ptr_mask;
+	ring->count_dw--;
+	ring->ring_free_dw--;
+}
+
 int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	/* r1xx-r5xx only has CP ring */
@@ -319,7 +252,7 @@ int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *ring, unsi
 		if (ndw < ring->ring_free_dw) {
 			break;
 		}
-		r = radeon_fence_wait_next(rdev, radeon_ring_index(rdev, ring));
+		r = radeon_fence_wait_next_locked(rdev, radeon_ring_index(rdev, ring));
 		if (r)
 			return r;
 	}
@@ -332,10 +265,10 @@ int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 {
 	int r;
 
-	mutex_lock(&ring->mutex);
+	mutex_lock(&rdev->ring_lock);
 	r = radeon_ring_alloc(rdev, ring, ndw);
 	if (r) {
-		mutex_unlock(&ring->mutex);
+		mutex_unlock(&rdev->ring_lock);
 		return r;
 	}
 	return 0;
@@ -360,13 +293,85 @@ void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	radeon_ring_commit(rdev, ring);
-	mutex_unlock(&ring->mutex);
+	mutex_unlock(&rdev->ring_lock);
 }
 
-void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *ring)
+void radeon_ring_undo(struct radeon_ring *ring)
 {
 	ring->wptr = ring->wptr_old;
-	mutex_unlock(&ring->mutex);
+}
+
+void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	radeon_ring_undo(ring);
+	mutex_unlock(&rdev->ring_lock);
+}
+
+void radeon_ring_force_activity(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	int r;
+
+	radeon_ring_free_size(rdev, ring);
+	if (ring->rptr == ring->wptr) {
+		r = radeon_ring_alloc(rdev, ring, 1);
+		if (!r) {
+			radeon_ring_write(ring, ring->nop);
+			radeon_ring_commit(rdev, ring);
+		}
+	}
+}
+
+void radeon_ring_lockup_update(struct radeon_ring *ring)
+{
+	ring->last_rptr = ring->rptr;
+	ring->last_activity = jiffies;
+}
+
+/**
+ * radeon_ring_test_lockup() - check if ring is lockedup by recording information
+ * @rdev:       radeon device structure
+ * @ring:       radeon_ring structure holding ring information
+ *
+ * We don't need to initialize the lockup tracking information as we will either
+ * have CP rptr to a different value of jiffies wrap around which will force
+ * initialization of the lockup tracking informations.
+ *
+ * A possible false positivie is if we get call after while and last_cp_rptr ==
+ * the current CP rptr, even if it's unlikely it might happen. To avoid this
+ * if the elapsed time since last call is bigger than 2 second than we return
+ * false and update the tracking information. Due to this the caller must call
+ * radeon_ring_test_lockup several time in less than 2sec for lockup to be reported
+ * the fencing code should be cautious about that.
+ *
+ * Caller should write to the ring to force CP to do something so we don't get
+ * false positive when CP is just gived nothing to do.
+ *
+ **/
+bool radeon_ring_test_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	unsigned long cjiffies, elapsed;
+	uint32_t rptr;
+
+	cjiffies = jiffies;
+	if (!time_after(cjiffies, ring->last_activity)) {
+		/* likely a wrap around */
+		radeon_ring_lockup_update(ring);
+		return false;
+	}
+	rptr = RREG32(ring->rptr_reg);
+	ring->rptr = (rptr & ring->ptr_reg_mask) >> ring->ptr_reg_shift;
+	if (ring->rptr != ring->last_rptr) {
+		/* CP is still working no lockup */
+		radeon_ring_lockup_update(ring);
+		return false;
+	}
+	elapsed = jiffies_to_msecs(cjiffies - ring->last_activity);
+	if (radeon_lockup_timeout && elapsed >= radeon_lockup_timeout) {
+		dev_err(rdev->dev, "GPU lockup CP stall for more than %lumsec\n", elapsed);
+		return true;
+	}
+	/* give a chance to the GPU ... */
+	return false;
 }
 
 int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ring_size,
@@ -385,8 +390,8 @@ int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 	/* Allocate ring buffer */
 	if (ring->ring_obj == NULL) {
 		r = radeon_bo_create(rdev, ring->ring_size, PAGE_SIZE, true,
-					RADEON_GEM_DOMAIN_GTT,
-					&ring->ring_obj);
+				     RADEON_GEM_DOMAIN_GTT,
+				     NULL, &ring->ring_obj);
 		if (r) {
 			dev_err(rdev->dev, "(%d) ring create failed\n", r);
 			return r;
@@ -411,6 +416,9 @@ int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 	}
 	ring->ptr_mask = (ring->ring_size / 4) - 1;
 	ring->ring_free_dw = ring->ring_size / 4;
+	if (radeon_debugfs_ring_init(rdev, ring)) {
+		DRM_ERROR("Failed to register debugfs file for rings !\n");
+	}
 	return 0;
 }
 
@@ -419,11 +427,12 @@ void radeon_ring_fini(struct radeon_device *rdev, struct radeon_ring *ring)
 	int r;
 	struct radeon_bo *ring_obj;
 
-	mutex_lock(&ring->mutex);
+	mutex_lock(&rdev->ring_lock);
 	ring_obj = ring->ring_obj;
+	ring->ready = false;
 	ring->ring = NULL;
 	ring->ring_obj = NULL;
-	mutex_unlock(&ring->mutex);
+	mutex_unlock(&rdev->ring_lock);
 
 	if (ring_obj) {
 		r = radeon_bo_reserve(ring_obj, false);
@@ -476,59 +485,48 @@ static struct drm_info_list radeon_debugfs_ring_info_list[] = {
 	{"radeon_ring_cp2", radeon_debugfs_ring_info, 0, &cayman_ring_type_cp2_index},
 };
 
-static int radeon_debugfs_ib_info(struct seq_file *m, void *data)
+static int radeon_debugfs_sa_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	struct radeon_ib *ib = &rdev->ib_pool.ibs[*((unsigned*)node->info_ent->data)];
-	unsigned i;
 
-	if (ib == NULL) {
-		return 0;
-	}
-	seq_printf(m, "IB %04u\n", ib->idx);
-	seq_printf(m, "IB fence %p\n", ib->fence);
-	seq_printf(m, "IB size %05u dwords\n", ib->length_dw);
-	for (i = 0; i < ib->length_dw; i++) {
-		seq_printf(m, "[%05u]=0x%08X\n", i, ib->ptr[i]);
-	}
+	radeon_sa_bo_dump_debug_info(&rdev->ring_tmp_bo, m);
+
 	return 0;
+
 }
 
-static struct drm_info_list radeon_debugfs_ib_list[RADEON_IB_POOL_SIZE];
-static char radeon_debugfs_ib_names[RADEON_IB_POOL_SIZE][32];
-static unsigned radeon_debugfs_ib_idx[RADEON_IB_POOL_SIZE];
+static struct drm_info_list radeon_debugfs_sa_list[] = {
+        {"radeon_sa_info", &radeon_debugfs_sa_info, 0, NULL},
+};
+
 #endif
 
-int radeon_debugfs_ring_init(struct radeon_device *rdev)
+int radeon_debugfs_ring_init(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 #if defined(CONFIG_DEBUG_FS)
-	if (rdev->family >= CHIP_CAYMAN)
-		return radeon_debugfs_add_files(rdev, radeon_debugfs_ring_info_list,
-						ARRAY_SIZE(radeon_debugfs_ring_info_list));
-	else
-		return radeon_debugfs_add_files(rdev, radeon_debugfs_ring_info_list, 1);
-#else
-	return 0;
+	unsigned i;
+	for (i = 0; i < ARRAY_SIZE(radeon_debugfs_ring_info_list); ++i) {
+		struct drm_info_list *info = &radeon_debugfs_ring_info_list[i];
+		int ridx = *(int*)radeon_debugfs_ring_info_list[i].data;
+		unsigned r;
+
+		if (&rdev->ring[ridx] != ring)
+			continue;
+
+		r = radeon_debugfs_add_files(rdev, info, 1);
+		if (r)
+			return r;
+	}
 #endif
+	return 0;
 }
 
-int radeon_debugfs_ib_init(struct radeon_device *rdev)
+int radeon_debugfs_sa_init(struct radeon_device *rdev)
 {
 #if defined(CONFIG_DEBUG_FS)
-	unsigned i;
-
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		sprintf(radeon_debugfs_ib_names[i], "radeon_ib_%04u", i);
-		radeon_debugfs_ib_idx[i] = i;
-		radeon_debugfs_ib_list[i].name = radeon_debugfs_ib_names[i];
-		radeon_debugfs_ib_list[i].show = &radeon_debugfs_ib_info;
-		radeon_debugfs_ib_list[i].driver_features = 0;
-		radeon_debugfs_ib_list[i].data = &radeon_debugfs_ib_idx[i];
-	}
-	return radeon_debugfs_add_files(rdev, radeon_debugfs_ib_list,
-					RADEON_IB_POOL_SIZE);
+	return radeon_debugfs_add_files(rdev, radeon_debugfs_sa_list, 1);
 #else
 	return 0;
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_sa.c b/drivers/gpu/drm/radeon/radeon_sa.c
index 4cce47e7dc0d..32059b745728 100644
--- a/drivers/gpu/drm/radeon/radeon_sa.c
+++ b/drivers/gpu/drm/radeon/radeon_sa.c
@@ -27,23 +27,45 @@
  * Authors:
  *    Jerome Glisse <glisse@freedesktop.org>
  */
+/* Algorithm:
+ *
+ * We store the last allocated bo in "hole", we always try to allocate
+ * after the last allocated bo. Principle is that in a linear GPU ring
+ * progression was is after last is the oldest bo we allocated and thus
+ * the first one that should no longer be in use by the GPU.
+ *
+ * If it's not the case we skip over the bo after last to the closest
+ * done bo if such one exist. If none exist and we are not asked to
+ * block we report failure to allocate.
+ *
+ * If we are asked to block we wait on all the oldest fence of all
+ * rings. We just wait for any of those fence to complete.
+ */
 #include "drmP.h"
 #include "drm.h"
 #include "radeon.h"
 
+static void radeon_sa_bo_remove_locked(struct radeon_sa_bo *sa_bo);
+static void radeon_sa_bo_try_free(struct radeon_sa_manager *sa_manager);
+
 int radeon_sa_bo_manager_init(struct radeon_device *rdev,
 			      struct radeon_sa_manager *sa_manager,
 			      unsigned size, u32 domain)
 {
-	int r;
+	int i, r;
 
+	spin_lock_init(&sa_manager->lock);
 	sa_manager->bo = NULL;
 	sa_manager->size = size;
 	sa_manager->domain = domain;
-	INIT_LIST_HEAD(&sa_manager->sa_bo);
+	sa_manager->hole = &sa_manager->olist;
+	INIT_LIST_HEAD(&sa_manager->olist);
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		INIT_LIST_HEAD(&sa_manager->flist[i]);
+	}
 
 	r = radeon_bo_create(rdev, size, RADEON_GPU_PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_CPU, &sa_manager->bo);
+			     RADEON_GEM_DOMAIN_CPU, NULL, &sa_manager->bo);
 	if (r) {
 		dev_err(rdev->dev, "(%d) failed to allocate bo for manager\n", r);
 		return r;
@@ -57,11 +79,15 @@ void radeon_sa_bo_manager_fini(struct radeon_device *rdev,
 {
 	struct radeon_sa_bo *sa_bo, *tmp;
 
-	if (!list_empty(&sa_manager->sa_bo)) {
-		dev_err(rdev->dev, "sa_manager is not empty, clearing anyway\n");
+	if (!list_empty(&sa_manager->olist)) {
+		sa_manager->hole = &sa_manager->olist,
+		radeon_sa_bo_try_free(sa_manager);
+		if (!list_empty(&sa_manager->olist)) {
+			dev_err(rdev->dev, "sa_manager is not empty, clearing anyway\n");
+		}
 	}
-	list_for_each_entry_safe(sa_bo, tmp, &sa_manager->sa_bo, list) {
-		list_del_init(&sa_bo->list);
+	list_for_each_entry_safe(sa_bo, tmp, &sa_manager->olist, olist) {
+		radeon_sa_bo_remove_locked(sa_bo);
 	}
 	radeon_bo_unref(&sa_manager->bo);
 	sa_manager->size = 0;
@@ -113,77 +139,248 @@ int radeon_sa_bo_manager_suspend(struct radeon_device *rdev,
 	return r;
 }
 
-/*
- * Principe is simple, we keep a list of sub allocation in offset
- * order (first entry has offset == 0, last entry has the highest
- * offset).
- *
- * When allocating new object we first check if there is room at
- * the end total_size - (last_object_offset + last_object_size) >=
- * alloc_size. If so we allocate new object there.
- *
- * When there is not enough room at the end, we start waiting for
- * each sub object until we reach object_offset+object_size >=
- * alloc_size, this object then become the sub object we return.
- *
- * Alignment can't be bigger than page size
- */
+static void radeon_sa_bo_remove_locked(struct radeon_sa_bo *sa_bo)
+{
+	struct radeon_sa_manager *sa_manager = sa_bo->manager;
+	if (sa_manager->hole == &sa_bo->olist) {
+		sa_manager->hole = sa_bo->olist.prev;
+	}
+	list_del_init(&sa_bo->olist);
+	list_del_init(&sa_bo->flist);
+	radeon_fence_unref(&sa_bo->fence);
+	kfree(sa_bo);
+}
+
+static void radeon_sa_bo_try_free(struct radeon_sa_manager *sa_manager)
+{
+	struct radeon_sa_bo *sa_bo, *tmp;
+
+	if (sa_manager->hole->next == &sa_manager->olist)
+		return;
+
+	sa_bo = list_entry(sa_manager->hole->next, struct radeon_sa_bo, olist);
+	list_for_each_entry_safe_from(sa_bo, tmp, &sa_manager->olist, olist) {
+		if (sa_bo->fence == NULL || !radeon_fence_signaled(sa_bo->fence)) {
+			return;
+		}
+		radeon_sa_bo_remove_locked(sa_bo);
+	}
+}
+
+static inline unsigned radeon_sa_bo_hole_soffset(struct radeon_sa_manager *sa_manager)
+{
+	struct list_head *hole = sa_manager->hole;
+
+	if (hole != &sa_manager->olist) {
+		return list_entry(hole, struct radeon_sa_bo, olist)->eoffset;
+	}
+	return 0;
+}
+
+static inline unsigned radeon_sa_bo_hole_eoffset(struct radeon_sa_manager *sa_manager)
+{
+	struct list_head *hole = sa_manager->hole;
+
+	if (hole->next != &sa_manager->olist) {
+		return list_entry(hole->next, struct radeon_sa_bo, olist)->soffset;
+	}
+	return sa_manager->size;
+}
+
+static bool radeon_sa_bo_try_alloc(struct radeon_sa_manager *sa_manager,
+				   struct radeon_sa_bo *sa_bo,
+				   unsigned size, unsigned align)
+{
+	unsigned soffset, eoffset, wasted;
+
+	soffset = radeon_sa_bo_hole_soffset(sa_manager);
+	eoffset = radeon_sa_bo_hole_eoffset(sa_manager);
+	wasted = (align - (soffset % align)) % align;
+
+	if ((eoffset - soffset) >= (size + wasted)) {
+		soffset += wasted;
+
+		sa_bo->manager = sa_manager;
+		sa_bo->soffset = soffset;
+		sa_bo->eoffset = soffset + size;
+		list_add(&sa_bo->olist, sa_manager->hole);
+		INIT_LIST_HEAD(&sa_bo->flist);
+		sa_manager->hole = &sa_bo->olist;
+		return true;
+	}
+	return false;
+}
+
+static bool radeon_sa_bo_next_hole(struct radeon_sa_manager *sa_manager,
+				   struct radeon_fence **fences,
+				   unsigned *tries)
+{
+	struct radeon_sa_bo *best_bo = NULL;
+	unsigned i, soffset, best, tmp;
+
+	/* if hole points to the end of the buffer */
+	if (sa_manager->hole->next == &sa_manager->olist) {
+		/* try again with its beginning */
+		sa_manager->hole = &sa_manager->olist;
+		return true;
+	}
+
+	soffset = radeon_sa_bo_hole_soffset(sa_manager);
+	/* to handle wrap around we add sa_manager->size */
+	best = sa_manager->size * 2;
+	/* go over all fence list and try to find the closest sa_bo
+	 * of the current last
+	 */
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		struct radeon_sa_bo *sa_bo;
+
+		if (list_empty(&sa_manager->flist[i])) {
+			continue;
+		}
+
+		sa_bo = list_first_entry(&sa_manager->flist[i],
+					 struct radeon_sa_bo, flist);
+
+		if (!radeon_fence_signaled(sa_bo->fence)) {
+			fences[i] = sa_bo->fence;
+			continue;
+		}
+
+		/* limit the number of tries each ring gets */
+		if (tries[i] > 2) {
+			continue;
+		}
+
+		tmp = sa_bo->soffset;
+		if (tmp < soffset) {
+			/* wrap around, pretend it's after */
+			tmp += sa_manager->size;
+		}
+		tmp -= soffset;
+		if (tmp < best) {
+			/* this sa bo is the closest one */
+			best = tmp;
+			best_bo = sa_bo;
+		}
+	}
+
+	if (best_bo) {
+		++tries[best_bo->fence->ring];
+		sa_manager->hole = best_bo->olist.prev;
+
+		/* we knew that this one is signaled,
+		   so it's save to remote it */
+		radeon_sa_bo_remove_locked(best_bo);
+		return true;
+	}
+	return false;
+}
+
 int radeon_sa_bo_new(struct radeon_device *rdev,
 		     struct radeon_sa_manager *sa_manager,
-		     struct radeon_sa_bo *sa_bo,
-		     unsigned size, unsigned align)
+		     struct radeon_sa_bo **sa_bo,
+		     unsigned size, unsigned align, bool block)
 {
-	struct radeon_sa_bo *tmp;
-	struct list_head *head;
-	unsigned offset = 0, wasted = 0;
+	struct radeon_fence *fences[RADEON_NUM_RINGS];
+	unsigned tries[RADEON_NUM_RINGS];
+	int i, r = -ENOMEM;
 
 	BUG_ON(align > RADEON_GPU_PAGE_SIZE);
 	BUG_ON(size > sa_manager->size);
 
-	/* no one ? */
-	head = sa_manager->sa_bo.prev;
-	if (list_empty(&sa_manager->sa_bo)) {
-		goto out;
+	*sa_bo = kmalloc(sizeof(struct radeon_sa_bo), GFP_KERNEL);
+	if ((*sa_bo) == NULL) {
+		return -ENOMEM;
 	}
+	(*sa_bo)->manager = sa_manager;
+	(*sa_bo)->fence = NULL;
+	INIT_LIST_HEAD(&(*sa_bo)->olist);
+	INIT_LIST_HEAD(&(*sa_bo)->flist);
 
-	/* look for a hole big enough */
-	offset = 0;
-	list_for_each_entry(tmp, &sa_manager->sa_bo, list) {
-		/* room before this object ? */
-		if ((tmp->offset - offset) >= size) {
-			head = tmp->list.prev;
-			goto out;
+	spin_lock(&sa_manager->lock);
+	do {
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			fences[i] = NULL;
+			tries[i] = 0;
 		}
-		offset = tmp->offset + tmp->size;
-		wasted = offset % align;
-		if (wasted) {
-			wasted = align - wasted;
+
+		do {
+			radeon_sa_bo_try_free(sa_manager);
+
+			if (radeon_sa_bo_try_alloc(sa_manager, *sa_bo,
+						   size, align)) {
+				spin_unlock(&sa_manager->lock);
+				return 0;
+			}
+
+			/* see if we can skip over some allocations */
+		} while (radeon_sa_bo_next_hole(sa_manager, fences, tries));
+
+		if (block) {
+			spin_unlock(&sa_manager->lock);
+			r = radeon_fence_wait_any(rdev, fences, false);
+			spin_lock(&sa_manager->lock);
+			if (r) {
+				/* if we have nothing to wait for we
+				   are practically out of memory */
+				if (r == -ENOENT) {
+					r = -ENOMEM;
+				}
+				goto out_err;
+			}
 		}
-		offset += wasted;
-	}
-	/* room at the end ? */
-	head = sa_manager->sa_bo.prev;
-	tmp = list_entry(head, struct radeon_sa_bo, list);
-	offset = tmp->offset + tmp->size;
-	wasted = offset % align;
-	if (wasted) {
-		wasted = align - wasted;
-	}
-	offset += wasted;
-	if ((sa_manager->size - offset) < size) {
-		/* failed to find somethings big enough */
-		return -ENOMEM;
+	} while (block);
+
+out_err:
+	spin_unlock(&sa_manager->lock);
+	kfree(*sa_bo);
+	*sa_bo = NULL;
+	return r;
+}
+
+void radeon_sa_bo_free(struct radeon_device *rdev, struct radeon_sa_bo **sa_bo,
+		       struct radeon_fence *fence)
+{
+	struct radeon_sa_manager *sa_manager;
+
+	if (sa_bo == NULL || *sa_bo == NULL) {
+		return;
 	}
 
-out:
-	sa_bo->manager = sa_manager;
-	sa_bo->offset = offset;
-	sa_bo->size = size;
-	list_add(&sa_bo->list, head);
-	return 0;
+	sa_manager = (*sa_bo)->manager;
+	spin_lock(&sa_manager->lock);
+	if (fence && fence->seq && fence->seq < RADEON_FENCE_NOTEMITED_SEQ) {
+		(*sa_bo)->fence = radeon_fence_ref(fence);
+		list_add_tail(&(*sa_bo)->flist,
+			      &sa_manager->flist[fence->ring]);
+	} else {
+		radeon_sa_bo_remove_locked(*sa_bo);
+	}
+	spin_unlock(&sa_manager->lock);
+	*sa_bo = NULL;
 }
 
-void radeon_sa_bo_free(struct radeon_device *rdev, struct radeon_sa_bo *sa_bo)
+#if defined(CONFIG_DEBUG_FS)
+void radeon_sa_bo_dump_debug_info(struct radeon_sa_manager *sa_manager,
+				  struct seq_file *m)
 {
-	list_del_init(&sa_bo->list);
+	struct radeon_sa_bo *i;
+
+	spin_lock(&sa_manager->lock);
+	list_for_each_entry(i, &sa_manager->olist, olist) {
+		if (&i->olist == sa_manager->hole) {
+			seq_printf(m, ">");
+		} else {
+			seq_printf(m, " ");
+		}
+		seq_printf(m, "[0x%08x 0x%08x] size %8d",
+			   i->soffset, i->eoffset, i->eoffset - i->soffset);
+		if (i->fence) {
+			seq_printf(m, " protected by 0x%016llx on ring %d",
+				   i->fence->seq, i->fence->ring);
+		}
+		seq_printf(m, "\n");
+	}
+	spin_unlock(&sa_manager->lock);
 }
+#endif
diff --git a/drivers/gpu/drm/radeon/radeon_semaphore.c b/drivers/gpu/drm/radeon/radeon_semaphore.c
index 61dd4e3c9209..e2ace5dce117 100644
--- a/drivers/gpu/drm/radeon/radeon_semaphore.c
+++ b/drivers/gpu/drm/radeon/radeon_semaphore.c
@@ -31,148 +31,107 @@
 #include "drm.h"
 #include "radeon.h"
 
-static int radeon_semaphore_add_bo(struct radeon_device *rdev)
-{
-	struct radeon_semaphore_bo *bo;
-	unsigned long irq_flags;
-	uint64_t gpu_addr;
-	uint32_t *cpu_ptr;
-	int r, i;
-
-
-	bo = kmalloc(sizeof(struct radeon_semaphore_bo), GFP_KERNEL);
-	if (bo == NULL) {
-		return -ENOMEM;
-	}
-	INIT_LIST_HEAD(&bo->free);
-	INIT_LIST_HEAD(&bo->list);
-	bo->nused = 0;
-
-	r = radeon_ib_get(rdev, 0, &bo->ib, RADEON_SEMAPHORE_BO_SIZE);
-	if (r) {
-		dev_err(rdev->dev, "failed to get a bo after 5 retry\n");
-		kfree(bo);
-		return r;
-	}
-	gpu_addr = rdev->ib_pool.sa_manager.gpu_addr;
-	gpu_addr += bo->ib->sa_bo.offset;
-	cpu_ptr = rdev->ib_pool.sa_manager.cpu_ptr;
-	cpu_ptr += (bo->ib->sa_bo.offset >> 2);
-	for (i = 0; i < (RADEON_SEMAPHORE_BO_SIZE/8); i++) {
-		bo->semaphores[i].gpu_addr = gpu_addr;
-		bo->semaphores[i].cpu_ptr = cpu_ptr;
-		bo->semaphores[i].bo = bo;
-		list_add_tail(&bo->semaphores[i].list, &bo->free);
-		gpu_addr += 8;
-		cpu_ptr += 2;
-	}
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	list_add_tail(&bo->list, &rdev->semaphore_drv.bo);
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
-	return 0;
-}
-
-static void radeon_semaphore_del_bo_locked(struct radeon_device *rdev,
-					   struct radeon_semaphore_bo *bo)
-{
-	radeon_sa_bo_free(rdev, &bo->ib->sa_bo);
-	radeon_fence_unref(&bo->ib->fence);
-	list_del(&bo->list);
-	kfree(bo);
-}
-
-void radeon_semaphore_shrink_locked(struct radeon_device *rdev)
-{
-	struct radeon_semaphore_bo *bo, *n;
-
-	if (list_empty(&rdev->semaphore_drv.bo)) {
-		return;
-	}
-	/* only shrink if first bo has free semaphore */
-	bo = list_first_entry(&rdev->semaphore_drv.bo, struct radeon_semaphore_bo, list);
-	if (list_empty(&bo->free)) {
-		return;
-	}
-	list_for_each_entry_safe_continue(bo, n, &rdev->semaphore_drv.bo, list) {
-		if (bo->nused)
-			continue;
-		radeon_semaphore_del_bo_locked(rdev, bo);
-	}
-}
 
 int radeon_semaphore_create(struct radeon_device *rdev,
 			    struct radeon_semaphore **semaphore)
 {
-	struct radeon_semaphore_bo *bo;
-	unsigned long irq_flags;
-	bool do_retry = true;
 	int r;
 
-retry:
-	*semaphore = NULL;
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	list_for_each_entry(bo, &rdev->semaphore_drv.bo, list) {
-		if (list_empty(&bo->free))
-			continue;
-		*semaphore = list_first_entry(&bo->free, struct radeon_semaphore, list);
-		(*semaphore)->cpu_ptr[0] = 0;
-		(*semaphore)->cpu_ptr[1] = 0;
-		list_del(&(*semaphore)->list);
-		bo->nused++;
-		break;
-	}
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
-
+	*semaphore = kmalloc(sizeof(struct radeon_semaphore), GFP_KERNEL);
 	if (*semaphore == NULL) {
-		if (do_retry) {
-			do_retry = false;
-			r = radeon_semaphore_add_bo(rdev);
-			if (r)
-				return r;
-			goto retry;
-		}
 		return -ENOMEM;
 	}
-
+	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo,
+			     &(*semaphore)->sa_bo, 8, 8, true);
+	if (r) {
+		kfree(*semaphore);
+		*semaphore = NULL;
+		return r;
+	}
+	(*semaphore)->waiters = 0;
+	(*semaphore)->gpu_addr = radeon_sa_bo_gpu_addr((*semaphore)->sa_bo);
+	*((uint64_t*)radeon_sa_bo_cpu_addr((*semaphore)->sa_bo)) = 0;
 	return 0;
 }
 
 void radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
 			          struct radeon_semaphore *semaphore)
 {
+	--semaphore->waiters;
 	radeon_semaphore_ring_emit(rdev, ring, &rdev->ring[ring], semaphore, false);
 }
 
 void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 			        struct radeon_semaphore *semaphore)
 {
+	++semaphore->waiters;
 	radeon_semaphore_ring_emit(rdev, ring, &rdev->ring[ring], semaphore, true);
 }
 
-void radeon_semaphore_free(struct radeon_device *rdev,
-			   struct radeon_semaphore *semaphore)
+int radeon_semaphore_sync_rings(struct radeon_device *rdev,
+				struct radeon_semaphore *semaphore,
+				bool sync_to[RADEON_NUM_RINGS],
+				int dst_ring)
 {
-	unsigned long irq_flags;
+	int i = 0, r;
+
+	mutex_lock(&rdev->ring_lock);
+	r = radeon_ring_alloc(rdev, &rdev->ring[dst_ring], RADEON_NUM_RINGS * 8);
+	if (r) {
+		goto error;
+	}
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		/* no need to sync to our own or unused rings */
+		if (!sync_to[i] || i == dst_ring)
+			continue;
 
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	semaphore->bo->nused--;
-	list_add_tail(&semaphore->list, &semaphore->bo->free);
-	radeon_semaphore_shrink_locked(rdev);
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+		/* prevent GPU deadlocks */
+		if (!rdev->ring[i].ready) {
+			dev_err(rdev->dev, "Trying to sync to a disabled ring!");
+			r = -EINVAL;
+			goto error;
+		}
+
+		r = radeon_ring_alloc(rdev, &rdev->ring[i], 8);
+		if (r) {
+			goto error;
+		}
+
+		radeon_semaphore_emit_signal(rdev, i, semaphore);
+		radeon_semaphore_emit_wait(rdev, dst_ring, semaphore);
+
+		radeon_ring_commit(rdev, &rdev->ring[i]);
+	}
+
+	radeon_ring_commit(rdev, &rdev->ring[dst_ring]);
+	mutex_unlock(&rdev->ring_lock);
+
+	return 0;
+
+error:
+	/* unlock all locks taken so far */
+	for (--i; i >= 0; --i) {
+		if (sync_to[i] || i == dst_ring) {
+			radeon_ring_undo(&rdev->ring[i]);
+		}
+	}
+	radeon_ring_undo(&rdev->ring[dst_ring]);
+	mutex_unlock(&rdev->ring_lock);
+	return r;
 }
 
-void radeon_semaphore_driver_fini(struct radeon_device *rdev)
+void radeon_semaphore_free(struct radeon_device *rdev,
+			   struct radeon_semaphore *semaphore,
+			   struct radeon_fence *fence)
 {
-	struct radeon_semaphore_bo *bo, *n;
-	unsigned long irq_flags;
-
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	/* we force to free everything */
-	list_for_each_entry_safe(bo, n, &rdev->semaphore_drv.bo, list) {
-		if (!list_empty(&bo->free)) {
-			dev_err(rdev->dev, "still in use semaphore\n");
-		}
-		radeon_semaphore_del_bo_locked(rdev, bo);
+	if (semaphore == NULL) {
+		return;
+	}
+	if (semaphore->waiters > 0) {
+		dev_err(rdev->dev, "semaphore %p has more waiters than signalers,"
+			" hardware lockup imminent!\n", semaphore);
 	}
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+	radeon_sa_bo_free(rdev, &semaphore->sa_bo, fence);
+	kfree(semaphore);
 }
diff --git a/drivers/gpu/drm/radeon/radeon_test.c b/drivers/gpu/drm/radeon/radeon_test.c
index dc5dcf483aa3..efff929ea49d 100644
--- a/drivers/gpu/drm/radeon/radeon_test.c
+++ b/drivers/gpu/drm/radeon/radeon_test.c
@@ -59,7 +59,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 	}
 
 	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				&vram_obj);
+			     NULL, &vram_obj);
 	if (r) {
 		DRM_ERROR("Failed to create VRAM object\n");
 		goto out_cleanup;
@@ -78,7 +78,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 		void **vram_start, **vram_end;
 
 		r = radeon_bo_create(rdev, size, PAGE_SIZE, true,
-					 RADEON_GEM_DOMAIN_GTT, gtt_obj + i);
+				     RADEON_GEM_DOMAIN_GTT, NULL, gtt_obj + i);
 		if (r) {
 			DRM_ERROR("Failed to create GTT object %d\n", i);
 			goto out_cleanup;
@@ -317,7 +317,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 
 out_cleanup:
 	if (semaphore)
-		radeon_semaphore_free(rdev, semaphore);
+		radeon_semaphore_free(rdev, semaphore, NULL);
 
 	if (fence1)
 		radeon_fence_unref(&fence1);
@@ -437,7 +437,7 @@ void radeon_test_ring_sync2(struct radeon_device *rdev,
 
 out_cleanup:
 	if (semaphore)
-		radeon_semaphore_free(rdev, semaphore);
+		radeon_semaphore_free(rdev, semaphore, NULL);
 
 	if (fenceA)
 		radeon_fence_unref(&fenceA);
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index f493c6403af5..c94a2257761f 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -222,8 +222,9 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 {
 	struct radeon_device *rdev;
 	uint64_t old_start, new_start;
-	struct radeon_fence *fence;
-	int r, i;
+	struct radeon_fence *fence, *old_fence;
+	struct radeon_semaphore *sem = NULL;
+	int r;
 
 	rdev = radeon_get_rdev(bo->bdev);
 	r = radeon_fence_create(rdev, &fence, radeon_copy_ring_index(rdev));
@@ -242,6 +243,7 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 		break;
 	default:
 		DRM_ERROR("Unknown placement %d\n", old_mem->mem_type);
+		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 	switch (new_mem->mem_type) {
@@ -253,42 +255,36 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 		break;
 	default:
 		DRM_ERROR("Unknown placement %d\n", old_mem->mem_type);
+		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 	if (!rdev->ring[radeon_copy_ring_index(rdev)].ready) {
 		DRM_ERROR("Trying to move memory with ring turned off.\n");
+		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 
 	BUILD_BUG_ON((PAGE_SIZE % RADEON_GPU_PAGE_SIZE) != 0);
 
 	/* sync other rings */
-	if (rdev->family >= CHIP_R600) {
-		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-			/* no need to sync to our own or unused rings */
-			if (i == radeon_copy_ring_index(rdev) || !rdev->ring[i].ready)
-				continue;
-
-			if (!fence->semaphore) {
-				r = radeon_semaphore_create(rdev, &fence->semaphore);
-				/* FIXME: handle semaphore error */
-				if (r)
-					continue;
-			}
+	old_fence = bo->sync_obj;
+	if (old_fence && old_fence->ring != fence->ring
+	    && !radeon_fence_signaled(old_fence)) {
+		bool sync_to_ring[RADEON_NUM_RINGS] = { };
+		sync_to_ring[old_fence->ring] = true;
+
+		r = radeon_semaphore_create(rdev, &sem);
+		if (r) {
+			radeon_fence_unref(&fence);
+			return r;
+		}
 
-			r = radeon_ring_lock(rdev, &rdev->ring[i], 3);
-			/* FIXME: handle ring lock error */
-			if (r)
-				continue;
-			radeon_semaphore_emit_signal(rdev, i, fence->semaphore);
-			radeon_ring_unlock_commit(rdev, &rdev->ring[i]);
-
-			r = radeon_ring_lock(rdev, &rdev->ring[radeon_copy_ring_index(rdev)], 3);
-			/* FIXME: handle ring lock error */
-			if (r)
-				continue;
-			radeon_semaphore_emit_wait(rdev, radeon_copy_ring_index(rdev), fence->semaphore);
-			radeon_ring_unlock_commit(rdev, &rdev->ring[radeon_copy_ring_index(rdev)]);
+		r = radeon_semaphore_sync_rings(rdev, sem,
+						sync_to_ring, fence->ring);
+		if (r) {
+			radeon_semaphore_free(rdev, sem, NULL);
+			radeon_fence_unref(&fence);
+			return r;
 		}
 	}
 
@@ -298,6 +294,7 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 	/* FIXME: handle copy error */
 	r = ttm_bo_move_accel_cleanup(bo, (void *)fence, NULL,
 				      evict, no_wait_reserve, no_wait_gpu, new_mem);
+	radeon_semaphore_free(rdev, sem, fence);
 	radeon_fence_unref(&fence);
 	return r;
 }
@@ -614,10 +611,18 @@ static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
 	struct radeon_ttm_tt *gtt = (void *)ttm;
 	unsigned i;
 	int r;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
 
 	if (ttm->state != tt_unpopulated)
 		return 0;
 
+	if (slave && ttm->sg) {
+		drm_prime_sg_to_page_addr_arrays(ttm->sg, ttm->pages,
+						 gtt->ttm.dma_address, ttm->num_pages);
+		ttm->state = tt_unbound;
+		return 0;
+	}
+
 	rdev = radeon_get_rdev(ttm->bdev);
 #if __OS_HAS_AGP
 	if (rdev->flags & RADEON_IS_AGP) {
@@ -658,6 +663,10 @@ static void radeon_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	struct radeon_device *rdev;
 	struct radeon_ttm_tt *gtt = (void *)ttm;
 	unsigned i;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
+
+	if (slave)
+		return;
 
 	rdev = radeon_get_rdev(ttm->bdev);
 #if __OS_HAS_AGP
@@ -729,8 +738,8 @@ int radeon_ttm_init(struct radeon_device *rdev)
 		return r;
 	}
 	r = radeon_bo_create(rdev, 256 * 1024, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_VRAM,
-				&rdev->stollen_vga_memory);
+			     RADEON_GEM_DOMAIN_VRAM,
+			     NULL, &rdev->stollen_vga_memory);
 	if (r) {
 		return r;
 	}
diff --git a/drivers/gpu/drm/radeon/rs400.c b/drivers/gpu/drm/radeon/rs400.c
index 4cf381b3a6d8..a464eb5e2df2 100644
--- a/drivers/gpu/drm/radeon/rs400.c
+++ b/drivers/gpu/drm/radeon/rs400.c
@@ -430,12 +430,9 @@ static int rs400_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index d25cf869d08d..25f9eef12c42 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -396,7 +396,6 @@ int rs600_asic_reset(struct radeon_device *rdev)
 	/* Check if GPU is idle */
 	if (G_000E40_GA_BUSY(status) || G_000E40_VAP_BUSY(status)) {
 		dev_err(rdev->dev, "failed to reset GPU\n");
-		rdev->gpu_lockup = true;
 		ret = -1;
 	} else
 		dev_info(rdev->dev, "GPU reset succeed\n");
@@ -553,6 +552,12 @@ int rs600_irq_set(struct radeon_device *rdev)
 		~S_007D08_DC_HOT_PLUG_DETECT1_INT_EN(1);
 	u32 hpd2 = RREG32(R_007D18_DC_HOT_PLUG_DETECT2_INT_CONTROL) &
 		~S_007D18_DC_HOT_PLUG_DETECT2_INT_EN(1);
+	u32 hdmi0;
+	if (ASIC_IS_DCE2(rdev))
+		hdmi0 = RREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL) &
+			~S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(1);
+	else
+		hdmi0 = 0;
 
 	if (!rdev->irq.installed) {
 		WARN(1, "Can't enable IRQ/MSI because no handler is installed\n");
@@ -579,10 +584,15 @@ int rs600_irq_set(struct radeon_device *rdev)
 	if (rdev->irq.hpd[1]) {
 		hpd2 |= S_007D18_DC_HOT_PLUG_DETECT2_INT_EN(1);
 	}
+	if (rdev->irq.afmt[0]) {
+		hdmi0 |= S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(1);
+	}
 	WREG32(R_000040_GEN_INT_CNTL, tmp);
 	WREG32(R_006540_DxMODE_INT_MASK, mode_int);
 	WREG32(R_007D08_DC_HOT_PLUG_DETECT1_INT_CONTROL, hpd1);
 	WREG32(R_007D18_DC_HOT_PLUG_DETECT2_INT_CONTROL, hpd2);
+	if (ASIC_IS_DCE2(rdev))
+		WREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
 	return 0;
 }
 
@@ -622,6 +632,17 @@ static inline u32 rs600_irq_ack(struct radeon_device *rdev)
 		rdev->irq.stat_regs.r500.disp_int = 0;
 	}
 
+	if (ASIC_IS_DCE2(rdev)) {
+		rdev->irq.stat_regs.r500.hdmi0_status = RREG32(R_007404_HDMI0_STATUS) &
+			S_007404_HDMI0_AZ_FORMAT_WTRIG(1);
+		if (G_007404_HDMI0_AZ_FORMAT_WTRIG(rdev->irq.stat_regs.r500.hdmi0_status)) {
+			tmp = RREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL);
+			tmp |= S_007408_HDMI0_AZ_FORMAT_WTRIG_ACK(1);
+			WREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL, tmp);
+		}
+	} else
+		rdev->irq.stat_regs.r500.hdmi0_status = 0;
+
 	if (irqs) {
 		WREG32(R_000044_GEN_INT_STATUS, irqs);
 	}
@@ -630,6 +651,9 @@ static inline u32 rs600_irq_ack(struct radeon_device *rdev)
 
 void rs600_irq_disable(struct radeon_device *rdev)
 {
+	u32 hdmi0 = RREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL) &
+		~S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(1);
+	WREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
 	WREG32(R_000040_GEN_INT_CNTL, 0);
 	WREG32(R_006540_DxMODE_INT_MASK, 0);
 	/* Wait and acknowledge irq */
@@ -641,15 +665,20 @@ int rs600_irq_process(struct radeon_device *rdev)
 {
 	u32 status, msi_rearm;
 	bool queue_hotplug = false;
+	bool queue_hdmi = false;
 
 	/* reset gui idle ack.  the status bit is broken */
 	rdev->irq.gui_idle_acked = false;
 
 	status = rs600_irq_ack(rdev);
-	if (!status && !rdev->irq.stat_regs.r500.disp_int) {
+	if (!status &&
+	    !rdev->irq.stat_regs.r500.disp_int &&
+	    !rdev->irq.stat_regs.r500.hdmi0_status) {
 		return IRQ_NONE;
 	}
-	while (status || rdev->irq.stat_regs.r500.disp_int) {
+	while (status ||
+	       rdev->irq.stat_regs.r500.disp_int ||
+	       rdev->irq.stat_regs.r500.hdmi0_status) {
 		/* SW interrupt */
 		if (G_000044_SW_INT(status)) {
 			radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
@@ -687,12 +716,18 @@ int rs600_irq_process(struct radeon_device *rdev)
 			queue_hotplug = true;
 			DRM_DEBUG("HPD2\n");
 		}
+		if (G_007404_HDMI0_AZ_FORMAT_WTRIG(rdev->irq.stat_regs.r500.hdmi0_status)) {
+			queue_hdmi = true;
+			DRM_DEBUG("HDMI0\n");
+		}
 		status = rs600_irq_ack(rdev);
 	}
 	/* reset gui idle ack.  the status bit is broken */
 	rdev->irq.gui_idle_acked = false;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
+	if (queue_hdmi)
+		schedule_work(&rdev->audio_work);
 	if (rdev->msi_enabled) {
 		switch (rdev->family) {
 		case CHIP_RS600:
@@ -883,12 +918,9 @@ static int rs600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/rs600d.h b/drivers/gpu/drm/radeon/rs600d.h
index a27c13ac47c3..f1f89414dc63 100644
--- a/drivers/gpu/drm/radeon/rs600d.h
+++ b/drivers/gpu/drm/radeon/rs600d.h
@@ -485,6 +485,20 @@
 #define   S_007D18_DC_HOT_PLUG_DETECT2_INT_EN(x)       (((x) & 0x1) << 16)
 #define   G_007D18_DC_HOT_PLUG_DETECT2_INT_EN(x)       (((x) >> 16) & 0x1)
 #define   C_007D18_DC_HOT_PLUG_DETECT2_INT_EN          0xFFFEFFFF
+#define R_007404_HDMI0_STATUS                          0x007404
+#define   S_007404_HDMI0_AZ_FORMAT_WTRIG(x)            (((x) & 0x1) << 28)
+#define   G_007404_HDMI0_AZ_FORMAT_WTRIG(x)            (((x) >> 28) & 0x1)
+#define   C_007404_HDMI0_AZ_FORMAT_WTRIG               0xEFFFFFFF
+#define   S_007404_HDMI0_AZ_FORMAT_WTRIG_INT(x)        (((x) & 0x1) << 29)
+#define   G_007404_HDMI0_AZ_FORMAT_WTRIG_INT(x)        (((x) >> 29) & 0x1)
+#define   C_007404_HDMI0_AZ_FORMAT_WTRIG_INT           0xDFFFFFFF
+#define R_007408_HDMI0_AUDIO_PACKET_CONTROL            0x007408
+#define   S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(x)       (((x) & 0x1) << 28)
+#define   G_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(x)       (((x) >> 28) & 0x1)
+#define   C_007408_HDMI0_AZ_FORMAT_WTRIG_MASK          0xEFFFFFFF
+#define   S_007408_HDMI0_AZ_FORMAT_WTRIG_ACK(x)        (((x) & 0x1) << 29)
+#define   G_007408_HDMI0_AZ_FORMAT_WTRIG_ACK(x)        (((x) >> 29) & 0x1)
+#define   C_007408_HDMI0_AZ_FORMAT_WTRIG_ACK           0xDFFFFFFF
 
 /* MC registers */
 #define R_000000_MC_STATUS                           0x000000
diff --git a/drivers/gpu/drm/radeon/rs690.c b/drivers/gpu/drm/radeon/rs690.c
index f2c3b9d75f18..3277ddecfe9f 100644
--- a/drivers/gpu/drm/radeon/rs690.c
+++ b/drivers/gpu/drm/radeon/rs690.c
@@ -647,12 +647,9 @@ static int rs690_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index d8d78fe17946..7f08cedb5333 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -412,12 +412,10 @@ static int rv515_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index cdab1aeaed6e..c2f473bc13b8 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -1114,12 +1114,9 @@ static int rv770_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "IB test failed (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
@@ -1178,10 +1175,6 @@ int rv770_init(struct radeon_device *rdev)
 {
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -1281,7 +1274,6 @@ void rv770_fini(struct radeon_device *rdev)
 	rv770_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_bo_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/rv770d.h b/drivers/gpu/drm/radeon/rv770d.h
index 79fa588e9ed5..9c549f702f2f 100644
--- a/drivers/gpu/drm/radeon/rv770d.h
+++ b/drivers/gpu/drm/radeon/rv770d.h
@@ -353,6 +353,197 @@
 
 #define	SRBM_STATUS				        0x0E50
 
+/* DCE 3.2 HDMI */
+#define HDMI_CONTROL                         0x7400
+#       define HDMI_KEEPOUT_MODE             (1 << 0)
+#       define HDMI_PACKET_GEN_VERSION       (1 << 4) /* 0 = r6xx compat */
+#       define HDMI_ERROR_ACK                (1 << 8)
+#       define HDMI_ERROR_MASK               (1 << 9)
+#define HDMI_STATUS                          0x7404
+#       define HDMI_ACTIVE_AVMUTE            (1 << 0)
+#       define HDMI_AUDIO_PACKET_ERROR       (1 << 16)
+#       define HDMI_VBI_PACKET_ERROR         (1 << 20)
+#define HDMI_AUDIO_PACKET_CONTROL            0x7408
+#       define HDMI_AUDIO_DELAY_EN(x)        (((x) & 3) << 4)
+#       define HDMI_AUDIO_PACKETS_PER_LINE(x)  (((x) & 0x1f) << 16)
+#define HDMI_ACR_PACKET_CONTROL              0x740c
+#       define HDMI_ACR_SEND                 (1 << 0)
+#       define HDMI_ACR_CONT                 (1 << 1)
+#       define HDMI_ACR_SELECT(x)            (((x) & 3) << 4)
+#       define HDMI_ACR_HW                   0
+#       define HDMI_ACR_32                   1
+#       define HDMI_ACR_44                   2
+#       define HDMI_ACR_48                   3
+#       define HDMI_ACR_SOURCE               (1 << 8) /* 0 - hw; 1 - cts value */
+#       define HDMI_ACR_AUTO_SEND            (1 << 12)
+#define HDMI_VBI_PACKET_CONTROL              0x7410
+#       define HDMI_NULL_SEND                (1 << 0)
+#       define HDMI_GC_SEND                  (1 << 4)
+#       define HDMI_GC_CONT                  (1 << 5) /* 0 - once; 1 - every frame */
+#define HDMI_INFOFRAME_CONTROL0              0x7414
+#       define HDMI_AVI_INFO_SEND            (1 << 0)
+#       define HDMI_AVI_INFO_CONT            (1 << 1)
+#       define HDMI_AUDIO_INFO_SEND          (1 << 4)
+#       define HDMI_AUDIO_INFO_CONT          (1 << 5)
+#       define HDMI_MPEG_INFO_SEND           (1 << 8)
+#       define HDMI_MPEG_INFO_CONT           (1 << 9)
+#define HDMI_INFOFRAME_CONTROL1              0x7418
+#       define HDMI_AVI_INFO_LINE(x)         (((x) & 0x3f) << 0)
+#       define HDMI_AUDIO_INFO_LINE(x)       (((x) & 0x3f) << 8)
+#       define HDMI_MPEG_INFO_LINE(x)        (((x) & 0x3f) << 16)
+#define HDMI_GENERIC_PACKET_CONTROL          0x741c
+#       define HDMI_GENERIC0_SEND            (1 << 0)
+#       define HDMI_GENERIC0_CONT            (1 << 1)
+#       define HDMI_GENERIC1_SEND            (1 << 4)
+#       define HDMI_GENERIC1_CONT            (1 << 5)
+#       define HDMI_GENERIC0_LINE(x)         (((x) & 0x3f) << 16)
+#       define HDMI_GENERIC1_LINE(x)         (((x) & 0x3f) << 24)
+#define HDMI_GC                              0x7428
+#       define HDMI_GC_AVMUTE                (1 << 0)
+#define AFMT_AUDIO_PACKET_CONTROL2           0x742c
+#       define AFMT_AUDIO_LAYOUT_OVRD        (1 << 0)
+#       define AFMT_AUDIO_LAYOUT_SELECT      (1 << 1)
+#       define AFMT_60958_CS_SOURCE          (1 << 4)
+#       define AFMT_AUDIO_CHANNEL_ENABLE(x)  (((x) & 0xff) << 8)
+#       define AFMT_DP_AUDIO_STREAM_ID(x)    (((x) & 0xff) << 16)
+#define AFMT_AVI_INFO0                       0x7454
+#       define AFMT_AVI_INFO_CHECKSUM(x)     (((x) & 0xff) << 0)
+#       define AFMT_AVI_INFO_S(x)            (((x) & 3) << 8)
+#       define AFMT_AVI_INFO_B(x)            (((x) & 3) << 10)
+#       define AFMT_AVI_INFO_A(x)            (((x) & 1) << 12)
+#       define AFMT_AVI_INFO_Y(x)            (((x) & 3) << 13)
+#       define AFMT_AVI_INFO_Y_RGB           0
+#       define AFMT_AVI_INFO_Y_YCBCR422      1
+#       define AFMT_AVI_INFO_Y_YCBCR444      2
+#       define AFMT_AVI_INFO_Y_A_B_S(x)      (((x) & 0xff) << 8)
+#       define AFMT_AVI_INFO_R(x)            (((x) & 0xf) << 16)
+#       define AFMT_AVI_INFO_M(x)            (((x) & 0x3) << 20)
+#       define AFMT_AVI_INFO_C(x)            (((x) & 0x3) << 22)
+#       define AFMT_AVI_INFO_C_M_R(x)        (((x) & 0xff) << 16)
+#       define AFMT_AVI_INFO_SC(x)           (((x) & 0x3) << 24)
+#       define AFMT_AVI_INFO_Q(x)            (((x) & 0x3) << 26)
+#       define AFMT_AVI_INFO_EC(x)           (((x) & 0x3) << 28)
+#       define AFMT_AVI_INFO_ITC(x)          (((x) & 0x1) << 31)
+#       define AFMT_AVI_INFO_ITC_EC_Q_SC(x)  (((x) & 0xff) << 24)
+#define AFMT_AVI_INFO1                       0x7458
+#       define AFMT_AVI_INFO_VIC(x)          (((x) & 0x7f) << 0) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_PR(x)           (((x) & 0xf) << 8) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_TOP(x)          (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO2                       0x745c
+#       define AFMT_AVI_INFO_BOTTOM(x)       (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_LEFT(x)         (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO3                       0x7460
+#       define AFMT_AVI_INFO_RIGHT(x)        (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_VERSION(x)      (((x) & 3) << 24)
+#define AFMT_MPEG_INFO0                      0x7464
+#       define AFMT_MPEG_INFO_CHECKSUM(x)    (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MB0(x)         (((x) & 0xff) << 8)
+#       define AFMT_MPEG_INFO_MB1(x)         (((x) & 0xff) << 16)
+#       define AFMT_MPEG_INFO_MB2(x)         (((x) & 0xff) << 24)
+#define AFMT_MPEG_INFO1                      0x7468
+#       define AFMT_MPEG_INFO_MB3(x)         (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MF(x)          (((x) & 3) << 8)
+#       define AFMT_MPEG_INFO_FR(x)          (((x) & 1) << 12)
+#define AFMT_GENERIC0_HDR                    0x746c
+#define AFMT_GENERIC0_0                      0x7470
+#define AFMT_GENERIC0_1                      0x7474
+#define AFMT_GENERIC0_2                      0x7478
+#define AFMT_GENERIC0_3                      0x747c
+#define AFMT_GENERIC0_4                      0x7480
+#define AFMT_GENERIC0_5                      0x7484
+#define AFMT_GENERIC0_6                      0x7488
+#define AFMT_GENERIC1_HDR                    0x748c
+#define AFMT_GENERIC1_0                      0x7490
+#define AFMT_GENERIC1_1                      0x7494
+#define AFMT_GENERIC1_2                      0x7498
+#define AFMT_GENERIC1_3                      0x749c
+#define AFMT_GENERIC1_4                      0x74a0
+#define AFMT_GENERIC1_5                      0x74a4
+#define AFMT_GENERIC1_6                      0x74a8
+#define HDMI_ACR_32_0                        0x74ac
+#       define HDMI_ACR_CTS_32(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_32_1                        0x74b0
+#       define HDMI_ACR_N_32(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_44_0                        0x74b4
+#       define HDMI_ACR_CTS_44(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_44_1                        0x74b8
+#       define HDMI_ACR_N_44(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_48_0                        0x74bc
+#       define HDMI_ACR_CTS_48(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_48_1                        0x74c0
+#       define HDMI_ACR_N_48(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_STATUS_0                    0x74c4
+#define HDMI_ACR_STATUS_1                    0x74c8
+#define AFMT_AUDIO_INFO0                     0x74cc
+#       define AFMT_AUDIO_INFO_CHECKSUM(x)   (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_CC(x)         (((x) & 7) << 8)
+#       define AFMT_AUDIO_INFO_CHECKSUM_OFFSET(x)   (((x) & 0xff) << 16)
+#define AFMT_AUDIO_INFO1                     0x74d0
+#       define AFMT_AUDIO_INFO_CA(x)         (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_LSV(x)        (((x) & 0xf) << 11)
+#       define AFMT_AUDIO_INFO_DM_INH(x)     (((x) & 1) << 15)
+#       define AFMT_AUDIO_INFO_DM_INH_LSV(x) (((x) & 0xff) << 8)
+#define AFMT_60958_0                         0x74d4
+#       define AFMT_60958_CS_A(x)            (((x) & 1) << 0)
+#       define AFMT_60958_CS_B(x)            (((x) & 1) << 1)
+#       define AFMT_60958_CS_C(x)            (((x) & 1) << 2)
+#       define AFMT_60958_CS_D(x)            (((x) & 3) << 3)
+#       define AFMT_60958_CS_MODE(x)         (((x) & 3) << 6)
+#       define AFMT_60958_CS_CATEGORY_CODE(x)      (((x) & 0xff) << 8)
+#       define AFMT_60958_CS_SOURCE_NUMBER(x)      (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_L(x)   (((x) & 0xf) << 20)
+#       define AFMT_60958_CS_SAMPLING_FREQUENCY(x) (((x) & 0xf) << 24)
+#       define AFMT_60958_CS_CLOCK_ACCURACY(x)     (((x) & 3) << 28)
+#define AFMT_60958_1                         0x74d8
+#       define AFMT_60958_CS_WORD_LENGTH(x)  (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_ORIGINAL_SAMPLING_FREQUENCY(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_VALID_L(x)      (((x) & 1) << 16)
+#       define AFMT_60958_CS_VALID_R(x)      (((x) & 1) << 18)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_R(x)   (((x) & 0xf) << 20)
+#define AFMT_AUDIO_CRC_CONTROL               0x74dc
+#       define AFMT_AUDIO_CRC_EN             (1 << 0)
+#define AFMT_RAMP_CONTROL0                   0x74e0
+#       define AFMT_RAMP_MAX_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_RAMP_DATA_SIGN           (1 << 31)
+#define AFMT_RAMP_CONTROL1                   0x74e4
+#       define AFMT_RAMP_MIN_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_AUDIO_TEST_CH_DISABLE(x) (((x) & 0xff) << 24)
+#define AFMT_RAMP_CONTROL2                   0x74e8
+#       define AFMT_RAMP_INC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_RAMP_CONTROL3                   0x74ec
+#       define AFMT_RAMP_DEC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_60958_2                         0x74f0
+#       define AFMT_60958_CS_CHANNEL_NUMBER_2(x)   (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_3(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_4(x)   (((x) & 0xf) << 8)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_5(x)   (((x) & 0xf) << 12)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_6(x)   (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_7(x)   (((x) & 0xf) << 20)
+#define AFMT_STATUS                          0x7600
+#       define AFMT_AUDIO_ENABLE             (1 << 4)
+#       define AFMT_AZ_FORMAT_WTRIG          (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_INT      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG      (1 << 30)
+#define AFMT_AUDIO_PACKET_CONTROL            0x7604
+#       define AFMT_AUDIO_SAMPLE_SEND        (1 << 0)
+#       define AFMT_AUDIO_TEST_EN            (1 << 12)
+#       define AFMT_AUDIO_CHANNEL_SWAP       (1 << 24)
+#       define AFMT_60958_CS_UPDATE          (1 << 26)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_MASK (1 << 27)
+#       define AFMT_AZ_FORMAT_WTRIG_MASK     (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_ACK      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_ACK  (1 << 30)
+#define AFMT_VBI_PACKET_CONTROL              0x7608
+#       define AFMT_GENERIC0_UPDATE          (1 << 2)
+#define AFMT_INFOFRAME_CONTROL0              0x760c
+#       define AFMT_AUDIO_INFO_SOURCE        (1 << 6) /* 0 - sound block; 1 - hmdi regs */
+#       define AFMT_AUDIO_INFO_UPDATE        (1 << 7)
+#       define AFMT_MPEG_INFO_UPDATE         (1 << 10)
+#define AFMT_GENERIC0_7                      0x7610
+/* second instance starts at 0x7800 */
+#define HDMI_OFFSET0                      (0x7400 - 0x7400)
+#define HDMI_OFFSET1                      (0x7800 - 0x7400)
+
 #define D1GRPH_PRIMARY_SURFACE_ADDRESS                    0x6110
 #define D1GRPH_PRIMARY_SURFACE_ADDRESS_HIGH               0x6914
 #define D2GRPH_PRIMARY_SURFACE_ADDRESS_HIGH               0x6114
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 27bda986fc2b..549732e56ca9 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -2217,8 +2217,6 @@ bool si_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 	u32 srbm_status;
 	u32 grbm_status, grbm_status2;
 	u32 grbm_status_se0, grbm_status_se1;
-	struct r100_gpu_lockup *lockup = &rdev->config.si.lockup;
-	int r;
 
 	srbm_status = RREG32(SRBM_STATUS);
 	grbm_status = RREG32(GRBM_STATUS);
@@ -2226,20 +2224,12 @@ bool si_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
 	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
 	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	/* XXX deal with CP0,1,2 */
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 static int si_gpu_soft_reset(struct radeon_device *rdev)
@@ -2275,6 +2265,7 @@ static int si_gpu_soft_reset(struct radeon_device *rdev)
 		      SOFT_RESET_GDS |
 		      SOFT_RESET_PA |
 		      SOFT_RESET_SC |
+		      SOFT_RESET_BCI |
 		      SOFT_RESET_SPI |
 		      SOFT_RESET_SX |
 		      SOFT_RESET_TC |
@@ -2985,7 +2976,8 @@ int si_rlc_init(struct radeon_device *rdev)
 	/* save restore block */
 	if (rdev->rlc.save_restore_obj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_VRAM, &rdev->rlc.save_restore_obj);
+				     RADEON_GEM_DOMAIN_VRAM, NULL,
+				     &rdev->rlc.save_restore_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create RLC sr bo failed\n", r);
 			return r;
@@ -3009,7 +3001,8 @@ int si_rlc_init(struct radeon_device *rdev)
 	/* clear state block */
 	if (rdev->rlc.clear_state_obj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_VRAM, &rdev->rlc.clear_state_obj);
+				     RADEON_GEM_DOMAIN_VRAM, NULL,
+				     &rdev->rlc.clear_state_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create RLC c bo failed\n", r);
 			si_rlc_fini(rdev);
@@ -3216,6 +3209,8 @@ static int si_irq_init(struct radeon_device *rdev)
 	/* force the active interrupt state to all disabled */
 	si_disable_interrupt_state(rdev);
 
+	pci_set_master(rdev->pdev);
+
 	/* enable irqs */
 	si_enable_interrupts(rdev);
 
@@ -3994,10 +3989,6 @@ int si_init(struct radeon_device *rdev)
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -4117,7 +4108,6 @@ void si_fini(struct radeon_device *rdev)
 	si_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
diff --git a/drivers/gpu/drm/savage/savage_bci.c b/drivers/gpu/drm/savage/savage_bci.c
index cb1ee4e0050a..6eb507a5d130 100644
--- a/drivers/gpu/drm/savage/savage_bci.c
+++ b/drivers/gpu/drm/savage/savage_bci.c
@@ -735,7 +735,7 @@ static int savage_do_init_bci(struct drm_device * dev, drm_savage_init_t * init)
 			return -EINVAL;
 		}
 		drm_core_ioremap(dev->agp_buffer_map, dev);
-		if (!dev->agp_buffer_map) {
+		if (!dev->agp_buffer_map->handle) {
 			DRM_ERROR("failed to ioremap DMA buffer region!\n");
 			savage_do_cleanup_bci(dev);
 			return -ENOMEM;
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index 1f5c67c579cf..36792bd4da77 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -343,6 +343,16 @@ static int ttm_bo_add_ttm(struct ttm_buffer_object *bo, bool zero_alloc)
 		if (unlikely(bo->ttm == NULL))
 			ret = -ENOMEM;
 		break;
+	case ttm_bo_type_sg:
+		bo->ttm = bdev->driver->ttm_tt_create(bdev, bo->num_pages << PAGE_SHIFT,
+						      page_flags | TTM_PAGE_FLAG_SG,
+						      glob->dummy_read_page);
+		if (unlikely(bo->ttm == NULL)) {
+			ret = -ENOMEM;
+			break;
+		}
+		bo->ttm->sg = bo->sg;
+		break;
 	default:
 		pr_err("Illegal buffer object type\n");
 		ret = -EINVAL;
@@ -1169,6 +1179,7 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 		bool interruptible,
 		struct file *persistent_swap_storage,
 		size_t acc_size,
+		struct sg_table *sg,
 		void (*destroy) (struct ttm_buffer_object *))
 {
 	int ret = 0;
@@ -1223,6 +1234,7 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 	bo->seq_valid = false;
 	bo->persistent_swap_storage = persistent_swap_storage;
 	bo->acc_size = acc_size;
+	bo->sg = sg;
 	atomic_inc(&bo->glob->bo_count);
 
 	ret = ttm_bo_check_placement(bo, placement);
@@ -1233,7 +1245,8 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 	 * For ttm_bo_type_device buffers, allocate
 	 * address space from the device.
 	 */
-	if (bo->type == ttm_bo_type_device) {
+	if (bo->type == ttm_bo_type_device ||
+	    bo->type == ttm_bo_type_sg) {
 		ret = ttm_bo_setup_vm(bo);
 		if (ret)
 			goto out_err;
@@ -1312,7 +1325,7 @@ int ttm_bo_create(struct ttm_bo_device *bdev,
 
 	ret = ttm_bo_init(bdev, bo, size, type, placement, page_alignment,
 				buffer_start, interruptible,
-				persistent_swap_storage, acc_size, NULL);
+			  persistent_swap_storage, acc_size, NULL, NULL);
 	if (likely(ret == 0))
 		*p_bo = bo;
 
diff --git a/drivers/gpu/drm/udl/udl_drv.c b/drivers/gpu/drm/udl/udl_drv.c
index 53673907a6a0..4d02c46a9420 100644
--- a/drivers/gpu/drm/udl/udl_drv.c
+++ b/drivers/gpu/drm/udl/udl_drv.c
@@ -38,7 +38,7 @@ static void udl_usb_disconnect(struct usb_interface *interface)
 	drm_unplug_dev(dev);
 }
 
-static struct vm_operations_struct udl_gem_vm_ops = {
+static const struct vm_operations_struct udl_gem_vm_ops = {
 	.fault = udl_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
@@ -57,7 +57,7 @@ static const struct file_operations udl_driver_fops = {
 };
 
 static struct drm_driver driver = {
-	.driver_features = DRIVER_MODESET | DRIVER_GEM,
+	.driver_features = DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
 	.load = udl_driver_load,
 	.unload = udl_driver_unload,
 
@@ -70,6 +70,10 @@ static struct drm_driver driver = {
 	.dumb_map_offset = udl_gem_mmap,
 	.dumb_destroy = udl_dumb_destroy,
 	.fops = &udl_driver_fops,
+
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_import = udl_gem_prime_import,
+
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/udl/udl_drv.h b/drivers/gpu/drm/udl/udl_drv.h
index 96820d03a303..fccd361f7b50 100644
--- a/drivers/gpu/drm/udl/udl_drv.h
+++ b/drivers/gpu/drm/udl/udl_drv.h
@@ -66,6 +66,7 @@ struct udl_gem_object {
 	struct drm_gem_object base;
 	struct page **pages;
 	void *vmapping;
+	struct sg_table *sg;
 };
 
 #define to_udl_bo(x) container_of(x, struct udl_gem_object, base)
@@ -118,6 +119,8 @@ int udl_gem_init_object(struct drm_gem_object *obj);
 void udl_gem_free_object(struct drm_gem_object *gem_obj);
 struct udl_gem_object *udl_gem_alloc_object(struct drm_device *dev,
 					    size_t size);
+struct drm_gem_object *udl_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf);
 
 int udl_gem_vmap(struct udl_gem_object *obj);
 void udl_gem_vunmap(struct udl_gem_object *obj);
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index 4d9c3a5d8a45..a029ee39b0c5 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -593,11 +593,20 @@ udl_fb_user_fb_create(struct drm_device *dev,
 	struct drm_gem_object *obj;
 	struct udl_framebuffer *ufb;
 	int ret;
+	uint32_t size;
 
 	obj = drm_gem_object_lookup(dev, file, mode_cmd->handles[0]);
 	if (obj == NULL)
 		return ERR_PTR(-ENOENT);
 
+	size = mode_cmd->pitches[0] * mode_cmd->height;
+	size = ALIGN(size, PAGE_SIZE);
+
+	if (size > obj->size) {
+		DRM_ERROR("object size not sufficient for fb %d %zu %d %d\n", size, obj->size, mode_cmd->pitches[0], mode_cmd->height);
+		return ERR_PTR(-ENOMEM);
+	}
+
 	ufb = kzalloc(sizeof(*ufb), GFP_KERNEL);
 	if (ufb == NULL)
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/gpu/drm/udl/udl_gem.c b/drivers/gpu/drm/udl/udl_gem.c
index 92f19ef329b0..40efd32f7dce 100644
--- a/drivers/gpu/drm/udl/udl_gem.c
+++ b/drivers/gpu/drm/udl/udl_gem.c
@@ -9,6 +9,7 @@
 #include "drmP.h"
 #include "udl_drv.h"
 #include <linux/shmem_fs.h>
+#include <linux/dma-buf.h>
 
 struct udl_gem_object *udl_gem_alloc_object(struct drm_device *dev,
 					    size_t size)
@@ -161,6 +162,12 @@ static void udl_gem_put_pages(struct udl_gem_object *obj)
 	int page_count = obj->base.size / PAGE_SIZE;
 	int i;
 
+	if (obj->base.import_attach) {
+		drm_free_large(obj->pages);
+		obj->pages = NULL;
+		return;
+	}
+
 	for (i = 0; i < page_count; i++)
 		page_cache_release(obj->pages[i]);
 
@@ -195,6 +202,9 @@ void udl_gem_free_object(struct drm_gem_object *gem_obj)
 {
 	struct udl_gem_object *obj = to_udl_bo(gem_obj);
 
+	if (gem_obj->import_attach)
+		drm_prime_gem_destroy(gem_obj, obj->sg);
+
 	if (obj->vmapping)
 		udl_gem_vunmap(obj);
 
@@ -239,3 +249,68 @@ unlock:
 	mutex_unlock(&dev->struct_mutex);
 	return ret;
 }
+
+static int udl_prime_create(struct drm_device *dev,
+			    size_t size,
+			    struct sg_table *sg,
+			    struct udl_gem_object **obj_p)
+{
+	struct udl_gem_object *obj;
+	int npages;
+	int i;
+	struct scatterlist *iter;
+
+	npages = size / PAGE_SIZE;
+
+	*obj_p = NULL;
+	obj = udl_gem_alloc_object(dev, npages * PAGE_SIZE);
+	if (!obj)
+		return -ENOMEM;
+
+	obj->sg = sg;
+	obj->pages = drm_malloc_ab(npages, sizeof(struct page *));
+	if (obj->pages == NULL) {
+		DRM_ERROR("obj pages is NULL %d\n", npages);
+		return -ENOMEM;
+	}
+
+	drm_prime_sg_to_page_addr_arrays(sg, obj->pages, NULL, npages);
+
+	*obj_p = obj;
+	return 0;
+}
+
+struct drm_gem_object *udl_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct udl_gem_object *uobj;
+	int ret;
+
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_PTR(PTR_ERR(attach));
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	ret = udl_prime_create(dev, dma_buf->size, sg, &uobj);
+	if (ret) {
+		goto fail_unmap;
+	}
+
+	uobj->base.import_attach = attach;
+
+	return &uobj->base;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/udl/udl_modeset.c b/drivers/gpu/drm/udl/udl_modeset.c
index b3ecb3d12a1d..0d7816789da1 100644
--- a/drivers/gpu/drm/udl/udl_modeset.c
+++ b/drivers/gpu/drm/udl/udl_modeset.c
@@ -395,7 +395,7 @@ int udl_modeset_init(struct drm_device *dev)
 	dev->mode_config.prefer_shadow = 0;
 	dev->mode_config.preferred_depth = 24;
 
-	dev->mode_config.funcs = (void *)&udl_mode_funcs;
+	dev->mode_config.funcs = &udl_mode_funcs;
 
 	drm_mode_create_dirty_info_property(dev);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index 2286d47e5022..6b0078ffa763 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -1178,7 +1178,7 @@ err_out:
 	return &vfb->base;
 }
 
-static struct drm_mode_config_funcs vmw_kms_funcs = {
+static const struct drm_mode_config_funcs vmw_kms_funcs = {
 	.fb_create = vmw_kms_fb_create,
 };
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
index a37abb581cbb..22bf9a21ec71 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
@@ -1567,7 +1567,7 @@ int vmw_dmabuf_init(struct vmw_private *dev_priv,
 	ret = ttm_bo_init(bdev, &vmw_bo->base, size,
 			  ttm_bo_type_device, placement,
 			  0, 0, interruptible,
-			  NULL, acc_size, bo_free);
+			  NULL, acc_size, NULL, bo_free);
 	return ret;
 }
 
diff --git a/drivers/gpu/vga/Kconfig b/drivers/gpu/vga/Kconfig
index 96c83a9a76bb..f34838839b08 100644
--- a/drivers/gpu/vga/Kconfig
+++ b/drivers/gpu/vga/Kconfig
@@ -21,6 +21,7 @@ config VGA_SWITCHEROO
 	bool "Laptop Hybrid Graphics - GPU switching support"
 	depends on X86
 	depends on ACPI
+	select VGA_ARB
 	help
 	  Many laptops released in 2008/9/10 have two GPUs with a multiplexer
 	  to switch between them. This adds support for dynamic switching when
diff --git a/drivers/gpu/vga/vga_switcheroo.c b/drivers/gpu/vga/vga_switcheroo.c
index 58434e804d91..38f9534ac513 100644
--- a/drivers/gpu/vga/vga_switcheroo.c
+++ b/drivers/gpu/vga/vga_switcheroo.c
@@ -28,15 +28,16 @@
 #include <linux/pci.h>
 #include <linux/vga_switcheroo.h>
 
+#include <linux/vgaarb.h>
+
 struct vga_switcheroo_client {
 	struct pci_dev *pdev;
 	struct fb_info *fb_info;
 	int pwr_state;
-	void (*set_gpu_state)(struct pci_dev *pdev, enum vga_switcheroo_state);
-	void (*reprobe)(struct pci_dev *pdev);
-	bool (*can_switch)(struct pci_dev *pdev);
+	const struct vga_switcheroo_client_ops *ops;
 	int id;
 	bool active;
+	struct list_head list;
 };
 
 static DEFINE_MUTEX(vgasr_mutex);
@@ -51,16 +52,23 @@ struct vgasr_priv {
 	struct dentry *switch_file;
 
 	int registered_clients;
-	struct vga_switcheroo_client clients[VGA_SWITCHEROO_MAX_CLIENTS];
+	struct list_head clients;
 
 	struct vga_switcheroo_handler *handler;
 };
 
+#define ID_BIT_AUDIO		0x100
+#define client_is_audio(c)	((c)->id & ID_BIT_AUDIO)
+#define client_is_vga(c)	((c)->id == -1 || !client_is_audio(c))
+#define client_id(c)		((c)->id & ~ID_BIT_AUDIO)
+
 static int vga_switcheroo_debugfs_init(struct vgasr_priv *priv);
 static void vga_switcheroo_debugfs_fini(struct vgasr_priv *priv);
 
 /* only one switcheroo per system */
-static struct vgasr_priv vgasr_priv;
+static struct vgasr_priv vgasr_priv = {
+	.clients = LIST_HEAD_INIT(vgasr_priv.clients),
+};
 
 int vga_switcheroo_register_handler(struct vga_switcheroo_handler *handler)
 {
@@ -86,72 +94,119 @@ EXPORT_SYMBOL(vga_switcheroo_unregister_handler);
 
 static void vga_switcheroo_enable(void)
 {
-	int i;
 	int ret;
+	struct vga_switcheroo_client *client;
+
 	/* call the handler to init */
 	vgasr_priv.handler->init();
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		ret = vgasr_priv.handler->get_client_id(vgasr_priv.clients[i].pdev);
+	list_for_each_entry(client, &vgasr_priv.clients, list) {
+		if (client->id != -1)
+			continue;
+		ret = vgasr_priv.handler->get_client_id(client->pdev);
 		if (ret < 0)
 			return;
 
-		vgasr_priv.clients[i].id = ret;
+		client->id = ret;
 	}
 	vga_switcheroo_debugfs_init(&vgasr_priv);
 	vgasr_priv.active = true;
 }
 
-int vga_switcheroo_register_client(struct pci_dev *pdev,
-				   void (*set_gpu_state)(struct pci_dev *pdev, enum vga_switcheroo_state),
-				   void (*reprobe)(struct pci_dev *pdev),
-				   bool (*can_switch)(struct pci_dev *pdev))
+static int register_client(struct pci_dev *pdev,
+			   const struct vga_switcheroo_client_ops *ops,
+			   int id, bool active)
 {
-	int index;
+	struct vga_switcheroo_client *client;
+
+	client = kzalloc(sizeof(*client), GFP_KERNEL);
+	if (!client)
+		return -ENOMEM;
+
+	client->pwr_state = VGA_SWITCHEROO_ON;
+	client->pdev = pdev;
+	client->ops = ops;
+	client->id = id;
+	client->active = active;
 
 	mutex_lock(&vgasr_mutex);
-	/* don't do IGD vs DIS here */
-	if (vgasr_priv.registered_clients & 1)
-		index = 1;
-	else
-		index = 0;
-
-	vgasr_priv.clients[index].pwr_state = VGA_SWITCHEROO_ON;
-	vgasr_priv.clients[index].pdev = pdev;
-	vgasr_priv.clients[index].set_gpu_state = set_gpu_state;
-	vgasr_priv.clients[index].reprobe = reprobe;
-	vgasr_priv.clients[index].can_switch = can_switch;
-	vgasr_priv.clients[index].id = -1;
-	if (pdev->resource[PCI_ROM_RESOURCE].flags & IORESOURCE_ROM_SHADOW)
-		vgasr_priv.clients[index].active = true;
-
-	vgasr_priv.registered_clients |= (1 << index);
+	list_add_tail(&client->list, &vgasr_priv.clients);
+	if (client_is_vga(client))
+		vgasr_priv.registered_clients++;
 
 	/* if we get two clients + handler */
-	if (vgasr_priv.registered_clients == 0x3 && vgasr_priv.handler) {
+	if (!vgasr_priv.active &&
+	    vgasr_priv.registered_clients == 2 && vgasr_priv.handler) {
 		printk(KERN_INFO "vga_switcheroo: enabled\n");
 		vga_switcheroo_enable();
 	}
 	mutex_unlock(&vgasr_mutex);
 	return 0;
 }
+
+int vga_switcheroo_register_client(struct pci_dev *pdev,
+				   const struct vga_switcheroo_client_ops *ops)
+{
+	return register_client(pdev, ops, -1,
+			       pdev == vga_default_device());
+}
 EXPORT_SYMBOL(vga_switcheroo_register_client);
 
+int vga_switcheroo_register_audio_client(struct pci_dev *pdev,
+					 const struct vga_switcheroo_client_ops *ops,
+					 int id, bool active)
+{
+	return register_client(pdev, ops, id | ID_BIT_AUDIO, active);
+}
+EXPORT_SYMBOL(vga_switcheroo_register_audio_client);
+
+static struct vga_switcheroo_client *
+find_client_from_pci(struct list_head *head, struct pci_dev *pdev)
+{
+	struct vga_switcheroo_client *client;
+	list_for_each_entry(client, head, list)
+		if (client->pdev == pdev)
+			return client;
+	return NULL;
+}
+
+static struct vga_switcheroo_client *
+find_client_from_id(struct list_head *head, int client_id)
+{
+	struct vga_switcheroo_client *client;
+	list_for_each_entry(client, head, list)
+		if (client->id == client_id)
+			return client;
+	return NULL;
+}
+
+static struct vga_switcheroo_client *
+find_active_client(struct list_head *head)
+{
+	struct vga_switcheroo_client *client;
+	list_for_each_entry(client, head, list)
+		if (client->active && client_is_vga(client))
+			return client;
+	return NULL;
+}
+
 void vga_switcheroo_unregister_client(struct pci_dev *pdev)
 {
-	int i;
+	struct vga_switcheroo_client *client;
 
 	mutex_lock(&vgasr_mutex);
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].pdev == pdev) {
-			vgasr_priv.registered_clients &= ~(1 << i);
-			break;
-		}
+	client = find_client_from_pci(&vgasr_priv.clients, pdev);
+	if (client) {
+		if (client_is_vga(client))
+			vgasr_priv.registered_clients--;
+		list_del(&client->list);
+		kfree(client);
+	}
+	if (vgasr_priv.active && vgasr_priv.registered_clients < 2) {
+		printk(KERN_INFO "vga_switcheroo: disabled\n");
+		vga_switcheroo_debugfs_fini(&vgasr_priv);
+		vgasr_priv.active = false;
 	}
-
-	printk(KERN_INFO "vga_switcheroo: disabled\n");
-	vga_switcheroo_debugfs_fini(&vgasr_priv);
-	vgasr_priv.active = false;
 	mutex_unlock(&vgasr_mutex);
 }
 EXPORT_SYMBOL(vga_switcheroo_unregister_client);
@@ -159,29 +214,29 @@ EXPORT_SYMBOL(vga_switcheroo_unregister_client);
 void vga_switcheroo_client_fb_set(struct pci_dev *pdev,
 				 struct fb_info *info)
 {
-	int i;
+	struct vga_switcheroo_client *client;
 
 	mutex_lock(&vgasr_mutex);
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].pdev == pdev) {
-			vgasr_priv.clients[i].fb_info = info;
-			break;
-		}
-	}
+	client = find_client_from_pci(&vgasr_priv.clients, pdev);
+	if (client)
+		client->fb_info = info;
 	mutex_unlock(&vgasr_mutex);
 }
 EXPORT_SYMBOL(vga_switcheroo_client_fb_set);
 
 static int vga_switcheroo_show(struct seq_file *m, void *v)
 {
-	int i;
+	struct vga_switcheroo_client *client;
+	int i = 0;
 	mutex_lock(&vgasr_mutex);
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		seq_printf(m, "%d:%s:%c:%s:%s\n", i,
-			   vgasr_priv.clients[i].id == VGA_SWITCHEROO_DIS ? "DIS" : "IGD",
-			   vgasr_priv.clients[i].active ? '+' : ' ',
-			   vgasr_priv.clients[i].pwr_state ? "Pwr" : "Off",
-			   pci_name(vgasr_priv.clients[i].pdev));
+	list_for_each_entry(client, &vgasr_priv.clients, list) {
+		seq_printf(m, "%d:%s%s:%c:%s:%s\n", i,
+			   client_id(client) == VGA_SWITCHEROO_DIS ? "DIS" : "IGD",
+			   client_is_vga(client) ? "" : "-Audio",
+			   client->active ? '+' : ' ',
+			   client->pwr_state ? "Pwr" : "Off",
+			   pci_name(client->pdev));
+		i++;
 	}
 	mutex_unlock(&vgasr_mutex);
 	return 0;
@@ -197,7 +252,7 @@ static int vga_switchon(struct vga_switcheroo_client *client)
 	if (vgasr_priv.handler->power_state)
 		vgasr_priv.handler->power_state(client->id, VGA_SWITCHEROO_ON);
 	/* call the driver callback to turn on device */
-	client->set_gpu_state(client->pdev, VGA_SWITCHEROO_ON);
+	client->ops->set_gpu_state(client->pdev, VGA_SWITCHEROO_ON);
 	client->pwr_state = VGA_SWITCHEROO_ON;
 	return 0;
 }
@@ -205,34 +260,39 @@ static int vga_switchon(struct vga_switcheroo_client *client)
 static int vga_switchoff(struct vga_switcheroo_client *client)
 {
 	/* call the driver callback to turn off device */
-	client->set_gpu_state(client->pdev, VGA_SWITCHEROO_OFF);
+	client->ops->set_gpu_state(client->pdev, VGA_SWITCHEROO_OFF);
 	if (vgasr_priv.handler->power_state)
 		vgasr_priv.handler->power_state(client->id, VGA_SWITCHEROO_OFF);
 	client->pwr_state = VGA_SWITCHEROO_OFF;
 	return 0;
 }
 
+static void set_audio_state(int id, int state)
+{
+	struct vga_switcheroo_client *client;
+
+	client = find_client_from_id(&vgasr_priv.clients, id | ID_BIT_AUDIO);
+	if (client && client->pwr_state != state) {
+		client->ops->set_gpu_state(client->pdev, state);
+		client->pwr_state = state;
+	}
+}
+
 /* stage one happens before delay */
 static int vga_switchto_stage1(struct vga_switcheroo_client *new_client)
 {
-	int i;
-	struct vga_switcheroo_client *active = NULL;
+	struct vga_switcheroo_client *active;
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].active == true) {
-			active = &vgasr_priv.clients[i];
-			break;
-		}
-	}
+	active = find_active_client(&vgasr_priv.clients);
 	if (!active)
 		return 0;
 
 	if (new_client->pwr_state == VGA_SWITCHEROO_OFF)
 		vga_switchon(new_client);
 
-	/* swap shadow resource to denote boot VGA device has changed so X starts on new device */
-	active->pdev->resource[PCI_ROM_RESOURCE].flags &= ~IORESOURCE_ROM_SHADOW;
-	new_client->pdev->resource[PCI_ROM_RESOURCE].flags |= IORESOURCE_ROM_SHADOW;
+	vga_set_default_device(new_client->pdev);
+	set_audio_state(new_client->id, VGA_SWITCHEROO_ON);
+
 	return 0;
 }
 
@@ -240,15 +300,9 @@ static int vga_switchto_stage1(struct vga_switcheroo_client *new_client)
 static int vga_switchto_stage2(struct vga_switcheroo_client *new_client)
 {
 	int ret;
-	int i;
-	struct vga_switcheroo_client *active = NULL;
+	struct vga_switcheroo_client *active;
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].active == true) {
-			active = &vgasr_priv.clients[i];
-			break;
-		}
-	}
+	active = find_active_client(&vgasr_priv.clients);
 	if (!active)
 		return 0;
 
@@ -264,8 +318,10 @@ static int vga_switchto_stage2(struct vga_switcheroo_client *new_client)
 	if (ret)
 		return ret;
 
-	if (new_client->reprobe)
-		new_client->reprobe(new_client->pdev);
+	if (new_client->ops->reprobe)
+		new_client->ops->reprobe(new_client->pdev);
+
+	set_audio_state(active->id, VGA_SWITCHEROO_OFF);
 
 	if (active->pwr_state == VGA_SWITCHEROO_ON)
 		vga_switchoff(active);
@@ -274,13 +330,26 @@ static int vga_switchto_stage2(struct vga_switcheroo_client *new_client)
 	return 0;
 }
 
+static bool check_can_switch(void)
+{
+	struct vga_switcheroo_client *client;
+
+	list_for_each_entry(client, &vgasr_priv.clients, list) {
+		if (!client->ops->can_switch(client->pdev)) {
+			printk(KERN_ERR "vga_switcheroo: client %x refused switch\n", client->id);
+			return false;
+		}
+	}
+	return true;
+}
+
 static ssize_t
 vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 			     size_t cnt, loff_t *ppos)
 {
 	char usercmd[64];
 	const char *pdev_name;
-	int i, ret;
+	int ret;
 	bool delay = false, can_switch;
 	bool just_mux = false;
 	int client_id = -1;
@@ -301,21 +370,21 @@ vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 
 	/* pwr off the device not in use */
 	if (strncmp(usercmd, "OFF", 3) == 0) {
-		for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-			if (vgasr_priv.clients[i].active)
+		list_for_each_entry(client, &vgasr_priv.clients, list) {
+			if (client->active)
 				continue;
-			if (vgasr_priv.clients[i].pwr_state == VGA_SWITCHEROO_ON)
-				vga_switchoff(&vgasr_priv.clients[i]);
+			if (client->pwr_state == VGA_SWITCHEROO_ON)
+				vga_switchoff(client);
 		}
 		goto out;
 	}
 	/* pwr on the device not in use */
 	if (strncmp(usercmd, "ON", 2) == 0) {
-		for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-			if (vgasr_priv.clients[i].active)
+		list_for_each_entry(client, &vgasr_priv.clients, list) {
+			if (client->active)
 				continue;
-			if (vgasr_priv.clients[i].pwr_state == VGA_SWITCHEROO_OFF)
-				vga_switchon(&vgasr_priv.clients[i]);
+			if (client->pwr_state == VGA_SWITCHEROO_OFF)
+				vga_switchon(client);
 		}
 		goto out;
 	}
@@ -348,13 +417,9 @@ vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 
 	if (client_id == -1)
 		goto out;
-
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].id == client_id) {
-			client = &vgasr_priv.clients[i];
-			break;
-		}
-	}
+	client = find_client_from_id(&vgasr_priv.clients, client_id);
+	if (!client)
+		goto out;
 
 	vgasr_priv.delayed_switch_active = false;
 
@@ -363,23 +428,16 @@ vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 		goto out;
 	}
 
-	if (client->active == true)
+	if (client->active)
 		goto out;
 
 	/* okay we want a switch - test if devices are willing to switch */
-	can_switch = true;
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		can_switch = vgasr_priv.clients[i].can_switch(vgasr_priv.clients[i].pdev);
-		if (can_switch == false) {
-			printk(KERN_ERR "vga_switcheroo: client %d refused switch\n", i);
-			break;
-		}
-	}
+	can_switch = check_can_switch();
 
 	if (can_switch == false && delay == false)
 		goto out;
 
-	if (can_switch == true) {
+	if (can_switch) {
 		pdev_name = pci_name(client->pdev);
 		ret = vga_switchto_stage1(client);
 		if (ret)
@@ -451,10 +509,8 @@ fail:
 
 int vga_switcheroo_process_delayed_switch(void)
 {
-	struct vga_switcheroo_client *client = NULL;
+	struct vga_switcheroo_client *client;
 	const char *pdev_name;
-	bool can_switch = true;
-	int i;
 	int ret;
 	int err = -EINVAL;
 
@@ -464,17 +520,9 @@ int vga_switcheroo_process_delayed_switch(void)
 
 	printk(KERN_INFO "vga_switcheroo: processing delayed switch to %d\n", vgasr_priv.delayed_client_id);
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].id == vgasr_priv.delayed_client_id)
-			client = &vgasr_priv.clients[i];
-		can_switch = vgasr_priv.clients[i].can_switch(vgasr_priv.clients[i].pdev);
-		if (can_switch == false) {
-			printk(KERN_ERR "vga_switcheroo: client %d refused switch\n", i);
-			break;
-		}
-	}
-
-	if (can_switch == false || client == NULL)
+	client = find_client_from_id(&vgasr_priv.clients,
+				     vgasr_priv.delayed_client_id);
+	if (!client || !check_can_switch())
 		goto err;
 
 	pdev_name = pci_name(client->pdev);
diff --git a/drivers/gpu/vga/vgaarb.c b/drivers/gpu/vga/vgaarb.c
index 111d956d8e7d..3df8fc0ec01a 100644
--- a/drivers/gpu/vga/vgaarb.c
+++ b/drivers/gpu/vga/vgaarb.c
@@ -136,6 +136,13 @@ struct pci_dev *vga_default_device(void)
 {
 	return vga_default;
 }
+
+EXPORT_SYMBOL_GPL(vga_default_device);
+
+void vga_set_default_device(struct pci_dev *pdev)
+{
+	vga_default = pdev;
+}
 #endif
 
 static inline void vga_irq_set_state(struct vga_device *vgadev, bool state)
@@ -605,10 +612,12 @@ static bool vga_arbiter_del_pci_device(struct pci_dev *pdev)
 		goto bail;
 	}
 
+#ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
 	if (vga_default == pdev) {
 		pci_dev_put(vga_default);
 		vga_default = NULL;
 	}
+#endif
 
 	if (vgadev->decodes & (VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM))
 		vga_decode_count--;
diff --git a/drivers/input/Kconfig b/drivers/input/Kconfig
index 332597980817..55f7e57d4e42 100644
--- a/drivers/input/Kconfig
+++ b/drivers/input/Kconfig
@@ -25,10 +25,6 @@ config INPUT
 
 if INPUT
 
-config INPUT_OF_MATRIX_KEYMAP
-	depends on USE_OF
-	bool
-
 config INPUT_FF_MEMLESS
 	tristate "Support for memoryless force-feedback devices"
 	help
@@ -68,6 +64,19 @@ config INPUT_SPARSEKMAP
 	  To compile this driver as a module, choose M here: the
 	  module will be called sparse-keymap.
 
+config INPUT_MATRIXKMAP
+	tristate "Matrix keymap support library"
+	help
+	  Say Y here if you are using a driver for an input
+	  device that uses matrix keymap. This option is only
+	  useful for out-of-tree drivers since in-tree drivers
+	  select it automatically.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called matrix-keymap.
+
 comment "Userland interfaces"
 
 config INPUT_MOUSEDEV
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index b173a13a73ca..5ca3f631497f 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -10,6 +10,7 @@ input-core-y := input.o input-compat.o input-mt.o ff-core.o
 obj-$(CONFIG_INPUT_FF_MEMLESS)	+= ff-memless.o
 obj-$(CONFIG_INPUT_POLLDEV)	+= input-polldev.o
 obj-$(CONFIG_INPUT_SPARSEKMAP)	+= sparse-keymap.o
+obj-$(CONFIG_INPUT_MATRIXKMAP)	+= matrix-keymap.o
 
 obj-$(CONFIG_INPUT_MOUSEDEV)	+= mousedev.o
 obj-$(CONFIG_INPUT_JOYDEV)	+= joydev.o
@@ -24,4 +25,3 @@ obj-$(CONFIG_INPUT_TOUCHSCREEN)	+= touchscreen/
 obj-$(CONFIG_INPUT_MISC)	+= misc/
 
 obj-$(CONFIG_INPUT_APMPOWER)	+= apm-power.o
-obj-$(CONFIG_INPUT_OF_MATRIX_KEYMAP) += of_keymap.o
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 4b2e10d5d641..6c58bfff01a3 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -180,7 +180,10 @@ static int evdev_grab(struct evdev *evdev, struct evdev_client *client)
 
 static int evdev_ungrab(struct evdev *evdev, struct evdev_client *client)
 {
-	if (evdev->grab != client)
+	struct evdev_client *grab = rcu_dereference_protected(evdev->grab,
+					lockdep_is_held(&evdev->mutex));
+
+	if (grab != client)
 		return  -EINVAL;
 
 	rcu_assign_pointer(evdev->grab, NULL);
@@ -259,8 +262,7 @@ static int evdev_release(struct inode *inode, struct file *file)
 	struct evdev *evdev = client->evdev;
 
 	mutex_lock(&evdev->mutex);
-	if (evdev->grab == client)
-		evdev_ungrab(evdev, client);
+	evdev_ungrab(evdev, client);
 	mutex_unlock(&evdev->mutex);
 
 	evdev_detach_client(evdev, client);
@@ -343,7 +345,7 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 	struct input_event event;
 	int retval = 0;
 
-	if (count < input_event_size())
+	if (count != 0 && count < input_event_size())
 		return -EINVAL;
 
 	retval = mutex_lock_interruptible(&evdev->mutex);
@@ -355,7 +357,8 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 		goto out;
 	}
 
-	do {
+	while (retval + input_event_size() <= count) {
+
 		if (input_event_from_user(buffer + retval, &event)) {
 			retval = -EFAULT;
 			goto out;
@@ -364,7 +367,7 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 
 		input_inject_event(&evdev->handle,
 				   event.type, event.code, event.value);
-	} while (retval + input_event_size() <= count);
+	}
 
  out:
 	mutex_unlock(&evdev->mutex);
@@ -395,35 +398,49 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	struct evdev_client *client = file->private_data;
 	struct evdev *evdev = client->evdev;
 	struct input_event event;
-	int retval = 0;
+	size_t read = 0;
+	int error;
 
-	if (count < input_event_size())
+	if (count != 0 && count < input_event_size())
 		return -EINVAL;
 
-	if (!(file->f_flags & O_NONBLOCK)) {
-		retval = wait_event_interruptible(evdev->wait,
-				client->packet_head != client->tail ||
-				!evdev->exist);
-		if (retval)
-			return retval;
-	}
+	for (;;) {
+		if (!evdev->exist)
+			return -ENODEV;
 
-	if (!evdev->exist)
-		return -ENODEV;
+		if (client->packet_head == client->tail &&
+		    (file->f_flags & O_NONBLOCK))
+			return -EAGAIN;
 
-	while (retval + input_event_size() <= count &&
-	       evdev_fetch_next_event(client, &event)) {
+		/*
+		 * count == 0 is special - no IO is done but we check
+		 * for error conditions (see above).
+		 */
+		if (count == 0)
+			break;
 
-		if (input_event_to_user(buffer + retval, &event))
-			return -EFAULT;
+		while (read + input_event_size() <= count &&
+		       evdev_fetch_next_event(client, &event)) {
 
-		retval += input_event_size();
-	}
+			if (input_event_to_user(buffer + read, &event))
+				return -EFAULT;
 
-	if (retval == 0 && (file->f_flags & O_NONBLOCK))
-		return -EAGAIN;
+			read += input_event_size();
+		}
 
-	return retval;
+		if (read)
+			break;
+
+		if (!(file->f_flags & O_NONBLOCK)) {
+			error = wait_event_interruptible(evdev->wait,
+					client->packet_head != client->tail ||
+					!evdev->exist);
+			if (error)
+				return error;
+		}
+	}
+
+	return read;
 }
 
 /* No kernel lock - fine */
diff --git a/drivers/input/ff-memless.c b/drivers/input/ff-memless.c
index 117a59aaa70e..5f558851d646 100644
--- a/drivers/input/ff-memless.c
+++ b/drivers/input/ff-memless.c
@@ -31,8 +31,7 @@
 #include <linux/mutex.h>
 #include <linux/spinlock.h>
 #include <linux/jiffies.h>
-
-#include "fixp-arith.h"
+#include <linux/fixp-arith.h>
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>");
diff --git a/drivers/input/gameport/emu10k1-gp.c b/drivers/input/gameport/emu10k1-gp.c
index 422aa0a6b77f..daceafe7ee7d 100644
--- a/drivers/input/gameport/emu10k1-gp.c
+++ b/drivers/input/gameport/emu10k1-gp.c
@@ -125,15 +125,4 @@ static struct pci_driver emu_driver = {
         .remove =       __devexit_p(emu_remove),
 };
 
-static int __init emu_init(void)
-{
-	return pci_register_driver(&emu_driver);
-}
-
-static void __exit emu_exit(void)
-{
-	pci_unregister_driver(&emu_driver);
-}
-
-module_init(emu_init);
-module_exit(emu_exit);
+module_pci_driver(emu_driver);
diff --git a/drivers/input/gameport/fm801-gp.c b/drivers/input/gameport/fm801-gp.c
index a3b70ff21018..48ad3829ff20 100644
--- a/drivers/input/gameport/fm801-gp.c
+++ b/drivers/input/gameport/fm801-gp.c
@@ -144,6 +144,7 @@ static const struct pci_device_id fm801_gp_id_table[] = {
 	{ PCI_VENDOR_ID_FORTEMEDIA, PCI_DEVICE_ID_FM801_GP, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0  },
 	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, fm801_gp_id_table);
 
 static struct pci_driver fm801_gp_driver = {
 	.name =		"FM801_gameport",
@@ -152,20 +153,7 @@ static struct pci_driver fm801_gp_driver = {
 	.remove =	__devexit_p(fm801_gp_remove),
 };
 
-static int __init fm801_gp_init(void)
-{
-	return pci_register_driver(&fm801_gp_driver);
-}
-
-static void __exit fm801_gp_exit(void)
-{
-	pci_unregister_driver(&fm801_gp_driver);
-}
-
-module_init(fm801_gp_init);
-module_exit(fm801_gp_exit);
-
-MODULE_DEVICE_TABLE(pci, fm801_gp_id_table);
+module_pci_driver(fm801_gp_driver);
 
 MODULE_DESCRIPTION("FM801 gameport driver");
 MODULE_AUTHOR("Takashi Iwai <tiwai@suse.de>");
diff --git a/drivers/input/joystick/a3d.c b/drivers/input/joystick/a3d.c
index 1639ab2b94b7..85bc8dc07cfc 100644
--- a/drivers/input/joystick/a3d.c
+++ b/drivers/input/joystick/a3d.c
@@ -413,15 +413,4 @@ static struct gameport_driver a3d_drv = {
 	.disconnect	= a3d_disconnect,
 };
 
-static int __init a3d_init(void)
-{
-	return gameport_register_driver(&a3d_drv);
-}
-
-static void __exit a3d_exit(void)
-{
-	gameport_unregister_driver(&a3d_drv);
-}
-
-module_init(a3d_init);
-module_exit(a3d_exit);
+module_gameport_driver(a3d_drv);
diff --git a/drivers/input/joystick/adi.c b/drivers/input/joystick/adi.c
index b992fbf91f2f..0cbfd2dfabf4 100644
--- a/drivers/input/joystick/adi.c
+++ b/drivers/input/joystick/adi.c
@@ -557,10 +557,6 @@ static void adi_disconnect(struct gameport *gameport)
 	kfree(port);
 }
 
-/*
- * The gameport device structure.
- */
-
 static struct gameport_driver adi_drv = {
 	.driver		= {
 		.name	= "adi",
@@ -570,15 +566,4 @@ static struct gameport_driver adi_drv = {
 	.disconnect	= adi_disconnect,
 };
 
-static int __init adi_init(void)
-{
-	return gameport_register_driver(&adi_drv);
-}
-
-static void __exit adi_exit(void)
-{
-	gameport_unregister_driver(&adi_drv);
-}
-
-module_init(adi_init);
-module_exit(adi_exit);
+module_gameport_driver(adi_drv);
diff --git a/drivers/input/joystick/cobra.c b/drivers/input/joystick/cobra.c
index 3497b87c3d05..65367e44d715 100644
--- a/drivers/input/joystick/cobra.c
+++ b/drivers/input/joystick/cobra.c
@@ -261,15 +261,4 @@ static struct gameport_driver cobra_drv = {
 	.disconnect	= cobra_disconnect,
 };
 
-static int __init cobra_init(void)
-{
-	return gameport_register_driver(&cobra_drv);
-}
-
-static void __exit cobra_exit(void)
-{
-	gameport_unregister_driver(&cobra_drv);
-}
-
-module_init(cobra_init);
-module_exit(cobra_exit);
+module_gameport_driver(cobra_drv);
diff --git a/drivers/input/joystick/gf2k.c b/drivers/input/joystick/gf2k.c
index 0536b1b2f018..ab1cf2882004 100644
--- a/drivers/input/joystick/gf2k.c
+++ b/drivers/input/joystick/gf2k.c
@@ -373,15 +373,4 @@ static struct gameport_driver gf2k_drv = {
 	.disconnect	= gf2k_disconnect,
 };
 
-static int __init gf2k_init(void)
-{
-	return gameport_register_driver(&gf2k_drv);
-}
-
-static void __exit gf2k_exit(void)
-{
-	gameport_unregister_driver(&gf2k_drv);
-}
-
-module_init(gf2k_init);
-module_exit(gf2k_exit);
+module_gameport_driver(gf2k_drv);
diff --git a/drivers/input/joystick/grip.c b/drivers/input/joystick/grip.c
index fc55899ba6c5..9e1beff57c33 100644
--- a/drivers/input/joystick/grip.c
+++ b/drivers/input/joystick/grip.c
@@ -424,15 +424,4 @@ static struct gameport_driver grip_drv = {
 	.disconnect	= grip_disconnect,
 };
 
-static int __init grip_init(void)
-{
-	return gameport_register_driver(&grip_drv);
-}
-
-static void __exit grip_exit(void)
-{
-	gameport_unregister_driver(&grip_drv);
-}
-
-module_init(grip_init);
-module_exit(grip_exit);
+module_gameport_driver(grip_drv);
diff --git a/drivers/input/joystick/grip_mp.c b/drivers/input/joystick/grip_mp.c
index 2d47baf47769..c0f9c7b7eb4e 100644
--- a/drivers/input/joystick/grip_mp.c
+++ b/drivers/input/joystick/grip_mp.c
@@ -687,15 +687,4 @@ static struct gameport_driver grip_drv = {
 	.disconnect	= grip_disconnect,
 };
 
-static int __init grip_init(void)
-{
-	return gameport_register_driver(&grip_drv);
-}
-
-static void __exit grip_exit(void)
-{
-	gameport_unregister_driver(&grip_drv);
-}
-
-module_init(grip_init);
-module_exit(grip_exit);
+module_gameport_driver(grip_drv);
diff --git a/drivers/input/joystick/guillemot.c b/drivers/input/joystick/guillemot.c
index 4058d4b272fe..55196f730af6 100644
--- a/drivers/input/joystick/guillemot.c
+++ b/drivers/input/joystick/guillemot.c
@@ -281,15 +281,4 @@ static struct gameport_driver guillemot_drv = {
 	.disconnect	= guillemot_disconnect,
 };
 
-static int __init guillemot_init(void)
-{
-	return gameport_register_driver(&guillemot_drv);
-}
-
-static void __exit guillemot_exit(void)
-{
-	gameport_unregister_driver(&guillemot_drv);
-}
-
-module_init(guillemot_init);
-module_exit(guillemot_exit);
+module_gameport_driver(guillemot_drv);
diff --git a/drivers/input/joystick/interact.c b/drivers/input/joystick/interact.c
index 16fb19d1ca25..88c22623a2e8 100644
--- a/drivers/input/joystick/interact.c
+++ b/drivers/input/joystick/interact.c
@@ -311,15 +311,4 @@ static struct gameport_driver interact_drv = {
 	.disconnect	= interact_disconnect,
 };
 
-static int __init interact_init(void)
-{
-	return gameport_register_driver(&interact_drv);
-}
-
-static void __exit interact_exit(void)
-{
-	gameport_unregister_driver(&interact_drv);
-}
-
-module_init(interact_init);
-module_exit(interact_exit);
+module_gameport_driver(interact_drv);
diff --git a/drivers/input/joystick/joydump.c b/drivers/input/joystick/joydump.c
index cd894a0564a2..7eb878bab968 100644
--- a/drivers/input/joystick/joydump.c
+++ b/drivers/input/joystick/joydump.c
@@ -159,15 +159,4 @@ static struct gameport_driver joydump_drv = {
 	.disconnect	= joydump_disconnect,
 };
 
-static int __init joydump_init(void)
-{
-	return gameport_register_driver(&joydump_drv);
-}
-
-static void __exit joydump_exit(void)
-{
-	gameport_unregister_driver(&joydump_drv);
-}
-
-module_init(joydump_init);
-module_exit(joydump_exit);
+module_gameport_driver(joydump_drv);
diff --git a/drivers/input/joystick/magellan.c b/drivers/input/joystick/magellan.c
index 40e40780747d..9fb153eef2fc 100644
--- a/drivers/input/joystick/magellan.c
+++ b/drivers/input/joystick/magellan.c
@@ -222,19 +222,4 @@ static struct serio_driver magellan_drv = {
 	.disconnect	= magellan_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init magellan_init(void)
-{
-	return serio_register_driver(&magellan_drv);
-}
-
-static void __exit magellan_exit(void)
-{
-	serio_unregister_driver(&magellan_drv);
-}
-
-module_init(magellan_init);
-module_exit(magellan_exit);
+module_serio_driver(magellan_drv);
diff --git a/drivers/input/joystick/sidewinder.c b/drivers/input/joystick/sidewinder.c
index b8d86115644b..04c69af37148 100644
--- a/drivers/input/joystick/sidewinder.c
+++ b/drivers/input/joystick/sidewinder.c
@@ -820,15 +820,4 @@ static struct gameport_driver sw_drv = {
 	.disconnect	= sw_disconnect,
 };
 
-static int __init sw_init(void)
-{
-	return gameport_register_driver(&sw_drv);
-}
-
-static void __exit sw_exit(void)
-{
-	gameport_unregister_driver(&sw_drv);
-}
-
-module_init(sw_init);
-module_exit(sw_exit);
+module_gameport_driver(sw_drv);
diff --git a/drivers/input/joystick/spaceball.c b/drivers/input/joystick/spaceball.c
index 0cd9b29356a8..80a7b27a457a 100644
--- a/drivers/input/joystick/spaceball.c
+++ b/drivers/input/joystick/spaceball.c
@@ -296,19 +296,4 @@ static struct serio_driver spaceball_drv = {
 	.disconnect	= spaceball_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init spaceball_init(void)
-{
-	return serio_register_driver(&spaceball_drv);
-}
-
-static void __exit spaceball_exit(void)
-{
-	serio_unregister_driver(&spaceball_drv);
-}
-
-module_init(spaceball_init);
-module_exit(spaceball_exit);
+module_serio_driver(spaceball_drv);
diff --git a/drivers/input/joystick/spaceorb.c b/drivers/input/joystick/spaceorb.c
index a694bf8e557b..a41f291652e6 100644
--- a/drivers/input/joystick/spaceorb.c
+++ b/drivers/input/joystick/spaceorb.c
@@ -237,19 +237,4 @@ static struct serio_driver spaceorb_drv = {
 	.disconnect	= spaceorb_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init spaceorb_init(void)
-{
-	return serio_register_driver(&spaceorb_drv);
-}
-
-static void __exit spaceorb_exit(void)
-{
-	serio_unregister_driver(&spaceorb_drv);
-}
-
-module_init(spaceorb_init);
-module_exit(spaceorb_exit);
+module_serio_driver(spaceorb_drv);
diff --git a/drivers/input/joystick/stinger.c b/drivers/input/joystick/stinger.c
index e0db9f5e4b41..0f51a60e14a7 100644
--- a/drivers/input/joystick/stinger.c
+++ b/drivers/input/joystick/stinger.c
@@ -208,19 +208,4 @@ static struct serio_driver stinger_drv = {
 	.disconnect	= stinger_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init stinger_init(void)
-{
-	return serio_register_driver(&stinger_drv);
-}
-
-static void __exit stinger_exit(void)
-{
-	serio_unregister_driver(&stinger_drv);
-}
-
-module_init(stinger_init);
-module_exit(stinger_exit);
+module_serio_driver(stinger_drv);
diff --git a/drivers/input/joystick/tmdc.c b/drivers/input/joystick/tmdc.c
index d6c609807115..5ef9bcdb0345 100644
--- a/drivers/input/joystick/tmdc.c
+++ b/drivers/input/joystick/tmdc.c
@@ -436,15 +436,4 @@ static struct gameport_driver tmdc_drv = {
 	.disconnect	= tmdc_disconnect,
 };
 
-static int __init tmdc_init(void)
-{
-	return gameport_register_driver(&tmdc_drv);
-}
-
-static void __exit tmdc_exit(void)
-{
-	gameport_unregister_driver(&tmdc_drv);
-}
-
-module_init(tmdc_init);
-module_exit(tmdc_exit);
+module_gameport_driver(tmdc_drv);
diff --git a/drivers/input/joystick/twidjoy.c b/drivers/input/joystick/twidjoy.c
index 3f4ec73c9553..2556a8193579 100644
--- a/drivers/input/joystick/twidjoy.c
+++ b/drivers/input/joystick/twidjoy.c
@@ -257,19 +257,4 @@ static struct serio_driver twidjoy_drv = {
 	.disconnect	= twidjoy_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init twidjoy_init(void)
-{
-	return serio_register_driver(&twidjoy_drv);
-}
-
-static void __exit twidjoy_exit(void)
-{
-	serio_unregister_driver(&twidjoy_drv);
-}
-
-module_init(twidjoy_init);
-module_exit(twidjoy_exit);
+module_serio_driver(twidjoy_drv);
diff --git a/drivers/input/joystick/warrior.c b/drivers/input/joystick/warrior.c
index f72c83e15e60..23b3071abb6e 100644
--- a/drivers/input/joystick/warrior.c
+++ b/drivers/input/joystick/warrior.c
@@ -217,19 +217,4 @@ static struct serio_driver warrior_drv = {
 	.disconnect	= warrior_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init warrior_init(void)
-{
-	return serio_register_driver(&warrior_drv);
-}
-
-static void __exit warrior_exit(void)
-{
-	serio_unregister_driver(&warrior_drv);
-}
-
-module_init(warrior_init);
-module_exit(warrior_exit);
+module_serio_driver(warrior_drv);
diff --git a/drivers/input/joystick/zhenhua.c b/drivers/input/joystick/zhenhua.c
index b5853125c898..c4de4388fd7f 100644
--- a/drivers/input/joystick/zhenhua.c
+++ b/drivers/input/joystick/zhenhua.c
@@ -225,19 +225,4 @@ static struct serio_driver zhenhua_drv = {
 	.disconnect	= zhenhua_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init zhenhua_init(void)
-{
-	return serio_register_driver(&zhenhua_drv);
-}
-
-static void __exit zhenhua_exit(void)
-{
-	serio_unregister_driver(&zhenhua_drv);
-}
-
-module_init(zhenhua_init);
-module_exit(zhenhua_exit);
+module_serio_driver(zhenhua_drv);
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index f354813a13e8..c0e11ecc646f 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -166,6 +166,7 @@ config KEYBOARD_LKKBD
 config KEYBOARD_EP93XX
 	tristate "EP93xx Matrix Keypad support"
 	depends on ARCH_EP93XX
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here to enable the matrix keypad on the Cirrus EP93XX.
 
@@ -224,6 +225,7 @@ config KEYBOARD_TCA6416
 config KEYBOARD_TCA8418
 	tristate "TCA8418 Keypad Support"
 	depends on I2C
+	select INPUT_MATRIXKMAP
 	help
 	  This driver implements basic keypad functionality
 	  for keys connected through TCA8418 keypad decoder.
@@ -240,6 +242,7 @@ config KEYBOARD_TCA8418
 config KEYBOARD_MATRIX
 	tristate "GPIO driven matrix keypad support"
 	depends on GENERIC_GPIO
+	select INPUT_MATRIXKMAP
 	help
 	  Enable support for GPIO driven matrix keypad.
 
@@ -309,6 +312,17 @@ config KEYBOARD_LM8323
 	  To compile this driver as a module, choose M here: the
 	  module will be called lm8323.
 
+config KEYBOARD_LM8333
+	tristate "LM8333 keypad chip"
+	depends on I2C
+	select INPUT_MATRIXKMAP
+	help
+	  If you say yes here you get support for the National Semiconductor
+	  LM8333 keypad controller.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called lm8333.
+
 config KEYBOARD_LOCOMO
 	tristate "LoCoMo Keyboard Support"
 	depends on SHARP_LOCOMO
@@ -366,6 +380,7 @@ config KEYBOARD_MPR121
 config KEYBOARD_IMX
 	tristate "IMX keypad support"
 	depends on ARCH_MXC
+	select INPUT_MATRIXKMAP
 	help
 	  Enable support for IMX keypad port.
 
@@ -384,6 +399,7 @@ config KEYBOARD_NEWTON
 config KEYBOARD_NOMADIK
 	tristate "ST-Ericsson Nomadik SKE keyboard"
 	depends on PLAT_NOMADIK
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use a keypad provided on the SKE controller
 	  used on the Ux500 and Nomadik platforms
@@ -394,7 +410,7 @@ config KEYBOARD_NOMADIK
 config KEYBOARD_TEGRA
 	tristate "NVIDIA Tegra internal matrix keyboard controller support"
 	depends on ARCH_TEGRA
-	select INPUT_OF_MATRIX_KEYMAP if USE_OF
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use a matrix keyboard connected directly
 	  to the internal keyboard controller on Tegra SoCs.
@@ -432,6 +448,7 @@ config KEYBOARD_PXA930_ROTARY
 config KEYBOARD_PMIC8XXX
 	tristate "Qualcomm PMIC8XXX keypad support"
 	depends on MFD_PM8XXX
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to enable the driver for the PMIC8XXX
 	  keypad provided as a reference design from Qualcomm. This is intended
@@ -443,6 +460,7 @@ config KEYBOARD_PMIC8XXX
 config KEYBOARD_SAMSUNG
 	tristate "Samsung keypad support"
 	depends on HAVE_CLK
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the keypad on your Samsung mobile
 	  device.
@@ -485,6 +503,7 @@ config KEYBOARD_SH_KEYSC
 config KEYBOARD_STMPE
 	tristate "STMPE keypad support"
 	depends on MFD_STMPE
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the keypad controller on STMPE I/O
 	  expanders.
@@ -505,6 +524,7 @@ config KEYBOARD_DAVINCI
 config KEYBOARD_OMAP
 	tristate "TI OMAP keypad support"
 	depends on (ARCH_OMAP1 || ARCH_OMAP2)
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the OMAP keypad.
 
@@ -512,9 +532,10 @@ config KEYBOARD_OMAP
 	  module will be called omap-keypad.
 
 config KEYBOARD_OMAP4
-	tristate "TI OMAP4 keypad support"
+	tristate "TI OMAP4+ keypad support"
+	select INPUT_MATRIXKMAP
 	help
-	  Say Y here if you want to use the OMAP4 keypad.
+	  Say Y here if you want to use the OMAP4+ keypad.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called omap4-keypad.
@@ -522,6 +543,7 @@ config KEYBOARD_OMAP4
 config KEYBOARD_SPEAR
 	tristate "ST SPEAR keyboard support"
 	depends on PLAT_SPEAR
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the SPEAR keyboard.
 
@@ -531,6 +553,7 @@ config KEYBOARD_SPEAR
 config KEYBOARD_TC3589X
 	tristate "TC3589X Keypad support"
 	depends on MFD_TC3589X
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the keypad controller on
 	  TC35892/3 I/O expander.
@@ -541,6 +564,7 @@ config KEYBOARD_TC3589X
 config KEYBOARD_TNETV107X
 	tristate "TI TNETV107X keypad support"
 	depends on ARCH_DAVINCI_TNETV107X
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the TNETV107X keypad.
 
@@ -550,6 +574,7 @@ config KEYBOARD_TNETV107X
 config KEYBOARD_TWL4030
 	tristate "TI TWL4030/TWL5030/TPS659x0 keypad support"
 	depends on TWL4030_CORE
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if your board use the keypad controller on
 	  TWL4030 family chips.  It's safe to say enable this
@@ -573,6 +598,7 @@ config KEYBOARD_XTKBD
 config KEYBOARD_W90P910
 	tristate "W90P910 Matrix Keypad support"
 	depends on ARCH_W90X900
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here to enable the matrix keypad on evaluation board
 	  based on W90P910.
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index df7061f12918..b03b02456a82 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_KEYBOARD_HP6XX)		+= jornada680_kbd.o
 obj-$(CONFIG_KEYBOARD_HP7XX)		+= jornada720_kbd.o
 obj-$(CONFIG_KEYBOARD_LKKBD)		+= lkkbd.o
 obj-$(CONFIG_KEYBOARD_LM8323)		+= lm8323.o
+obj-$(CONFIG_KEYBOARD_LM8333)		+= lm8333.o
 obj-$(CONFIG_KEYBOARD_LOCOMO)		+= locomokbd.o
 obj-$(CONFIG_KEYBOARD_MAPLE)		+= maple_keyb.o
 obj-$(CONFIG_KEYBOARD_MATRIX)		+= matrix_keypad.o
diff --git a/drivers/input/keyboard/adp5588-keys.c b/drivers/input/keyboard/adp5588-keys.c
index 39ebffac207e..b083bf10f139 100644
--- a/drivers/input/keyboard/adp5588-keys.c
+++ b/drivers/input/keyboard/adp5588-keys.c
@@ -197,6 +197,7 @@ static int __devinit adp5588_gpio_add(struct adp5588_kpad *kpad)
 	kpad->gc.base = gpio_data->gpio_start;
 	kpad->gc.label = kpad->client->name;
 	kpad->gc.owner = THIS_MODULE;
+	kpad->gc.names = gpio_data->names;
 
 	mutex_init(&kpad->gpio_lock);
 
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index e05a2e7073c6..add5ffd9fe26 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -433,7 +433,7 @@ static irqreturn_t atkbd_interrupt(struct serio *serio, unsigned char data,
 		if (printk_ratelimit())
 			dev_warn(&serio->dev,
 				 "Spurious %s on %s. "
-				 "Some program might be trying access hardware directly.\n",
+				 "Some program might be trying to access hardware directly.\n",
 				 data == ATKBD_RET_ACK ? "ACK" : "NAK", serio->phys);
 		goto out;
 	case ATKBD_RET_ERR:
diff --git a/drivers/input/keyboard/ep93xx_keypad.c b/drivers/input/keyboard/ep93xx_keypad.c
index 0ba69f3fcb52..c46fc8185469 100644
--- a/drivers/input/keyboard/ep93xx_keypad.c
+++ b/drivers/input/keyboard/ep93xx_keypad.c
@@ -182,16 +182,10 @@ static void ep93xx_keypad_close(struct input_dev *pdev)
 }
 
 
-#ifdef CONFIG_PM
-/*
- * NOTE: I don't know if this is correct, or will work on the ep93xx.
- *
- * None of the existing ep93xx drivers have power management support.
- * But, this is basically what the pxa27x_keypad driver does.
- */
-static int ep93xx_keypad_suspend(struct platform_device *pdev,
-				 pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int ep93xx_keypad_suspend(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct ep93xx_keypad *keypad = platform_get_drvdata(pdev);
 	struct input_dev *input_dev = keypad->input_dev;
 
@@ -210,8 +204,9 @@ static int ep93xx_keypad_suspend(struct platform_device *pdev,
 	return 0;
 }
 
-static int ep93xx_keypad_resume(struct platform_device *pdev)
+static int ep93xx_keypad_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct ep93xx_keypad *keypad = platform_get_drvdata(pdev);
 	struct input_dev *input_dev = keypad->input_dev;
 
@@ -232,10 +227,10 @@ static int ep93xx_keypad_resume(struct platform_device *pdev)
 
 	return 0;
 }
-#else	/* !CONFIG_PM */
-#define ep93xx_keypad_suspend	NULL
-#define ep93xx_keypad_resume	NULL
-#endif	/* !CONFIG_PM */
+#endif
+
+static SIMPLE_DEV_PM_OPS(ep93xx_keypad_pm_ops,
+			 ep93xx_keypad_suspend, ep93xx_keypad_resume);
 
 static int __devinit ep93xx_keypad_probe(struct platform_device *pdev)
 {
@@ -308,19 +303,16 @@ static int __devinit ep93xx_keypad_probe(struct platform_device *pdev)
 	input_dev->open = ep93xx_keypad_open;
 	input_dev->close = ep93xx_keypad_close;
 	input_dev->dev.parent = &pdev->dev;
-	input_dev->keycode = keypad->keycodes;
-	input_dev->keycodesize = sizeof(keypad->keycodes[0]);
-	input_dev->keycodemax = ARRAY_SIZE(keypad->keycodes);
 
-	input_set_drvdata(input_dev, keypad);
+	err = matrix_keypad_build_keymap(keymap_data, NULL,
+					 EP93XX_MATRIX_ROWS, EP93XX_MATRIX_COLS,
+					 keypad->keycodes, input_dev);
+	if (err)
+		goto failed_free_dev;
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY);
 	if (keypad->pdata->flags & EP93XX_KEYPAD_AUTOREPEAT)
-		input_dev->evbit[0] |= BIT_MASK(EV_REP);
-
-	matrix_keypad_build_keymap(keymap_data, 3,
-				   input_dev->keycode, input_dev->keybit);
-	platform_set_drvdata(pdev, keypad);
+		__set_bit(EV_REP, input_dev->evbit);
+	input_set_drvdata(input_dev, keypad);
 
 	err = request_irq(keypad->irq, ep93xx_keypad_irq_handler,
 			  0, pdev->name, keypad);
@@ -331,6 +323,7 @@ static int __devinit ep93xx_keypad_probe(struct platform_device *pdev)
 	if (err)
 		goto failed_free_irq;
 
+	platform_set_drvdata(pdev, keypad);
 	device_init_wakeup(&pdev->dev, 1);
 
 	return 0;
@@ -384,11 +377,10 @@ static struct platform_driver ep93xx_keypad_driver = {
 	.driver		= {
 		.name	= "ep93xx-keypad",
 		.owner	= THIS_MODULE,
+		.pm	= &ep93xx_keypad_pm_ops,
 	},
 	.probe		= ep93xx_keypad_probe,
 	.remove		= __devexit_p(ep93xx_keypad_remove),
-	.suspend	= ep93xx_keypad_suspend,
-	.resume		= ep93xx_keypad_resume,
 };
 module_platform_driver(ep93xx_keypad_driver);
 
diff --git a/drivers/input/keyboard/hil_kbd.c b/drivers/input/keyboard/hil_kbd.c
index fed31e0947a1..589e3c258f3f 100644
--- a/drivers/input/keyboard/hil_kbd.c
+++ b/drivers/input/keyboard/hil_kbd.c
@@ -583,15 +583,4 @@ static struct serio_driver hil_serio_drv = {
 	.interrupt	= hil_dev_interrupt
 };
 
-static int __init hil_dev_init(void)
-{
-	return serio_register_driver(&hil_serio_drv);
-}
-
-static void __exit hil_dev_exit(void)
-{
-	serio_unregister_driver(&hil_serio_drv);
-}
-
-module_init(hil_dev_init);
-module_exit(hil_dev_exit);
+module_serio_driver(hil_serio_drv);
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index fb87b3bcadb9..6ee7421e2321 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -481,7 +481,7 @@ static int __devinit imx_keypad_probe(struct platform_device *pdev)
 	}
 
 	if (keypad->rows_en_mask > ((1 << MAX_MATRIX_KEY_ROWS) - 1) ||
-	   keypad->cols_en_mask > ((1 << MAX_MATRIX_KEY_COLS) - 1)) {
+	    keypad->cols_en_mask > ((1 << MAX_MATRIX_KEY_COLS) - 1)) {
 		dev_err(&pdev->dev,
 			"invalid key data (too many rows or colums)\n");
 		error = -EINVAL;
@@ -496,14 +496,17 @@ static int __devinit imx_keypad_probe(struct platform_device *pdev)
 	input_dev->dev.parent = &pdev->dev;
 	input_dev->open = imx_keypad_open;
 	input_dev->close = imx_keypad_close;
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
-	input_dev->keycode = keypad->keycodes;
-	input_dev->keycodesize = sizeof(keypad->keycodes[0]);
-	input_dev->keycodemax = ARRAY_SIZE(keypad->keycodes);
 
-	matrix_keypad_build_keymap(keymap_data, MATRIX_ROW_SHIFT,
-				keypad->keycodes, input_dev->keybit);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   MAX_MATRIX_KEY_ROWS,
+					   MAX_MATRIX_KEY_COLS,
+					   keypad->keycodes, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto failed_clock_put;
+	}
 
+	__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 	input_set_drvdata(input_dev, keypad);
 
diff --git a/drivers/input/keyboard/lkkbd.c b/drivers/input/keyboard/lkkbd.c
index fa9bb6d235e2..fc0a63c2f278 100644
--- a/drivers/input/keyboard/lkkbd.c
+++ b/drivers/input/keyboard/lkkbd.c
@@ -731,19 +731,4 @@ static struct serio_driver lkkbd_drv = {
 	.interrupt	= lkkbd_interrupt,
 };
 
-/*
- * The functions for insering/removing us as a module.
- */
-static int __init lkkbd_init(void)
-{
-	return serio_register_driver(&lkkbd_drv);
-}
-
-static void __exit lkkbd_exit(void)
-{
-	serio_unregister_driver(&lkkbd_drv);
-}
-
-module_init(lkkbd_init);
-module_exit(lkkbd_exit);
-
+module_serio_driver(lkkbd_drv);
diff --git a/drivers/input/keyboard/lm8333.c b/drivers/input/keyboard/lm8333.c
new file mode 100644
index 000000000000..ca168a6679de
--- /dev/null
+++ b/drivers/input/keyboard/lm8333.c
@@ -0,0 +1,235 @@
+/*
+ * LM8333 keypad driver
+ * Copyright (C) 2012 Wolfram Sang, Pengutronix <w.sang@pengutronix.de>
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/input/matrix_keypad.h>
+#include <linux/input/lm8333.h>
+
+#define LM8333_FIFO_READ		0x20
+#define LM8333_DEBOUNCE			0x22
+#define LM8333_READ_INT			0xD0
+#define LM8333_ACTIVE			0xE4
+#define LM8333_READ_ERROR		0xF0
+
+#define LM8333_KEYPAD_IRQ		(1 << 0)
+#define LM8333_ERROR_IRQ		(1 << 3)
+
+#define LM8333_ERROR_KEYOVR		0x04
+#define LM8333_ERROR_FIFOOVR		0x40
+
+#define LM8333_FIFO_TRANSFER_SIZE	16
+
+#define LM8333_NUM_ROWS		8
+#define LM8333_NUM_COLS		16
+#define LM8333_ROW_SHIFT	4
+
+struct lm8333 {
+	struct i2c_client *client;
+	struct input_dev *input;
+	unsigned short keycodes[LM8333_NUM_ROWS << LM8333_ROW_SHIFT];
+};
+
+/* The accessors try twice because the first access may be needed for wakeup */
+#define LM8333_READ_RETRIES 2
+
+int lm8333_read8(struct lm8333 *lm8333, u8 cmd)
+{
+	int retries = 0, ret;
+
+	do {
+		ret = i2c_smbus_read_byte_data(lm8333->client, cmd);
+	} while (ret < 0 && retries++ < LM8333_READ_RETRIES);
+
+	return ret;
+}
+
+int lm8333_write8(struct lm8333 *lm8333, u8 cmd, u8 val)
+{
+	int retries = 0, ret;
+
+	do {
+		ret = i2c_smbus_write_byte_data(lm8333->client, cmd, val);
+	} while (ret < 0 && retries++ < LM8333_READ_RETRIES);
+
+	return ret;
+}
+
+int lm8333_read_block(struct lm8333 *lm8333, u8 cmd, u8 len, u8 *buf)
+{
+	int retries = 0, ret;
+
+	do {
+		ret = i2c_smbus_read_i2c_block_data(lm8333->client,
+						    cmd, len, buf);
+	} while (ret < 0 && retries++ < LM8333_READ_RETRIES);
+
+	return ret;
+}
+
+static void lm8333_key_handler(struct lm8333 *lm8333)
+{
+	struct input_dev *input = lm8333->input;
+	u8 keys[LM8333_FIFO_TRANSFER_SIZE];
+	u8 code, pressed;
+	int i, ret;
+
+	ret = lm8333_read_block(lm8333, LM8333_FIFO_READ,
+				LM8333_FIFO_TRANSFER_SIZE, keys);
+	if (ret != LM8333_FIFO_TRANSFER_SIZE) {
+		dev_err(&lm8333->client->dev,
+			"Error %d while reading FIFO\n", ret);
+		return;
+	}
+
+	for (i = 0; keys[i] && i < LM8333_FIFO_TRANSFER_SIZE; i++) {
+		pressed = keys[i] & 0x80;
+		code = keys[i] & 0x7f;
+
+		input_event(input, EV_MSC, MSC_SCAN, code);
+		input_report_key(input, lm8333->keycodes[code], pressed);
+	}
+
+	input_sync(input);
+}
+
+static irqreturn_t lm8333_irq_thread(int irq, void *data)
+{
+	struct lm8333 *lm8333 = data;
+	u8 status = lm8333_read8(lm8333, LM8333_READ_INT);
+
+	if (!status)
+		return IRQ_NONE;
+
+	if (status & LM8333_ERROR_IRQ) {
+		u8 err = lm8333_read8(lm8333, LM8333_READ_ERROR);
+
+		if (err & (LM8333_ERROR_KEYOVR | LM8333_ERROR_FIFOOVR)) {
+			u8 dummy[LM8333_FIFO_TRANSFER_SIZE];
+
+			lm8333_read_block(lm8333, LM8333_FIFO_READ,
+					LM8333_FIFO_TRANSFER_SIZE, dummy);
+		}
+		dev_err(&lm8333->client->dev, "Got error %02x\n", err);
+	}
+
+	if (status & LM8333_KEYPAD_IRQ)
+		lm8333_key_handler(lm8333);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit lm8333_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	const struct lm8333_platform_data *pdata = client->dev.platform_data;
+	struct lm8333 *lm8333;
+	struct input_dev *input;
+	int err, active_time;
+
+	if (!pdata)
+		return -EINVAL;
+
+	active_time = pdata->active_time ?: 500;
+	if (active_time / 3 <= pdata->debounce_time / 3) {
+		dev_err(&client->dev, "Active time not big enough!\n");
+		return -EINVAL;
+	}
+
+	lm8333 = kzalloc(sizeof(*lm8333), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!lm8333 || !input) {
+		err = -ENOMEM;
+		goto free_mem;
+	}
+
+	lm8333->client = client;
+	lm8333->input = input;
+
+	input->name = client->name;
+	input->dev.parent = &client->dev;
+	input->id.bustype = BUS_I2C;
+
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+
+	err = matrix_keypad_build_keymap(pdata->matrix_data, NULL,
+					 LM8333_NUM_ROWS, LM8333_NUM_COLS,
+					 lm8333->keycodes, input);
+	if (err)
+		goto free_mem;
+
+	if (pdata->debounce_time) {
+		err = lm8333_write8(lm8333, LM8333_DEBOUNCE,
+				    pdata->debounce_time / 3);
+		if (err)
+			dev_warn(&client->dev, "Unable to set debounce time\n");
+	}
+
+	if (pdata->active_time) {
+		err = lm8333_write8(lm8333, LM8333_ACTIVE,
+				    pdata->active_time / 3);
+		if (err)
+			dev_warn(&client->dev, "Unable to set active time\n");
+	}
+
+	err = request_threaded_irq(client->irq, NULL, lm8333_irq_thread,
+				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				   "lm8333", lm8333);
+	if (err)
+		goto free_mem;
+
+	err = input_register_device(input);
+	if (err)
+		goto free_irq;
+
+	i2c_set_clientdata(client, lm8333);
+	return 0;
+
+ free_irq:
+	free_irq(client->irq, lm8333);
+ free_mem:
+	input_free_device(input);
+	kfree(lm8333);
+	return err;
+}
+
+static int __devexit lm8333_remove(struct i2c_client *client)
+{
+	struct lm8333 *lm8333 = i2c_get_clientdata(client);
+
+	free_irq(client->irq, lm8333);
+	input_unregister_device(lm8333->input);
+	kfree(lm8333);
+
+	return 0;
+}
+
+static const struct i2c_device_id lm8333_id[] = {
+	{ "lm8333", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm8333_id);
+
+static struct i2c_driver lm8333_driver = {
+	.driver = {
+		.name		= "lm8333",
+		.owner		= THIS_MODULE,
+	},
+	.probe		= lm8333_probe,
+	.remove		= __devexit_p(lm8333_remove),
+	.id_table	= lm8333_id,
+};
+module_i2c_driver(lm8333_driver);
+
+MODULE_AUTHOR("Wolfram Sang <w.sang@pengutronix.de>");
+MODULE_DESCRIPTION("LM8333 keyboard driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/keyboard/matrix_keypad.c b/drivers/input/keyboard/matrix_keypad.c
index 9b223d73de32..18b72372028a 100644
--- a/drivers/input/keyboard/matrix_keypad.c
+++ b/drivers/input/keyboard/matrix_keypad.c
@@ -27,7 +27,6 @@
 struct matrix_keypad {
 	const struct matrix_keypad_platform_data *pdata;
 	struct input_dev *input_dev;
-	unsigned short *keycodes;
 	unsigned int row_shift;
 
 	DECLARE_BITMAP(disabled_gpios, MATRIX_MAX_ROWS);
@@ -38,6 +37,8 @@ struct matrix_keypad {
 	bool scan_pending;
 	bool stopped;
 	bool gpio_all_disabled;
+
+	unsigned short keycodes[];
 };
 
 /*
@@ -224,7 +225,7 @@ static void matrix_keypad_stop(struct input_dev *dev)
 	disable_row_irqs(keypad);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static void matrix_keypad_enable_wakeup(struct matrix_keypad *keypad)
 {
 	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
@@ -293,16 +294,16 @@ static int matrix_keypad_resume(struct device *dev)
 
 	return 0;
 }
-
-static const SIMPLE_DEV_PM_OPS(matrix_keypad_pm_ops,
-				matrix_keypad_suspend, matrix_keypad_resume);
 #endif
 
-static int __devinit init_matrix_gpio(struct platform_device *pdev,
-					struct matrix_keypad *keypad)
+static SIMPLE_DEV_PM_OPS(matrix_keypad_pm_ops,
+			 matrix_keypad_suspend, matrix_keypad_resume);
+
+static int __devinit matrix_keypad_init_gpio(struct platform_device *pdev,
+					     struct matrix_keypad *keypad)
 {
 	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
-	int i, err = -EINVAL;
+	int i, err;
 
 	/* initialized strobe lines as outputs, activated */
 	for (i = 0; i < pdata->num_col_gpios; i++) {
@@ -348,8 +349,7 @@ static int __devinit init_matrix_gpio(struct platform_device *pdev,
 					"matrix-keypad", keypad);
 			if (err) {
 				dev_err(&pdev->dev,
-					"Unable to acquire interrupt "
-					"for GPIO line %i\n",
+					"Unable to acquire interrupt for GPIO line %i\n",
 					pdata->row_gpios[i]);
 				goto err_free_irqs;
 			}
@@ -375,14 +375,33 @@ err_free_cols:
 	return err;
 }
 
+static void matrix_keypad_free_gpio(struct matrix_keypad *keypad)
+{
+	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
+	int i;
+
+	if (pdata->clustered_irq > 0) {
+		free_irq(pdata->clustered_irq, keypad);
+	} else {
+		for (i = 0; i < pdata->num_row_gpios; i++)
+			free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
+	}
+
+	for (i = 0; i < pdata->num_row_gpios; i++)
+		gpio_free(pdata->row_gpios[i]);
+
+	for (i = 0; i < pdata->num_col_gpios; i++)
+		gpio_free(pdata->col_gpios[i]);
+}
+
 static int __devinit matrix_keypad_probe(struct platform_device *pdev)
 {
 	const struct matrix_keypad_platform_data *pdata;
 	const struct matrix_keymap_data *keymap_data;
 	struct matrix_keypad *keypad;
 	struct input_dev *input_dev;
-	unsigned short *keycodes;
 	unsigned int row_shift;
+	size_t keymap_size;
 	int err;
 
 	pdata = pdev->dev.platform_data;
@@ -398,20 +417,18 @@ static int __devinit matrix_keypad_probe(struct platform_device *pdev)
 	}
 
 	row_shift = get_count_order(pdata->num_col_gpios);
-
-	keypad = kzalloc(sizeof(struct matrix_keypad), GFP_KERNEL);
-	keycodes = kzalloc((pdata->num_row_gpios << row_shift) *
-				sizeof(*keycodes),
-			   GFP_KERNEL);
+	keymap_size = (pdata->num_row_gpios << row_shift) *
+			sizeof(keypad->keycodes[0]);
+	keypad = kzalloc(sizeof(struct matrix_keypad) + keymap_size,
+			 GFP_KERNEL);
 	input_dev = input_allocate_device();
-	if (!keypad || !keycodes || !input_dev) {
+	if (!keypad || !input_dev) {
 		err = -ENOMEM;
 		goto err_free_mem;
 	}
 
 	keypad->input_dev = input_dev;
 	keypad->pdata = pdata;
-	keypad->keycodes = keycodes;
 	keypad->row_shift = row_shift;
 	keypad->stopped = true;
 	INIT_DELAYED_WORK(&keypad->work, matrix_keypad_scan);
@@ -420,38 +437,38 @@ static int __devinit matrix_keypad_probe(struct platform_device *pdev)
 	input_dev->name		= pdev->name;
 	input_dev->id.bustype	= BUS_HOST;
 	input_dev->dev.parent	= &pdev->dev;
-	input_dev->evbit[0]	= BIT_MASK(EV_KEY);
-	if (!pdata->no_autorepeat)
-		input_dev->evbit[0] |= BIT_MASK(EV_REP);
 	input_dev->open		= matrix_keypad_start;
 	input_dev->close	= matrix_keypad_stop;
 
-	input_dev->keycode	= keycodes;
-	input_dev->keycodesize	= sizeof(*keycodes);
-	input_dev->keycodemax	= pdata->num_row_gpios << row_shift;
-
-	matrix_keypad_build_keymap(keymap_data, row_shift,
-				   input_dev->keycode, input_dev->keybit);
+	err = matrix_keypad_build_keymap(keymap_data, NULL,
+					 pdata->num_row_gpios,
+					 pdata->num_col_gpios,
+					 keypad->keycodes, input_dev);
+	if (err)
+		goto err_free_mem;
 
+	if (!pdata->no_autorepeat)
+		__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 	input_set_drvdata(input_dev, keypad);
 
-	err = init_matrix_gpio(pdev, keypad);
+	err = matrix_keypad_init_gpio(pdev, keypad);
 	if (err)
 		goto err_free_mem;
 
 	err = input_register_device(keypad->input_dev);
 	if (err)
-		goto err_free_mem;
+		goto err_free_gpio;
 
 	device_init_wakeup(&pdev->dev, pdata->wakeup);
 	platform_set_drvdata(pdev, keypad);
 
 	return 0;
 
+err_free_gpio:
+	matrix_keypad_free_gpio(keypad);
 err_free_mem:
 	input_free_device(input_dev);
-	kfree(keycodes);
 	kfree(keypad);
 	return err;
 }
@@ -459,29 +476,15 @@ err_free_mem:
 static int __devexit matrix_keypad_remove(struct platform_device *pdev)
 {
 	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
-	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
-	int i;
 
 	device_init_wakeup(&pdev->dev, 0);
 
-	if (pdata->clustered_irq > 0) {
-		free_irq(pdata->clustered_irq, keypad);
-	} else {
-		for (i = 0; i < pdata->num_row_gpios; i++)
-			free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
-	}
-
-	for (i = 0; i < pdata->num_row_gpios; i++)
-		gpio_free(pdata->row_gpios[i]);
-
-	for (i = 0; i < pdata->num_col_gpios; i++)
-		gpio_free(pdata->col_gpios[i]);
-
+	matrix_keypad_free_gpio(keypad);
 	input_unregister_device(keypad->input_dev);
-	platform_set_drvdata(pdev, NULL);
-	kfree(keypad->keycodes);
 	kfree(keypad);
 
+	platform_set_drvdata(pdev, NULL);
+
 	return 0;
 }
 
@@ -491,9 +494,7 @@ static struct platform_driver matrix_keypad_driver = {
 	.driver		= {
 		.name	= "matrix-keypad",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	= &matrix_keypad_pm_ops,
-#endif
 	},
 };
 module_platform_driver(matrix_keypad_driver);
diff --git a/drivers/input/keyboard/newtonkbd.c b/drivers/input/keyboard/newtonkbd.c
index 48d1cab0aa1c..f971898ad591 100644
--- a/drivers/input/keyboard/newtonkbd.c
+++ b/drivers/input/keyboard/newtonkbd.c
@@ -166,15 +166,4 @@ static struct serio_driver nkbd_drv = {
 	.disconnect	= nkbd_disconnect,
 };
 
-static int __init nkbd_init(void)
-{
-	return serio_register_driver(&nkbd_drv);
-}
-
-static void __exit nkbd_exit(void)
-{
-	serio_unregister_driver(&nkbd_drv);
-}
-
-module_init(nkbd_init);
-module_exit(nkbd_exit);
+module_serio_driver(nkbd_drv);
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index 101e245944e7..4ea4341a68c5 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -39,7 +39,8 @@
 #define SKE_KPRISA	(0x1 << 2)
 
 #define SKE_KEYPAD_ROW_SHIFT	3
-#define SKE_KPD_KEYMAP_SIZE	(8 * 8)
+#define SKE_KPD_NUM_ROWS	8
+#define SKE_KPD_NUM_COLS	8
 
 /* keypad auto scan registers */
 #define SKE_ASR0	0x20
@@ -63,7 +64,7 @@ struct ske_keypad {
 	void __iomem *reg_base;
 	struct input_dev *input;
 	const struct ske_keypad_platform_data *board;
-	unsigned short keymap[SKE_KPD_KEYMAP_SIZE];
+	unsigned short keymap[SKE_KPD_NUM_ROWS * SKE_KPD_NUM_COLS];
 	struct clk *clk;
 	spinlock_t ske_keypad_lock;
 };
@@ -261,19 +262,18 @@ static int __init ske_keypad_probe(struct platform_device *pdev)
 	input->name = "ux500-ske-keypad";
 	input->dev.parent = &pdev->dev;
 
-	input->keycode = keypad->keymap;
-	input->keycodesize = sizeof(keypad->keymap[0]);
-	input->keycodemax = ARRAY_SIZE(keypad->keymap);
+	error = matrix_keypad_build_keymap(plat->keymap_data, NULL,
+					   SKE_KPD_NUM_ROWS, SKE_KPD_NUM_COLS,
+					   keypad->keymap, input);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to build keymap\n");
+		goto err_iounmap;
+	}
 
 	input_set_capability(input, EV_MSC, MSC_SCAN);
-
-	__set_bit(EV_KEY, input->evbit);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
-	matrix_keypad_build_keymap(plat->keymap_data, SKE_KEYPAD_ROW_SHIFT,
-			input->keycode, input->keybit);
-
 	clk_enable(keypad->clk);
 
 	/* go through board initialization helpers */
diff --git a/drivers/input/keyboard/omap-keypad.c b/drivers/input/keyboard/omap-keypad.c
index 6b630d9d3dff..a0222db4dc86 100644
--- a/drivers/input/keyboard/omap-keypad.c
+++ b/drivers/input/keyboard/omap-keypad.c
@@ -61,6 +61,7 @@ struct omap_kp {
 	unsigned int cols;
 	unsigned long delay;
 	unsigned int debounce;
+	unsigned short keymap[];
 };
 
 static DECLARE_TASKLET_DISABLED(kp_tasklet, omap_kp_tasklet, 0);
@@ -316,13 +317,6 @@ static int __devinit omap_kp_probe(struct platform_device *pdev)
 	if (!cpu_is_omap24xx())
 		omap_writew(1, OMAP1_MPUIO_BASE + OMAP_MPUIO_KBD_MASKIT);
 
-	input_dev->keycode      = &omap_kp[1];
-	input_dev->keycodesize  = sizeof(unsigned short);
-	input_dev->keycodemax   = keycodemax;
-
-	if (pdata->rep)
-		__set_bit(EV_REP, input_dev->evbit);
-
 	if (pdata->delay)
 		omap_kp->delay = pdata->delay;
 
@@ -371,9 +365,6 @@ static int __devinit omap_kp_probe(struct platform_device *pdev)
 		goto err2;
 
 	/* setup input device */
-	__set_bit(EV_KEY, input_dev->evbit);
-	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
-			input_dev->keycode, input_dev->keybit);
 	input_dev->name = "omap-keypad";
 	input_dev->phys = "omap-keypad/input0";
 	input_dev->dev.parent = &pdev->dev;
@@ -383,6 +374,15 @@ static int __devinit omap_kp_probe(struct platform_device *pdev)
 	input_dev->id.product = 0x0001;
 	input_dev->id.version = 0x0100;
 
+	if (pdata->rep)
+		__set_bit(EV_REP, input_dev->evbit);
+
+	ret = matrix_keypad_build_keymap(pdata->keymap_data, NULL,
+					 pdata->rows, pdata->cols,
+					 omap_kp->keymap, input_dev);
+	if (ret < 0)
+		goto err3;
+
 	ret = input_register_device(omap_kp->input);
 	if (ret < 0) {
 		printk(KERN_ERR "Unable to register omap-keypad input device\n");
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index e809ac095a38..aed5f6999ce2 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -68,19 +68,52 @@
 
 #define OMAP4_MASK_IRQSTATUSDISABLE	0xFFFF
 
+enum {
+	KBD_REVISION_OMAP4 = 0,
+	KBD_REVISION_OMAP5,
+};
+
 struct omap4_keypad {
 	struct input_dev *input;
 
 	void __iomem *base;
-	int irq;
+	unsigned int irq;
 
 	unsigned int rows;
 	unsigned int cols;
+	u32 reg_offset;
+	u32 irqreg_offset;
 	unsigned int row_shift;
 	unsigned char key_state[8];
 	unsigned short keymap[];
 };
 
+static int kbd_readl(struct omap4_keypad *keypad_data, u32 offset)
+{
+	return __raw_readl(keypad_data->base +
+				keypad_data->reg_offset + offset);
+}
+
+static void kbd_writel(struct omap4_keypad *keypad_data, u32 offset, u32 value)
+{
+	__raw_writel(value,
+		     keypad_data->base + keypad_data->reg_offset + offset);
+}
+
+static int kbd_read_irqreg(struct omap4_keypad *keypad_data, u32 offset)
+{
+	return __raw_readl(keypad_data->base +
+				keypad_data->irqreg_offset + offset);
+}
+
+static void kbd_write_irqreg(struct omap4_keypad *keypad_data,
+			     u32 offset, u32 value)
+{
+	__raw_writel(value,
+		     keypad_data->base + keypad_data->irqreg_offset + offset);
+}
+
+
 /* Interrupt handler */
 static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 {
@@ -91,12 +124,11 @@ static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 	u32 *new_state = (u32 *) key_state;
 
 	/* Disable interrupts */
-	__raw_writel(OMAP4_VAL_IRQDISABLE,
-		     keypad_data->base + OMAP4_KBD_IRQENABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+			 OMAP4_VAL_IRQDISABLE);
 
-	*new_state = __raw_readl(keypad_data->base + OMAP4_KBD_FULLCODE31_0);
-	*(new_state + 1) = __raw_readl(keypad_data->base
-						+ OMAP4_KBD_FULLCODE63_32);
+	*new_state = kbd_readl(keypad_data, OMAP4_KBD_FULLCODE31_0);
+	*(new_state + 1) = kbd_readl(keypad_data, OMAP4_KBD_FULLCODE63_32);
 
 	for (row = 0; row < keypad_data->rows; row++) {
 		changed = key_state[row] ^ keypad_data->key_state[row];
@@ -121,12 +153,13 @@ static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 		sizeof(keypad_data->key_state));
 
 	/* clear pending interrupts */
-	__raw_writel(__raw_readl(keypad_data->base + OMAP4_KBD_IRQSTATUS),
-			keypad_data->base + OMAP4_KBD_IRQSTATUS);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS,
+			 kbd_read_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS));
 
 	/* enable interrupts */
-	__raw_writel(OMAP4_DEF_IRQENABLE_EVENTEN | OMAP4_DEF_IRQENABLE_LONGKEY,
-			keypad_data->base + OMAP4_KBD_IRQENABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+		OMAP4_DEF_IRQENABLE_EVENTEN |
+				OMAP4_DEF_IRQENABLE_LONGKEY);
 
 	return IRQ_HANDLED;
 }
@@ -139,16 +172,17 @@ static int omap4_keypad_open(struct input_dev *input)
 
 	disable_irq(keypad_data->irq);
 
-	__raw_writel(OMAP4_VAL_FUNCTIONALCFG,
-			keypad_data->base + OMAP4_KBD_CTRL);
-	__raw_writel(OMAP4_VAL_DEBOUNCINGTIME,
-			keypad_data->base + OMAP4_KBD_DEBOUNCINGTIME);
-	__raw_writel(OMAP4_VAL_IRQDISABLE,
-			keypad_data->base + OMAP4_KBD_IRQSTATUS);
-	__raw_writel(OMAP4_DEF_IRQENABLE_EVENTEN | OMAP4_DEF_IRQENABLE_LONGKEY,
-			keypad_data->base + OMAP4_KBD_IRQENABLE);
-	__raw_writel(OMAP4_DEF_WUP_EVENT_ENA | OMAP4_DEF_WUP_LONG_KEY_ENA,
-			keypad_data->base + OMAP4_KBD_WAKEUPENABLE);
+	kbd_writel(keypad_data, OMAP4_KBD_CTRL,
+			OMAP4_VAL_FUNCTIONALCFG);
+	kbd_writel(keypad_data, OMAP4_KBD_DEBOUNCINGTIME,
+			OMAP4_VAL_DEBOUNCINGTIME);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS,
+			OMAP4_VAL_IRQDISABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+			OMAP4_DEF_IRQENABLE_EVENTEN |
+				OMAP4_DEF_IRQENABLE_LONGKEY);
+	kbd_writel(keypad_data, OMAP4_KBD_WAKEUPENABLE,
+			OMAP4_DEF_WUP_EVENT_ENA | OMAP4_DEF_WUP_LONG_KEY_ENA);
 
 	enable_irq(keypad_data->irq);
 
@@ -162,12 +196,12 @@ static void omap4_keypad_close(struct input_dev *input)
 	disable_irq(keypad_data->irq);
 
 	/* Disable interrupts */
-	__raw_writel(OMAP4_VAL_IRQDISABLE,
-		     keypad_data->base + OMAP4_KBD_IRQENABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+			 OMAP4_VAL_IRQDISABLE);
 
 	/* clear pending interrupts */
-	__raw_writel(__raw_readl(keypad_data->base + OMAP4_KBD_IRQSTATUS),
-			keypad_data->base + OMAP4_KBD_IRQSTATUS);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS,
+			 kbd_read_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS));
 
 	enable_irq(keypad_data->irq);
 
@@ -182,6 +216,7 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	struct resource *res;
 	resource_size_t size;
 	unsigned int row_shift, max_keys;
+	int rev;
 	int irq;
 	int error;
 
@@ -241,11 +276,40 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	keypad_data->rows = pdata->rows;
 	keypad_data->cols = pdata->cols;
 
+	/*
+	* Enable clocks for the keypad module so that we can read
+	* revision register.
+	*/
+	pm_runtime_enable(&pdev->dev);
+	error = pm_runtime_get_sync(&pdev->dev);
+	if (error) {
+		dev_err(&pdev->dev, "pm_runtime_get_sync() failed\n");
+		goto err_unmap;
+	}
+	rev = __raw_readl(keypad_data->base + OMAP4_KBD_REVISION);
+	rev &= 0x03 << 30;
+	rev >>= 30;
+	switch (rev) {
+	case KBD_REVISION_OMAP4:
+		keypad_data->reg_offset = 0x00;
+		keypad_data->irqreg_offset = 0x00;
+		break;
+	case KBD_REVISION_OMAP5:
+		keypad_data->reg_offset = 0x10;
+		keypad_data->irqreg_offset = 0x0c;
+		break;
+	default:
+		dev_err(&pdev->dev,
+			"Keypad reports unsupported revision %d", rev);
+		error = -EINVAL;
+		goto err_pm_put_sync;
+	}
+
 	/* input device allocation */
 	keypad_data->input = input_dev = input_allocate_device();
 	if (!input_dev) {
 		error = -ENOMEM;
-		goto err_unmap;
+		goto err_pm_put_sync;
 	}
 
 	input_dev->name = pdev->name;
@@ -258,20 +322,19 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	input_dev->open = omap4_keypad_open;
 	input_dev->close = omap4_keypad_close;
 
-	input_dev->keycode	= keypad_data->keymap;
-	input_dev->keycodesize	= sizeof(keypad_data->keymap[0]);
-	input_dev->keycodemax	= max_keys;
+	error = matrix_keypad_build_keymap(pdata->keymap_data, NULL,
+					   pdata->rows, pdata->cols,
+					   keypad_data->keymap, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto err_free_input;
+	}
 
-	__set_bit(EV_KEY, input_dev->evbit);
 	__set_bit(EV_REP, input_dev->evbit);
-
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
 	input_set_drvdata(input_dev, keypad_data);
 
-	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
-			input_dev->keycode, input_dev->keybit);
-
 	error = request_irq(keypad_data->irq, omap4_keypad_interrupt,
 			     IRQF_TRIGGER_RISING,
 			     "omap4-keypad", keypad_data);
@@ -280,7 +343,7 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 		goto err_free_input;
 	}
 
-	pm_runtime_enable(&pdev->dev);
+	pm_runtime_put_sync(&pdev->dev);
 
 	error = input_register_device(keypad_data->input);
 	if (error < 0) {
@@ -296,6 +359,8 @@ err_pm_disable:
 	free_irq(keypad_data->irq, keypad_data);
 err_free_input:
 	input_free_device(input_dev);
+err_pm_put_sync:
+	pm_runtime_put_sync(&pdev->dev);
 err_unmap:
 	iounmap(keypad_data->base);
 err_release_mem:
diff --git a/drivers/input/keyboard/pmic8xxx-keypad.c b/drivers/input/keyboard/pmic8xxx-keypad.c
index 01a1c9f8a383..52c34657d301 100644
--- a/drivers/input/keyboard/pmic8xxx-keypad.c
+++ b/drivers/input/keyboard/pmic8xxx-keypad.c
@@ -626,21 +626,21 @@ static int __devinit pmic8xxx_kp_probe(struct platform_device *pdev)
 	kp->input->id.product	= 0x0001;
 	kp->input->id.vendor	= 0x0001;
 
-	kp->input->evbit[0]	= BIT_MASK(EV_KEY);
-
-	if (pdata->rep)
-		__set_bit(EV_REP, kp->input->evbit);
-
-	kp->input->keycode	= kp->keycodes;
-	kp->input->keycodemax	= PM8XXX_MATRIX_MAX_SIZE;
-	kp->input->keycodesize	= sizeof(kp->keycodes);
 	kp->input->open		= pmic8xxx_kp_open;
 	kp->input->close	= pmic8xxx_kp_close;
 
-	matrix_keypad_build_keymap(keymap_data, PM8XXX_ROW_SHIFT,
-					kp->input->keycode, kp->input->keybit);
+	rc = matrix_keypad_build_keymap(keymap_data, NULL,
+					PM8XXX_MAX_ROWS, PM8XXX_MAX_COLS,
+					kp->keycodes, kp->input);
+	if (rc) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto err_get_irq;
+	}
 
+	if (pdata->rep)
+		__set_bit(EV_REP, kp->input->evbit);
 	input_set_capability(kp->input, EV_MSC, MSC_SCAN);
+
 	input_set_drvdata(kp->input, kp);
 
 	/* initialize keypad state */
diff --git a/drivers/input/keyboard/samsung-keypad.c b/drivers/input/keyboard/samsung-keypad.c
index 2391ae884fee..a061ba603a29 100644
--- a/drivers/input/keyboard/samsung-keypad.c
+++ b/drivers/input/keyboard/samsung-keypad.c
@@ -454,23 +454,23 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 	input_dev->name = pdev->name;
 	input_dev->id.bustype = BUS_HOST;
 	input_dev->dev.parent = &pdev->dev;
-	input_set_drvdata(input_dev, keypad);
 
 	input_dev->open = samsung_keypad_open;
 	input_dev->close = samsung_keypad_close;
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY);
-	if (!pdata->no_autorepeat)
-		input_dev->evbit[0] |= BIT_MASK(EV_REP);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   pdata->rows, pdata->cols,
+					   keypad->keycodes, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto err_put_clk;
+	}
 
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
+	if (!pdata->no_autorepeat)
+		__set_bit(EV_REP, input_dev->evbit);
 
-	input_dev->keycode = keypad->keycodes;
-	input_dev->keycodesize = sizeof(keypad->keycodes[0]);
-	input_dev->keycodemax = pdata->rows << row_shift;
-
-	matrix_keypad_build_keymap(keymap_data, row_shift,
-			input_dev->keycode, input_dev->keybit);
+	input_set_drvdata(input_dev, keypad);
 
 	keypad->irq = platform_get_irq(pdev, 0);
 	if (keypad->irq < 0) {
diff --git a/drivers/input/keyboard/spear-keyboard.c b/drivers/input/keyboard/spear-keyboard.c
index 3b6b528f02fd..6f287f7e1538 100644
--- a/drivers/input/keyboard/spear-keyboard.c
+++ b/drivers/input/keyboard/spear-keyboard.c
@@ -19,6 +19,7 @@
 #include <linux/irq.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pm_wakeup.h>
 #include <linux/slab.h>
@@ -49,7 +50,9 @@
 #define KEY_VALUE	0x00FFFFFF
 #define ROW_MASK	0xF0
 #define COLUMN_MASK	0x0F
-#define ROW_SHIFT	4
+#define NUM_ROWS	16
+#define NUM_COLS	16
+
 #define KEY_MATRIX_SHIFT	6
 
 struct spear_kbd {
@@ -60,7 +63,8 @@ struct spear_kbd {
 	unsigned int irq;
 	unsigned int mode;
 	unsigned short last_key;
-	unsigned short keycodes[256];
+	unsigned short keycodes[NUM_ROWS * NUM_COLS];
+	bool rep;
 };
 
 static irqreturn_t spear_kbd_interrupt(int irq, void *dev_id)
@@ -136,27 +140,49 @@ static void spear_kbd_close(struct input_dev *dev)
 	kbd->last_key = KEY_RESERVED;
 }
 
-static int __devinit spear_kbd_probe(struct platform_device *pdev)
+#ifdef CONFIG_OF
+static int __devinit spear_kbd_parse_dt(struct platform_device *pdev,
+                                        struct spear_kbd *kbd)
 {
-	const struct kbd_platform_data *pdata = pdev->dev.platform_data;
-	const struct matrix_keymap_data *keymap;
-	struct spear_kbd *kbd;
-	struct input_dev *input_dev;
-	struct resource *res;
-	int irq;
+	struct device_node *np = pdev->dev.of_node;
 	int error;
+	u32 val;
 
-	if (!pdata) {
-		dev_err(&pdev->dev, "Invalid platform data\n");
+	if (!np) {
+		dev_err(&pdev->dev, "Missing DT data\n");
 		return -EINVAL;
 	}
 
-	keymap = pdata->keymap;
-	if (!keymap) {
-		dev_err(&pdev->dev, "no keymap defined\n");
-		return -EINVAL;
+	if (of_property_read_bool(np, "autorepeat"))
+		kbd->rep = true;
+
+	error = of_property_read_u32(np, "st,mode", &val);
+	if (error) {
+		dev_err(&pdev->dev, "DT: Invalid or missing mode\n");
+		return error;
 	}
 
+	kbd->mode = val;
+	return 0;
+}
+#else
+static inline int spear_kbd_parse_dt(struct platform_device *pdev,
+				     struct spear_kbd *kbd)
+{
+	return -ENOSYS;
+}
+#endif
+
+static int __devinit spear_kbd_probe(struct platform_device *pdev)
+{
+	struct kbd_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	const struct matrix_keymap_data *keymap = pdata ? pdata->keymap : NULL;
+	struct spear_kbd *kbd;
+	struct input_dev *input_dev;
+	struct resource *res;
+	int irq;
+	int error;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "no keyboard resource defined\n");
@@ -179,7 +205,15 @@ static int __devinit spear_kbd_probe(struct platform_device *pdev)
 
 	kbd->input = input_dev;
 	kbd->irq = irq;
-	kbd->mode = pdata->mode;
+
+	if (!pdata) {
+		error = spear_kbd_parse_dt(pdev, kbd);
+		if (error)
+			goto err_free_mem;
+	} else {
+		kbd->mode = pdata->mode;
+		kbd->rep = pdata->rep;
+	}
 
 	kbd->res = request_mem_region(res->start, resource_size(res),
 				      pdev->name);
@@ -212,18 +246,17 @@ static int __devinit spear_kbd_probe(struct platform_device *pdev)
 	input_dev->open = spear_kbd_open;
 	input_dev->close = spear_kbd_close;
 
-	__set_bit(EV_KEY, input_dev->evbit);
-	if (pdata->rep)
+	error = matrix_keypad_build_keymap(keymap, NULL, NUM_ROWS, NUM_COLS,
+					   kbd->keycodes, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to build keymap\n");
+		goto err_put_clk;
+	}
+
+	if (kbd->rep)
 		__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
-	input_dev->keycode = kbd->keycodes;
-	input_dev->keycodesize = sizeof(kbd->keycodes[0]);
-	input_dev->keycodemax = ARRAY_SIZE(kbd->keycodes);
-
-	matrix_keypad_build_keymap(keymap, ROW_SHIFT,
-			input_dev->keycode, input_dev->keybit);
-
 	input_set_drvdata(input_dev, kbd);
 
 	error = request_irq(irq, spear_kbd_interrupt, 0, "keyboard", kbd);
@@ -317,6 +350,14 @@ static int spear_kbd_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(spear_kbd_pm_ops, spear_kbd_suspend, spear_kbd_resume);
 
+#ifdef CONFIG_OF
+static const struct of_device_id spear_kbd_id_table[] = {
+	{ .compatible = "st,spear300-kbd" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, spear_kbd_id_table);
+#endif
+
 static struct platform_driver spear_kbd_driver = {
 	.probe		= spear_kbd_probe,
 	.remove		= __devexit_p(spear_kbd_remove),
@@ -324,6 +365,7 @@ static struct platform_driver spear_kbd_driver = {
 		.name	= "keyboard",
 		.owner	= THIS_MODULE,
 		.pm	= &spear_kbd_pm_ops,
+		.of_match_table = of_match_ptr(spear_kbd_id_table),
 	},
 };
 module_platform_driver(spear_kbd_driver);
diff --git a/drivers/input/keyboard/stmpe-keypad.c b/drivers/input/keyboard/stmpe-keypad.c
index 9397cf9c625c..470a8778dec1 100644
--- a/drivers/input/keyboard/stmpe-keypad.c
+++ b/drivers/input/keyboard/stmpe-keypad.c
@@ -289,19 +289,17 @@ static int __devinit stmpe_keypad_probe(struct platform_device *pdev)
 	input->id.bustype = BUS_I2C;
 	input->dev.parent = &pdev->dev;
 
-	input_set_capability(input, EV_MSC, MSC_SCAN);
+	ret = matrix_keypad_build_keymap(plat->keymap_data, NULL,
+					 STMPE_KEYPAD_MAX_ROWS,
+					 STMPE_KEYPAD_MAX_COLS,
+					 keypad->keymap, input);
+	if (ret)
+		goto out_freeinput;
 
-	__set_bit(EV_KEY, input->evbit);
+	input_set_capability(input, EV_MSC, MSC_SCAN);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
-	input->keycode = keypad->keymap;
-	input->keycodesize = sizeof(keypad->keymap[0]);
-	input->keycodemax = ARRAY_SIZE(keypad->keymap);
-
-	matrix_keypad_build_keymap(plat->keymap_data, STMPE_KEYPAD_ROW_SHIFT,
-				   input->keycode, input->keybit);
-
 	for (i = 0; i < plat->keymap_data->keymap_size; i++) {
 		unsigned int key = plat->keymap_data->keymap[i];
 
diff --git a/drivers/input/keyboard/stowaway.c b/drivers/input/keyboard/stowaway.c
index 7437219370b1..cc612c5d5427 100644
--- a/drivers/input/keyboard/stowaway.c
+++ b/drivers/input/keyboard/stowaway.c
@@ -170,15 +170,4 @@ static struct serio_driver skbd_drv = {
 	.disconnect	= skbd_disconnect,
 };
 
-static int __init skbd_init(void)
-{
-	return serio_register_driver(&skbd_drv);
-}
-
-static void __exit skbd_exit(void)
-{
-	serio_unregister_driver(&skbd_drv);
-}
-
-module_init(skbd_init);
-module_exit(skbd_exit);
+module_serio_driver(skbd_drv);
diff --git a/drivers/input/keyboard/sunkbd.c b/drivers/input/keyboard/sunkbd.c
index a99a04b03ee4..5f836b1638c1 100644
--- a/drivers/input/keyboard/sunkbd.c
+++ b/drivers/input/keyboard/sunkbd.c
@@ -369,19 +369,4 @@ static struct serio_driver sunkbd_drv = {
 	.disconnect	= sunkbd_disconnect,
 };
 
-/*
- * The functions for insering/removing us as a module.
- */
-
-static int __init sunkbd_init(void)
-{
-	return serio_register_driver(&sunkbd_drv);
-}
-
-static void __exit sunkbd_exit(void)
-{
-	serio_unregister_driver(&sunkbd_drv);
-}
-
-module_init(sunkbd_init);
-module_exit(sunkbd_exit);
+module_serio_driver(sunkbd_drv);
diff --git a/drivers/input/keyboard/tc3589x-keypad.c b/drivers/input/keyboard/tc3589x-keypad.c
index 2dee3e4e7c6f..7d498e698508 100644
--- a/drivers/input/keyboard/tc3589x-keypad.c
+++ b/drivers/input/keyboard/tc3589x-keypad.c
@@ -78,7 +78,7 @@
  * @input:      pointer to input device object
  * @board:      keypad platform device
  * @krow:	number of rows
- * @kcol:	number of coloumns
+ * @kcol:	number of columns
  * @keymap:     matrix scan code table for keycodes
  * @keypad_stopped: holds keypad status
  */
@@ -96,21 +96,15 @@ static int tc3589x_keypad_init_key_hardware(struct tc_keypad *keypad)
 {
 	int ret;
 	struct tc3589x *tc3589x = keypad->tc3589x;
-	u8 settle_time = keypad->board->settle_time;
-	u8 dbounce_period = keypad->board->debounce_period;
-	u8 rows = keypad->board->krow & 0xf;	/* mask out the nibble */
-	u8 column = keypad->board->kcol & 0xf;	/* mask out the nibble */
-
-	/* validate platform configurations */
-	if (keypad->board->kcol > TC3589x_MAX_KPCOL ||
-	    keypad->board->krow > TC3589x_MAX_KPROW ||
-	    keypad->board->debounce_period > TC3589x_MAX_DEBOUNCE_SETTLE ||
-	    keypad->board->settle_time > TC3589x_MAX_DEBOUNCE_SETTLE)
+	const struct tc3589x_keypad_platform_data *board = keypad->board;
+
+	/* validate platform configuration */
+	if (board->kcol > TC3589x_MAX_KPCOL || board->krow > TC3589x_MAX_KPROW)
 		return -EINVAL;
 
 	/* configure KBDSIZE 4 LSbits for cols and 4 MSbits for rows */
 	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDSIZE,
-			(rows << KP_ROW_SHIFT) | column);
+			(board->krow << KP_ROW_SHIFT) | board->kcol);
 	if (ret < 0)
 		return ret;
 
@@ -124,12 +118,14 @@ static int tc3589x_keypad_init_key_hardware(struct tc_keypad *keypad)
 		return ret;
 
 	/* Configure settle time */
-	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDSETTLE_REG, settle_time);
+	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDSETTLE_REG,
+				board->settle_time);
 	if (ret < 0)
 		return ret;
 
 	/* Configure debounce time */
-	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDBOUNCE, dbounce_period);
+	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDBOUNCE,
+				board->debounce_period);
 	if (ret < 0)
 		return ret;
 
@@ -337,23 +333,22 @@ static int __devinit tc3589x_keypad_probe(struct platform_device *pdev)
 	input->name = pdev->name;
 	input->dev.parent = &pdev->dev;
 
-	input->keycode = keypad->keymap;
-	input->keycodesize = sizeof(keypad->keymap[0]);
-	input->keycodemax = ARRAY_SIZE(keypad->keymap);
-
 	input->open = tc3589x_keypad_open;
 	input->close = tc3589x_keypad_close;
 
-	input_set_drvdata(input, keypad);
+	error = matrix_keypad_build_keymap(plat->keymap_data, NULL,
+					   TC3589x_MAX_KPROW, TC3589x_MAX_KPCOL,
+					   keypad->keymap, input);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to build keymap\n");
+		goto err_free_mem;
+	}
 
 	input_set_capability(input, EV_MSC, MSC_SCAN);
-
-	__set_bit(EV_KEY, input->evbit);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
-	matrix_keypad_build_keymap(plat->keymap_data, 0x3,
-			input->keycode, input->keybit);
+	input_set_drvdata(input, keypad);
 
 	error = request_threaded_irq(irq, NULL,
 			tc3589x_keypad_irq, plat->irqtype,
diff --git a/drivers/input/keyboard/tca8418_keypad.c b/drivers/input/keyboard/tca8418_keypad.c
index 958ec107bfbc..5f87b28b3192 100644
--- a/drivers/input/keyboard/tca8418_keypad.c
+++ b/drivers/input/keyboard/tca8418_keypad.c
@@ -342,21 +342,20 @@ static int __devinit tca8418_keypad_probe(struct i2c_client *client,
 	input->id.product = 0x001;
 	input->id.version = 0x0001;
 
-	input->keycode     = keypad_data->keymap;
-	input->keycodesize = sizeof(keypad_data->keymap[0]);
-	input->keycodemax  = max_keys;
+	error = matrix_keypad_build_keymap(pdata->keymap_data, NULL,
+					   pdata->rows, pdata->cols,
+					   keypad_data->keymap, input);
+	if (error) {
+		dev_dbg(&client->dev, "Failed to build keymap\n");
+		goto fail2;
+	}
 
-	__set_bit(EV_KEY, input->evbit);
 	if (pdata->rep)
 		__set_bit(EV_REP, input->evbit);
-
 	input_set_capability(input, EV_MSC, MSC_SCAN);
 
 	input_set_drvdata(input, keypad_data);
 
-	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
-			input->keycode, input->keybit);
-
 	if (pdata->irq_is_gpio)
 		client->irq = gpio_to_irq(client->irq);
 
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index fe4ac95ca6c8..4ffe64d53107 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -619,8 +619,8 @@ tegra_kbc_check_pin_cfg(const struct tegra_kbc_platform_data *pdata,
 }
 
 #ifdef CONFIG_OF
-static struct tegra_kbc_platform_data * __devinit
-tegra_kbc_dt_parse_pdata(struct platform_device *pdev)
+static struct tegra_kbc_platform_data * __devinit tegra_kbc_dt_parse_pdata(
+	struct platform_device *pdev)
 {
 	struct tegra_kbc_platform_data *pdata;
 	struct device_node *np = pdev->dev.of_node;
@@ -660,10 +660,6 @@ tegra_kbc_dt_parse_pdata(struct platform_device *pdev)
 		pdata->pin_cfg[KBC_MAX_ROW + i].type = PIN_CFG_COL;
 	}
 
-	pdata->keymap_data = matrix_keyboard_of_fill_keymap(np, "linux,keymap");
-
-	/* FIXME: Add handling of linux,fn-keymap here */
-
 	return pdata;
 }
 #else
@@ -674,10 +670,36 @@ static inline struct tegra_kbc_platform_data *tegra_kbc_dt_parse_pdata(
 }
 #endif
 
+static int __devinit tegra_kbd_setup_keymap(struct tegra_kbc *kbc)
+{
+	const struct tegra_kbc_platform_data *pdata = kbc->pdata;
+	const struct matrix_keymap_data *keymap_data = pdata->keymap_data;
+	unsigned int keymap_rows = KBC_MAX_KEY;
+	int retval;
+
+	if (keymap_data && pdata->use_fn_map)
+		keymap_rows *= 2;
+
+	retval = matrix_keypad_build_keymap(keymap_data, NULL,
+					    keymap_rows, KBC_MAX_COL,
+					    kbc->keycode, kbc->idev);
+	if (retval == -ENOSYS || retval == -ENOENT) {
+		/*
+		 * If there is no OF support in kernel or keymap
+		 * property is missing, use default keymap.
+		 */
+		retval = matrix_keypad_build_keymap(
+					&tegra_kbc_default_keymap_data, NULL,
+					keymap_rows, KBC_MAX_COL,
+					kbc->keycode, kbc->idev);
+	}
+
+	return retval;
+}
+
 static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 {
 	const struct tegra_kbc_platform_data *pdata = pdev->dev.platform_data;
-	const struct matrix_keymap_data *keymap_data;
 	struct tegra_kbc *kbc;
 	struct input_dev *input_dev;
 	struct resource *res;
@@ -757,29 +779,26 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	kbc->repoll_dly = KBC_ROW_SCAN_DLY + scan_time_rows + pdata->repeat_cnt;
 	kbc->repoll_dly = DIV_ROUND_UP(kbc->repoll_dly, KBC_CYCLE_MS);
 
+	kbc->wakeup_key = pdata->wakeup_key;
+	kbc->use_fn_map = pdata->use_fn_map;
+	kbc->use_ghost_filter = pdata->use_ghost_filter;
+
 	input_dev->name = pdev->name;
 	input_dev->id.bustype = BUS_HOST;
 	input_dev->dev.parent = &pdev->dev;
 	input_dev->open = tegra_kbc_open;
 	input_dev->close = tegra_kbc_close;
 
-	input_set_drvdata(input_dev, kbc);
+	err = tegra_kbd_setup_keymap(kbc);
+	if (err) {
+		dev_err(&pdev->dev, "failed to setup keymap\n");
+		goto err_put_clk;
+	}
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
+	__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
-	input_dev->keycode = kbc->keycode;
-	input_dev->keycodesize = sizeof(kbc->keycode[0]);
-	input_dev->keycodemax = KBC_MAX_KEY;
-	if (pdata->use_fn_map)
-		input_dev->keycodemax *= 2;
-
-	kbc->use_fn_map = pdata->use_fn_map;
-	kbc->use_ghost_filter = pdata->use_ghost_filter;
-	keymap_data = pdata->keymap_data ?: &tegra_kbc_default_keymap_data;
-	matrix_keypad_build_keymap(keymap_data, KBC_ROW_SHIFT,
-				   input_dev->keycode, input_dev->keybit);
-	kbc->wakeup_key = pdata->wakeup_key;
+	input_set_drvdata(input_dev, kbc);
 
 	err = request_irq(kbc->irq, tegra_kbc_isr,
 			  IRQF_NO_SUSPEND | IRQF_TRIGGER_HIGH, pdev->name, kbc);
@@ -799,9 +818,6 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, kbc);
 	device_init_wakeup(&pdev->dev, pdata->wakeup);
 
-	if (!pdev->dev.platform_data)
-		matrix_keyboard_of_free_keymap(pdata->keymap_data);
-
 	return 0;
 
 err_free_irq:
@@ -816,10 +832,8 @@ err_free_mem:
 	input_free_device(input_dev);
 	kfree(kbc);
 err_free_pdata:
-	if (!pdev->dev.platform_data) {
-		matrix_keyboard_of_free_keymap(pdata->keymap_data);
+	if (!pdev->dev.platform_data)
 		kfree(pdata);
-	}
 
 	return err;
 }
diff --git a/drivers/input/keyboard/tnetv107x-keypad.c b/drivers/input/keyboard/tnetv107x-keypad.c
index fb39c94b6fdd..a4a445fb7020 100644
--- a/drivers/input/keyboard/tnetv107x-keypad.c
+++ b/drivers/input/keyboard/tnetv107x-keypad.c
@@ -247,15 +247,11 @@ static int __devinit keypad_probe(struct platform_device *pdev)
 		error = -ENOMEM;
 		goto error_input;
 	}
-	input_set_drvdata(kp->input_dev, kp);
 
 	kp->input_dev->name	  = pdev->name;
 	kp->input_dev->dev.parent = &pdev->dev;
 	kp->input_dev->open	  = keypad_start;
 	kp->input_dev->close	  = keypad_stop;
-	kp->input_dev->evbit[0]	  = BIT_MASK(EV_KEY);
-	if (!pdata->no_autorepeat)
-		kp->input_dev->evbit[0] |= BIT_MASK(EV_REP);
 
 	clk_enable(kp->clk);
 	rev = keypad_read(kp, rev);
@@ -264,15 +260,20 @@ static int __devinit keypad_probe(struct platform_device *pdev)
 	kp->input_dev->id.version = ((rev >> 16) & 0xfff);
 	clk_disable(kp->clk);
 
-	kp->input_dev->keycode     = kp->keycodes;
-	kp->input_dev->keycodesize = sizeof(kp->keycodes[0]);
-	kp->input_dev->keycodemax  = kp->rows << kp->row_shift;
-
-	matrix_keypad_build_keymap(keymap_data, kp->row_shift, kp->keycodes,
-				   kp->input_dev->keybit);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   kp->rows, kp->cols,
+					   kp->keycodes, kp->input_dev);
+	if (error) {
+		dev_err(dev, "Failed to build keymap\n");
+		goto error_reg;
+	}
 
+	if (!pdata->no_autorepeat)
+		kp->input_dev->evbit[0] |= BIT_MASK(EV_REP);
 	input_set_capability(kp->input_dev, EV_MSC, MSC_SCAN);
 
+	input_set_drvdata(kp->input_dev, kp);
+
 	error = input_register_device(kp->input_dev);
 	if (error < 0) {
 		dev_err(dev, "Could not register input device\n");
diff --git a/drivers/input/keyboard/twl4030_keypad.c b/drivers/input/keyboard/twl4030_keypad.c
index 67bec14e8b96..a2c6f79aa101 100644
--- a/drivers/input/keyboard/twl4030_keypad.c
+++ b/drivers/input/keyboard/twl4030_keypad.c
@@ -361,14 +361,6 @@ static int __devinit twl4030_kp_probe(struct platform_device *pdev)
 	kp->irq = platform_get_irq(pdev, 0);
 
 	/* setup input device */
-	__set_bit(EV_KEY, input->evbit);
-
-	/* Enable auto repeat feature of Linux input subsystem */
-	if (pdata->rep)
-		__set_bit(EV_REP, input->evbit);
-
-	input_set_capability(input, EV_MSC, MSC_SCAN);
-
 	input->name		= "TWL4030 Keypad";
 	input->phys		= "twl4030_keypad/input0";
 	input->dev.parent	= &pdev->dev;
@@ -378,12 +370,19 @@ static int __devinit twl4030_kp_probe(struct platform_device *pdev)
 	input->id.product	= 0x0001;
 	input->id.version	= 0x0003;
 
-	input->keycode		= kp->keymap;
-	input->keycodesize	= sizeof(kp->keymap[0]);
-	input->keycodemax	= ARRAY_SIZE(kp->keymap);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   TWL4030_MAX_ROWS,
+					   1 << TWL4030_ROW_SHIFT,
+					   kp->keymap, input);
+	if (error) {
+		dev_err(kp->dbg_dev, "Failed to build keymap\n");
+		goto err1;
+	}
 
-	matrix_keypad_build_keymap(keymap_data, TWL4030_ROW_SHIFT,
-				   input->keycode, input->keybit);
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+	/* Enable auto repeat feature of Linux input subsystem */
+	if (pdata->rep)
+		__set_bit(EV_REP, input->evbit);
 
 	error = input_register_device(input);
 	if (error) {
diff --git a/drivers/input/keyboard/w90p910_keypad.c b/drivers/input/keyboard/w90p910_keypad.c
index 99bbb7e775ae..085ede4d972d 100644
--- a/drivers/input/keyboard/w90p910_keypad.c
+++ b/drivers/input/keyboard/w90p910_keypad.c
@@ -42,7 +42,8 @@
 #define KGET_RAW(n)		(((n) & KEY0R) >> 3)
 #define KGET_COLUMN(n)		((n) & KEY0C)
 
-#define W90P910_MAX_KEY_NUM	(8 * 8)
+#define W90P910_NUM_ROWS	8
+#define W90P910_NUM_COLS	8
 #define W90P910_ROW_SHIFT	3
 
 struct w90p910_keypad {
@@ -51,7 +52,7 @@ struct w90p910_keypad {
 	struct input_dev *input_dev;
 	void __iomem *mmio_base;
 	int irq;
-	unsigned short keymap[W90P910_MAX_KEY_NUM];
+	unsigned short keymap[W90P910_NUM_ROWS * W90P910_NUM_COLS];
 };
 
 static void w90p910_keypad_scan_matrix(struct w90p910_keypad *keypad,
@@ -190,17 +191,13 @@ static int __devinit w90p910_keypad_probe(struct platform_device *pdev)
 	input_dev->close = w90p910_keypad_close;
 	input_dev->dev.parent = &pdev->dev;
 
-	input_dev->keycode = keypad->keymap;
-	input_dev->keycodesize = sizeof(keypad->keymap[0]);
-	input_dev->keycodemax = ARRAY_SIZE(keypad->keymap);
-
-	input_set_drvdata(input_dev, keypad);
-
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
-	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
-
-	matrix_keypad_build_keymap(keymap_data, W90P910_ROW_SHIFT,
-				   input_dev->keycode, input_dev->keybit);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   W90P910_NUM_ROWS, W90P910_NUM_COLS,
+					   keypad->keymap, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto failed_put_clk;
+	}
 
 	error = request_irq(keypad->irq, w90p910_keypad_irq_handler,
 			    0, pdev->name, keypad);
@@ -209,6 +206,10 @@ static int __devinit w90p910_keypad_probe(struct platform_device *pdev)
 		goto failed_put_clk;
 	}
 
+	__set_bit(EV_REP, input_dev->evbit);
+	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
+	input_set_drvdata(input_dev, keypad);
+
 	/* Register the input device */
 	error = input_register_device(input_dev);
 	if (error) {
diff --git a/drivers/input/keyboard/xtkbd.c b/drivers/input/keyboard/xtkbd.c
index 37b01d777a4a..d050d9d0011b 100644
--- a/drivers/input/keyboard/xtkbd.c
+++ b/drivers/input/keyboard/xtkbd.c
@@ -169,15 +169,4 @@ static struct serio_driver xtkbd_drv = {
 	.disconnect	= xtkbd_disconnect,
 };
 
-static int __init xtkbd_init(void)
-{
-	return serio_register_driver(&xtkbd_drv);
-}
-
-static void __exit xtkbd_exit(void)
-{
-	serio_unregister_driver(&xtkbd_drv);
-}
-
-module_init(xtkbd_init);
-module_exit(xtkbd_exit);
+module_serio_driver(xtkbd_drv);
diff --git a/drivers/input/matrix-keymap.c b/drivers/input/matrix-keymap.c
new file mode 100644
index 000000000000..443ad64b7f2a
--- /dev/null
+++ b/drivers/input/matrix-keymap.c
@@ -0,0 +1,163 @@
+/*
+ * Helpers for matrix keyboard bindings
+ *
+ * Copyright (C) 2012 Google, Inc
+ *
+ * Author:
+ *	Olof Johansson <olof@lixom.net>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/input.h>
+#include <linux/of.h>
+#include <linux/export.h>
+#include <linux/input/matrix_keypad.h>
+
+static bool matrix_keypad_map_key(struct input_dev *input_dev,
+				  unsigned int rows, unsigned int cols,
+				  unsigned int row_shift, unsigned int key)
+{
+	unsigned short *keymap = input_dev->keycode;
+	unsigned int row = KEY_ROW(key);
+	unsigned int col = KEY_COL(key);
+	unsigned short code = KEY_VAL(key);
+
+	if (row >= rows || col >= cols) {
+		dev_err(input_dev->dev.parent,
+			"%s: invalid keymap entry 0x%x (row: %d, col: %d, rows: %d, cols: %d)\n",
+			__func__, key, row, col, rows, cols);
+		return false;
+	}
+
+	keymap[MATRIX_SCAN_CODE(row, col, row_shift)] = code;
+	__set_bit(code, input_dev->keybit);
+
+	return true;
+}
+
+#ifdef CONFIG_OF
+static int matrix_keypad_parse_of_keymap(const char *propname,
+					 unsigned int rows, unsigned int cols,
+					 struct input_dev *input_dev)
+{
+	struct device *dev = input_dev->dev.parent;
+	struct device_node *np = dev->of_node;
+	unsigned int row_shift = get_count_order(cols);
+	unsigned int max_keys = rows << row_shift;
+	unsigned int proplen, i, size;
+	const __be32 *prop;
+
+	if (!np)
+		return -ENOENT;
+
+	if (!propname)
+		propname = "linux,keymap";
+
+	prop = of_get_property(np, propname, &proplen);
+	if (!prop) {
+		dev_err(dev, "OF: %s property not defined in %s\n",
+			propname, np->full_name);
+		return -ENOENT;
+	}
+
+	if (proplen % sizeof(u32)) {
+		dev_err(dev, "OF: Malformed keycode property %s in %s\n",
+			propname, np->full_name);
+		return -EINVAL;
+	}
+
+	size = proplen / sizeof(u32);
+	if (size > max_keys) {
+		dev_err(dev, "OF: %s size overflow\n", propname);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < size; i++) {
+		unsigned int key = be32_to_cpup(prop + i);
+
+		if (!matrix_keypad_map_key(input_dev, rows, cols,
+					   row_shift, key))
+			return -EINVAL;
+	}
+
+	return 0;
+}
+#else
+static int matrix_keypad_parse_of_keymap(const char *propname,
+					 unsigned int rows, unsigned int cols,
+					 struct input_dev *input_dev)
+{
+	return -ENOSYS;
+}
+#endif
+
+/**
+ * matrix_keypad_build_keymap - convert platform keymap into matrix keymap
+ * @keymap_data: keymap supplied by the platform code
+ * @keymap_name: name of device tree property containing keymap (if device
+ *	tree support is enabled).
+ * @rows: number of rows in target keymap array
+ * @cols: number of cols in target keymap array
+ * @keymap: expanded version of keymap that is suitable for use by
+ * matrix keyboard driver
+ * @input_dev: input devices for which we are setting up the keymap
+ *
+ * This function converts platform keymap (encoded with KEY() macro) into
+ * an array of keycodes that is suitable for using in a standard matrix
+ * keyboard driver that uses row and col as indices.
+ *
+ * If @keymap_data is not supplied and device tree support is enabled
+ * it will attempt load the keymap from property specified by @keymap_name
+ * argument (or "linux,keymap" if @keymap_name is %NULL).
+ *
+ * Callers are expected to set up input_dev->dev.parent before calling this
+ * function.
+ */
+int matrix_keypad_build_keymap(const struct matrix_keymap_data *keymap_data,
+			       const char *keymap_name,
+			       unsigned int rows, unsigned int cols,
+			       unsigned short *keymap,
+			       struct input_dev *input_dev)
+{
+	unsigned int row_shift = get_count_order(cols);
+	int i;
+	int error;
+
+	input_dev->keycode = keymap;
+	input_dev->keycodesize = sizeof(*keymap);
+	input_dev->keycodemax = rows << row_shift;
+
+	__set_bit(EV_KEY, input_dev->evbit);
+
+	if (keymap_data) {
+		for (i = 0; i < keymap_data->keymap_size; i++) {
+			unsigned int key = keymap_data->keymap[i];
+
+			if (!matrix_keypad_map_key(input_dev, rows, cols,
+						   row_shift, key))
+				return -EINVAL;
+		}
+	} else {
+		error = matrix_keypad_parse_of_keymap(keymap_name, rows, cols,
+						      input_dev);
+		if (error)
+			return error;
+	}
+
+	__clear_bit(KEY_RESERVED, input_dev->keybit);
+
+	return 0;
+}
+EXPORT_SYMBOL(matrix_keypad_build_keymap);
diff --git a/drivers/input/misc/cma3000_d0x.c b/drivers/input/misc/cma3000_d0x.c
index 06517e60e50c..a3735a01e9fd 100644
--- a/drivers/input/misc/cma3000_d0x.c
+++ b/drivers/input/misc/cma3000_d0x.c
@@ -318,7 +318,7 @@ struct cma3000_accl_data *cma3000_init(struct device *dev, int irq,
 	mutex_init(&data->mutex);
 
 	data->mode = pdata->mode;
-	if (data->mode < CMAMODE_DEFAULT || data->mode > CMAMODE_POFF) {
+	if (data->mode > CMAMODE_POFF) {
 		data->mode = CMAMODE_MOTDET;
 		dev_warn(dev,
 			 "Invalid mode specified, assuming Motion Detect\n");
diff --git a/drivers/input/misc/mpu3050.c b/drivers/input/misc/mpu3050.c
index 5403c571b6a5..306f84c2d8fb 100644
--- a/drivers/input/misc/mpu3050.c
+++ b/drivers/input/misc/mpu3050.c
@@ -367,7 +367,7 @@ static int __devinit mpu3050_probe(struct i2c_client *client,
 
 	error = request_threaded_irq(client->irq,
 				     NULL, mpu3050_interrupt_thread,
-				     IRQF_TRIGGER_RISING,
+				     IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 				     "mpu3050", sensor);
 	if (error) {
 		dev_err(&client->dev,
diff --git a/drivers/input/misc/twl6040-vibra.c b/drivers/input/misc/twl6040-vibra.c
index 14e94f56cb7d..c34f6c0371c4 100644
--- a/drivers/input/misc/twl6040-vibra.c
+++ b/drivers/input/misc/twl6040-vibra.c
@@ -27,6 +27,7 @@
  */
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/workqueue.h>
 #include <linux/input.h>
 #include <linux/mfd/twl6040.h>
@@ -258,10 +259,13 @@ static SIMPLE_DEV_PM_OPS(twl6040_vibra_pm_ops, twl6040_vibra_suspend, NULL);
 static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 {
 	struct twl6040_vibra_data *pdata = pdev->dev.platform_data;
+	struct device_node *node = pdev->dev.of_node;
 	struct vibra_info *info;
+	int vddvibl_uV = 0;
+	int vddvibr_uV = 0;
 	int ret;
 
-	if (!pdata) {
+	if (!pdata && !node) {
 		dev_err(&pdev->dev, "platform_data not available\n");
 		return -EINVAL;
 	}
@@ -273,11 +277,26 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 	}
 
 	info->dev = &pdev->dev;
+
 	info->twl6040 = dev_get_drvdata(pdev->dev.parent);
-	info->vibldrv_res = pdata->vibldrv_res;
-	info->vibrdrv_res = pdata->vibrdrv_res;
-	info->viblmotor_res = pdata->viblmotor_res;
-	info->vibrmotor_res = pdata->vibrmotor_res;
+	if (pdata) {
+		info->vibldrv_res = pdata->vibldrv_res;
+		info->vibrdrv_res = pdata->vibrdrv_res;
+		info->viblmotor_res = pdata->viblmotor_res;
+		info->vibrmotor_res = pdata->vibrmotor_res;
+		vddvibl_uV = pdata->vddvibl_uV;
+		vddvibr_uV = pdata->vddvibr_uV;
+	} else {
+		of_property_read_u32(node, "vibldrv_res", &info->vibldrv_res);
+		of_property_read_u32(node, "vibrdrv_res", &info->vibrdrv_res);
+		of_property_read_u32(node, "viblmotor_res",
+				     &info->viblmotor_res);
+		of_property_read_u32(node, "vibrmotor_res",
+				     &info->vibrmotor_res);
+		of_property_read_u32(node, "vddvibl_uV", &vddvibl_uV);
+		of_property_read_u32(node, "vddvibr_uV", &vddvibr_uV);
+	}
+
 	if ((!info->vibldrv_res && !info->viblmotor_res) ||
 	    (!info->vibrdrv_res && !info->vibrmotor_res)) {
 		dev_err(info->dev, "invalid vibra driver/motor resistance\n");
@@ -339,10 +358,9 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 		goto err_regulator;
 	}
 
-	if (pdata->vddvibl_uV) {
+	if (vddvibl_uV) {
 		ret = regulator_set_voltage(info->supplies[0].consumer,
-					    pdata->vddvibl_uV,
-					    pdata->vddvibl_uV);
+					    vddvibl_uV, vddvibl_uV);
 		if (ret) {
 			dev_err(info->dev, "failed to set VDDVIBL volt %d\n",
 				ret);
@@ -350,10 +368,9 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 		}
 	}
 
-	if (pdata->vddvibr_uV) {
+	if (vddvibr_uV) {
 		ret = regulator_set_voltage(info->supplies[1].consumer,
-					    pdata->vddvibr_uV,
-					    pdata->vddvibr_uV);
+					    vddvibr_uV, vddvibr_uV);
 		if (ret) {
 			dev_err(info->dev, "failed to set VDDVIBR volt %d\n",
 				ret);
@@ -401,6 +418,12 @@ static int __devexit twl6040_vibra_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id twl6040_vibra_of_match[] = {
+	{.compatible = "ti,twl6040-vibra", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, twl6040_vibra_of_match);
+
 static struct platform_driver twl6040_vibra_driver = {
 	.probe		= twl6040_vibra_probe,
 	.remove		= __devexit_p(twl6040_vibra_remove),
@@ -408,6 +431,7 @@ static struct platform_driver twl6040_vibra_driver = {
 		.name	= "twl6040-vibra",
 		.owner	= THIS_MODULE,
 		.pm	= &twl6040_vibra_pm_ops,
+		.of_match_table = twl6040_vibra_of_match,
 	},
 };
 module_platform_driver(twl6040_vibra_driver);
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index 9b8db821d5f0..cd6268cf7cd5 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -339,4 +339,16 @@ config MOUSE_SYNAPTICS_USB
 	  To compile this driver as a module, choose M here: the
 	  module will be called synaptics_usb.
 
+config MOUSE_NAVPOINT_PXA27x
+	tristate "Synaptics NavPoint (PXA27x SSP/SPI)"
+	depends on PXA27x && PXA_SSP
+	help
+	  This driver adds support for the Synaptics NavPoint touchpad connected
+	  to a PXA27x SSP port in SPI slave mode. The device emulates a mouse;
+	  a tap or tap-and-a-half drag gesture emulates the left mouse button.
+	  For example, use the xf86-input-evdev driver for an X pointing device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called navpoint.
+
 endif
diff --git a/drivers/input/mouse/Makefile b/drivers/input/mouse/Makefile
index 4718effeb8d9..46ba7556fd4f 100644
--- a/drivers/input/mouse/Makefile
+++ b/drivers/input/mouse/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_MOUSE_GPIO)		+= gpio_mouse.o
 obj-$(CONFIG_MOUSE_INPORT)		+= inport.o
 obj-$(CONFIG_MOUSE_LOGIBM)		+= logibm.o
 obj-$(CONFIG_MOUSE_MAPLE)		+= maplemouse.o
+obj-$(CONFIG_MOUSE_NAVPOINT_PXA27x)	+= navpoint.o
 obj-$(CONFIG_MOUSE_PC110PAD)		+= pc110pad.o
 obj-$(CONFIG_MOUSE_PS2)			+= psmouse.o
 obj-$(CONFIG_MOUSE_PXA930_TRKBALL)	+= pxa930_trkball.o
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 4c6a72d3d48c..4a1347e91bdc 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -553,10 +553,7 @@ static void alps_process_touchpad_packet_v3(struct psmouse *psmouse)
 
 	alps_report_semi_mt_data(dev, fingers, x1, y1, x2, y2);
 
-	input_report_key(dev, BTN_TOOL_FINGER, fingers == 1);
-	input_report_key(dev, BTN_TOOL_DOUBLETAP, fingers == 2);
-	input_report_key(dev, BTN_TOOL_TRIPLETAP, fingers == 3);
-	input_report_key(dev, BTN_TOOL_QUADTAP, fingers == 4);
+	input_mt_report_finger_count(dev, fingers);
 
 	input_report_key(dev, BTN_LEFT, left);
 	input_report_key(dev, BTN_RIGHT, right);
@@ -604,10 +601,54 @@ static void alps_process_packet_v3(struct psmouse *psmouse)
 
 static void alps_process_packet_v4(struct psmouse *psmouse)
 {
+	struct alps_data *priv = psmouse->private;
 	unsigned char *packet = psmouse->packet;
 	struct input_dev *dev = psmouse->dev;
+	int offset;
 	int x, y, z;
 	int left, right;
+	int x1, y1, x2, y2;
+	int fingers = 0;
+	unsigned int x_bitmap, y_bitmap;
+
+	/*
+	 * v4 has a 6-byte encoding for bitmap data, but this data is
+	 * broken up between 3 normal packets. Use priv->multi_packet to
+	 * track our position in the bitmap packet.
+	 */
+	if (packet[6] & 0x40) {
+		/* sync, reset position */
+		priv->multi_packet = 0;
+	}
+
+	if (WARN_ON_ONCE(priv->multi_packet > 2))
+		return;
+
+	offset = 2 * priv->multi_packet;
+	priv->multi_data[offset] = packet[6];
+	priv->multi_data[offset + 1] = packet[7];
+
+	if (++priv->multi_packet > 2) {
+		priv->multi_packet = 0;
+
+		x_bitmap = ((priv->multi_data[2] & 0x1f) << 10) |
+			   ((priv->multi_data[3] & 0x60) << 3) |
+			   ((priv->multi_data[0] & 0x3f) << 2) |
+			   ((priv->multi_data[1] & 0x60) >> 5);
+		y_bitmap = ((priv->multi_data[5] & 0x01) << 10) |
+			   ((priv->multi_data[3] & 0x1f) << 5) |
+			    (priv->multi_data[1] & 0x1f);
+
+		fingers = alps_process_bitmap(x_bitmap, y_bitmap,
+					      &x1, &y1, &x2, &y2);
+
+		/* Store MT data.*/
+		priv->fingers = fingers;
+		priv->x1 = x1;
+		priv->x2 = x2;
+		priv->y1 = y1;
+		priv->y2 = y2;
+	}
 
 	left = packet[4] & 0x01;
 	right = packet[4] & 0x02;
@@ -617,21 +658,41 @@ static void alps_process_packet_v4(struct psmouse *psmouse)
 	y = ((packet[2] & 0x7f) << 4) | (packet[3] & 0x0f);
 	z = packet[5] & 0x7f;
 
+	/*
+	 * If there were no contacts in the bitmap, use ST
+	 * points in MT reports.
+	 * If there were two contacts or more, report MT data.
+	 */
+	if (priv->fingers < 2) {
+		x1 = x;
+		y1 = y;
+		fingers = z > 0 ? 1 : 0;
+	} else {
+		fingers = priv->fingers;
+		x1 = priv->x1;
+		x2 = priv->x2;
+		y1 = priv->y1;
+		y2 = priv->y2;
+	}
+
 	if (z >= 64)
 		input_report_key(dev, BTN_TOUCH, 1);
 	else
 		input_report_key(dev, BTN_TOUCH, 0);
 
+	alps_report_semi_mt_data(dev, fingers, x1, y1, x2, y2);
+
+	input_mt_report_finger_count(dev, fingers);
+
+	input_report_key(dev, BTN_LEFT, left);
+	input_report_key(dev, BTN_RIGHT, right);
+
 	if (z > 0) {
 		input_report_abs(dev, ABS_X, x);
 		input_report_abs(dev, ABS_Y, y);
 	}
 	input_report_abs(dev, ABS_PRESSURE, z);
 
-	input_report_key(dev, BTN_TOOL_FINGER, z > 0);
-	input_report_key(dev, BTN_LEFT, left);
-	input_report_key(dev, BTN_RIGHT, right);
-
 	input_sync(dev);
 }
 
@@ -1557,6 +1618,7 @@ int alps_init(struct psmouse *psmouse)
 		input_set_abs_params(dev1, ABS_Y, 0, 767, 0, 0);
 		break;
 	case ALPS_PROTO_V3:
+	case ALPS_PROTO_V4:
 		set_bit(INPUT_PROP_SEMI_MT, dev1->propbit);
 		input_mt_init_slots(dev1, 2);
 		input_set_abs_params(dev1, ABS_MT_POSITION_X, 0, ALPS_V3_X_MAX, 0, 0);
@@ -1565,8 +1627,7 @@ int alps_init(struct psmouse *psmouse)
 		set_bit(BTN_TOOL_DOUBLETAP, dev1->keybit);
 		set_bit(BTN_TOOL_TRIPLETAP, dev1->keybit);
 		set_bit(BTN_TOOL_QUADTAP, dev1->keybit);
-		/* fall through */
-	case ALPS_PROTO_V4:
+
 		input_set_abs_params(dev1, ABS_X, 0, ALPS_V3_X_MAX, 0, 0);
 		input_set_abs_params(dev1, ABS_Y, 0, ALPS_V3_Y_MAX, 0, 0);
 		break;
diff --git a/drivers/input/mouse/alps.h b/drivers/input/mouse/alps.h
index a00a4ab92a0f..ae1ac354c778 100644
--- a/drivers/input/mouse/alps.h
+++ b/drivers/input/mouse/alps.h
@@ -39,6 +39,8 @@ struct alps_data {
 	int prev_fin;			/* Finger bit from previous packet */
 	int multi_packet;		/* Multi-packet data in progress */
 	unsigned char multi_data[6];	/* Saved multi-packet data */
+	int x1, x2, y1, y2;		/* Coordinates from last MT report */
+	int fingers;			/* Number of fingers from MT report */
 	u8 quirks;
 	struct timer_list timer;
 };
diff --git a/drivers/input/mouse/navpoint.c b/drivers/input/mouse/navpoint.c
new file mode 100644
index 000000000000..c29ae7654d5e
--- /dev/null
+++ b/drivers/input/mouse/navpoint.c
@@ -0,0 +1,369 @@
+/*
+ * Synaptics NavPoint (PXA27x SSP/SPI) driver.
+ *
+ * Copyright (C) 2012 Paul Parsons <lost.distance@yahoo.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 <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/input.h>
+#include <linux/input/navpoint.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/pxa2xx_ssp.h>
+#include <linux/slab.h>
+
+/*
+ * Synaptics Modular Embedded Protocol: Module Packet Format.
+ * Module header byte 2:0 = Length (# bytes that follow)
+ * Module header byte 4:3 = Control
+ * Module header byte 7:5 = Module Address
+ */
+#define HEADER_LENGTH(byte)	((byte) & 0x07)
+#define HEADER_CONTROL(byte)	(((byte) >> 3) & 0x03)
+#define HEADER_ADDRESS(byte)	((byte) >> 5)
+
+struct navpoint {
+	struct ssp_device	*ssp;
+	struct input_dev	*input;
+	struct device		*dev;
+	int			gpio;
+	int			index;
+	u8			data[1 + HEADER_LENGTH(0xff)];
+};
+
+/*
+ * Initialization values for SSCR0_x, SSCR1_x, SSSR_x.
+ */
+static const u32 sscr0 = 0
+	| SSCR0_TUM		/* TIM = 1; No TUR interrupts */
+	| SSCR0_RIM		/* RIM = 1; No ROR interrupts */
+	| SSCR0_SSE		/* SSE = 1; SSP enabled */
+	| SSCR0_Motorola	/* FRF = 0; Motorola SPI */
+	| SSCR0_DataSize(16)	/* DSS = 15; Data size = 16-bit */
+	;
+static const u32 sscr1 = 0
+	| SSCR1_SCFR		/* SCFR = 1; SSPSCLK only during transfers */
+	| SSCR1_SCLKDIR		/* SCLKDIR = 1; Slave mode */
+	| SSCR1_SFRMDIR		/* SFRMDIR = 1; Slave mode */
+	| SSCR1_RWOT		/* RWOT = 1; Receive without transmit mode */
+	| SSCR1_RxTresh(1)	/* RFT = 0; Receive FIFO threshold = 1 */
+	| SSCR1_SPH		/* SPH = 1; SSPSCLK inactive 0.5 + 1 cycles */
+	| SSCR1_RIE		/* RIE = 1; Receive FIFO interrupt enabled */
+	;
+static const u32 sssr = 0
+	| SSSR_BCE		/* BCE = 1; Clear BCE */
+	| SSSR_TUR		/* TUR = 1; Clear TUR */
+	| SSSR_EOC		/* EOC = 1; Clear EOC */
+	| SSSR_TINT		/* TINT = 1; Clear TINT */
+	| SSSR_PINT		/* PINT = 1; Clear PINT */
+	| SSSR_ROR		/* ROR = 1; Clear ROR */
+	;
+
+/*
+ * MEP Query $22: Touchpad Coordinate Range Query is not supported by
+ * the NavPoint module, so sampled values provide the default limits.
+ */
+#define NAVPOINT_X_MIN		1278
+#define NAVPOINT_X_MAX		5340
+#define NAVPOINT_Y_MIN		1572
+#define NAVPOINT_Y_MAX		4396
+#define NAVPOINT_PRESSURE_MIN	0
+#define NAVPOINT_PRESSURE_MAX	255
+
+static void navpoint_packet(struct navpoint *navpoint)
+{
+	int finger;
+	int gesture;
+	int x, y, z;
+
+	switch (navpoint->data[0]) {
+	case 0xff:	/* Garbage (packet?) between reset and Hello packet */
+	case 0x00:	/* Module 0, NULL packet */
+		break;
+
+	case 0x0e:	/* Module 0, Absolute packet */
+		finger = (navpoint->data[1] & 0x01);
+		gesture = (navpoint->data[1] & 0x02);
+		x = ((navpoint->data[2] & 0x1f) << 8) | navpoint->data[3];
+		y = ((navpoint->data[4] & 0x1f) << 8) | navpoint->data[5];
+		z = navpoint->data[6];
+		input_report_key(navpoint->input, BTN_TOUCH, finger);
+		input_report_abs(navpoint->input, ABS_X, x);
+		input_report_abs(navpoint->input, ABS_Y, y);
+		input_report_abs(navpoint->input, ABS_PRESSURE, z);
+		input_report_key(navpoint->input, BTN_TOOL_FINGER, finger);
+		input_report_key(navpoint->input, BTN_LEFT, gesture);
+		input_sync(navpoint->input);
+		break;
+
+	case 0x19:	/* Module 0, Hello packet */
+		if ((navpoint->data[1] & 0xf0) == 0x10)
+			break;
+		/* FALLTHROUGH */
+	default:
+		dev_warn(navpoint->dev,
+			 "spurious packet: data=0x%02x,0x%02x,...\n",
+			 navpoint->data[0], navpoint->data[1]);
+		break;
+	}
+}
+
+static irqreturn_t navpoint_irq(int irq, void *dev_id)
+{
+	struct navpoint *navpoint = dev_id;
+	struct ssp_device *ssp = navpoint->ssp;
+	irqreturn_t ret = IRQ_NONE;
+	u32 status;
+
+	status = pxa_ssp_read_reg(ssp, SSSR);
+	if (status & sssr) {
+		dev_warn(navpoint->dev,
+			 "unexpected interrupt: status=0x%08x\n", status);
+		pxa_ssp_write_reg(ssp, SSSR, (status & sssr));
+		ret = IRQ_HANDLED;
+	}
+
+	while (status & SSSR_RNE) {
+		u32 data;
+
+		data = pxa_ssp_read_reg(ssp, SSDR);
+		navpoint->data[navpoint->index + 0] = (data >> 8);
+		navpoint->data[navpoint->index + 1] = data;
+		navpoint->index += 2;
+		if (HEADER_LENGTH(navpoint->data[0]) < navpoint->index) {
+			navpoint_packet(navpoint);
+			navpoint->index = 0;
+		}
+		status = pxa_ssp_read_reg(ssp, SSSR);
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static void navpoint_up(struct navpoint *navpoint)
+{
+	struct ssp_device *ssp = navpoint->ssp;
+	int timeout;
+
+	clk_prepare_enable(ssp->clk);
+
+	pxa_ssp_write_reg(ssp, SSCR1, sscr1);
+	pxa_ssp_write_reg(ssp, SSSR, sssr);
+	pxa_ssp_write_reg(ssp, SSTO, 0);
+	pxa_ssp_write_reg(ssp, SSCR0, sscr0);	/* SSCR0_SSE written last */
+
+	/* Wait until SSP port is ready for slave clock operations */
+	for (timeout = 100; timeout != 0; --timeout) {
+		if (!(pxa_ssp_read_reg(ssp, SSSR) & SSSR_CSS))
+			break;
+		msleep(1);
+	}
+
+	if (timeout == 0)
+		dev_err(navpoint->dev,
+			"timeout waiting for SSSR[CSS] to clear\n");
+
+	if (gpio_is_valid(navpoint->gpio))
+		gpio_set_value(navpoint->gpio, 1);
+}
+
+static void navpoint_down(struct navpoint *navpoint)
+{
+	struct ssp_device *ssp = navpoint->ssp;
+
+	if (gpio_is_valid(navpoint->gpio))
+		gpio_set_value(navpoint->gpio, 0);
+
+	pxa_ssp_write_reg(ssp, SSCR0, 0);
+
+	clk_disable_unprepare(ssp->clk);
+}
+
+static int navpoint_open(struct input_dev *input)
+{
+	struct navpoint *navpoint = input_get_drvdata(input);
+
+	navpoint_up(navpoint);
+
+	return 0;
+}
+
+static void navpoint_close(struct input_dev *input)
+{
+	struct navpoint *navpoint = input_get_drvdata(input);
+
+	navpoint_down(navpoint);
+}
+
+static int __devinit navpoint_probe(struct platform_device *pdev)
+{
+	const struct navpoint_platform_data *pdata =
+					dev_get_platdata(&pdev->dev);
+	struct ssp_device *ssp;
+	struct input_dev *input;
+	struct navpoint *navpoint;
+	int error;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	if (gpio_is_valid(pdata->gpio)) {
+		error = gpio_request_one(pdata->gpio, GPIOF_OUT_INIT_LOW,
+					 "SYNAPTICS_ON");
+		if (error)
+			return error;
+	}
+
+	ssp = pxa_ssp_request(pdata->port, pdev->name);
+	if (!ssp) {
+		error = -ENODEV;
+		goto err_free_gpio;
+	}
+
+	/* HaRET does not disable devices before jumping into Linux */
+	if (pxa_ssp_read_reg(ssp, SSCR0) & SSCR0_SSE) {
+		pxa_ssp_write_reg(ssp, SSCR0, 0);
+		dev_warn(&pdev->dev, "ssp%d already enabled\n", pdata->port);
+	}
+
+	navpoint = kzalloc(sizeof(*navpoint), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!navpoint || !input) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	navpoint->ssp = ssp;
+	navpoint->input = input;
+	navpoint->dev = &pdev->dev;
+	navpoint->gpio = pdata->gpio;
+
+	input->name = pdev->name;
+	input->dev.parent = &pdev->dev;
+
+	__set_bit(EV_KEY, input->evbit);
+	__set_bit(EV_ABS, input->evbit);
+	__set_bit(BTN_LEFT, input->keybit);
+	__set_bit(BTN_TOUCH, input->keybit);
+	__set_bit(BTN_TOOL_FINGER, input->keybit);
+
+	input_set_abs_params(input, ABS_X,
+			     NAVPOINT_X_MIN, NAVPOINT_X_MAX, 0, 0);
+	input_set_abs_params(input, ABS_Y,
+			     NAVPOINT_Y_MIN, NAVPOINT_Y_MAX, 0, 0);
+	input_set_abs_params(input, ABS_PRESSURE,
+			     NAVPOINT_PRESSURE_MIN, NAVPOINT_PRESSURE_MAX,
+			     0, 0);
+
+	input->open = navpoint_open;
+	input->close = navpoint_close;
+
+	input_set_drvdata(input, navpoint);
+
+	error = request_irq(ssp->irq, navpoint_irq, 0, pdev->name, navpoint);
+	if (error)
+		goto err_free_mem;
+
+	error = input_register_device(input);
+	if (error)
+		goto err_free_irq;
+
+	platform_set_drvdata(pdev, navpoint);
+	dev_dbg(&pdev->dev, "ssp%d, irq %d\n", pdata->port, ssp->irq);
+
+	return 0;
+
+err_free_irq:
+	free_irq(ssp->irq, &pdev->dev);
+err_free_mem:
+	input_free_device(input);
+	kfree(navpoint);
+	pxa_ssp_free(ssp);
+err_free_gpio:
+	if (gpio_is_valid(pdata->gpio))
+		gpio_free(pdata->gpio);
+
+	return error;
+}
+
+static int __devexit navpoint_remove(struct platform_device *pdev)
+{
+	const struct navpoint_platform_data *pdata =
+					dev_get_platdata(&pdev->dev);
+	struct navpoint *navpoint = platform_get_drvdata(pdev);
+	struct ssp_device *ssp = navpoint->ssp;
+
+	free_irq(ssp->irq, navpoint);
+
+	input_unregister_device(navpoint->input);
+	kfree(navpoint);
+
+	pxa_ssp_free(ssp);
+
+	if (gpio_is_valid(pdata->gpio))
+		gpio_free(pdata->gpio);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int navpoint_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct navpoint *navpoint = platform_get_drvdata(pdev);
+	struct input_dev *input = navpoint->input;
+
+	mutex_lock(&input->mutex);
+	if (input->users)
+		navpoint_down(navpoint);
+	mutex_unlock(&input->mutex);
+
+	return 0;
+}
+
+static int navpoint_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct navpoint *navpoint = platform_get_drvdata(pdev);
+	struct input_dev *input = navpoint->input;
+
+	mutex_lock(&input->mutex);
+	if (input->users)
+		navpoint_up(navpoint);
+	mutex_unlock(&input->mutex);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(navpoint_pm_ops, navpoint_suspend, navpoint_resume);
+
+static struct platform_driver navpoint_driver = {
+	.probe		= navpoint_probe,
+	.remove		= __devexit_p(navpoint_remove),
+	.driver = {
+		.name	= "navpoint",
+		.owner	= THIS_MODULE,
+		.pm	= &navpoint_pm_ops,
+	},
+};
+
+module_platform_driver(navpoint_driver);
+
+MODULE_AUTHOR("Paul Parsons <lost.distance@yahoo.com>");
+MODULE_DESCRIPTION("Synaptics NavPoint (PXA27x SSP/SPI) driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:navpoint");
diff --git a/drivers/input/mouse/sentelic.c b/drivers/input/mouse/sentelic.c
index 661a0ca3b3d6..3f5649f19082 100644
--- a/drivers/input/mouse/sentelic.c
+++ b/drivers/input/mouse/sentelic.c
@@ -41,7 +41,7 @@
 #define	GET_ABS_Y(packet)	((packet[2] << 2) | (packet[3] & 0x03))
 
 /** Driver version. */
-static const char fsp_drv_ver[] = "1.0.0-K";
+static const char fsp_drv_ver[] = "1.1.0-K";
 
 /*
  * Make sure that the value being sent to FSP will not conflict with
@@ -303,6 +303,27 @@ static int fsp_get_revision(struct psmouse *psmouse, int *rev)
 	return 0;
 }
 
+static int fsp_get_sn(struct psmouse *psmouse, int *sn)
+{
+	int v0, v1, v2;
+	int rc = -EIO;
+
+	/* production number since Cx is available at: 0x0b40 ~ 0x0b42 */
+	if (fsp_page_reg_write(psmouse, FSP_PAGE_0B))
+		goto out;
+	if (fsp_reg_read(psmouse, FSP_REG_SN0, &v0))
+		goto out;
+	if (fsp_reg_read(psmouse, FSP_REG_SN1, &v1))
+		goto out;
+	if (fsp_reg_read(psmouse, FSP_REG_SN2, &v2))
+		goto out;
+	*sn = (v0 << 16) | (v1 << 8) | v2;
+	rc = 0;
+out:
+	fsp_page_reg_write(psmouse, FSP_PAGE_DEFAULT);
+	return rc;
+}
+
 static int fsp_get_buttons(struct psmouse *psmouse, int *btn)
 {
 	static const int buttons[] = {
@@ -1000,16 +1021,21 @@ static int fsp_reconnect(struct psmouse *psmouse)
 int fsp_init(struct psmouse *psmouse)
 {
 	struct fsp_data *priv;
-	int ver, rev;
+	int ver, rev, sn = 0;
 	int error;
 
 	if (fsp_get_version(psmouse, &ver) ||
 	    fsp_get_revision(psmouse, &rev)) {
 		return -ENODEV;
 	}
+	if (ver >= FSP_VER_STL3888_C0) {
+		/* firmware information is only available since C0 */
+		fsp_get_sn(psmouse, &sn);
+	}
 
-	psmouse_info(psmouse, "Finger Sensing Pad, hw: %d.%d.%d, sw: %s\n",
-		     ver >> 4, ver & 0x0F, rev, fsp_drv_ver);
+	psmouse_info(psmouse,
+		     "Finger Sensing Pad, hw: %d.%d.%d, sn: %x, sw: %s\n",
+		     ver >> 4, ver & 0x0F, rev, sn, fsp_drv_ver);
 
 	psmouse->private = priv = kzalloc(sizeof(struct fsp_data), GFP_KERNEL);
 	if (!priv)
diff --git a/drivers/input/mouse/sentelic.h b/drivers/input/mouse/sentelic.h
index 334de19e5ddb..aa697ece405b 100644
--- a/drivers/input/mouse/sentelic.h
+++ b/drivers/input/mouse/sentelic.h
@@ -65,6 +65,14 @@
 #define	FSP_BIT_SWC1_GST_GRP1	BIT(6)
 #define	FSP_BIT_SWC1_BX_COMPAT	BIT(7)
 
+#define	FSP_PAGE_0B		(0x0b)
+#define	FSP_PAGE_82		(0x82)
+#define	FSP_PAGE_DEFAULT	FSP_PAGE_82
+
+#define	FSP_REG_SN0		(0x40)
+#define	FSP_REG_SN1		(0x41)
+#define	FSP_REG_SN2		(0x42)
+
 /* Finger-sensing Pad packet formating related definitions */
 
 /* absolute packet type */
diff --git a/drivers/input/mouse/sermouse.c b/drivers/input/mouse/sermouse.c
index 17ff137b9bd5..d5928fd0c914 100644
--- a/drivers/input/mouse/sermouse.c
+++ b/drivers/input/mouse/sermouse.c
@@ -355,15 +355,4 @@ static struct serio_driver sermouse_drv = {
 	.disconnect	= sermouse_disconnect,
 };
 
-static int __init sermouse_init(void)
-{
-	return serio_register_driver(&sermouse_drv);
-}
-
-static void __exit sermouse_exit(void)
-{
-	serio_unregister_driver(&sermouse_drv);
-}
-
-module_init(sermouse_init);
-module_exit(sermouse_exit);
+module_serio_driver(sermouse_drv);
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index a4b14a41cbf4..c703d53be3a0 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -45,16 +45,6 @@
 #define YMIN_NOMINAL 1408
 #define YMAX_NOMINAL 4448
 
-/*
- * Synaptics touchpads report the y coordinate from bottom to top, which is
- * opposite from what userspace expects.
- * This function is used to invert y before reporting.
- */
-static int synaptics_invert_y(int y)
-{
-	return YMAX_NOMINAL + YMIN_NOMINAL - y;
-}
-
 
 /*****************************************************************************
  *	Stuff we need even when we do not want native Synaptics support
@@ -112,6 +102,16 @@ void synaptics_reset(struct psmouse *psmouse)
  ****************************************************************************/
 
 /*
+ * Synaptics touchpads report the y coordinate from bottom to top, which is
+ * opposite from what userspace expects.
+ * This function is used to invert y before reporting.
+ */
+static int synaptics_invert_y(int y)
+{
+	return YMAX_NOMINAL + YMIN_NOMINAL - y;
+}
+
+/*
  * Send a command to the synpatics touchpad by special commands
  */
 static int synaptics_send_cmd(struct psmouse *psmouse, unsigned char c, unsigned char *param)
diff --git a/drivers/input/mouse/vsxxxaa.c b/drivers/input/mouse/vsxxxaa.c
index eb9a3cfbeefa..e900d465aaf6 100644
--- a/drivers/input/mouse/vsxxxaa.c
+++ b/drivers/input/mouse/vsxxxaa.c
@@ -548,16 +548,4 @@ static struct serio_driver vsxxxaa_drv = {
 	.disconnect	= vsxxxaa_disconnect,
 };
 
-static int __init vsxxxaa_init(void)
-{
-	return serio_register_driver(&vsxxxaa_drv);
-}
-
-static void __exit vsxxxaa_exit(void)
-{
-	serio_unregister_driver(&vsxxxaa_drv);
-}
-
-module_init(vsxxxaa_init);
-module_exit(vsxxxaa_exit);
-
+module_serio_driver(vsxxxaa_drv);
diff --git a/drivers/input/of_keymap.c b/drivers/input/of_keymap.c
deleted file mode 100644
index 061493d57682..000000000000
--- a/drivers/input/of_keymap.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * Helpers for open firmware matrix keyboard bindings
- *
- * Copyright (C) 2012 Google, Inc
- *
- * Author:
- *	Olof Johansson <olof@lixom.net>
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/input.h>
-#include <linux/of.h>
-#include <linux/input/matrix_keypad.h>
-#include <linux/export.h>
-#include <linux/gfp.h>
-#include <linux/slab.h>
-
-struct matrix_keymap_data *
-matrix_keyboard_of_fill_keymap(struct device_node *np,
-			       const char *propname)
-{
-	struct matrix_keymap_data *kd;
-	u32 *keymap;
-	int proplen, i;
-	const __be32 *prop;
-
-	if (!np)
-		return NULL;
-
-	if (!propname)
-		propname = "linux,keymap";
-
-	prop = of_get_property(np, propname, &proplen);
-	if (!prop)
-		return NULL;
-
-	if (proplen % sizeof(u32)) {
-		pr_warn("Malformed keymap property %s in %s\n",
-			propname, np->full_name);
-		return NULL;
-	}
-
-	kd = kzalloc(sizeof(*kd), GFP_KERNEL);
-	if (!kd)
-		return NULL;
-
-	kd->keymap = keymap = kzalloc(proplen, GFP_KERNEL);
-	if (!kd->keymap) {
-		kfree(kd);
-		return NULL;
-	}
-
-	kd->keymap_size = proplen / sizeof(u32);
-
-	for (i = 0; i < kd->keymap_size; i++) {
-		u32 tmp = be32_to_cpup(prop + i);
-		int key_code, row, col;
-
-		row = (tmp >> 24) & 0xff;
-		col = (tmp >> 16) & 0xff;
-		key_code = tmp & 0xffff;
-		keymap[i] = KEY(row, col, key_code);
-	}
-
-	return kd;
-}
-EXPORT_SYMBOL_GPL(matrix_keyboard_of_fill_keymap);
-
-void matrix_keyboard_of_free_keymap(const struct matrix_keymap_data *kd)
-{
-	if (kd) {
-		kfree(kd->keymap);
-		kfree(kd);
-	}
-}
-EXPORT_SYMBOL_GPL(matrix_keyboard_of_free_keymap);
diff --git a/drivers/input/serio/pcips2.c b/drivers/input/serio/pcips2.c
index 43494742541c..0c42497aaaf4 100644
--- a/drivers/input/serio/pcips2.c
+++ b/drivers/input/serio/pcips2.c
@@ -206,6 +206,7 @@ static const struct pci_device_id pcips2_ids[] = {
 	},
 	{ 0, }
 };
+MODULE_DEVICE_TABLE(pci, pcips2_ids);
 
 static struct pci_driver pcips2_driver = {
 	.name			= "pcips2",
@@ -214,20 +215,8 @@ static struct pci_driver pcips2_driver = {
 	.remove			= __devexit_p(pcips2_remove),
 };
 
-static int __init pcips2_init(void)
-{
-	return pci_register_driver(&pcips2_driver);
-}
-
-static void __exit pcips2_exit(void)
-{
-	pci_unregister_driver(&pcips2_driver);
-}
-
-module_init(pcips2_init);
-module_exit(pcips2_exit);
+module_pci_driver(pcips2_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Russell King <rmk@arm.linux.org.uk>");
 MODULE_DESCRIPTION("PCI PS/2 keyboard/mouse driver");
-MODULE_DEVICE_TABLE(pci, pcips2_ids);
diff --git a/drivers/input/serio/ps2mult.c b/drivers/input/serio/ps2mult.c
index 15aa81c9f1fb..a76fb64f03db 100644
--- a/drivers/input/serio/ps2mult.c
+++ b/drivers/input/serio/ps2mult.c
@@ -304,15 +304,4 @@ static struct serio_driver ps2mult_drv = {
 	.reconnect	= ps2mult_reconnect,
 };
 
-static int __init ps2mult_init(void)
-{
-	return serio_register_driver(&ps2mult_drv);
-}
-
-static void __exit ps2mult_exit(void)
-{
-	serio_unregister_driver(&ps2mult_drv);
-}
-
-module_init(ps2mult_init);
-module_exit(ps2mult_exit);
+module_serio_driver(ps2mult_drv);
diff --git a/drivers/input/serio/serio_raw.c b/drivers/input/serio/serio_raw.c
index 4494233d331a..59df2e7317a3 100644
--- a/drivers/input/serio/serio_raw.c
+++ b/drivers/input/serio/serio_raw.c
@@ -165,31 +165,38 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 	struct serio_raw *serio_raw = client->serio_raw;
 	char uninitialized_var(c);
 	ssize_t read = 0;
-	int retval;
+	int error;
 
-	if (serio_raw->dead)
-		return -ENODEV;
+	for (;;) {
+		if (serio_raw->dead)
+			return -ENODEV;
 
-	if (serio_raw->head == serio_raw->tail && (file->f_flags & O_NONBLOCK))
-		return -EAGAIN;
+		if (serio_raw->head == serio_raw->tail &&
+		    (file->f_flags & O_NONBLOCK))
+			return -EAGAIN;
 
-	retval = wait_event_interruptible(serio_raw->wait,
-			serio_raw->head != serio_raw->tail || serio_raw->dead);
-	if (retval)
-		return retval;
+		if (count == 0)
+			break;
 
-	if (serio_raw->dead)
-		return -ENODEV;
+		while (read < count && serio_raw_fetch_byte(serio_raw, &c)) {
+			if (put_user(c, buffer++))
+				return -EFAULT;
+			read++;
+		}
 
-	while (read < count && serio_raw_fetch_byte(serio_raw, &c)) {
-		if (put_user(c, buffer++)) {
-			retval = -EFAULT;
+		if (read)
 			break;
+
+		if (!(file->f_flags & O_NONBLOCK)) {
+			error = wait_event_interruptible(serio_raw->wait,
+					serio_raw->head != serio_raw->tail ||
+					serio_raw->dead);
+			if (error)
+				return error;
 		}
-		read++;
 	}
 
-	return read ?: retval;
+	return read;
 }
 
 static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
@@ -197,8 +204,7 @@ static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
 {
 	struct serio_raw_client *client = file->private_data;
 	struct serio_raw *serio_raw = client->serio_raw;
-	ssize_t written = 0;
-	int retval;
+	int retval = 0;
 	unsigned char c;
 
 	retval = mutex_lock_interruptible(&serio_raw_mutex);
@@ -218,16 +224,20 @@ static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
 			retval = -EFAULT;
 			goto out;
 		}
+
 		if (serio_write(serio_raw->serio, c)) {
-			retval = -EIO;
+			/* Either signal error or partial write */
+			if (retval == 0)
+				retval = -EIO;
 			goto out;
 		}
-		written++;
+
+		retval++;
 	}
 
 out:
 	mutex_unlock(&serio_raw_mutex);
-	return written ?: retval;
+	return retval;
 }
 
 static unsigned int serio_raw_poll(struct file *file, poll_table *wait)
@@ -432,15 +442,4 @@ static struct serio_driver serio_raw_drv = {
 	.manual_bind	= true,
 };
 
-static int __init serio_raw_init(void)
-{
-	return serio_register_driver(&serio_raw_drv);
-}
-
-static void __exit serio_raw_exit(void)
-{
-	serio_unregister_driver(&serio_raw_drv);
-}
-
-module_init(serio_raw_init);
-module_exit(serio_raw_exit);
+module_serio_driver(serio_raw_drv);
diff --git a/drivers/input/serio/xilinx_ps2.c b/drivers/input/serio/xilinx_ps2.c
index d96d4c2a76a9..1e983bec7d86 100644
--- a/drivers/input/serio/xilinx_ps2.c
+++ b/drivers/input/serio/xilinx_ps2.c
@@ -73,7 +73,8 @@ struct xps2data {
 	spinlock_t lock;
 	void __iomem *base_address;	/* virt. address of control registers */
 	unsigned int flags;
-	struct serio serio;		/* serio */
+	struct serio *serio;		/* serio */
+	struct device *dev;
 };
 
 /************************************/
@@ -119,7 +120,7 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 
 	/* Check which interrupt is active */
 	if (intr_sr & XPS2_IPIXR_RX_OVF)
-		dev_warn(drvdata->serio.dev.parent, "receive overrun error\n");
+		dev_warn(drvdata->dev, "receive overrun error\n");
 
 	if (intr_sr & XPS2_IPIXR_RX_ERR)
 		drvdata->flags |= SERIO_PARITY;
@@ -132,10 +133,10 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 
 		/* Error, if a byte is not received */
 		if (status) {
-			dev_err(drvdata->serio.dev.parent,
+			dev_err(drvdata->dev,
 				"wrong rcvd byte count (%d)\n", status);
 		} else {
-			serio_interrupt(&drvdata->serio, c, drvdata->flags);
+			serio_interrupt(drvdata->serio, c, drvdata->flags);
 			drvdata->flags = 0;
 		}
 	}
@@ -193,7 +194,7 @@ static int sxps2_open(struct serio *pserio)
 	error = request_irq(drvdata->irq, &xps2_interrupt, 0,
 				DRIVER_NAME, drvdata);
 	if (error) {
-		dev_err(drvdata->serio.dev.parent,
+		dev_err(drvdata->dev,
 			"Couldn't allocate interrupt %d\n", drvdata->irq);
 		return error;
 	}
@@ -259,15 +260,16 @@ static int __devinit xps2_of_probe(struct platform_device *ofdev)
 	}
 
 	drvdata = kzalloc(sizeof(struct xps2data), GFP_KERNEL);
-	if (!drvdata) {
-		dev_err(dev, "Couldn't allocate device private record\n");
-		return -ENOMEM;
+	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
+	if (!drvdata || !serio) {
+		error = -ENOMEM;
+		goto failed1;
 	}
 
-	dev_set_drvdata(dev, drvdata);
-
 	spin_lock_init(&drvdata->lock);
 	drvdata->irq = r_irq.start;
+	drvdata->serio = serio;
+	drvdata->dev = dev;
 
 	phys_addr = r_mem.start;
 	remap_size = resource_size(&r_mem);
@@ -298,7 +300,6 @@ static int __devinit xps2_of_probe(struct platform_device *ofdev)
 		 (unsigned long long)phys_addr, drvdata->base_address,
 		 drvdata->irq);
 
-	serio = &drvdata->serio;
 	serio->id.type = SERIO_8042;
 	serio->write = sxps2_write;
 	serio->open = sxps2_open;
@@ -312,13 +313,14 @@ static int __devinit xps2_of_probe(struct platform_device *ofdev)
 
 	serio_register_port(serio);
 
+	platform_set_drvdata(ofdev, drvdata);
 	return 0;		/* success */
 
 failed2:
 	release_mem_region(phys_addr, remap_size);
 failed1:
+	kfree(serio);
 	kfree(drvdata);
-	dev_set_drvdata(dev, NULL);
 
 	return error;
 }
@@ -333,22 +335,21 @@ failed1:
  */
 static int __devexit xps2_of_remove(struct platform_device *of_dev)
 {
-	struct device *dev = &of_dev->dev;
-	struct xps2data *drvdata = dev_get_drvdata(dev);
+	struct xps2data *drvdata = platform_get_drvdata(of_dev);
 	struct resource r_mem; /* IO mem resources */
 
-	serio_unregister_port(&drvdata->serio);
+	serio_unregister_port(drvdata->serio);
 	iounmap(drvdata->base_address);
 
 	/* Get iospace of the device */
 	if (of_address_to_resource(of_dev->dev.of_node, 0, &r_mem))
-		dev_err(dev, "invalid address\n");
+		dev_err(drvdata->dev, "invalid address\n");
 	else
 		release_mem_region(r_mem.start, resource_size(&r_mem));
 
 	kfree(drvdata);
 
-	dev_set_drvdata(dev, NULL);
+	platform_set_drvdata(of_dev, NULL);
 
 	return 0;
 }
diff --git a/drivers/input/tablet/aiptek.c b/drivers/input/tablet/aiptek.c
index 755a39e4c9e9..ee83c3904ee8 100644
--- a/drivers/input/tablet/aiptek.c
+++ b/drivers/input/tablet/aiptek.c
@@ -1862,7 +1862,7 @@ aiptek_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	if (i == ARRAY_SIZE(speeds)) {
 		dev_info(&intf->dev,
 			 "Aiptek tried all speeds, no sane response\n");
-		goto fail2;
+		goto fail3;
 	}
 
 	/* Associate this driver's struct with the usb interface.
diff --git a/drivers/input/tablet/wacom.h b/drivers/input/tablet/wacom.h
index b4842d0e61dd..b79d45198d82 100644
--- a/drivers/input/tablet/wacom.h
+++ b/drivers/input/tablet/wacom.h
@@ -135,6 +135,6 @@ extern const struct usb_device_id wacom_ids[];
 
 void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len);
 void wacom_setup_device_quirks(struct wacom_features *features);
-void wacom_setup_input_capabilities(struct input_dev *input_dev,
-				    struct wacom_wac *wacom_wac);
+int wacom_setup_input_capabilities(struct input_dev *input_dev,
+				   struct wacom_wac *wacom_wac);
 #endif
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 79a0509882d4..cad5602d3ce4 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -28,6 +28,7 @@
 #define HID_USAGE_Y_TILT		0x3e
 #define HID_USAGE_FINGER		0x22
 #define HID_USAGE_STYLUS		0x20
+#define HID_USAGE_CONTACTMAX		0x55
 #define HID_COLLECTION			0xa1
 #define HID_COLLECTION_LOGICAL		0x02
 #define HID_COLLECTION_END		0xc0
@@ -204,6 +205,27 @@ static int wacom_parse_logical_collection(unsigned char *report,
 	return length;
 }
 
+static void wacom_retrieve_report_data(struct usb_interface *intf,
+				       struct wacom_features *features)
+{
+	int result = 0;
+	unsigned char *rep_data;
+
+	rep_data = kmalloc(2, GFP_KERNEL);
+	if (rep_data) {
+
+		rep_data[0] = 12;
+		result = wacom_get_report(intf, WAC_HID_FEATURE_REPORT,
+					  rep_data[0], &rep_data, 2,
+					  WAC_MSG_RETRIES);
+
+		if (result >= 0 && rep_data[1] > 2)
+			features->touch_max = rep_data[1];
+
+		kfree(rep_data);
+	}
+}
+
 /*
  * Interface Descriptor of wacom devices can be incomplete and
  * inconsistent so wacom_features table is used to store stylus
@@ -236,6 +258,9 @@ static int wacom_parse_logical_collection(unsigned char *report,
  * 3rd gen Bamboo Touch no longer define a Digitizer-Finger Pysical
  * Collection. Instead they define a Logical Collection with a single
  * Logical Maximum for both X and Y.
+ *
+ * Intuos5 touch interface does not contain useful data. We deal with
+ * this after returning from this function.
  */
 static int wacom_parse_hid(struct usb_interface *intf,
 			   struct hid_descriptor *hid_desc,
@@ -295,6 +320,10 @@ static int wacom_parse_hid(struct usb_interface *intf,
 							/* need to reset back */
 							features->pktlen = WACOM_PKGLEN_TPC2FG;
 						}
+
+						if (features->type == MTSCREEN)
+							features->pktlen = WACOM_PKGLEN_MTOUCH;
+
 						if (features->type == BAMBOO_PT) {
 							/* need to reset back */
 							features->pktlen = WACOM_PKGLEN_BBTOUCH;
@@ -327,18 +356,15 @@ static int wacom_parse_hid(struct usb_interface *intf,
 			case HID_USAGE_Y:
 				if (usage == WCM_DESKTOP) {
 					if (finger) {
-						features->device_type = BTN_TOOL_FINGER;
-						if (features->type == TABLETPC2FG) {
-							/* need to reset back */
-							features->pktlen = WACOM_PKGLEN_TPC2FG;
+						int type = features->type;
+
+						if (type == TABLETPC2FG || type == MTSCREEN) {
 							features->y_max =
 								get_unaligned_le16(&report[i + 3]);
 							features->y_phy =
 								get_unaligned_le16(&report[i + 6]);
 							i += 7;
-						} else if (features->type == BAMBOO_PT) {
-							/* need to reset back */
-							features->pktlen = WACOM_PKGLEN_BBTOUCH;
+						} else if (type == BAMBOO_PT) {
 							features->y_phy =
 								get_unaligned_le16(&report[i + 3]);
 							features->y_max =
@@ -352,10 +378,6 @@ static int wacom_parse_hid(struct usb_interface *intf,
 							i += 4;
 						}
 					} else if (pen) {
-						/* penabled only accepts exact bytes of data */
-						if (features->type == TABLETPC2FG)
-							features->pktlen = WACOM_PKGLEN_GRAPHIRE;
-						features->device_type = BTN_TOOL_PEN;
 						features->y_max =
 							get_unaligned_le16(&report[i + 3]);
 						i += 4;
@@ -377,6 +399,11 @@ static int wacom_parse_hid(struct usb_interface *intf,
 				pen = 1;
 				i++;
 				break;
+
+			case HID_USAGE_CONTACTMAX:
+				wacom_retrieve_report_data(intf, features);
+				i++;
+				break;
 			}
 			break;
 
@@ -413,22 +440,29 @@ static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_feat
 	if (!rep_data)
 		return error;
 
-	/* ask to report tablet data if it is MT Tablet PC or
-	 * not a Tablet PC */
-	if (features->type == TABLETPC2FG) {
-		do {
-			rep_data[0] = 3;
-			rep_data[1] = 4;
-			rep_data[2] = 0;
-			rep_data[3] = 0;
-			report_id = 3;
-			error = wacom_set_report(intf, WAC_HID_FEATURE_REPORT,
-						 report_id, rep_data, 4, 1);
-			if (error >= 0)
-				error = wacom_get_report(intf,
-						WAC_HID_FEATURE_REPORT,
-						report_id, rep_data, 4, 1);
-		} while ((error < 0 || rep_data[1] != 4) && limit++ < WAC_MSG_RETRIES);
+	/* ask to report Wacom data */
+	if (features->device_type == BTN_TOOL_FINGER) {
+		/* if it is an MT Tablet PC touch */
+		if (features->type == TABLETPC2FG ||
+		    features->type == MTSCREEN) {
+			do {
+				rep_data[0] = 3;
+				rep_data[1] = 4;
+				rep_data[2] = 0;
+				rep_data[3] = 0;
+				report_id = 3;
+				error = wacom_set_report(intf,
+							 WAC_HID_FEATURE_REPORT,
+							 report_id,
+							 rep_data, 4, 1);
+				if (error >= 0)
+					error = wacom_get_report(intf,
+							WAC_HID_FEATURE_REPORT,
+							report_id,
+							rep_data, 4, 1);
+			} while ((error < 0 || rep_data[1] != 4) &&
+				 limit++ < WAC_MSG_RETRIES);
+		}
 	} else if (features->type != TABLETPC &&
 		   features->type != WIRELESS &&
 		   features->device_type == BTN_TOOL_PEN) {
@@ -450,7 +484,7 @@ static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_feat
 }
 
 static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
-		struct wacom_features *features)
+					 struct wacom_features *features)
 {
 	int error = 0;
 	struct usb_host_interface *interface = intf->cur_altsetting;
@@ -478,16 +512,21 @@ static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
 		}
 	}
 
-	/* only Tablet PCs and Bamboo P&T need to retrieve the info */
-	if ((features->type != TABLETPC) && (features->type != TABLETPC2FG) &&
-	    (features->type != BAMBOO_PT))
+	/* only devices that support touch need to retrieve the info */
+	if (features->type != TABLETPC &&
+	    features->type != TABLETPC2FG &&
+	    features->type != BAMBOO_PT &&
+	    features->type != MTSCREEN) {
 		goto out;
+	}
 
-	if (usb_get_extra_descriptor(interface, HID_DEVICET_HID, &hid_desc)) {
-		if (usb_get_extra_descriptor(&interface->endpoint[0],
-				HID_DEVICET_REPORT, &hid_desc)) {
-			printk("wacom: can not retrieve extra class descriptor\n");
-			error = 1;
+	error = usb_get_extra_descriptor(interface, HID_DEVICET_HID, &hid_desc);
+	if (error) {
+		error = usb_get_extra_descriptor(&interface->endpoint[0],
+						 HID_DEVICET_REPORT, &hid_desc);
+		if (error) {
+			dev_err(&intf->dev,
+				"can not retrieve extra class descriptor\n");
 			goto out;
 		}
 	}
@@ -577,23 +616,39 @@ static void wacom_remove_shared_data(struct wacom_wac *wacom)
 static int wacom_led_control(struct wacom *wacom)
 {
 	unsigned char *buf;
-	int retval, led = 0;
+	int retval;
 
 	buf = kzalloc(9, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
-	if (wacom->wacom_wac.features.type == WACOM_21UX2 ||
-	    wacom->wacom_wac.features.type == WACOM_24HD)
-		led = (wacom->led.select[1] << 4) | 0x40;
-
-	led |=  wacom->led.select[0] | 0x4;
-
-	buf[0] = WAC_CMD_LED_CONTROL;
-	buf[1] = led;
-	buf[2] = wacom->led.llv;
-	buf[3] = wacom->led.hlv;
-	buf[4] = wacom->led.img_lum;
+	if (wacom->wacom_wac.features.type >= INTUOS5S &&
+	    wacom->wacom_wac.features.type <= INTUOS5L)	{
+		/*
+		 * Touch Ring and crop mark LED luminance may take on
+		 * one of four values:
+		 *    0 = Low; 1 = Medium; 2 = High; 3 = Off
+		 */
+		int ring_led = wacom->led.select[0] & 0x03;
+		int ring_lum = (((wacom->led.llv & 0x60) >> 5) - 1) & 0x03;
+		int crop_lum = 0;
+
+		buf[0] = WAC_CMD_LED_CONTROL;
+		buf[1] = (crop_lum << 4) | (ring_lum << 2) | (ring_led);
+	}
+	else {
+		int led = wacom->led.select[0] | 0x4;
+
+		if (wacom->wacom_wac.features.type == WACOM_21UX2 ||
+		    wacom->wacom_wac.features.type == WACOM_24HD)
+			led |= (wacom->led.select[1] << 4) | 0x40;
+
+		buf[0] = WAC_CMD_LED_CONTROL;
+		buf[1] = led;
+		buf[2] = wacom->led.llv;
+		buf[3] = wacom->led.hlv;
+		buf[4] = wacom->led.img_lum;
+	}
 
 	retval = wacom_set_report(wacom->intf, 0x03, WAC_CMD_LED_CONTROL,
 				  buf, 9, WAC_CMD_RETRIES);
@@ -786,6 +841,17 @@ static struct attribute_group intuos4_led_attr_group = {
 	.attrs = intuos4_led_attrs,
 };
 
+static struct attribute *intuos5_led_attrs[] = {
+	&dev_attr_status0_luminance.attr,
+	&dev_attr_status_led0_select.attr,
+	NULL
+};
+
+static struct attribute_group intuos5_led_attr_group = {
+	.name = "wacom_led",
+	.attrs = intuos5_led_attrs,
+};
+
 static int wacom_initialize_leds(struct wacom *wacom)
 {
 	int error;
@@ -815,6 +881,19 @@ static int wacom_initialize_leds(struct wacom *wacom)
 					   &cintiq_led_attr_group);
 		break;
 
+	case INTUOS5S:
+	case INTUOS5:
+	case INTUOS5L:
+		wacom->led.select[0] = 0;
+		wacom->led.select[1] = 0;
+		wacom->led.llv = 32;
+		wacom->led.hlv = 0;
+		wacom->led.img_lum = 0;
+
+		error = sysfs_create_group(&wacom->intf->dev.kobj,
+					   &intuos5_led_attr_group);
+		break;
+
 	default:
 		return 0;
 	}
@@ -843,6 +922,13 @@ static void wacom_destroy_leds(struct wacom *wacom)
 		sysfs_remove_group(&wacom->intf->dev.kobj,
 				   &cintiq_led_attr_group);
 		break;
+
+	case INTUOS5S:
+	case INTUOS5:
+	case INTUOS5L:
+		sysfs_remove_group(&wacom->intf->dev.kobj,
+				   &intuos5_led_attr_group);
+		break;
 	}
 }
 
@@ -904,8 +990,10 @@ static int wacom_register_input(struct wacom *wacom)
 	int error;
 
 	input_dev = input_allocate_device();
-	if (!input_dev)
-		return -ENOMEM;
+	if (!input_dev) {
+		error = -ENOMEM;
+		goto fail1;
+	}
 
 	input_dev->name = wacom_wac->name;
 	input_dev->dev.parent = &intf->dev;
@@ -915,14 +1003,20 @@ static int wacom_register_input(struct wacom *wacom)
 	input_set_drvdata(input_dev, wacom);
 
 	wacom_wac->input = input_dev;
-	wacom_setup_input_capabilities(input_dev, wacom_wac);
+	error = wacom_setup_input_capabilities(input_dev, wacom_wac);
+	if (error)
+		goto fail1;
 
 	error = input_register_device(input_dev);
-	if (error) {
-		input_free_device(input_dev);
-		wacom_wac->input = NULL;
-	}
+	if (error)
+		goto fail2;
 
+	return 0;
+
+fail2:
+	input_free_device(input_dev);
+	wacom_wac->input = NULL;
+fail1:
 	return error;
 }
 
@@ -941,22 +1035,22 @@ static void wacom_wireless_work(struct work_struct *work)
 	wacom = usb_get_intfdata(usbdev->config->interface[1]);
 	if (wacom->wacom_wac.input)
 		input_unregister_device(wacom->wacom_wac.input);
-	wacom->wacom_wac.input = 0;
+	wacom->wacom_wac.input = NULL;
 
 	/* Touch interface */
 	wacom = usb_get_intfdata(usbdev->config->interface[2]);
 	if (wacom->wacom_wac.input)
 		input_unregister_device(wacom->wacom_wac.input);
-	wacom->wacom_wac.input = 0;
+	wacom->wacom_wac.input = NULL;
 
 	if (wacom_wac->pid == 0) {
-		printk(KERN_INFO "wacom: wireless tablet disconnected\n");
+		dev_info(&wacom->intf->dev, "wireless tablet disconnected\n");
 	} else {
 		const struct usb_device_id *id = wacom_ids;
 
-		printk(KERN_INFO
-		       "wacom: wireless tablet connected with PID %x\n",
-		       wacom_wac->pid);
+		dev_info(&wacom->intf->dev,
+			 "wireless tablet connected with PID %x\n",
+			 wacom_wac->pid);
 
 		while (id->match_flags) {
 			if (id->idVendor == USB_VENDOR_ID_WACOM &&
@@ -966,8 +1060,8 @@ static void wacom_wireless_work(struct work_struct *work)
 		}
 
 		if (!id->match_flags) {
-			printk(KERN_INFO
-			       "wacom: ignorning unknown PID.\n");
+			dev_info(&wacom->intf->dev,
+				 "ignoring unknown PID.\n");
 			return;
 		}
 
@@ -1038,11 +1132,33 @@ static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *i
 
 	endpoint = &intf->cur_altsetting->endpoint[0].desc;
 
-	/* Retrieve the physical and logical size for OEM devices */
+	/* Retrieve the physical and logical size for touch devices */
 	error = wacom_retrieve_hid_descriptor(intf, features);
 	if (error)
 		goto fail3;
 
+	/*
+	 * Intuos5 has no useful data about its touch interface in its
+	 * HID descriptor. If this is the touch interface (wMaxPacketSize
+	 * of WACOM_PKGLEN_BBTOUCH3), override the table values.
+	 */
+	if (features->type >= INTUOS5S && features->type <= INTUOS5L) {
+		if (endpoint->wMaxPacketSize == WACOM_PKGLEN_BBTOUCH3) {
+			features->device_type = BTN_TOOL_FINGER;
+			features->pktlen = WACOM_PKGLEN_BBTOUCH3;
+
+			features->x_phy =
+				(features->x_max * 100) / features->x_resolution;
+			features->y_phy =
+				(features->y_max * 100) / features->y_resolution;
+
+			features->x_max = 4096;
+			features->y_max = 4096;
+		} else {
+			features->device_type = BTN_TOOL_PEN;
+		}
+	}
+
 	wacom_setup_device_quirks(features);
 
 	strlcpy(wacom_wac->name, features->name, sizeof(wacom_wac->name));
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index b327790e9a0c..004bc1bb1544 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -61,7 +61,8 @@ static int wacom_penpartner_irq(struct wacom_wac *wacom)
 		break;
 
 	default:
-		printk(KERN_INFO "wacom_penpartner_irq: received unknown report #%d\n", data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		return 0;
         }
 
@@ -76,8 +77,8 @@ static int wacom_pl_irq(struct wacom_wac *wacom)
 	int prox, pressure;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
-		dev_dbg(&input->dev,
-			"wacom_pl_irq: received unknown report #%d\n", data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		return 0;
 	}
 
@@ -147,7 +148,8 @@ static int wacom_ptu_irq(struct wacom_wac *wacom)
 	struct input_dev *input = wacom->input;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
-		printk(KERN_INFO "wacom_ptu_irq: received unknown report #%d\n", data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		return 0;
 	}
 
@@ -176,7 +178,8 @@ static int wacom_dtu_irq(struct wacom_wac *wacom)
 	struct input_dev *input = wacom->input;
 	int prox = data[1] & 0x20, pressure;
 
-	dev_dbg(&input->dev, "wacom_dtu_irq: received report #%d\n", data[0]);
+	dev_dbg(input->dev.parent,
+		"%s: received report #%d", __func__, data[0]);
 
 	if (prox) {
 		/* Going into proximity select tool */
@@ -212,9 +215,8 @@ static int wacom_graphire_irq(struct wacom_wac *wacom)
 	int retval = 0;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
-		dev_dbg(&input->dev,
-			"wacom_graphire_irq: received unknown report #%d\n",
-			data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		goto exit;
 	}
 
@@ -324,6 +326,9 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
 	/* Enter report */
 	if ((data[1] & 0xfc) == 0xc0) {
+		if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+			wacom->shared->stylus_in_proximity = true;
+
 		/* serial number of the tool */
 		wacom->serial[idx] = ((data[3] & 0x0f) << 28) +
 			(data[4] << 20) + (data[5] << 12) +
@@ -409,6 +414,9 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
 	/* Exit report */
 	if ((data[1] & 0xfe) == 0x80) {
+		if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+			wacom->shared->stylus_in_proximity = false;
+
 		/*
 		 * Reset all states otherwise we lose the initial states
 		 * when in-prox next time
@@ -455,6 +463,7 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 	if ((data[1] & 0xb8) == 0xa0) {
 		t = (data[6] << 2) | ((data[7] >> 6) & 3);
 		if ((features->type >= INTUOS4S && features->type <= INTUOS4L) ||
+                    (features->type >= INTUOS5S && features->type <= INTUOS5L) ||
 		    features->type == WACOM_21UX2 || features->type == WACOM_24HD) {
 			t = (t << 1) | (data[1] & 1);
 		}
@@ -485,11 +494,13 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 	unsigned int t;
 	int idx = 0, result;
 
-	if (data[0] != WACOM_REPORT_PENABLED && data[0] != WACOM_REPORT_INTUOSREAD
-		&& data[0] != WACOM_REPORT_INTUOSWRITE && data[0] != WACOM_REPORT_INTUOSPAD) {
-		dev_dbg(&input->dev,
-			"wacom_intuos_irq: received unknown report #%d\n",
-			data[0]);
+	if (data[0] != WACOM_REPORT_PENABLED &&
+	    data[0] != WACOM_REPORT_INTUOSREAD &&
+	    data[0] != WACOM_REPORT_INTUOSWRITE &&
+	    data[0] != WACOM_REPORT_INTUOSPAD &&
+	    data[0] != WACOM_REPORT_INTUOS5PAD) {
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
                 return 0;
 	}
 
@@ -498,7 +509,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 		idx = data[1] & 0x01;
 
 	/* pad packets. Works as a second tool and is always in prox */
-	if (data[0] == WACOM_REPORT_INTUOSPAD) {
+	if (data[0] == WACOM_REPORT_INTUOSPAD || data[0] == WACOM_REPORT_INTUOS5PAD) {
 		if (features->type >= INTUOS4S && features->type <= INTUOS4L) {
 			input_report_key(input, BTN_0, (data[2] & 0x01));
 			input_report_key(input, BTN_1, (data[3] & 0x01));
@@ -574,6 +585,34 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				input_report_key(input, wacom->tool[1], 0);
 				input_report_abs(input, ABS_MISC, 0);
 			}
+		} else if (features->type >= INTUOS5S && features->type <= INTUOS5L) {
+			int i;
+
+			/* Touch ring mode switch has no capacitive sensor */
+			input_report_key(input, BTN_0, (data[3] & 0x01));
+
+			/*
+			 * ExpressKeys on Intuos5 have a capacitive sensor in
+			 * addition to the mechanical switch. Switch data is
+			 * stored in data[4], capacitive data in data[5].
+			 */
+			for (i = 0; i < 8; i++)
+				input_report_key(input, BTN_1 + i, data[4] & (1 << i));
+
+			if (data[2] & 0x80) {
+				input_report_abs(input, ABS_WHEEL, (data[2] & 0x7f));
+			} else {
+				/* Out of proximity, clear wheel value. */
+				input_report_abs(input, ABS_WHEEL, 0);
+			}
+
+			if (data[2] | (data[3] & 0x01) | data[4]) {
+				input_report_key(input, wacom->tool[1], 1);
+				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
+			} else {
+				input_report_key(input, wacom->tool[1], 0);
+				input_report_abs(input, ABS_MISC, 0);
+			}
 		} else {
 			if (features->type == WACOM_21UX2) {
 				input_report_key(input, BTN_0, (data[5] & 0x01));
@@ -637,7 +676,9 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 	    (features->type == INTUOS3 ||
 	     features->type == INTUOS3S ||
 	     features->type == INTUOS4 ||
-	     features->type == INTUOS4S)) {
+	     features->type == INTUOS4S ||
+	     features->type == INTUOS5 ||
+	     features->type == INTUOS5S)) {
 
 		return 0;
 	}
@@ -690,7 +731,8 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 
 		} else if (wacom->tool[idx] == BTN_TOOL_MOUSE) {
 			/* I4 mouse */
-			if (features->type >= INTUOS4S && features->type <= INTUOS4L) {
+			if ((features->type >= INTUOS4S && features->type <= INTUOS4L) ||
+			    (features->type >= INTUOS5S && features->type <= INTUOS5L)) {
 				input_report_key(input, BTN_LEFT,   data[6] & 0x01);
 				input_report_key(input, BTN_MIDDLE, data[6] & 0x02);
 				input_report_key(input, BTN_RIGHT,  data[6] & 0x04);
@@ -717,7 +759,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				}
 			}
 		} else if ((features->type < INTUOS3S || features->type == INTUOS3L ||
-				features->type == INTUOS4L) &&
+				features->type == INTUOS4L || features->type == INTUOS5L) &&
 			   wacom->tool[idx] == BTN_TOOL_LENS) {
 			/* Lens cursor packets */
 			input_report_key(input, BTN_LEFT,   data[8] & 0x01);
@@ -734,6 +776,72 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 	return 1;
 }
 
+static int find_slot_from_contactid(struct wacom_wac *wacom, int contactid)
+{
+	int touch_max = wacom->features.touch_max;
+	int i;
+
+	if (!wacom->slots)
+		return -1;
+
+	for (i = 0; i < touch_max; ++i) {
+		if (wacom->slots[i] == contactid)
+			return i;
+	}
+	for (i = 0; i < touch_max; ++i) {
+		if (wacom->slots[i] == -1)
+			return i;
+	}
+	return -1;
+}
+
+static int wacom_mt_touch(struct wacom_wac *wacom)
+{
+	struct input_dev *input = wacom->input;
+	char *data = wacom->data;
+	int i;
+	int current_num_contacts = data[2];
+	int contacts_to_send = 0;
+
+	/*
+	 * First packet resets the counter since only the first
+	 * packet in series will have non-zero current_num_contacts.
+	 */
+	if (current_num_contacts)
+		wacom->num_contacts_left = current_num_contacts;
+
+	/* There are at most 5 contacts per packet */
+	contacts_to_send = min(5, wacom->num_contacts_left);
+
+	for (i = 0; i < contacts_to_send; i++) {
+		int offset = (WACOM_BYTES_PER_MT_PACKET * i) + 3;
+		bool touch = data[offset] & 0x1;
+		int id = le16_to_cpup((__le16 *)&data[offset + 1]);
+		int slot = find_slot_from_contactid(wacom, id);
+
+		if (slot < 0)
+			continue;
+
+		input_mt_slot(input, slot);
+		input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
+		if (touch) {
+			int x = le16_to_cpup((__le16 *)&data[offset + 7]);
+			int y = le16_to_cpup((__le16 *)&data[offset + 9]);
+			input_report_abs(input, ABS_MT_POSITION_X, x);
+			input_report_abs(input, ABS_MT_POSITION_Y, y);
+		}
+		wacom->slots[slot] = touch ? id : -1;
+	}
+
+	input_mt_report_pointer_emulation(input, true);
+
+	wacom->num_contacts_left -= contacts_to_send;
+	if (wacom->num_contacts_left < 0)
+		wacom->num_contacts_left = 0;
+
+	return 1;
+}
+
 static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 {
 	struct input_dev *input = wacom->input;
@@ -772,6 +880,9 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 	bool prox;
 	int x = 0, y = 0;
 
+	if (wacom->features.touch_max > 1 || len > WACOM_PKGLEN_TPC2FG)
+		return 0;
+
 	if (!wacom->shared->stylus_in_proximity) {
 		if (len == WACOM_PKGLEN_TPC1FG) {
 			prox = data[0] & 0x01;
@@ -835,15 +946,15 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 {
 	char *data = wacom->data;
 
-	dev_dbg(&wacom->input->dev, "wacom_tpc_irq: received report #%d\n",
-		data[0]);
+	dev_dbg(wacom->input->dev.parent,
+		"%s: received report #%d\n", __func__, data[0]);
 
 	switch (len) {
 	case WACOM_PKGLEN_TPC1FG:
-		 return wacom_tpc_single_touch(wacom, len);
+		return wacom_tpc_single_touch(wacom, len);
 
 	case WACOM_PKGLEN_TPC2FG:
- 		return wacom_tpc_mt_touch(wacom);
+		return wacom_tpc_mt_touch(wacom);
 
 	default:
 		switch (data[0]) {
@@ -852,6 +963,9 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 		case WACOM_REPORT_TPCST:
 			return wacom_tpc_single_touch(wacom, len);
 
+		case WACOM_REPORT_TPCMT:
+			return wacom_mt_touch(wacom);
+
 		case WACOM_REPORT_PENABLED:
 			return wacom_tpc_pen(wacom);
 		}
@@ -1120,8 +1234,18 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 		sync = wacom_intuos_irq(wacom_wac);
 		break;
 
+	case INTUOS5S:
+	case INTUOS5:
+	case INTUOS5L:
+		if (len == WACOM_PKGLEN_BBTOUCH3)
+			sync = wacom_bpt3_touch(wacom_wac);
+		else
+			sync = wacom_intuos_irq(wacom_wac);
+		break;
+
 	case TABLETPC:
 	case TABLETPC2FG:
+	case MTSCREEN:
 		sync = wacom_tpc_irq(wacom_wac, len);
 		break;
 
@@ -1194,7 +1318,9 @@ void wacom_setup_device_quirks(struct wacom_features *features)
 
 	/* these device have multiple inputs */
 	if (features->type == TABLETPC || features->type == TABLETPC2FG ||
-	    features->type == BAMBOO_PT || features->type == WIRELESS)
+	    features->type == BAMBOO_PT || features->type == WIRELESS ||
+	    (features->type >= INTUOS5S && features->type <= INTUOS5L) ||
+	    features->type == MTSCREEN)
 		features->quirks |= WACOM_QUIRK_MULTI_INPUT;
 
 	/* quirk for bamboo touch with 2 low res touches */
@@ -1225,8 +1351,8 @@ static unsigned int wacom_calculate_touch_res(unsigned int logical_max,
        return (logical_max * 100) / physical_max;
 }
 
-void wacom_setup_input_capabilities(struct input_dev *input_dev,
-				    struct wacom_wac *wacom_wac)
+int wacom_setup_input_capabilities(struct input_dev *input_dev,
+				   struct wacom_wac *wacom_wac)
 {
 	struct wacom_features *features = &wacom_wac->features;
 	int i;
@@ -1361,6 +1487,50 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 		wacom_setup_intuos(wacom_wac);
 		break;
 
+	case INTUOS5:
+	case INTUOS5L:
+		if (features->device_type == BTN_TOOL_PEN) {
+			__set_bit(BTN_7, input_dev->keybit);
+			__set_bit(BTN_8, input_dev->keybit);
+		}
+		/* fall through */
+
+	case INTUOS5S:
+		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+
+		if (features->device_type == BTN_TOOL_PEN) {
+			for (i = 0; i < 7; i++)
+				__set_bit(BTN_0 + i, input_dev->keybit);
+
+			input_set_abs_params(input_dev, ABS_DISTANCE, 0,
+					      features->distance_max,
+					      0, 0);
+
+			input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+
+			wacom_setup_intuos(wacom_wac);
+		} else if (features->device_type == BTN_TOOL_FINGER) {
+			__clear_bit(ABS_MISC, input_dev->absbit);
+
+			__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
+			__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
+			__set_bit(BTN_TOOL_TRIPLETAP, input_dev->keybit);
+			__set_bit(BTN_TOOL_QUADTAP, input_dev->keybit);
+
+			input_mt_init_slots(input_dev, features->touch_max);
+
+			input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
+			                     0, 255, 0, 0);
+
+			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
+					     0, features->x_max,
+					     features->x_fuzz, 0);
+			input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
+					     0, features->y_max,
+					     features->y_fuzz, 0);
+		}
+		break;
+
 	case INTUOS4:
 	case INTUOS4L:
 		__set_bit(BTN_7, input_dev->keybit);
@@ -1378,9 +1548,19 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 		break;
 
 	case TABLETPC2FG:
+	case MTSCREEN:
 		if (features->device_type == BTN_TOOL_FINGER) {
 
-			input_mt_init_slots(input_dev, 2);
+			wacom_wac->slots = kmalloc(features->touch_max *
+							sizeof(int),
+						   GFP_KERNEL);
+			if (!wacom_wac->slots)
+				return -ENOMEM;
+
+			for (i = 0; i < features->touch_max; i++)
+				wacom_wac->slots[i] = -1;
+
+			input_mt_init_slots(input_dev, features->touch_max);
 			input_set_abs_params(input_dev, ABS_MT_TOOL_TYPE,
 					0, MT_TOOL_MAX, 0, 0);
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
@@ -1435,6 +1615,7 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 			__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
 			__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
+			input_mt_init_slots(input_dev, features->touch_max);
 
 			if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
 				__set_bit(BTN_TOOL_TRIPLETAP,
@@ -1442,13 +1623,9 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 				__set_bit(BTN_TOOL_QUADTAP,
 					  input_dev->keybit);
 
-				input_mt_init_slots(input_dev, 16);
-
 				input_set_abs_params(input_dev,
 						     ABS_MT_TOUCH_MAJOR,
 						     0, 255, 0, 0);
-			} else {
-				input_mt_init_slots(input_dev, 2);
 			}
 
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
@@ -1468,6 +1645,7 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 		}
 		break;
 	}
+	return 0;
 }
 
 static const struct wacom_features wacom_features_0x00 =
@@ -1635,6 +1813,24 @@ static const struct wacom_features wacom_features_0xBB =
 static const struct wacom_features wacom_features_0xBC =
 	{ "Wacom Intuos4 WL",     WACOM_PKGLEN_INTUOS,    40840, 25400, 2047,
 	  63, INTUOS4, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+static const struct wacom_features wacom_features_0x26 =
+	{ "Wacom Intuos5 touch S", WACOM_PKGLEN_INTUOS,  31496, 19685, 2047,
+	  63, INTUOS5S, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  .touch_max = 16 };
+static const struct wacom_features wacom_features_0x27 =
+	{ "Wacom Intuos5 touch M", WACOM_PKGLEN_INTUOS,  44704, 27940, 2047,
+	  63, INTUOS5, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  .touch_max = 16 };
+static const struct wacom_features wacom_features_0x28 =
+	{ "Wacom Intuos5 touch L", WACOM_PKGLEN_INTUOS, 65024, 40640, 2047,
+	  63, INTUOS5L, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  .touch_max = 16 };
+static const struct wacom_features wacom_features_0x29 =
+	{ "Wacom Intuos5 S", WACOM_PKGLEN_INTUOS,  31496, 19685, 2047,
+	  63, INTUOS5S, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+static const struct wacom_features wacom_features_0x2A =
+	{ "Wacom Intuos5 M", WACOM_PKGLEN_INTUOS,  44704, 27940, 2047,
+	  63, INTUOS5, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0xF4 =
 	{ "Wacom Cintiq 24HD",    WACOM_PKGLEN_INTUOS,   104480, 65600, 2047,
 	  63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
@@ -1676,13 +1872,19 @@ static const struct wacom_features wacom_features_0x9F =
 	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xE2 =
 	{ "Wacom ISDv4 E2",       WACOM_PKGLEN_TPC2FG,    26202, 16325,  255,
-	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xE3 =
 	{ "Wacom ISDv4 E3",       WACOM_PKGLEN_TPC2FG,    26202, 16325,  255,
-	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
+static const struct wacom_features wacom_features_0xE5 =
+	{ "Wacom ISDv4 E5",       WACOM_PKGLEN_MTOUCH,    26202, 16325,  255,
+	  0, MTSCREEN, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xE6 =
 	{ "Wacom ISDv4 E6",       WACOM_PKGLEN_TPC2FG,    27760, 15694,  255,
-	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	.touch_max = 2 };
 static const struct wacom_features wacom_features_0xEC =
 	{ "Wacom ISDv4 EC",       WACOM_PKGLEN_GRAPHIRE,  25710, 14500,  255,
 	  0, TABLETPC,    WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1691,19 +1893,22 @@ static const struct wacom_features wacom_features_0x47 =
 	  31, INTUOS, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x84 =
 	{ "Wacom Wireless Receiver", WACOM_PKGLEN_WIRELESS, 0, 0, 0,
-	  0, WIRELESS, 0, 0 };
+	  0, WIRELESS, 0, 0, .touch_max = 16 };
 static const struct wacom_features wacom_features_0xD0 =
 	{ "Wacom Bamboo 2FG",     WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD1 =
 	{ "Wacom Bamboo 2FG 4x5", WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD2 =
 	{ "Wacom Bamboo Craft",   WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
 	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xD3 =
 	{ "Wacom Bamboo 2FG 6x8", WACOM_PKGLEN_BBFUN,     21648, 13700, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD4 =
 	{ "Wacom Bamboo Pen",     WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
 	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1712,28 +1917,35 @@ static const struct wacom_features wacom_features_0xD5 =
 	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xD6 =
 	{ "Wacom BambooPT 2FG 4x5", WACOM_PKGLEN_BBFUN,   14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD7 =
 	{ "Wacom BambooPT 2FG Small", WACOM_PKGLEN_BBFUN, 14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD8 =
 	{ "Wacom Bamboo Comic 2FG", WACOM_PKGLEN_BBFUN,   21648, 13700, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xDA =
 	{ "Wacom Bamboo 2FG 4x5 SE", WACOM_PKGLEN_BBFUN,  14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static struct wacom_features wacom_features_0xDB =
 	{ "Wacom Bamboo 2FG 6x8 SE", WACOM_PKGLEN_BBFUN,  21648, 13700, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xDD =
         { "Wacom Bamboo Connect", WACOM_PKGLEN_BBPEN,     14720,  9200, 1023,
           31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xDE =
         { "Wacom Bamboo 16FG 4x5", WACOM_PKGLEN_BBPEN,    14720,  9200, 1023,
-          31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 16 };
 static const struct wacom_features wacom_features_0xDF =
         { "Wacom Bamboo 16FG 6x8", WACOM_PKGLEN_BBPEN,    21648, 13700, 1023,
-          31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 16 };
 static const struct wacom_features wacom_features_0x6004 =
 	{ "ISD-V4",               WACOM_PKGLEN_GRAPHIRE,  12800,  8000,  255,
 	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1807,6 +2019,11 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0xBA) },
 	{ USB_DEVICE_WACOM(0xBB) },
 	{ USB_DEVICE_WACOM(0xBC) },
+	{ USB_DEVICE_WACOM(0x26) },
+	{ USB_DEVICE_WACOM(0x27) },
+	{ USB_DEVICE_WACOM(0x28) },
+	{ USB_DEVICE_WACOM(0x29) },
+	{ USB_DEVICE_WACOM(0x2A) },
 	{ USB_DEVICE_WACOM(0x3F) },
 	{ USB_DEVICE_WACOM(0xC5) },
 	{ USB_DEVICE_WACOM(0xC6) },
@@ -1842,6 +2059,7 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x9F) },
 	{ USB_DEVICE_WACOM(0xE2) },
 	{ USB_DEVICE_WACOM(0xE3) },
+	{ USB_DEVICE_WACOM(0xE5) },
 	{ USB_DEVICE_WACOM(0xE6) },
 	{ USB_DEVICE_WACOM(0xEC) },
 	{ USB_DEVICE_WACOM(0x47) },
diff --git a/drivers/input/tablet/wacom_wac.h b/drivers/input/tablet/wacom_wac.h
index ba5a334e54d6..78fbd3f42009 100644
--- a/drivers/input/tablet/wacom_wac.h
+++ b/drivers/input/tablet/wacom_wac.h
@@ -25,6 +25,10 @@
 #define WACOM_PKGLEN_BBTOUCH3	64
 #define WACOM_PKGLEN_BBPEN	10
 #define WACOM_PKGLEN_WIRELESS	32
+#define WACOM_PKGLEN_MTOUCH	62
+
+/* wacom data size per MT contact */
+#define WACOM_BYTES_PER_MT_PACKET	11
 
 /* device IDs */
 #define STYLUS_DEVICE_ID	0x02
@@ -38,8 +42,10 @@
 #define WACOM_REPORT_INTUOSREAD		5
 #define WACOM_REPORT_INTUOSWRITE	6
 #define WACOM_REPORT_INTUOSPAD		12
+#define WACOM_REPORT_INTUOS5PAD		3
 #define WACOM_REPORT_TPC1FG		6
 #define WACOM_REPORT_TPC2FG		13
+#define WACOM_REPORT_TPCMT		13
 #define WACOM_REPORT_TPCHID		15
 #define WACOM_REPORT_TPCST		16
 
@@ -65,6 +71,9 @@ enum {
 	INTUOS4S,
 	INTUOS4,
 	INTUOS4L,
+	INTUOS5S,
+	INTUOS5,
+	INTUOS5L,
 	WACOM_24HD,
 	WACOM_21UX2,
 	CINTIQ,
@@ -72,6 +81,7 @@ enum {
 	WACOM_MO,
 	TABLETPC,
 	TABLETPC2FG,
+	MTSCREEN,
 	MAX_TYPE
 };
 
@@ -95,6 +105,7 @@ struct wacom_features {
 	int pressure_fuzz;
 	int distance_fuzz;
 	unsigned quirks;
+	unsigned touch_max;
 };
 
 struct wacom_shared {
@@ -113,6 +124,8 @@ struct wacom_wac {
 	struct input_dev *input;
 	int pid;
 	int battery_capacity;
+	int num_contacts_left;
+	int *slots;
 };
 
 #endif
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 75838d7710ce..98d263504eea 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -187,6 +187,23 @@ config TOUCHSCREEN_DA9034
 	  Say Y here to enable the support for the touchscreen found
 	  on Dialog Semiconductor DA9034 PMIC.
 
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called da9034-ts.
+
+config TOUCHSCREEN_DA9052
+	tristate "Dialog DA9052/DA9053 TSI"
+	depends on PMIC_DA9052
+	help
+	  Say Y here to support the touchscreen found on Dialog Semiconductor
+	  DA9052-BC and DA9053-AA/Bx PMICs.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called da9052_tsi.
+
 config TOUCHSCREEN_DYNAPRO
 	tristate "Dynapro serial touchscreen"
 	select SERIO
@@ -306,6 +323,18 @@ config TOUCHSCREEN_WACOM_W8001
 	  To compile this driver as a module, choose M here: the
 	  module will be called wacom_w8001.
 
+config TOUCHSCREEN_WACOM_I2C
+	tristate "Wacom Tablet support (I2C)"
+	depends on I2C
+	help
+	  Say Y here if you want to use the I2C version of the Wacom
+	  Pen Tablet.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called wacom_i2c.
+
 config TOUCHSCREEN_LPC32XX
 	tristate "LPC32XX touchscreen controller"
 	depends on ARCH_LPC32XX
@@ -635,6 +664,7 @@ config TOUCHSCREEN_USB_COMPOSITE
 	  - Zytronic controllers
 	  - Elo TouchSystems 2700 IntelliTouch
 	  - EasyTouch USB Touch Controller from Data Modul
+	  - e2i (Mimo monitors)
 
 	  Have a look at <http://linux.chapter7.ch/touchkit/> for
 	  a usage description and the required user-space stuff.
@@ -721,7 +751,7 @@ config TOUCHSCREEN_USB_ELO
 
 config TOUCHSCREEN_USB_E2I
 	default y
-	bool "e2i Touchscreen controller (e.g. from Mimo 740)"
+	bool "e2i Touchscreen controller (e.g. from Mimo 740)" if EXPERT
 	depends on TOUCHSCREEN_USB_COMPOSITE
 
 config TOUCHSCREEN_USB_ZYTRONIC
@@ -744,7 +774,7 @@ config TOUCHSCREEN_USB_EASYTOUCH
 	bool "EasyTouch USB Touch controller device support" if EMBEDDED
 	depends on TOUCHSCREEN_USB_COMPOSITE
 	help
-	  Say Y here if you have a EasyTouch USB Touch controller device support.
+	  Say Y here if you have an EasyTouch USB Touch controller.
 	  If unsure, say N.
 
 config TOUCHSCREEN_TOUCHIT213
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 3d5cf8cbf89c..eb8bfe1c1a46 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -22,6 +22,7 @@ obj-$(CONFIG_TOUCHSCREEN_CYTTSP_CORE)	+= cyttsp_core.o
 obj-$(CONFIG_TOUCHSCREEN_CYTTSP_I2C)	+= cyttsp_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_CYTTSP_SPI)	+= cyttsp_spi.o
 obj-$(CONFIG_TOUCHSCREEN_DA9034)	+= da9034-ts.o
+obj-$(CONFIG_TOUCHSCREEN_DA9052)	+= da9052_tsi.o
 obj-$(CONFIG_TOUCHSCREEN_DYNAPRO)	+= dynapro.o
 obj-$(CONFIG_TOUCHSCREEN_HAMPSHIRE)	+= hampshire.o
 obj-$(CONFIG_TOUCHSCREEN_GUNZE)		+= gunze.o
@@ -59,6 +60,7 @@ obj-$(CONFIG_TOUCHSCREEN_TSC2005)	+= tsc2005.o
 obj-$(CONFIG_TOUCHSCREEN_TSC2007)	+= tsc2007.o
 obj-$(CONFIG_TOUCHSCREEN_UCB1400)	+= ucb1400_ts.o
 obj-$(CONFIG_TOUCHSCREEN_WACOM_W8001)	+= wacom_w8001.o
+obj-$(CONFIG_TOUCHSCREEN_WACOM_I2C)	+= wacom_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_WM831X)	+= wm831x-ts.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX)	+= wm97xx-ts.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9705)	+= wm9705.o
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 19d4ea65ea01..42e645062c20 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -236,7 +236,6 @@ struct mxt_object {
 struct mxt_message {
 	u8 reportid;
 	u8 message[7];
-	u8 checksum;
 };
 
 struct mxt_finger {
@@ -326,17 +325,12 @@ static bool mxt_object_writable(unsigned int type)
 }
 
 static void mxt_dump_message(struct device *dev,
-				  struct mxt_message *message)
+			     struct mxt_message *message)
 {
-	dev_dbg(dev, "reportid:\t0x%x\n", message->reportid);
-	dev_dbg(dev, "message1:\t0x%x\n", message->message[0]);
-	dev_dbg(dev, "message2:\t0x%x\n", message->message[1]);
-	dev_dbg(dev, "message3:\t0x%x\n", message->message[2]);
-	dev_dbg(dev, "message4:\t0x%x\n", message->message[3]);
-	dev_dbg(dev, "message5:\t0x%x\n", message->message[4]);
-	dev_dbg(dev, "message6:\t0x%x\n", message->message[5]);
-	dev_dbg(dev, "message7:\t0x%x\n", message->message[6]);
-	dev_dbg(dev, "checksum:\t0x%x\n", message->checksum);
+	dev_dbg(dev, "reportid: %u\tmessage: %02x %02x %02x %02x %02x %02x %02x\n",
+		message->reportid, message->message[0], message->message[1],
+		message->message[2], message->message[3], message->message[4],
+		message->message[5], message->message[6]);
 }
 
 static int mxt_check_bootloader(struct i2c_client *client,
@@ -506,7 +500,7 @@ static int mxt_write_object(struct mxt_data *data,
 	u16 reg;
 
 	object = mxt_get_object(data, type);
-	if (!object)
+	if (!object || offset >= object->size + 1)
 		return -EINVAL;
 
 	reg = object->start_address;
@@ -1049,8 +1043,8 @@ static ssize_t mxt_update_fw_store(struct device *dev,
 	return count;
 }
 
-static DEVICE_ATTR(object, 0444, mxt_object_show, NULL);
-static DEVICE_ATTR(update_fw, 0664, NULL, mxt_update_fw_store);
+static DEVICE_ATTR(object, S_IRUGO, mxt_object_show, NULL);
+static DEVICE_ATTR(update_fw, S_IWUSR, NULL, mxt_update_fw_store);
 
 static struct attribute *mxt_attrs[] = {
 	&dev_attr_object.attr,
@@ -1201,7 +1195,7 @@ static int __devexit mxt_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int mxt_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
@@ -1239,13 +1233,10 @@ static int mxt_resume(struct device *dev)
 
 	return 0;
 }
-
-static const struct dev_pm_ops mxt_pm_ops = {
-	.suspend	= mxt_suspend,
-	.resume		= mxt_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(mxt_pm_ops, mxt_suspend, mxt_resume);
+
 static const struct i2c_device_id mxt_id[] = {
 	{ "qt602240_ts", 0 },
 	{ "atmel_mxt_ts", 0 },
@@ -1258,9 +1249,7 @@ static struct i2c_driver mxt_driver = {
 	.driver = {
 		.name	= "atmel_mxt_ts",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	= &mxt_pm_ops,
-#endif
 	},
 	.probe		= mxt_probe,
 	.remove		= __devexit_p(mxt_remove),
diff --git a/drivers/input/touchscreen/da9052_tsi.c b/drivers/input/touchscreen/da9052_tsi.c
new file mode 100644
index 000000000000..e8df341090c0
--- /dev/null
+++ b/drivers/input/touchscreen/da9052_tsi.c
@@ -0,0 +1,370 @@
+/*
+ * TSI driver for Dialog DA9052
+ *
+ * Copyright(c) 2012 Dialog Semiconductor Ltd.
+ *
+ * Author: David Dajun Chen <dchen@diasemi.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 <linux/module.h>
+#include <linux/input.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+
+#include <linux/mfd/da9052/reg.h>
+#include <linux/mfd/da9052/da9052.h>
+
+#define TSI_PEN_DOWN_STATUS 0x40
+
+struct da9052_tsi {
+	struct da9052 *da9052;
+	struct input_dev *dev;
+	struct delayed_work ts_pen_work;
+	struct mutex mutex;
+	unsigned int irq_pendwn;
+	unsigned int irq_datardy;
+	bool stopped;
+	bool adc_on;
+};
+
+static void da9052_ts_adc_toggle(struct da9052_tsi *tsi, bool on)
+{
+	da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 1 << 0, on);
+	tsi->adc_on = on;
+}
+
+static irqreturn_t da9052_ts_pendwn_irq(int irq, void *data)
+{
+	struct da9052_tsi *tsi = data;
+
+	if (!tsi->stopped) {
+		/* Mask PEN_DOWN event and unmask TSI_READY event */
+		disable_irq_nosync(tsi->irq_pendwn);
+		enable_irq(tsi->irq_datardy);
+
+		da9052_ts_adc_toggle(tsi, true);
+
+		schedule_delayed_work(&tsi->ts_pen_work, HZ / 50);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void da9052_ts_read(struct da9052_tsi *tsi)
+{
+	struct input_dev *input = tsi->dev;
+	int ret;
+	u16 x, y, z;
+	u8 v;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_X_MSB_REG);
+	if (ret < 0)
+		return;
+
+	x = (u16) ret;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_Y_MSB_REG);
+	if (ret < 0)
+		return;
+
+	y = (u16) ret;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_Z_MSB_REG);
+	if (ret < 0)
+		return;
+
+	z = (u16) ret;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_LSB_REG);
+	if (ret < 0)
+		return;
+
+	v = (u8) ret;
+
+	x = ((x << 2) & 0x3fc) | (v & 0x3);
+	y = ((y << 2) & 0x3fc) | ((v & 0xc) >> 2);
+	z = ((z << 2) & 0x3fc) | ((v & 0x30) >> 4);
+
+	input_report_key(input, BTN_TOUCH, 1);
+	input_report_abs(input, ABS_X, x);
+	input_report_abs(input, ABS_Y, y);
+	input_report_abs(input, ABS_PRESSURE, z);
+	input_sync(input);
+}
+
+static irqreturn_t da9052_ts_datardy_irq(int irq, void *data)
+{
+	struct da9052_tsi *tsi = data;
+
+	da9052_ts_read(tsi);
+
+	return IRQ_HANDLED;
+}
+
+static void da9052_ts_pen_work(struct work_struct *work)
+{
+	struct da9052_tsi *tsi = container_of(work, struct da9052_tsi,
+					      ts_pen_work.work);
+	if (!tsi->stopped) {
+		int ret = da9052_reg_read(tsi->da9052, DA9052_TSI_LSB_REG);
+		if (ret < 0 || (ret & TSI_PEN_DOWN_STATUS)) {
+			/* Pen is still DOWN (or read error) */
+			schedule_delayed_work(&tsi->ts_pen_work, HZ / 50);
+		} else {
+			struct input_dev *input = tsi->dev;
+
+			/* Pen UP */
+			da9052_ts_adc_toggle(tsi, false);
+
+			/* Report Pen UP */
+			input_report_key(input, BTN_TOUCH, 0);
+			input_report_abs(input, ABS_PRESSURE, 0);
+			input_sync(input);
+
+			/*
+			 * FIXME: Fixes the unhandled irq issue when quick
+			 * pen down and pen up events occurs
+			 */
+			ret = da9052_reg_update(tsi->da9052,
+						DA9052_EVENT_B_REG, 0xC0, 0xC0);
+			if (ret < 0)
+				return;
+
+			/* Mask TSI_READY event and unmask PEN_DOWN event */
+			disable_irq(tsi->irq_datardy);
+			enable_irq(tsi->irq_pendwn);
+		}
+	}
+}
+
+static int __devinit da9052_ts_configure_gpio(struct da9052 *da9052)
+{
+	int error;
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_2_3_REG, 0x30, 0);
+	if (error < 0)
+		return error;
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_4_5_REG, 0x33, 0);
+	if (error < 0)
+		return error;
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_6_7_REG, 0x33, 0);
+	if (error < 0)
+		return error;
+
+	return 0;
+}
+
+static int __devinit da9052_configure_tsi(struct da9052_tsi *tsi)
+{
+	int error;
+
+	error = da9052_ts_configure_gpio(tsi->da9052);
+	if (error)
+		return error;
+
+	/* Measure TSI sample every 1ms */
+	error = da9052_reg_update(tsi->da9052, DA9052_ADC_CONT_REG,
+				  1 << 6, 1 << 6);
+	if (error < 0)
+		return error;
+
+	/* TSI_DELAY: 3 slots, TSI_SKIP: 0 slots, TSI_MODE: XYZP */
+	error = da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 0xFC, 0xC0);
+	if (error < 0)
+		return error;
+
+	/* Supply TSIRef through LD09 */
+	error = da9052_reg_write(tsi->da9052, DA9052_LDO9_REG, 0x59);
+	if (error < 0)
+		return error;
+
+	return 0;
+}
+
+static int da9052_ts_input_open(struct input_dev *input_dev)
+{
+	struct da9052_tsi *tsi = input_get_drvdata(input_dev);
+
+	tsi->stopped = false;
+	mb();
+
+	/* Unmask PEN_DOWN event */
+	enable_irq(tsi->irq_pendwn);
+
+	/* Enable Pen Detect Circuit */
+	return da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG,
+				 1 << 1, 1 << 1);
+}
+
+static void da9052_ts_input_close(struct input_dev *input_dev)
+{
+	struct da9052_tsi *tsi = input_get_drvdata(input_dev);
+
+	tsi->stopped = true;
+	mb();
+	disable_irq(tsi->irq_pendwn);
+	cancel_delayed_work_sync(&tsi->ts_pen_work);
+
+	if (tsi->adc_on) {
+		disable_irq(tsi->irq_datardy);
+		da9052_ts_adc_toggle(tsi, false);
+
+		/*
+		 * If ADC was on that means that pendwn IRQ was disabled
+		 * twice and we need to enable it to keep enable/disable
+		 * counter balanced. IRQ is still off though.
+		 */
+		enable_irq(tsi->irq_pendwn);
+	}
+
+	/* Disable Pen Detect Circuit */
+	da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 1 << 1, 0);
+}
+
+static int __devinit da9052_ts_probe(struct platform_device *pdev)
+{
+	struct da9052 *da9052;
+	struct da9052_tsi *tsi;
+	struct input_dev *input_dev;
+	int irq_pendwn;
+	int irq_datardy;
+	int error;
+
+	da9052 = dev_get_drvdata(pdev->dev.parent);
+	if (!da9052)
+		return -EINVAL;
+
+	irq_pendwn = platform_get_irq_byname(pdev, "PENDWN");
+	irq_datardy = platform_get_irq_byname(pdev, "TSIRDY");
+	if (irq_pendwn < 0 || irq_datardy < 0) {
+		dev_err(da9052->dev, "Unable to determine device interrupts\n");
+		return -ENXIO;
+	}
+
+	tsi = kzalloc(sizeof(struct da9052_tsi), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!tsi || !input_dev) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	tsi->da9052 = da9052;
+	tsi->dev = input_dev;
+	tsi->irq_pendwn = da9052->irq_base + irq_pendwn;
+	tsi->irq_datardy = da9052->irq_base + irq_datardy;
+	tsi->stopped = true;
+	INIT_DELAYED_WORK(&tsi->ts_pen_work, da9052_ts_pen_work);
+
+	input_dev->id.version = 0x0101;
+	input_dev->id.vendor = 0x15B6;
+	input_dev->id.product = 0x9052;
+	input_dev->name = "Dialog DA9052 TouchScreen Driver";
+	input_dev->dev.parent = &pdev->dev;
+	input_dev->open = da9052_ts_input_open;
+	input_dev->close = da9052_ts_input_close;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	input_set_abs_params(input_dev, ABS_X, 0, 1023, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, 1023, 0, 0);
+	input_set_abs_params(input_dev, ABS_PRESSURE, 0, 1023, 0, 0);
+
+	input_set_drvdata(input_dev, tsi);
+
+	/* Disable Pen Detect Circuit */
+	da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 1 << 1, 0);
+
+	/* Disable ADC */
+	da9052_ts_adc_toggle(tsi, false);
+
+	error = request_threaded_irq(tsi->irq_pendwn,
+				     NULL, da9052_ts_pendwn_irq,
+				     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				     "PENDWN", tsi);
+	if (error) {
+		dev_err(tsi->da9052->dev,
+			"Failed to register PENDWN IRQ %d, error = %d\n",
+			tsi->irq_pendwn, error);
+		goto err_free_mem;
+	}
+
+	error = request_threaded_irq(tsi->irq_datardy,
+				     NULL, da9052_ts_datardy_irq,
+				     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				     "TSIRDY", tsi);
+	if (error) {
+		dev_err(tsi->da9052->dev,
+			"Failed to register TSIRDY IRQ %d, error = %d\n",
+			tsi->irq_datardy, error);
+		goto err_free_pendwn_irq;
+	}
+
+	/* Mask PEN_DOWN and TSI_READY events */
+	disable_irq(tsi->irq_pendwn);
+	disable_irq(tsi->irq_datardy);
+
+	error = da9052_configure_tsi(tsi);
+	if (error)
+		goto err_free_datardy_irq;
+
+	error = input_register_device(tsi->dev);
+	if (error)
+		goto err_free_datardy_irq;
+
+	platform_set_drvdata(pdev, tsi);
+
+	return 0;
+
+err_free_datardy_irq:
+	free_irq(tsi->irq_datardy, tsi);
+err_free_pendwn_irq:
+	free_irq(tsi->irq_pendwn, tsi);
+err_free_mem:
+	kfree(tsi);
+	input_free_device(input_dev);
+
+	return error;
+}
+
+static int  __devexit da9052_ts_remove(struct platform_device *pdev)
+{
+	struct da9052_tsi *tsi = platform_get_drvdata(pdev);
+
+	da9052_reg_write(tsi->da9052, DA9052_LDO9_REG, 0x19);
+
+	free_irq(tsi->irq_pendwn, tsi);
+	free_irq(tsi->irq_datardy, tsi);
+
+	input_unregister_device(tsi->dev);
+	kfree(tsi);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver da9052_tsi_driver = {
+	.probe	= da9052_ts_probe,
+	.remove	= __devexit_p(da9052_ts_remove),
+	.driver	= {
+		.name	= "da9052-tsi",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(da9052_tsi_driver);
+
+MODULE_DESCRIPTION("Touchscreen driver for Dialog Semiconductor DA9052");
+MODULE_AUTHOR("Anthony Olech <Anthony.Olech@diasemi.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9052-tsi");
diff --git a/drivers/input/touchscreen/dynapro.c b/drivers/input/touchscreen/dynapro.c
index 455353908bdf..1809677a6513 100644
--- a/drivers/input/touchscreen/dynapro.c
+++ b/drivers/input/touchscreen/dynapro.c
@@ -188,19 +188,4 @@ static struct serio_driver dynapro_drv = {
 	.disconnect	= dynapro_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init dynapro_init(void)
-{
-	return serio_register_driver(&dynapro_drv);
-}
-
-static void __exit dynapro_exit(void)
-{
-	serio_unregister_driver(&dynapro_drv);
-}
-
-module_init(dynapro_init);
-module_exit(dynapro_exit);
+module_serio_driver(dynapro_drv);
diff --git a/drivers/input/touchscreen/elo.c b/drivers/input/touchscreen/elo.c
index 486d31ba9c09..957423d1471d 100644
--- a/drivers/input/touchscreen/elo.c
+++ b/drivers/input/touchscreen/elo.c
@@ -405,19 +405,4 @@ static struct serio_driver elo_drv = {
 	.disconnect	= elo_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init elo_init(void)
-{
-	return serio_register_driver(&elo_drv);
-}
-
-static void __exit elo_exit(void)
-{
-	serio_unregister_driver(&elo_drv);
-}
-
-module_init(elo_init);
-module_exit(elo_exit);
+module_serio_driver(elo_drv);
diff --git a/drivers/input/touchscreen/fujitsu_ts.c b/drivers/input/touchscreen/fujitsu_ts.c
index 80b21800355f..10794ddbdf58 100644
--- a/drivers/input/touchscreen/fujitsu_ts.c
+++ b/drivers/input/touchscreen/fujitsu_ts.c
@@ -175,15 +175,4 @@ static struct serio_driver fujitsu_drv = {
 	.disconnect	= fujitsu_disconnect,
 };
 
-static int __init fujitsu_init(void)
-{
-	return serio_register_driver(&fujitsu_drv);
-}
-
-static void __exit fujitsu_exit(void)
-{
-	serio_unregister_driver(&fujitsu_drv);
-}
-
-module_init(fujitsu_init);
-module_exit(fujitsu_exit);
+module_serio_driver(fujitsu_drv);
diff --git a/drivers/input/touchscreen/gunze.c b/drivers/input/touchscreen/gunze.c
index a54f90e02ab6..41c71766bf18 100644
--- a/drivers/input/touchscreen/gunze.c
+++ b/drivers/input/touchscreen/gunze.c
@@ -186,19 +186,4 @@ static struct serio_driver gunze_drv = {
 	.disconnect	= gunze_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init gunze_init(void)
-{
-	return serio_register_driver(&gunze_drv);
-}
-
-static void __exit gunze_exit(void)
-{
-	serio_unregister_driver(&gunze_drv);
-}
-
-module_init(gunze_init);
-module_exit(gunze_exit);
+module_serio_driver(gunze_drv);
diff --git a/drivers/input/touchscreen/h3600_ts_input.c b/drivers/input/touchscreen/h3600_ts_input.c
index 6107e563e681..b9e8686a6f1c 100644
--- a/drivers/input/touchscreen/h3600_ts_input.c
+++ b/drivers/input/touchscreen/h3600_ts_input.c
@@ -476,19 +476,4 @@ static struct serio_driver h3600ts_drv = {
 	.disconnect	= h3600ts_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init h3600ts_init(void)
-{
-	return serio_register_driver(&h3600ts_drv);
-}
-
-static void __exit h3600ts_exit(void)
-{
-	serio_unregister_driver(&h3600ts_drv);
-}
-
-module_init(h3600ts_init);
-module_exit(h3600ts_exit);
+module_serio_driver(h3600ts_drv);
diff --git a/drivers/input/touchscreen/hampshire.c b/drivers/input/touchscreen/hampshire.c
index 2da6cc31bb21..0cc47ea98acf 100644
--- a/drivers/input/touchscreen/hampshire.c
+++ b/drivers/input/touchscreen/hampshire.c
@@ -187,19 +187,4 @@ static struct serio_driver hampshire_drv = {
 	.disconnect	= hampshire_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init hampshire_init(void)
-{
-	return serio_register_driver(&hampshire_drv);
-}
-
-static void __exit hampshire_exit(void)
-{
-	serio_unregister_driver(&hampshire_drv);
-}
-
-module_init(hampshire_init);
-module_exit(hampshire_exit);
+module_serio_driver(hampshire_drv);
diff --git a/drivers/input/touchscreen/inexio.c b/drivers/input/touchscreen/inexio.c
index 192ade0a0fb9..a29c99c32245 100644
--- a/drivers/input/touchscreen/inexio.c
+++ b/drivers/input/touchscreen/inexio.c
@@ -189,19 +189,4 @@ static struct serio_driver inexio_drv = {
 	.disconnect	= inexio_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init inexio_init(void)
-{
-	return serio_register_driver(&inexio_drv);
-}
-
-static void __exit inexio_exit(void)
-{
-	serio_unregister_driver(&inexio_drv);
-}
-
-module_init(inexio_init);
-module_exit(inexio_exit);
+module_serio_driver(inexio_drv);
diff --git a/drivers/input/touchscreen/lpc32xx_ts.c b/drivers/input/touchscreen/lpc32xx_ts.c
index afcd0691ec67..4c2b8ed3bf16 100644
--- a/drivers/input/touchscreen/lpc32xx_ts.c
+++ b/drivers/input/touchscreen/lpc32xx_ts.c
@@ -22,6 +22,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/of.h>
 
 /*
  * Touchscreen controller register offsets
@@ -383,6 +384,14 @@ static const struct dev_pm_ops lpc32xx_ts_pm_ops = {
 #define LPC32XX_TS_PM_OPS NULL
 #endif
 
+#ifdef CONFIG_OF
+static struct of_device_id lpc32xx_tsc_of_match[] = {
+	{ .compatible = "nxp,lpc3220-tsc", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_tsc_of_match);
+#endif
+
 static struct platform_driver lpc32xx_ts_driver = {
 	.probe		= lpc32xx_ts_probe,
 	.remove		= __devexit_p(lpc32xx_ts_remove),
@@ -390,6 +399,7 @@ static struct platform_driver lpc32xx_ts_driver = {
 		.name	= MOD_NAME,
 		.owner	= THIS_MODULE,
 		.pm	= LPC32XX_TS_PM_OPS,
+		.of_match_table = of_match_ptr(lpc32xx_tsc_of_match),
 	},
 };
 module_platform_driver(lpc32xx_ts_driver);
diff --git a/drivers/input/touchscreen/mtouch.c b/drivers/input/touchscreen/mtouch.c
index 9077228418b7..eb66b7c37c2f 100644
--- a/drivers/input/touchscreen/mtouch.c
+++ b/drivers/input/touchscreen/mtouch.c
@@ -202,19 +202,4 @@ static struct serio_driver mtouch_drv = {
 	.disconnect	= mtouch_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init mtouch_init(void)
-{
-	return serio_register_driver(&mtouch_drv);
-}
-
-static void __exit mtouch_exit(void)
-{
-	serio_unregister_driver(&mtouch_drv);
-}
-
-module_init(mtouch_init);
-module_exit(mtouch_exit);
+module_serio_driver(mtouch_drv);
diff --git a/drivers/input/touchscreen/penmount.c b/drivers/input/touchscreen/penmount.c
index 4c012fb2b01e..4ccde45b9da2 100644
--- a/drivers/input/touchscreen/penmount.c
+++ b/drivers/input/touchscreen/penmount.c
@@ -317,19 +317,4 @@ static struct serio_driver pm_drv = {
 	.disconnect	= pm_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init pm_init(void)
-{
-	return serio_register_driver(&pm_drv);
-}
-
-static void __exit pm_exit(void)
-{
-	serio_unregister_driver(&pm_drv);
-}
-
-module_init(pm_init);
-module_exit(pm_exit);
+module_serio_driver(pm_drv);
diff --git a/drivers/input/touchscreen/st1232.c b/drivers/input/touchscreen/st1232.c
index cbbf71b22696..6cb68a1981bf 100644
--- a/drivers/input/touchscreen/st1232.c
+++ b/drivers/input/touchscreen/st1232.c
@@ -218,7 +218,7 @@ static int __devexit st1232_ts_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int st1232_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
@@ -243,18 +243,25 @@ static int st1232_ts_resume(struct device *dev)
 	return 0;
 }
 
-static const struct dev_pm_ops st1232_ts_pm_ops = {
-	.suspend	= st1232_ts_suspend,
-	.resume		= st1232_ts_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(st1232_ts_pm_ops,
+			 st1232_ts_suspend, st1232_ts_resume);
+
 static const struct i2c_device_id st1232_ts_id[] = {
 	{ ST1232_TS_NAME, 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, st1232_ts_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id st1232_ts_dt_ids[] __devinitconst = {
+	{ .compatible = "sitronix,st1232", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, st1232_ts_dt_ids);
+#endif
+
 static struct i2c_driver st1232_ts_driver = {
 	.probe		= st1232_ts_probe,
 	.remove		= __devexit_p(st1232_ts_remove),
@@ -262,9 +269,8 @@ static struct i2c_driver st1232_ts_driver = {
 	.driver = {
 		.name	= ST1232_TS_NAME,
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
+		.of_match_table = of_match_ptr(st1232_ts_dt_ids),
 		.pm	= &st1232_ts_pm_ops,
-#endif
 	},
 };
 
diff --git a/drivers/input/touchscreen/touchit213.c b/drivers/input/touchscreen/touchit213.c
index d1297ba19daf..5f29e5b8e1c1 100644
--- a/drivers/input/touchscreen/touchit213.c
+++ b/drivers/input/touchscreen/touchit213.c
@@ -216,19 +216,4 @@ static struct serio_driver touchit213_drv = {
 	.disconnect	= touchit213_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init touchit213_init(void)
-{
-	return serio_register_driver(&touchit213_drv);
-}
-
-static void __exit touchit213_exit(void)
-{
-	serio_unregister_driver(&touchit213_drv);
-}
-
-module_init(touchit213_init);
-module_exit(touchit213_exit);
+module_serio_driver(touchit213_drv);
diff --git a/drivers/input/touchscreen/touchright.c b/drivers/input/touchscreen/touchright.c
index 3a5c142c2a78..8a2887daf194 100644
--- a/drivers/input/touchscreen/touchright.c
+++ b/drivers/input/touchscreen/touchright.c
@@ -176,19 +176,4 @@ static struct serio_driver tr_drv = {
 	.disconnect	= tr_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init tr_init(void)
-{
-	return serio_register_driver(&tr_drv);
-}
-
-static void __exit tr_exit(void)
-{
-	serio_unregister_driver(&tr_drv);
-}
-
-module_init(tr_init);
-module_exit(tr_exit);
+module_serio_driver(tr_drv);
diff --git a/drivers/input/touchscreen/touchwin.c b/drivers/input/touchscreen/touchwin.c
index 763a656a59f8..588cdcb839dd 100644
--- a/drivers/input/touchscreen/touchwin.c
+++ b/drivers/input/touchscreen/touchwin.c
@@ -183,19 +183,4 @@ static struct serio_driver tw_drv = {
 	.disconnect	= tw_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init tw_init(void)
-{
-	return serio_register_driver(&tw_drv);
-}
-
-static void __exit tw_exit(void)
-{
-	serio_unregister_driver(&tw_drv);
-}
-
-module_init(tw_init);
-module_exit(tw_exit);
+module_serio_driver(tw_drv);
diff --git a/drivers/input/touchscreen/tsc40.c b/drivers/input/touchscreen/tsc40.c
index 29d5ed4dd31c..63209aaa55f0 100644
--- a/drivers/input/touchscreen/tsc40.c
+++ b/drivers/input/touchscreen/tsc40.c
@@ -167,17 +167,7 @@ static struct serio_driver tsc_drv = {
 	.disconnect     = tsc_disconnect,
 };
 
-static int __init tsc_ser_init(void)
-{
-	return serio_register_driver(&tsc_drv);
-}
-module_init(tsc_ser_init);
-
-static void __exit tsc_exit(void)
-{
-	serio_unregister_driver(&tsc_drv);
-}
-module_exit(tsc_exit);
+module_serio_driver(tsc_drv);
 
 MODULE_AUTHOR("Sebastian Andrzej Siewior <bigeasy@linutronix.de>");
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/input/touchscreen/wacom_i2c.c b/drivers/input/touchscreen/wacom_i2c.c
new file mode 100644
index 000000000000..35572575d34a
--- /dev/null
+++ b/drivers/input/touchscreen/wacom_i2c.c
@@ -0,0 +1,282 @@
+/*
+ * Wacom Penabled Driver for I2C
+ *
+ * Copyright (c) 2011 Tatsunosuke Tobita, Wacom.
+ * <tobita.tatsunosuke@wacom.co.jp>
+ *
+ * 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 of 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/input.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <asm/unaligned.h>
+
+#define WACOM_CMD_QUERY0	0x04
+#define WACOM_CMD_QUERY1	0x00
+#define WACOM_CMD_QUERY2	0x33
+#define WACOM_CMD_QUERY3	0x02
+#define WACOM_CMD_THROW0	0x05
+#define WACOM_CMD_THROW1	0x00
+#define WACOM_QUERY_SIZE	19
+#define WACOM_RETRY_CNT		100
+
+struct wacom_features {
+	int x_max;
+	int y_max;
+	int pressure_max;
+	char fw_version;
+};
+
+struct wacom_i2c {
+	struct i2c_client *client;
+	struct input_dev *input;
+	u8 data[WACOM_QUERY_SIZE];
+};
+
+static int wacom_query_device(struct i2c_client *client,
+			      struct wacom_features *features)
+{
+	int ret;
+	u8 cmd1[] = { WACOM_CMD_QUERY0, WACOM_CMD_QUERY1,
+			WACOM_CMD_QUERY2, WACOM_CMD_QUERY3 };
+	u8 cmd2[] = { WACOM_CMD_THROW0, WACOM_CMD_THROW1 };
+	u8 data[WACOM_QUERY_SIZE];
+	struct i2c_msg msgs[] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = sizeof(cmd1),
+			.buf = cmd1,
+		},
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = sizeof(cmd2),
+			.buf = cmd2,
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = sizeof(data),
+			.buf = data,
+		},
+	};
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		return ret;
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	features->x_max = get_unaligned_le16(&data[3]);
+	features->y_max = get_unaligned_le16(&data[5]);
+	features->pressure_max = get_unaligned_le16(&data[11]);
+	features->fw_version = get_unaligned_le16(&data[13]);
+
+	dev_dbg(&client->dev,
+		"x_max:%d, y_max:%d, pressure:%d, fw:%d\n",
+		features->x_max, features->y_max,
+		features->pressure_max, features->fw_version);
+
+	return 0;
+}
+
+static irqreturn_t wacom_i2c_irq(int irq, void *dev_id)
+{
+	struct wacom_i2c *wac_i2c = dev_id;
+	struct input_dev *input = wac_i2c->input;
+	u8 *data = wac_i2c->data;
+	unsigned int x, y, pressure;
+	unsigned char tsw, f1, f2, ers;
+	int error;
+
+	error = i2c_master_recv(wac_i2c->client,
+				wac_i2c->data, sizeof(wac_i2c->data));
+	if (error < 0)
+		goto out;
+
+	tsw = data[3] & 0x01;
+	ers = data[3] & 0x04;
+	f1 = data[3] & 0x02;
+	f2 = data[3] & 0x10;
+	x = le16_to_cpup((__le16 *)&data[4]);
+	y = le16_to_cpup((__le16 *)&data[6]);
+	pressure = le16_to_cpup((__le16 *)&data[8]);
+
+	input_report_key(input, BTN_TOUCH, tsw || ers);
+	input_report_key(input, BTN_TOOL_PEN, tsw);
+	input_report_key(input, BTN_TOOL_RUBBER, ers);
+	input_report_key(input, BTN_STYLUS, f1);
+	input_report_key(input, BTN_STYLUS2, f2);
+	input_report_abs(input, ABS_X, x);
+	input_report_abs(input, ABS_Y, y);
+	input_report_abs(input, ABS_PRESSURE, pressure);
+	input_sync(input);
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int wacom_i2c_open(struct input_dev *dev)
+{
+	struct wacom_i2c *wac_i2c = input_get_drvdata(dev);
+	struct i2c_client *client = wac_i2c->client;
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static void wacom_i2c_close(struct input_dev *dev)
+{
+	struct wacom_i2c *wac_i2c = input_get_drvdata(dev);
+	struct i2c_client *client = wac_i2c->client;
+
+	disable_irq(client->irq);
+}
+
+static int __devinit wacom_i2c_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct wacom_i2c *wac_i2c;
+	struct input_dev *input;
+	struct wacom_features features;
+	int error;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "i2c_check_functionality error\n");
+		return -EIO;
+	}
+
+	error = wacom_query_device(client, &features);
+	if (error)
+		return error;
+
+	wac_i2c = kzalloc(sizeof(*wac_i2c), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!wac_i2c || !input) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	wac_i2c->client = client;
+	wac_i2c->input = input;
+
+	input->name = "Wacom I2C Digitizer";
+	input->id.bustype = BUS_I2C;
+	input->id.vendor = 0x56a;
+	input->id.version = features.fw_version;
+	input->dev.parent = &client->dev;
+	input->open = wacom_i2c_open;
+	input->close = wacom_i2c_close;
+
+	input->evbit[0] |= BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+
+	__set_bit(BTN_TOOL_PEN, input->keybit);
+	__set_bit(BTN_TOOL_RUBBER, input->keybit);
+	__set_bit(BTN_STYLUS, input->keybit);
+	__set_bit(BTN_STYLUS2, input->keybit);
+	__set_bit(BTN_TOUCH, input->keybit);
+
+	input_set_abs_params(input, ABS_X, 0, features.x_max, 0, 0);
+	input_set_abs_params(input, ABS_Y, 0, features.y_max, 0, 0);
+	input_set_abs_params(input, ABS_PRESSURE,
+			     0, features.pressure_max, 0, 0);
+
+	input_set_drvdata(input, wac_i2c);
+
+	error = request_threaded_irq(client->irq, NULL, wacom_i2c_irq,
+				     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				     "wacom_i2c", wac_i2c);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to enable IRQ, error: %d\n", error);
+		goto err_free_mem;
+	}
+
+	/* Disable the IRQ, we'll enable it in wac_i2c_open() */
+	disable_irq(client->irq);
+
+	error = input_register_device(wac_i2c->input);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to register input device, error: %d\n", error);
+		goto err_free_irq;
+	}
+
+	i2c_set_clientdata(client, wac_i2c);
+	return 0;
+
+err_free_irq:
+	free_irq(client->irq, wac_i2c);
+err_free_mem:
+	input_free_device(input);
+	kfree(wac_i2c);
+
+	return error;
+}
+
+static int __devexit wacom_i2c_remove(struct i2c_client *client)
+{
+	struct wacom_i2c *wac_i2c = i2c_get_clientdata(client);
+
+	free_irq(client->irq, wac_i2c);
+	input_unregister_device(wac_i2c->input);
+	kfree(wac_i2c);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int wacom_i2c_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	disable_irq(client->irq);
+
+	return 0;
+}
+
+static int wacom_i2c_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(wacom_i2c_pm, wacom_i2c_suspend, wacom_i2c_resume);
+
+static const struct i2c_device_id wacom_i2c_id[] = {
+	{ "WAC_I2C_EMR", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, wacom_i2c_id);
+
+static struct i2c_driver wacom_i2c_driver = {
+	.driver	= {
+		.name	= "wacom_i2c",
+		.owner	= THIS_MODULE,
+		.pm	= &wacom_i2c_pm,
+	},
+
+	.probe		= wacom_i2c_probe,
+	.remove		= __devexit_p(wacom_i2c_remove),
+	.id_table	= wacom_i2c_id,
+};
+module_i2c_driver(wacom_i2c_driver);
+
+MODULE_AUTHOR("Tatsunosuke Tobita <tobita.tatsunosuke@wacom.co.jp>");
+MODULE_DESCRIPTION("WACOM EMR I2C Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/wacom_w8001.c b/drivers/input/touchscreen/wacom_w8001.c
index 1569a3934ab2..8f9ad2f893b8 100644
--- a/drivers/input/touchscreen/wacom_w8001.c
+++ b/drivers/input/touchscreen/wacom_w8001.c
@@ -594,15 +594,4 @@ static struct serio_driver w8001_drv = {
 	.disconnect	= w8001_disconnect,
 };
 
-static int __init w8001_init(void)
-{
-	return serio_register_driver(&w8001_drv);
-}
-
-static void __exit w8001_exit(void)
-{
-	serio_unregister_driver(&w8001_drv);
-}
-
-module_init(w8001_init);
-module_exit(w8001_exit);
+module_serio_driver(w8001_drv);
diff --git a/drivers/isdn/hardware/mISDN/hfcsusb.h b/drivers/isdn/hardware/mISDN/hfcsusb.h
index cb1231b08f78..4157311d569d 100644
--- a/drivers/isdn/hardware/mISDN/hfcsusb.h
+++ b/drivers/isdn/hardware/mISDN/hfcsusb.h
@@ -410,6 +410,12 @@ static struct usb_device_id hfcsusb_idtab[] = {
 			{LED_SCHEME1, {0x88, -64, -32, -16},
 					"ZyXEL OMNI.NET USB II"}),
 	},
+	{
+		USB_DEVICE(0x1ae7, 0x0525),
+		.driver_info = (unsigned long) &((struct hfcsusb_vdata)
+			{LED_SCHEME1, {0x88, -64, -32, -16},
+					"X-Tensions USB ISDN TA XC-525"}),
+	},
 	{ }
 };
 
diff --git a/drivers/media/common/saa7146_fops.c b/drivers/media/common/saa7146_fops.c
index 71f8e018e564..7d42c11c8684 100644
--- a/drivers/media/common/saa7146_fops.c
+++ b/drivers/media/common/saa7146_fops.c
@@ -198,7 +198,6 @@ static int fops_open(struct file *file)
 	struct saa7146_dev *dev = video_drvdata(file);
 	struct saa7146_fh *fh = NULL;
 	int result = 0;
-
 	enum v4l2_buf_type type;
 
 	DEB_EE("file:%p, dev:%s\n", file, video_device_node_name(vdev));
@@ -227,11 +226,12 @@ static int fops_open(struct file *file)
 		goto out;
 	}
 
-	file->private_data = fh;
+	v4l2_fh_init(&fh->fh, vdev);
+
+	file->private_data = &fh->fh;
 	fh->dev = dev;
-	fh->type = type;
 
-	if( fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
 		DEB_S("initializing vbi...\n");
 		if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
 			result = saa7146_vbi_uops.open(dev,file);
@@ -252,6 +252,7 @@ static int fops_open(struct file *file)
 	}
 
 	result = 0;
+	v4l2_fh_add(&fh->fh);
 out:
 	if (fh && result != 0) {
 		kfree(fh);
@@ -263,6 +264,7 @@ out:
 
 static int fops_release(struct file *file)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh  *fh  = file->private_data;
 	struct saa7146_dev *dev = fh->dev;
 
@@ -271,7 +273,7 @@ static int fops_release(struct file *file)
 	if (mutex_lock_interruptible(&saa7146_devices_lock))
 		return -ERESTARTSYS;
 
-	if( fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
 		if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
 			saa7146_vbi_uops.release(dev,file);
 		if (dev->ext_vv_data->vbi_fops.release)
@@ -280,6 +282,8 @@ static int fops_release(struct file *file)
 		saa7146_video_uops.release(dev,file);
 	}
 
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	module_put(dev->ext->module);
 	file->private_data = NULL;
 	kfree(fh);
@@ -291,19 +295,22 @@ static int fops_release(struct file *file)
 
 static int fops_mmap(struct file *file, struct vm_area_struct * vma)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 	struct videobuf_queue *q;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE: {
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER: {
 		DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: file:%p, vma:%p\n",
 		       file, vma);
 		q = &fh->video_q;
 		break;
 		}
-	case V4L2_BUF_TYPE_VBI_CAPTURE: {
+	case VFL_TYPE_VBI: {
 		DEB_EE("V4L2_BUF_TYPE_VBI_CAPTURE: file:%p, vma:%p\n",
 		       file, vma);
+		if (fh->dev->ext_vv_data->capabilities & V4L2_CAP_SLICED_VBI_OUTPUT)
+			return -ENODEV;
 		q = &fh->vbi_q;
 		break;
 		}
@@ -317,15 +324,19 @@ static int fops_mmap(struct file *file, struct vm_area_struct * vma)
 
 static unsigned int fops_poll(struct file *file, struct poll_table_struct *wait)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 	struct videobuf_buffer *buf = NULL;
 	struct videobuf_queue *q;
+	unsigned int res = v4l2_ctrl_poll(file, wait);
 
 	DEB_EE("file:%p, poll:%p\n", file, wait);
 
-	if (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type) {
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
+		if (fh->dev->ext_vv_data->capabilities & V4L2_CAP_SLICED_VBI_OUTPUT)
+			return res | POLLOUT | POLLWRNORM;
 		if( 0 == fh->vbi_q.streaming )
-			return videobuf_poll_stream(file, &fh->vbi_q, wait);
+			return res | videobuf_poll_stream(file, &fh->vbi_q, wait);
 		q = &fh->vbi_q;
 	} else {
 		DEB_D("using video queue\n");
@@ -337,31 +348,32 @@ static unsigned int fops_poll(struct file *file, struct poll_table_struct *wait)
 
 	if (!buf) {
 		DEB_D("buf == NULL!\n");
-		return POLLERR;
+		return res | POLLERR;
 	}
 
 	poll_wait(file, &buf->done, wait);
 	if (buf->state == VIDEOBUF_DONE || buf->state == VIDEOBUF_ERROR) {
 		DEB_D("poll succeeded!\n");
-		return POLLIN|POLLRDNORM;
+		return res | POLLIN | POLLRDNORM;
 	}
 
 	DEB_D("nothing to poll for, buf->state:%d\n", buf->state);
-	return 0;
+	return res;
 }
 
 static ssize_t fops_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
 /*
 		DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: file:%p, data:%p, count:%lun",
 		       file, data, (unsigned long)count);
 */
 		return saa7146_video_uops.read(file,data,count,ppos);
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case VFL_TYPE_VBI:
 /*
 		DEB_EE("V4L2_BUF_TYPE_VBI_CAPTURE: file:%p, data:%p, count:%lu\n",
 		       file, data, (unsigned long)count);
@@ -377,12 +389,13 @@ static ssize_t fops_read(struct file *file, char __user *data, size_t count, lof
 
 static ssize_t fops_write(struct file *file, const char __user *data, size_t count, loff_t *ppos)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
 		return -EINVAL;
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case VFL_TYPE_VBI:
 		if (fh->dev->ext_vv_data->vbi_fops.write)
 			return fh->dev->ext_vv_data->vbi_fops.write(file, data, count, ppos);
 		else
@@ -429,8 +442,15 @@ static void vv_callback(struct saa7146_dev *dev, unsigned long status)
 	}
 }
 
+static const struct v4l2_ctrl_ops saa7146_ctrl_ops = {
+	.s_ctrl = saa7146_s_ctrl,
+};
+
 int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+	struct v4l2_pix_format *fmt;
+	struct v4l2_vbi_format *vbi;
 	struct saa7146_vv *vv;
 	int err;
 
@@ -438,12 +458,32 @@ int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 	if (err)
 		return err;
 
+	v4l2_ctrl_handler_init(hdl, 6);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (hdl->error) {
+		err = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
+
 	vv = kzalloc(sizeof(struct saa7146_vv), GFP_KERNEL);
 	if (vv == NULL) {
 		ERR("out of memory. aborting.\n");
+		v4l2_ctrl_handler_free(hdl);
 		return -ENOMEM;
 	}
-	ext_vv->ops = saa7146_video_ioctl_ops;
+	ext_vv->vid_ops = saa7146_video_ioctl_ops;
+	ext_vv->vbi_ops = saa7146_vbi_ioctl_ops;
 	ext_vv->core_ops = &saa7146_video_ioctl_ops;
 
 	DEB_EE("dev:%p\n", dev);
@@ -463,6 +503,7 @@ int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 	if( NULL == vv->d_clipping.cpu_addr ) {
 		ERR("out of memory. aborting.\n");
 		kfree(vv);
+		v4l2_ctrl_handler_free(hdl);
 		return -1;
 	}
 	memset(vv->d_clipping.cpu_addr, 0x0, SAA7146_CLIPPING_MEM);
@@ -471,6 +512,39 @@ int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 	if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
 		saa7146_vbi_uops.init(dev,vv);
 
+	fmt = &vv->ov_fb.fmt;
+	fmt->width = vv->standard->h_max_out;
+	fmt->height = vv->standard->v_max_out;
+	fmt->pixelformat = V4L2_PIX_FMT_RGB565;
+	fmt->bytesperline = 2 * fmt->width;
+	fmt->sizeimage = fmt->bytesperline * fmt->height;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt = &vv->video_fmt;
+	fmt->width = 384;
+	fmt->height = 288;
+	fmt->pixelformat = V4L2_PIX_FMT_BGR24;
+	fmt->field = V4L2_FIELD_ANY;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	fmt->bytesperline = 3 * fmt->width;
+	fmt->sizeimage = fmt->bytesperline * fmt->height;
+
+	vbi = &vv->vbi_fmt;
+	vbi->sampling_rate	= 27000000;
+	vbi->offset		= 248; /* todo */
+	vbi->samples_per_line	= 720 * 2;
+	vbi->sample_format	= V4L2_PIX_FMT_GREY;
+
+	/* fixme: this only works for PAL */
+	vbi->start[0] = 5;
+	vbi->count[0] = 16;
+	vbi->start[1] = 312;
+	vbi->count[1] = 16;
+
+	init_timer(&vv->vbi_read_timeout);
+
+	vv->ov_fb.capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	vv->ov_fb.flags = V4L2_FBUF_FLAG_PRIMARY;
 	dev->vv_data = vv;
 	dev->vv_callback = &vv_callback;
 
@@ -486,6 +560,7 @@ int saa7146_vv_release(struct saa7146_dev* dev)
 
 	v4l2_device_unregister(&dev->v4l2_dev);
 	pci_free_consistent(dev->pci, SAA7146_CLIPPING_MEM, vv->d_clipping.cpu_addr, vv->d_clipping.dma_handle);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 	kfree(vv);
 	dev->vv_data = NULL;
 	dev->vv_callback = NULL;
@@ -509,10 +584,19 @@ int saa7146_register_device(struct video_device **vid, struct saa7146_dev* dev,
 		return -ENOMEM;
 
 	vfd->fops = &video_fops;
-	vfd->ioctl_ops = &dev->ext_vv_data->ops;
+	if (type == VFL_TYPE_GRABBER)
+		vfd->ioctl_ops = &dev->ext_vv_data->vid_ops;
+	else
+		vfd->ioctl_ops = &dev->ext_vv_data->vbi_ops;
 	vfd->release = video_device_release;
+	/* Locking in file operations other than ioctl should be done by
+	   the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &dev->v4l2_lock;
+	vfd->v4l2_dev = &dev->v4l2_dev;
 	vfd->tvnorms = 0;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	for (i = 0; i < dev->ext_vv_data->num_stds; i++)
 		vfd->tvnorms |= dev->ext_vv_data->stds[i].id;
 	strlcpy(vfd->name, name, sizeof(vfd->name));
diff --git a/drivers/media/common/saa7146_hlp.c b/drivers/media/common/saa7146_hlp.c
index bc1f545c95cb..be746d1aee9a 100644
--- a/drivers/media/common/saa7146_hlp.c
+++ b/drivers/media/common/saa7146_hlp.c
@@ -343,9 +343,9 @@ static void calculate_clipping_registers_rect(struct saa7146_dev *dev, struct sa
 	struct saa7146_vv *vv = dev->vv_data;
 	__le32 *clipping = vv->d_clipping.cpu_addr;
 
-	int width = fh->ov.win.w.width;
-	int height =  fh->ov.win.w.height;
-	int clipcount = fh->ov.nclips;
+	int width = vv->ov.win.w.width;
+	int height =  vv->ov.win.w.height;
+	int clipcount = vv->ov.nclips;
 
 	u32 line_list[32];
 	u32 pixel_list[32];
@@ -365,10 +365,10 @@ static void calculate_clipping_registers_rect(struct saa7146_dev *dev, struct sa
 	for(i = 0; i < clipcount; i++) {
 		int l = 0, r = 0, t = 0, b = 0;
 
-		x[i] = fh->ov.clips[i].c.left;
-		y[i] = fh->ov.clips[i].c.top;
-		w[i] = fh->ov.clips[i].c.width;
-		h[i] = fh->ov.clips[i].c.height;
+		x[i] = vv->ov.clips[i].c.left;
+		y[i] = vv->ov.clips[i].c.top;
+		w[i] = vv->ov.clips[i].c.width;
+		h[i] = vv->ov.clips[i].c.height;
 
 		if( w[i] < 0) {
 			x[i] += w[i]; w[i] = -w[i];
@@ -485,13 +485,14 @@ static void saa7146_disable_clipping(struct saa7146_dev *dev)
 static void saa7146_set_clipping_rect(struct saa7146_fh *fh)
 {
 	struct saa7146_dev *dev = fh->dev;
-	enum v4l2_field field = fh->ov.win.field;
+	struct saa7146_vv *vv = dev->vv_data;
+	enum v4l2_field field = vv->ov.win.field;
 	struct	saa7146_video_dma vdma2;
 	u32 clip_format;
 	u32 arbtr_ctrl;
 
 	/* check clipcount, disable clipping if clipcount == 0*/
-	if( fh->ov.nclips == 0 ) {
+	if (vv->ov.nclips == 0) {
 		saa7146_disable_clipping(dev);
 		return;
 	}
@@ -651,8 +652,8 @@ int saa7146_enable_overlay(struct saa7146_fh *fh)
 	struct saa7146_dev *dev = fh->dev;
 	struct saa7146_vv *vv = dev->vv_data;
 
-	saa7146_set_window(dev, fh->ov.win.w.width, fh->ov.win.w.height, fh->ov.win.field);
-	saa7146_set_position(dev, fh->ov.win.w.left, fh->ov.win.w.top, fh->ov.win.w.height, fh->ov.win.field, vv->ov_fmt->pixelformat);
+	saa7146_set_window(dev, vv->ov.win.w.width, vv->ov.win.w.height, vv->ov.win.field);
+	saa7146_set_position(dev, vv->ov.win.w.left, vv->ov.win.w.top, vv->ov.win.w.height, vv->ov.win.field, vv->ov_fmt->pixelformat);
 	saa7146_set_output_format(dev, vv->ov_fmt->trans);
 	saa7146_set_clipping_rect(fh);
 
diff --git a/drivers/media/common/saa7146_vbi.c b/drivers/media/common/saa7146_vbi.c
index b2e718343739..1e71e374bbfe 100644
--- a/drivers/media/common/saa7146_vbi.c
+++ b/drivers/media/common/saa7146_vbi.c
@@ -211,7 +211,7 @@ static int buffer_activate(struct saa7146_dev *dev,
 	DEB_VBI("dev:%p, buf:%p, next:%p\n", dev, buf, next);
 	saa7146_set_vbi_capture(dev,buf,next);
 
-	mod_timer(&vv->vbi_q.timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&vv->vbi_dmaq.timeout, jiffies+BUFFER_TIMEOUT);
 	return 0;
 }
 
@@ -294,7 +294,7 @@ static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
 	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
 
 	DEB_VBI("vb:%p\n", vb);
-	saa7146_buffer_queue(dev,&vv->vbi_q,buf);
+	saa7146_buffer_queue(dev, &vv->vbi_dmaq, buf);
 }
 
 static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
@@ -335,16 +335,15 @@ static void vbi_stop(struct saa7146_fh *fh, struct file *file)
 	/* shut down dma 3 transfers */
 	saa7146_write(dev, MC1, MASK_20);
 
-	if (vv->vbi_q.curr) {
-		saa7146_buffer_finish(dev,&vv->vbi_q,VIDEOBUF_DONE);
-	}
+	if (vv->vbi_dmaq.curr)
+		saa7146_buffer_finish(dev, &vv->vbi_dmaq, VIDEOBUF_DONE);
 
 	videobuf_queue_cancel(&fh->vbi_q);
 
 	vv->vbi_streaming = NULL;
 
-	del_timer(&vv->vbi_q.timeout);
-	del_timer(&fh->vbi_read_timeout);
+	del_timer(&vv->vbi_dmaq.timeout);
+	del_timer(&vv->vbi_read_timeout);
 
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
@@ -364,12 +363,12 @@ static void vbi_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 {
 	DEB_VBI("dev:%p\n", dev);
 
-	INIT_LIST_HEAD(&vv->vbi_q.queue);
+	INIT_LIST_HEAD(&vv->vbi_dmaq.queue);
 
-	init_timer(&vv->vbi_q.timeout);
-	vv->vbi_q.timeout.function = saa7146_buffer_timeout;
-	vv->vbi_q.timeout.data     = (unsigned long)(&vv->vbi_q);
-	vv->vbi_q.dev              = dev;
+	init_timer(&vv->vbi_dmaq.timeout);
+	vv->vbi_dmaq.timeout.function = saa7146_buffer_timeout;
+	vv->vbi_dmaq.timeout.data     = (unsigned long)(&vv->vbi_dmaq);
+	vv->vbi_dmaq.dev              = dev;
 
 	init_waitqueue_head(&vv->vbi_wq);
 }
@@ -377,6 +376,7 @@ static void vbi_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 static int vbi_open(struct saa7146_dev *dev, struct file *file)
 {
 	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = fh->dev->vv_data;
 
 	u32 arbtr_ctrl	= saa7146_read(dev, PCI_BT_V1);
 	int ret = 0;
@@ -395,19 +395,6 @@ static int vbi_open(struct saa7146_dev *dev, struct file *file)
 	saa7146_write(dev, PCI_BT_V1, arbtr_ctrl);
 	saa7146_write(dev, MC2, (MASK_04|MASK_20));
 
-	memset(&fh->vbi_fmt,0,sizeof(fh->vbi_fmt));
-
-	fh->vbi_fmt.sampling_rate	= 27000000;
-	fh->vbi_fmt.offset		= 248; /* todo */
-	fh->vbi_fmt.samples_per_line	= vbi_pixel_to_capture;
-	fh->vbi_fmt.sample_format	= V4L2_PIX_FMT_GREY;
-
-	/* fixme: this only works for PAL */
-	fh->vbi_fmt.start[0] = 5;
-	fh->vbi_fmt.count[0] = 16;
-	fh->vbi_fmt.start[1] = 312;
-	fh->vbi_fmt.count[1] = 16;
-
 	videobuf_queue_sg_init(&fh->vbi_q, &vbi_qops,
 			    &dev->pci->dev, &dev->slock,
 			    V4L2_BUF_TYPE_VBI_CAPTURE,
@@ -415,9 +402,8 @@ static int vbi_open(struct saa7146_dev *dev, struct file *file)
 			    sizeof(struct saa7146_buf),
 			    file, &dev->v4l2_lock);
 
-	init_timer(&fh->vbi_read_timeout);
-	fh->vbi_read_timeout.function = vbi_read_timeout;
-	fh->vbi_read_timeout.data = (unsigned long)file;
+	vv->vbi_read_timeout.function = vbi_read_timeout;
+	vv->vbi_read_timeout.data = (unsigned long)file;
 
 	/* initialize the brs */
 	if ( 0 != (SAA7146_USE_PORT_B_FOR_VBI & dev->ext_vv_data->flags)) {
@@ -453,16 +439,16 @@ static void vbi_irq_done(struct saa7146_dev *dev, unsigned long status)
 	struct saa7146_vv *vv = dev->vv_data;
 	spin_lock(&dev->slock);
 
-	if (vv->vbi_q.curr) {
-		DEB_VBI("dev:%p, curr:%p\n", dev, vv->vbi_q.curr);
+	if (vv->vbi_dmaq.curr) {
+		DEB_VBI("dev:%p, curr:%p\n", dev, vv->vbi_dmaq.curr);
 		/* this must be += 2, one count for each field */
 		vv->vbi_fieldcount+=2;
-		vv->vbi_q.curr->vb.field_count = vv->vbi_fieldcount;
-		saa7146_buffer_finish(dev,&vv->vbi_q,VIDEOBUF_DONE);
+		vv->vbi_dmaq.curr->vb.field_count = vv->vbi_fieldcount;
+		saa7146_buffer_finish(dev, &vv->vbi_dmaq, VIDEOBUF_DONE);
 	} else {
 		DEB_VBI("dev:%p\n", dev);
 	}
-	saa7146_buffer_next(dev,&vv->vbi_q,1);
+	saa7146_buffer_next(dev, &vv->vbi_dmaq, 1);
 
 	spin_unlock(&dev->slock);
 }
@@ -488,7 +474,7 @@ static ssize_t vbi_read(struct file *file, char __user *data, size_t count, loff
 		return -EBUSY;
 	}
 
-	mod_timer(&fh->vbi_read_timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&vv->vbi_read_timeout, jiffies+BUFFER_TIMEOUT);
 	ret = videobuf_read_stream(&fh->vbi_q, data, count, ppos, 1,
 				   file->f_flags & O_NONBLOCK);
 /*
diff --git a/drivers/media/common/saa7146_video.c b/drivers/media/common/saa7146_video.c
index ce30533fd972..6d14785d4747 100644
--- a/drivers/media/common/saa7146_video.c
+++ b/drivers/media/common/saa7146_video.c
@@ -2,6 +2,8 @@
 
 #include <media/saa7146_vv.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
 #include <linux/module.h>
 
 static int max_memory = 32;
@@ -112,8 +114,8 @@ int saa7146_start_preview(struct saa7146_fh *fh)
 
 	DEB_EE("dev:%p, fh:%p\n", dev, fh);
 
-	/* check if we have overlay informations */
-	if( NULL == fh->ov.fh ) {
+	/* check if we have overlay information */
+	if (vv->ov.fh == NULL) {
 		DEB_D("no overlay data available. try S_FMT first.\n");
 		return -EAGAIN;
 	}
@@ -139,19 +141,18 @@ int saa7146_start_preview(struct saa7146_fh *fh)
 		return -EBUSY;
 	}
 
-	fmt.fmt.win = fh->ov.win;
+	fmt.fmt.win = vv->ov.win;
 	err = vidioc_try_fmt_vid_overlay(NULL, fh, &fmt);
 	if (0 != err) {
 		saa7146_res_free(vv->video_fh, RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP);
 		return -EBUSY;
 	}
-	fh->ov.win = fmt.fmt.win;
-	vv->ov_data = &fh->ov;
+	vv->ov.win = fmt.fmt.win;
 
 	DEB_D("%dx%d+%d+%d %s field=%s\n",
-	      fh->ov.win.w.width, fh->ov.win.w.height,
-	      fh->ov.win.w.left, fh->ov.win.w.top,
-	      vv->ov_fmt->name, v4l2_field_names[fh->ov.win.field]);
+	      vv->ov.win.w.width, vv->ov.win.w.height,
+	      vv->ov.win.w.left, vv->ov.win.w.top,
+	      vv->ov_fmt->name, v4l2_field_names[vv->ov.win.field]);
 
 	if (0 != (ret = saa7146_enable_overlay(fh))) {
 		DEB_D("enabling overlay failed: %d\n", ret);
@@ -202,65 +203,6 @@ int saa7146_stop_preview(struct saa7146_fh *fh)
 EXPORT_SYMBOL_GPL(saa7146_stop_preview);
 
 /********************************************************************************/
-/* device controls */
-
-static struct v4l2_queryctrl controls[] = {
-	{
-		.id		= V4L2_CID_BRIGHTNESS,
-		.name		= "Brightness",
-		.minimum	= 0,
-		.maximum	= 255,
-		.step		= 1,
-		.default_value	= 128,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.flags 		= V4L2_CTRL_FLAG_SLIDER,
-	},{
-		.id		= V4L2_CID_CONTRAST,
-		.name		= "Contrast",
-		.minimum	= 0,
-		.maximum	= 127,
-		.step		= 1,
-		.default_value	= 64,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.flags 		= V4L2_CTRL_FLAG_SLIDER,
-	},{
-		.id		= V4L2_CID_SATURATION,
-		.name		= "Saturation",
-		.minimum	= 0,
-		.maximum	= 127,
-		.step		= 1,
-		.default_value	= 64,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.flags 		= V4L2_CTRL_FLAG_SLIDER,
-	},{
-		.id		= V4L2_CID_VFLIP,
-		.name		= "Vertical Flip",
-		.minimum	= 0,
-		.maximum	= 1,
-		.type		= V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id		= V4L2_CID_HFLIP,
-		.name		= "Horizontal Flip",
-		.minimum	= 0,
-		.maximum	= 1,
-		.type		= V4L2_CTRL_TYPE_BOOLEAN,
-	},
-};
-static int NUM_CONTROLS = sizeof(controls)/sizeof(struct v4l2_queryctrl);
-
-#define V4L2_CID_PRIVATE_LASTP1      (V4L2_CID_PRIVATE_BASE + 0)
-
-static struct v4l2_queryctrl* ctrl_by_id(int id)
-{
-	int i;
-
-	for (i = 0; i < NUM_CONTROLS; i++)
-		if (controls[i].id == id)
-			return controls+i;
-	return NULL;
-}
-
-/********************************************************************************/
 /* common pagetable functions */
 
 static int saa7146_pgtable_build(struct saa7146_dev *dev, struct saa7146_buf *buf)
@@ -413,7 +355,7 @@ static int video_begin(struct saa7146_fh *fh)
 		}
 	}
 
-	fmt = saa7146_format_by_fourcc(dev,fh->video_fmt.pixelformat);
+	fmt = saa7146_format_by_fourcc(dev, vv->video_fmt.pixelformat);
 	/* we need to have a valid format set here */
 	BUG_ON(NULL == fmt);
 
@@ -465,7 +407,7 @@ static int video_end(struct saa7146_fh *fh, struct file *file)
 		return -EBUSY;
 	}
 
-	fmt = saa7146_format_by_fourcc(dev,fh->video_fmt.pixelformat);
+	fmt = saa7146_format_by_fourcc(dev, vv->video_fmt.pixelformat);
 	/* we need to have a valid format set here */
 	BUG_ON(NULL == fmt);
 
@@ -504,18 +446,25 @@ static int video_end(struct saa7146_fh *fh, struct file *file)
 
 static int vidioc_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
 	strcpy((char *)cap->driver, "saa7146 v4l2");
 	strlcpy((char *)cap->card, dev->ext->name, sizeof(cap->card));
 	sprintf((char *)cap->bus_info, "PCI:%s", pci_name(dev->pci));
-	cap->version = SAA7146_VERSION_CODE;
-	cap->capabilities =
+	cap->device_caps =
 		V4L2_CAP_VIDEO_CAPTURE |
 		V4L2_CAP_VIDEO_OVERLAY |
 		V4L2_CAP_READWRITE |
 		V4L2_CAP_STREAMING;
-	cap->capabilities |= dev->ext_vv_data->capabilities;
+	cap->device_caps |= dev->ext_vv_data->capabilities;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER)
+		cap->device_caps &=
+			~(V4L2_CAP_VBI_CAPTURE | V4L2_CAP_SLICED_VBI_OUTPUT);
+	else
+		cap->device_caps &=
+			~(V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY | V4L2_CAP_AUDIO);
 	return 0;
 }
 
@@ -526,6 +475,7 @@ static int vidioc_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *f
 
 	*fb = vv->ov_fb;
 	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	fb->flags = V4L2_FBUF_FLAG_PRIMARY;
 	return 0;
 }
 
@@ -579,135 +529,58 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtd
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *c)
+int saa7146_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	const struct v4l2_queryctrl *ctrl;
-
-	if ((c->id <  V4L2_CID_BASE ||
-	     c->id >= V4L2_CID_LASTP1) &&
-	    (c->id <  V4L2_CID_PRIVATE_BASE ||
-	     c->id >= V4L2_CID_PRIVATE_LASTP1))
-		return -EINVAL;
-
-	ctrl = ctrl_by_id(c->id);
-	if (ctrl == NULL)
-		return -EINVAL;
-
-	DEB_EE("VIDIOC_QUERYCTRL: id:%d\n", c->id);
-	*c = *ctrl;
-	return 0;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_dev *dev = container_of(ctrl->handler,
+				struct saa7146_dev, ctrl_handler);
 	struct saa7146_vv *vv = dev->vv_data;
-	const struct v4l2_queryctrl *ctrl;
-	u32 value = 0;
+	u32 val;
 
-	ctrl = ctrl_by_id(c->id);
-	if (NULL == ctrl)
-		return -EINVAL;
-	switch (c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		value = saa7146_read(dev, BCS_CTRL);
-		c->value = 0xff & (value >> 24);
-		DEB_D("V4L2_CID_BRIGHTNESS: %d\n", c->value);
-		break;
-	case V4L2_CID_CONTRAST:
-		value = saa7146_read(dev, BCS_CTRL);
-		c->value = 0x7f & (value >> 16);
-		DEB_D("V4L2_CID_CONTRAST: %d\n", c->value);
-		break;
-	case V4L2_CID_SATURATION:
-		value = saa7146_read(dev, BCS_CTRL);
-		c->value = 0x7f & (value >> 0);
-		DEB_D("V4L2_CID_SATURATION: %d\n", c->value);
-		break;
-	case V4L2_CID_VFLIP:
-		c->value = vv->vflip;
-		DEB_D("V4L2_CID_VFLIP: %d\n", c->value);
-		break;
-	case V4L2_CID_HFLIP:
-		c->value = vv->hflip;
-		DEB_D("V4L2_CID_HFLIP: %d\n", c->value);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *c)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct saa7146_vv *vv = dev->vv_data;
-	const struct v4l2_queryctrl *ctrl;
-
-	ctrl = ctrl_by_id(c->id);
-	if (NULL == ctrl) {
-		DEB_D("unknown control %d\n", c->id);
-		return -EINVAL;
-	}
-
-	switch (ctrl->type) {
-	case V4L2_CTRL_TYPE_BOOLEAN:
-	case V4L2_CTRL_TYPE_MENU:
-	case V4L2_CTRL_TYPE_INTEGER:
-		if (c->value < ctrl->minimum)
-			c->value = ctrl->minimum;
-		if (c->value > ctrl->maximum)
-			c->value = ctrl->maximum;
-		break;
-	default:
-		/* nothing */;
-	}
-
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS: {
-		u32 value = saa7146_read(dev, BCS_CTRL);
-		value &= 0x00ffffff;
-		value |= (c->value << 24);
-		saa7146_write(dev, BCS_CTRL, value);
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0x00ffffff;
+		val |= (ctrl->val << 24);
+		saa7146_write(dev, BCS_CTRL, val);
 		saa7146_write(dev, MC2, MASK_22 | MASK_06);
 		break;
-	}
-	case V4L2_CID_CONTRAST: {
-		u32 value = saa7146_read(dev, BCS_CTRL);
-		value &= 0xff00ffff;
-		value |= (c->value << 16);
-		saa7146_write(dev, BCS_CTRL, value);
+
+	case V4L2_CID_CONTRAST:
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0xff00ffff;
+		val |= (ctrl->val << 16);
+		saa7146_write(dev, BCS_CTRL, val);
 		saa7146_write(dev, MC2, MASK_22 | MASK_06);
 		break;
-	}
-	case V4L2_CID_SATURATION: {
-		u32 value = saa7146_read(dev, BCS_CTRL);
-		value &= 0xffffff00;
-		value |= (c->value << 0);
-		saa7146_write(dev, BCS_CTRL, value);
+
+	case V4L2_CID_SATURATION:
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0xffffff00;
+		val |= (ctrl->val << 0);
+		saa7146_write(dev, BCS_CTRL, val);
 		saa7146_write(dev, MC2, MASK_22 | MASK_06);
 		break;
-	}
+
 	case V4L2_CID_HFLIP:
 		/* fixme: we can support changing VFLIP and HFLIP here... */
-		if (IS_CAPTURE_ACTIVE(fh) != 0) {
-			DEB_D("V4L2_CID_HFLIP while active capture\n");
+		if ((vv->video_status & STATUS_CAPTURE))
 			return -EBUSY;
-		}
-		vv->hflip = c->value;
+		vv->hflip = ctrl->val;
 		break;
+
 	case V4L2_CID_VFLIP:
-		if (IS_CAPTURE_ACTIVE(fh) != 0) {
-			DEB_D("V4L2_CID_VFLIP while active capture\n");
+		if ((vv->video_status & STATUS_CAPTURE))
 			return -EBUSY;
-		}
-		vv->vflip = c->value;
+		vv->vflip = ctrl->val;
 		break;
+
 	default:
 		return -EINVAL;
 	}
 
-	if (IS_OVERLAY_ACTIVE(fh) != 0) {
+	if ((vv->video_status & STATUS_OVERLAY) != 0) { /* CHECK: && (vv->video_fh == fh)) */
+		struct saa7146_fh *fh = vv->video_fh;
+
 		saa7146_stop_preview(fh);
 		saa7146_start_preview(fh);
 	}
@@ -720,6 +593,8 @@ static int vidioc_g_parm(struct file *file, void *fh,
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct saa7146_vv *vv = dev->vv_data;
 
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 	parm->parm.capture.readbuffers = 1;
 	v4l2_video_std_frame_period(vv->standard->id,
 				    &parm->parm.capture.timeperframe);
@@ -728,19 +603,28 @@ static int vidioc_g_parm(struct file *file, void *fh,
 
 static int vidioc_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 {
-	f->fmt.pix = ((struct saa7146_fh *)fh)->video_fmt;
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.pix = vv->video_fmt;
 	return 0;
 }
 
 static int vidioc_g_fmt_vid_overlay(struct file *file, void *fh, struct v4l2_format *f)
 {
-	f->fmt.win = ((struct saa7146_fh *)fh)->ov.win;
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.win = vv->ov.win;
 	return 0;
 }
 
 static int vidioc_g_fmt_vbi_cap(struct file *file, void *fh, struct v4l2_format *f)
 {
-	f->fmt.vbi = ((struct saa7146_fh *)fh)->vbi_fmt;
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.vbi = vv->vbi_fmt;
 	return 0;
 }
 
@@ -787,6 +671,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_forma
 	}
 
 	f->fmt.pix.field = field;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	if (f->fmt.pix.width > maxw)
 		f->fmt.pix.width = maxw;
 	if (f->fmt.pix.height > maxh)
@@ -883,9 +768,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *__fh, struct v4l2_forma
 	err = vidioc_try_fmt_vid_cap(file, fh, f);
 	if (0 != err)
 		return err;
-	fh->video_fmt = f->fmt.pix;
+	vv->video_fmt = f->fmt.pix;
 	DEB_EE("set to pixelformat '%4.4s'\n",
-	       (char *)&fh->video_fmt.pixelformat);
+	       (char *)&vv->video_fmt.pixelformat);
 	return 0;
 }
 
@@ -900,17 +785,17 @@ static int vidioc_s_fmt_vid_overlay(struct file *file, void *__fh, struct v4l2_f
 	err = vidioc_try_fmt_vid_overlay(file, fh, f);
 	if (0 != err)
 		return err;
-	fh->ov.win    = f->fmt.win;
-	fh->ov.nclips = f->fmt.win.clipcount;
-	if (fh->ov.nclips > 16)
-		fh->ov.nclips = 16;
-	if (copy_from_user(fh->ov.clips, f->fmt.win.clips,
-				sizeof(struct v4l2_clip) * fh->ov.nclips)) {
+	vv->ov.win    = f->fmt.win;
+	vv->ov.nclips = f->fmt.win.clipcount;
+	if (vv->ov.nclips > 16)
+		vv->ov.nclips = 16;
+	if (copy_from_user(vv->ov.clips, f->fmt.win.clips,
+				sizeof(struct v4l2_clip) * vv->ov.nclips)) {
 		return -EFAULT;
 	}
 
-	/* fh->ov.fh is used to indicate that we have valid overlay informations, too */
-	fh->ov.fh = fh;
+	/* vv->ov.fh is used to indicate that we have valid overlay informations, too */
+	vv->ov.fh = fh;
 
 	/* check if our current overlay is active */
 	if (IS_OVERLAY_ACTIVE(fh) != 0) {
@@ -1111,10 +996,14 @@ static int vidioc_g_chip_ident(struct file *file, void *__fh,
 
 	chip->ident = V4L2_IDENT_NONE;
 	chip->revision = 0;
-	if (chip->match.type == V4L2_CHIP_MATCH_HOST && !chip->match.addr) {
-		chip->ident = V4L2_IDENT_SAA7146;
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
+		if (v4l2_chip_match_host(&chip->match))
+			chip->ident = V4L2_IDENT_SAA7146;
 		return 0;
 	}
+	if (chip->match.type != V4L2_CHIP_MATCH_I2C_DRIVER &&
+	    chip->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
 	return v4l2_device_call_until_err(&dev->v4l2_dev, 0,
 			core, g_chip_ident, chip);
 }
@@ -1129,7 +1018,6 @@ const struct v4l2_ioctl_ops saa7146_video_ioctl_ops = {
 	.vidioc_g_fmt_vid_overlay    = vidioc_g_fmt_vid_overlay,
 	.vidioc_try_fmt_vid_overlay  = vidioc_try_fmt_vid_overlay,
 	.vidioc_s_fmt_vid_overlay    = vidioc_s_fmt_vid_overlay,
-	.vidioc_g_fmt_vbi_cap        = vidioc_g_fmt_vbi_cap,
 	.vidioc_g_chip_ident         = vidioc_g_chip_ident,
 
 	.vidioc_overlay 	     = vidioc_overlay,
@@ -1141,12 +1029,29 @@ const struct v4l2_ioctl_ops saa7146_video_ioctl_ops = {
 	.vidioc_dqbuf                = vidioc_dqbuf,
 	.vidioc_g_std                = vidioc_g_std,
 	.vidioc_s_std                = vidioc_s_std,
-	.vidioc_queryctrl            = vidioc_queryctrl,
-	.vidioc_g_ctrl               = vidioc_g_ctrl,
-	.vidioc_s_ctrl               = vidioc_s_ctrl,
 	.vidioc_streamon             = vidioc_streamon,
 	.vidioc_streamoff            = vidioc_streamoff,
 	.vidioc_g_parm 		     = vidioc_g_parm,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
+};
+
+const struct v4l2_ioctl_ops saa7146_vbi_ioctl_ops = {
+	.vidioc_querycap             = vidioc_querycap,
+	.vidioc_g_fmt_vbi_cap        = vidioc_g_fmt_vbi_cap,
+	.vidioc_g_chip_ident         = vidioc_g_chip_ident,
+
+	.vidioc_reqbufs              = vidioc_reqbufs,
+	.vidioc_querybuf             = vidioc_querybuf,
+	.vidioc_qbuf                 = vidioc_qbuf,
+	.vidioc_dqbuf                = vidioc_dqbuf,
+	.vidioc_g_std                = vidioc_g_std,
+	.vidioc_s_std                = vidioc_s_std,
+	.vidioc_streamon             = vidioc_streamon,
+	.vidioc_streamoff            = vidioc_streamoff,
+	.vidioc_g_parm		     = vidioc_g_parm,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
 };
 
 /*********************************************************************************/
@@ -1161,7 +1066,7 @@ static int buffer_activate (struct saa7146_dev *dev,
 	buf->vb.state = VIDEOBUF_ACTIVE;
 	saa7146_set_capture(dev,buf,next);
 
-	mod_timer(&vv->video_q.timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&vv->video_dmaq.timeout, jiffies+BUFFER_TIMEOUT);
 	return 0;
 }
 
@@ -1185,44 +1090,44 @@ static int buffer_prepare(struct videobuf_queue *q,
 	DEB_CAP("vbuf:%p\n", vb);
 
 	/* sanity checks */
-	if (fh->video_fmt.width  < 48 ||
-	    fh->video_fmt.height < 32 ||
-	    fh->video_fmt.width  > vv->standard->h_max_out ||
-	    fh->video_fmt.height > vv->standard->v_max_out) {
+	if (vv->video_fmt.width  < 48 ||
+	    vv->video_fmt.height < 32 ||
+	    vv->video_fmt.width  > vv->standard->h_max_out ||
+	    vv->video_fmt.height > vv->standard->v_max_out) {
 		DEB_D("w (%d) / h (%d) out of bounds\n",
-		      fh->video_fmt.width, fh->video_fmt.height);
+		      vv->video_fmt.width, vv->video_fmt.height);
 		return -EINVAL;
 	}
 
-	size = fh->video_fmt.sizeimage;
+	size = vv->video_fmt.sizeimage;
 	if (0 != buf->vb.baddr && buf->vb.bsize < size) {
 		DEB_D("size mismatch\n");
 		return -EINVAL;
 	}
 
 	DEB_CAP("buffer_prepare [size=%dx%d,bytes=%d,fields=%s]\n",
-		fh->video_fmt.width, fh->video_fmt.height,
-		size, v4l2_field_names[fh->video_fmt.field]);
-	if (buf->vb.width  != fh->video_fmt.width  ||
-	    buf->vb.bytesperline != fh->video_fmt.bytesperline ||
-	    buf->vb.height != fh->video_fmt.height ||
+		vv->video_fmt.width, vv->video_fmt.height,
+		size, v4l2_field_names[vv->video_fmt.field]);
+	if (buf->vb.width  != vv->video_fmt.width  ||
+	    buf->vb.bytesperline != vv->video_fmt.bytesperline ||
+	    buf->vb.height != vv->video_fmt.height ||
 	    buf->vb.size   != size ||
 	    buf->vb.field  != field      ||
-	    buf->vb.field  != fh->video_fmt.field  ||
-	    buf->fmt       != &fh->video_fmt) {
+	    buf->vb.field  != vv->video_fmt.field  ||
+	    buf->fmt       != &vv->video_fmt) {
 		saa7146_dma_free(dev,q,buf);
 	}
 
 	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
 		struct saa7146_format *sfmt;
 
-		buf->vb.bytesperline  = fh->video_fmt.bytesperline;
-		buf->vb.width  = fh->video_fmt.width;
-		buf->vb.height = fh->video_fmt.height;
+		buf->vb.bytesperline  = vv->video_fmt.bytesperline;
+		buf->vb.width  = vv->video_fmt.width;
+		buf->vb.height = vv->video_fmt.height;
 		buf->vb.size   = size;
 		buf->vb.field  = field;
-		buf->fmt       = &fh->video_fmt;
-		buf->vb.field  = fh->video_fmt.field;
+		buf->fmt       = &vv->video_fmt;
+		buf->vb.field  = vv->video_fmt.field;
 
 		sfmt = saa7146_format_by_fourcc(dev,buf->fmt->pixelformat);
 
@@ -1258,11 +1163,12 @@ static int buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned
 {
 	struct file *file = q->priv_data;
 	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = fh->dev->vv_data;
 
 	if (0 == *count || *count > MAX_SAA7146_CAPTURE_BUFFERS)
 		*count = MAX_SAA7146_CAPTURE_BUFFERS;
 
-	*size = fh->video_fmt.sizeimage;
+	*size = vv->video_fmt.sizeimage;
 
 	/* check if we exceed the "max_memory" parameter */
 	if( (*count * *size) > (max_memory*1048576) ) {
@@ -1283,7 +1189,7 @@ static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
 	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
 
 	DEB_CAP("vbuf:%p\n", vb);
-	saa7146_buffer_queue(fh->dev,&vv->video_q,buf);
+	saa7146_buffer_queue(fh->dev, &vv->video_dmaq, buf);
 }
 
 static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
@@ -1312,12 +1218,12 @@ static struct videobuf_queue_ops video_qops = {
 
 static void video_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 {
-	INIT_LIST_HEAD(&vv->video_q.queue);
+	INIT_LIST_HEAD(&vv->video_dmaq.queue);
 
-	init_timer(&vv->video_q.timeout);
-	vv->video_q.timeout.function = saa7146_buffer_timeout;
-	vv->video_q.timeout.data     = (unsigned long)(&vv->video_q);
-	vv->video_q.dev              = dev;
+	init_timer(&vv->video_dmaq.timeout);
+	vv->video_dmaq.timeout.function = saa7146_buffer_timeout;
+	vv->video_dmaq.timeout.data     = (unsigned long)(&vv->video_dmaq);
+	vv->video_dmaq.dev              = dev;
 
 	/* set some default values */
 	vv->standard = &dev->ext_vv_data->stds[0];
@@ -1331,15 +1237,6 @@ static void video_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 static int video_open(struct saa7146_dev *dev, struct file *file)
 {
 	struct saa7146_fh *fh = file->private_data;
-	struct saa7146_format *sfmt;
-
-	fh->video_fmt.width = 384;
-	fh->video_fmt.height = 288;
-	fh->video_fmt.pixelformat = V4L2_PIX_FMT_BGR24;
-	fh->video_fmt.bytesperline = 0;
-	fh->video_fmt.field = V4L2_FIELD_ANY;
-	sfmt = saa7146_format_by_fourcc(dev,fh->video_fmt.pixelformat);
-	fh->video_fmt.sizeimage = (fh->video_fmt.width * fh->video_fmt.height * sfmt->depth)/8;
 
 	videobuf_queue_sg_init(&fh->video_q, &video_qops,
 			    &dev->pci->dev, &dev->slock,
@@ -1371,7 +1268,7 @@ static void video_close(struct saa7146_dev *dev, struct file *file)
 static void video_irq_done(struct saa7146_dev *dev, unsigned long st)
 {
 	struct saa7146_vv *vv = dev->vv_data;
-	struct saa7146_dmaqueue *q = &vv->video_q;
+	struct saa7146_dmaqueue *q = &vv->video_dmaq;
 
 	spin_lock(&dev->slock);
 	DEB_CAP("called\n");
diff --git a/drivers/media/common/tuners/Kconfig b/drivers/media/common/tuners/Kconfig
index 4a6d5cef3964..bbf4945149a9 100644
--- a/drivers/media/common/tuners/Kconfig
+++ b/drivers/media/common/tuners/Kconfig
@@ -204,6 +204,27 @@ config MEDIA_TUNER_TDA18218
 	help
 	  NXP TDA18218 silicon tuner driver.
 
+config MEDIA_TUNER_FC0011
+	tristate "Fitipower FC0011 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Fitipower FC0011 silicon tuner driver.
+
+config MEDIA_TUNER_FC0012
+	tristate "Fitipower FC0012 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Fitipower FC0012 silicon tuner driver.
+
+config MEDIA_TUNER_FC0013
+	tristate "Fitipower FC0013 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Fitipower FC0013 silicon tuner driver.
+
 config MEDIA_TUNER_TDA18212
 	tristate "NXP TDA18212 silicon tuner"
 	depends on VIDEO_MEDIA && I2C
@@ -211,4 +232,10 @@ config MEDIA_TUNER_TDA18212
 	help
 	  NXP TDA18212 silicon tuner driver.
 
+config MEDIA_TUNER_TUA9001
+	tristate "Infineon TUA 9001 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Infineon TUA 9001 silicon tuner driver.
 endmenu
diff --git a/drivers/media/common/tuners/Makefile b/drivers/media/common/tuners/Makefile
index f80407eb8998..891b80e60808 100644
--- a/drivers/media/common/tuners/Makefile
+++ b/drivers/media/common/tuners/Makefile
@@ -28,6 +28,10 @@ obj-$(CONFIG_MEDIA_TUNER_MC44S803) += mc44s803.o
 obj-$(CONFIG_MEDIA_TUNER_MAX2165) += max2165.o
 obj-$(CONFIG_MEDIA_TUNER_TDA18218) += tda18218.o
 obj-$(CONFIG_MEDIA_TUNER_TDA18212) += tda18212.o
+obj-$(CONFIG_MEDIA_TUNER_TUA9001) += tua9001.o
+obj-$(CONFIG_MEDIA_TUNER_FC0011) += fc0011.o
+obj-$(CONFIG_MEDIA_TUNER_FC0012) += fc0012.o
+obj-$(CONFIG_MEDIA_TUNER_FC0013) += fc0013.o
 
 ccflags-y += -I$(srctree)/drivers/media/dvb/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb/frontends
diff --git a/drivers/media/common/tuners/fc0011.c b/drivers/media/common/tuners/fc0011.c
new file mode 100644
index 000000000000..e4882546c283
--- /dev/null
+++ b/drivers/media/common/tuners/fc0011.c
@@ -0,0 +1,524 @@
+/*
+ * Fitipower FC0011 tuner driver
+ *
+ * Copyright (C) 2012 Michael Buesch <m@bues.ch>
+ *
+ * Derived from FC0012 tuner driver:
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "fc0011.h"
+
+
+/* Tuner registers */
+enum {
+	FC11_REG_0,
+	FC11_REG_FA,		/* FA */
+	FC11_REG_FP,		/* FP */
+	FC11_REG_XINHI,		/* XIN high 8 bit */
+	FC11_REG_XINLO,		/* XIN low 8 bit */
+	FC11_REG_VCO,		/* VCO */
+	FC11_REG_VCOSEL,	/* VCO select */
+	FC11_REG_7,		/* Unknown tuner reg 7 */
+	FC11_REG_8,		/* Unknown tuner reg 8 */
+	FC11_REG_9,
+	FC11_REG_10,		/* Unknown tuner reg 10 */
+	FC11_REG_11,		/* Unknown tuner reg 11 */
+	FC11_REG_12,
+	FC11_REG_RCCAL,		/* RC calibrate */
+	FC11_REG_VCOCAL,	/* VCO calibrate */
+	FC11_REG_15,
+	FC11_REG_16,		/* Unknown tuner reg 16 */
+	FC11_REG_17,
+
+	FC11_NR_REGS,		/* Number of registers */
+};
+
+enum FC11_REG_VCOSEL_bits {
+	FC11_VCOSEL_2		= 0x08, /* VCO select 2 */
+	FC11_VCOSEL_1		= 0x10, /* VCO select 1 */
+	FC11_VCOSEL_CLKOUT	= 0x20, /* Fix clock out */
+	FC11_VCOSEL_BW7M	= 0x40, /* 7MHz bw */
+	FC11_VCOSEL_BW6M	= 0x80, /* 6MHz bw */
+};
+
+enum FC11_REG_RCCAL_bits {
+	FC11_RCCAL_FORCE	= 0x10, /* force */
+};
+
+enum FC11_REG_VCOCAL_bits {
+	FC11_VCOCAL_RUN		= 0,	/* VCO calibration run */
+	FC11_VCOCAL_VALUEMASK	= 0x3F,	/* VCO calibration value mask */
+	FC11_VCOCAL_OK		= 0x40,	/* VCO calibration Ok */
+	FC11_VCOCAL_RESET	= 0x80, /* VCO calibration reset */
+};
+
+
+struct fc0011_priv {
+	struct i2c_adapter *i2c;
+	u8 addr;
+
+	u32 frequency;
+	u32 bandwidth;
+};
+
+
+static int fc0011_writereg(struct fc0011_priv *priv, u8 reg, u8 val)
+{
+	u8 buf[2] = { reg, val };
+	struct i2c_msg msg = { .addr = priv->addr,
+		.flags = 0, .buf = buf, .len = 2 };
+
+	if (i2c_transfer(priv->i2c, &msg, 1) != 1) {
+		dev_err(&priv->i2c->dev,
+			"I2C write reg failed, reg: %02x, val: %02x\n",
+			reg, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int fc0011_readreg(struct fc0011_priv *priv, u8 reg, u8 *val)
+{
+	u8 dummy;
+	struct i2c_msg msg[2] = {
+		{ .addr = priv->addr,
+		  .flags = 0, .buf = &reg, .len = 1 },
+		{ .addr = priv->addr,
+		  .flags = I2C_M_RD, .buf = val ? : &dummy, .len = 1 },
+	};
+
+	if (i2c_transfer(priv->i2c, msg, 2) != 2) {
+		dev_err(&priv->i2c->dev,
+			"I2C read failed, reg: %02x\n", reg);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int fc0011_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+
+	return 0;
+}
+
+static int fc0011_init(struct dvb_frontend *fe)
+{
+	struct fc0011_priv *priv = fe->tuner_priv;
+	int err;
+
+	if (WARN_ON(!fe->callback))
+		return -EINVAL;
+
+	err = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			   FC0011_FE_CALLBACK_POWER, priv->addr);
+	if (err) {
+		dev_err(&priv->i2c->dev, "Power-on callback failed\n");
+		return err;
+	}
+	err = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			   FC0011_FE_CALLBACK_RESET, priv->addr);
+	if (err) {
+		dev_err(&priv->i2c->dev, "Reset callback failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+/* Initiate VCO calibration */
+static int fc0011_vcocal_trigger(struct fc0011_priv *priv)
+{
+	int err;
+
+	err = fc0011_writereg(priv, FC11_REG_VCOCAL, FC11_VCOCAL_RESET);
+	if (err)
+		return err;
+	err = fc0011_writereg(priv, FC11_REG_VCOCAL, FC11_VCOCAL_RUN);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+/* Read VCO calibration value */
+static int fc0011_vcocal_read(struct fc0011_priv *priv, u8 *value)
+{
+	int err;
+
+	err = fc0011_writereg(priv, FC11_REG_VCOCAL, FC11_VCOCAL_RUN);
+	if (err)
+		return err;
+	usleep_range(10000, 20000);
+	err = fc0011_readreg(priv, FC11_REG_VCOCAL, value);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static int fc0011_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct fc0011_priv *priv = fe->tuner_priv;
+	int err;
+	unsigned int i, vco_retries;
+	u32 freq = p->frequency / 1000;
+	u32 bandwidth = p->bandwidth_hz / 1000;
+	u32 fvco, xin, xdiv, xdivr;
+	u16 frac;
+	u8 fa, fp, vco_sel, vco_cal;
+	u8 regs[FC11_NR_REGS] = { };
+
+	regs[FC11_REG_7] = 0x0F;
+	regs[FC11_REG_8] = 0x3E;
+	regs[FC11_REG_10] = 0xB8;
+	regs[FC11_REG_11] = 0x80;
+	regs[FC11_REG_RCCAL] = 0x04;
+	err = fc0011_writereg(priv, FC11_REG_7, regs[FC11_REG_7]);
+	err |= fc0011_writereg(priv, FC11_REG_8, regs[FC11_REG_8]);
+	err |= fc0011_writereg(priv, FC11_REG_10, regs[FC11_REG_10]);
+	err |= fc0011_writereg(priv, FC11_REG_11, regs[FC11_REG_11]);
+	err |= fc0011_writereg(priv, FC11_REG_RCCAL, regs[FC11_REG_RCCAL]);
+	if (err)
+		return -EIO;
+
+	/* Set VCO freq and VCO div */
+	if (freq < 54000) {
+		fvco = freq * 64;
+		regs[FC11_REG_VCO] = 0x82;
+	} else if (freq < 108000) {
+		fvco = freq * 32;
+		regs[FC11_REG_VCO] = 0x42;
+	} else if (freq < 216000) {
+		fvco = freq * 16;
+		regs[FC11_REG_VCO] = 0x22;
+	} else if (freq < 432000) {
+		fvco = freq * 8;
+		regs[FC11_REG_VCO] = 0x12;
+	} else {
+		fvco = freq * 4;
+		regs[FC11_REG_VCO] = 0x0A;
+	}
+
+	/* Calc XIN. The PLL reference frequency is 18 MHz. */
+	xdiv = fvco / 18000;
+	frac = fvco - xdiv * 18000;
+	frac = (frac << 15) / 18000;
+	if (frac >= 16384)
+		frac += 32786;
+	if (!frac)
+		xin = 0;
+	else if (frac < 511)
+		xin = 512;
+	else if (frac < 65026)
+		xin = frac;
+	else
+		xin = 65024;
+	regs[FC11_REG_XINHI] = xin >> 8;
+	regs[FC11_REG_XINLO] = xin;
+
+	/* Calc FP and FA */
+	xdivr = xdiv;
+	if (fvco - xdiv * 18000 >= 9000)
+		xdivr += 1; /* round */
+	fp = xdivr / 8;
+	fa = xdivr - fp * 8;
+	if (fa < 2) {
+		fp -= 1;
+		fa += 8;
+	}
+	if (fp > 0x1F) {
+		fp &= 0x1F;
+		fa &= 0xF;
+	}
+	if (fa >= fp) {
+		dev_warn(&priv->i2c->dev,
+			 "fa %02X >= fp %02X, but trying to continue\n",
+			 (unsigned int)(u8)fa, (unsigned int)(u8)fp);
+	}
+	regs[FC11_REG_FA] = fa;
+	regs[FC11_REG_FP] = fp;
+
+	/* Select bandwidth */
+	switch (bandwidth) {
+	case 8000:
+		break;
+	case 7000:
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_BW7M;
+		break;
+	default:
+		dev_warn(&priv->i2c->dev, "Unsupported bandwidth %u kHz. "
+			 "Using 6000 kHz.\n",
+			 bandwidth);
+		bandwidth = 6000;
+		/* fallthrough */
+	case 6000:
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_BW6M;
+		break;
+	}
+
+	/* Pre VCO select */
+	if (fvco < 2320000) {
+		vco_sel = 0;
+		regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+	} else if (fvco < 3080000) {
+		vco_sel = 1;
+		regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+	} else {
+		vco_sel = 2;
+		regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_2;
+	}
+
+	/* Fix for low freqs */
+	if (freq < 45000) {
+		regs[FC11_REG_FA] = 0x6;
+		regs[FC11_REG_FP] = 0x11;
+	}
+
+	/* Clock out fix */
+	regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_CLKOUT;
+
+	/* Write the cached registers */
+	for (i = FC11_REG_FA; i <= FC11_REG_VCOSEL; i++) {
+		err = fc0011_writereg(priv, i, regs[i]);
+		if (err)
+			return err;
+	}
+
+	/* VCO calibration */
+	err = fc0011_vcocal_trigger(priv);
+	if (err)
+		return err;
+	err = fc0011_vcocal_read(priv, &vco_cal);
+	if (err)
+		return err;
+	vco_retries = 0;
+	while (!(vco_cal & FC11_VCOCAL_OK) && vco_retries < 3) {
+		/* Reset the tuner and try again */
+		err = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+				   FC0011_FE_CALLBACK_RESET, priv->addr);
+		if (err) {
+			dev_err(&priv->i2c->dev, "Failed to reset tuner\n");
+			return err;
+		}
+		/* Reinit tuner config */
+		err = 0;
+		for (i = FC11_REG_FA; i <= FC11_REG_VCOSEL; i++)
+			err |= fc0011_writereg(priv, i, regs[i]);
+		err |= fc0011_writereg(priv, FC11_REG_7, regs[FC11_REG_7]);
+		err |= fc0011_writereg(priv, FC11_REG_8, regs[FC11_REG_8]);
+		err |= fc0011_writereg(priv, FC11_REG_10, regs[FC11_REG_10]);
+		err |= fc0011_writereg(priv, FC11_REG_11, regs[FC11_REG_11]);
+		err |= fc0011_writereg(priv, FC11_REG_RCCAL, regs[FC11_REG_RCCAL]);
+		if (err)
+			return -EIO;
+		/* VCO calibration */
+		err = fc0011_vcocal_trigger(priv);
+		if (err)
+			return err;
+		err = fc0011_vcocal_read(priv, &vco_cal);
+		if (err)
+			return err;
+		vco_retries++;
+	}
+	if (!(vco_cal & FC11_VCOCAL_OK)) {
+		dev_err(&priv->i2c->dev,
+			"Failed to read VCO calibration value (got %02X)\n",
+			(unsigned int)vco_cal);
+		return -EIO;
+	}
+	vco_cal &= FC11_VCOCAL_VALUEMASK;
+
+	switch (vco_sel) {
+	case 0:
+		if (vco_cal < 8) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		} else {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+		}
+		break;
+	case 1:
+		if (vco_cal < 5) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_2;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		} else if (vco_cal <= 48) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+		} else {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		}
+		break;
+	case 2:
+		if (vco_cal > 53) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		} else {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_2;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+		}
+		break;
+	}
+	err = fc0011_vcocal_read(priv, NULL);
+	if (err)
+		return err;
+	usleep_range(10000, 50000);
+
+	err = fc0011_readreg(priv, FC11_REG_RCCAL, &regs[FC11_REG_RCCAL]);
+	if (err)
+		return err;
+	regs[FC11_REG_RCCAL] |= FC11_RCCAL_FORCE;
+	err = fc0011_writereg(priv, FC11_REG_RCCAL, regs[FC11_REG_RCCAL]);
+	if (err)
+		return err;
+	err = fc0011_writereg(priv, FC11_REG_16, 0xB);
+	if (err)
+		return err;
+
+	dev_dbg(&priv->i2c->dev, "Tuned to "
+		"fa=%02X fp=%02X xin=%02X%02X vco=%02X vcosel=%02X "
+		"vcocal=%02X(%u) bw=%u\n",
+		(unsigned int)regs[FC11_REG_FA],
+		(unsigned int)regs[FC11_REG_FP],
+		(unsigned int)regs[FC11_REG_XINHI],
+		(unsigned int)regs[FC11_REG_XINLO],
+		(unsigned int)regs[FC11_REG_VCO],
+		(unsigned int)regs[FC11_REG_VCOSEL],
+		(unsigned int)vco_cal, vco_retries,
+		(unsigned int)bandwidth);
+
+	priv->frequency = p->frequency;
+	priv->bandwidth = p->bandwidth_hz;
+
+	return 0;
+}
+
+static int fc0011_get_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct fc0011_priv *priv = fe->tuner_priv;
+
+	*frequency = priv->frequency;
+
+	return 0;
+}
+
+static int fc0011_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	*frequency = 0;
+
+	return 0;
+}
+
+static int fc0011_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
+{
+	struct fc0011_priv *priv = fe->tuner_priv;
+
+	*bandwidth = priv->bandwidth;
+
+	return 0;
+}
+
+static const struct dvb_tuner_ops fc0011_tuner_ops = {
+	.info = {
+		.name		= "Fitipower FC0011",
+
+		.frequency_min	= 45000000,
+		.frequency_max	= 1000000000,
+	},
+
+	.release		= fc0011_release,
+	.init			= fc0011_init,
+
+	.set_params		= fc0011_set_params,
+
+	.get_frequency		= fc0011_get_frequency,
+	.get_if_frequency	= fc0011_get_if_frequency,
+	.get_bandwidth		= fc0011_get_bandwidth,
+};
+
+struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
+				   struct i2c_adapter *i2c,
+				   const struct fc0011_config *config)
+{
+	struct fc0011_priv *priv;
+
+	priv = kzalloc(sizeof(struct fc0011_priv), GFP_KERNEL);
+	if (!priv)
+		return NULL;
+
+	priv->i2c = i2c;
+	priv->addr = config->i2c_address;
+
+	fe->tuner_priv = priv;
+	fe->ops.tuner_ops = fc0011_tuner_ops;
+
+	dev_info(&priv->i2c->dev, "Fitipower FC0011 tuner attached\n");
+
+	return fe;
+}
+EXPORT_SYMBOL(fc0011_attach);
+
+MODULE_DESCRIPTION("Fitipower FC0011 silicon tuner driver");
+MODULE_AUTHOR("Michael Buesch <m@bues.ch>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/tuners/fc0011.h b/drivers/media/common/tuners/fc0011.h
new file mode 100644
index 000000000000..0ee581f122d2
--- /dev/null
+++ b/drivers/media/common/tuners/fc0011.h
@@ -0,0 +1,41 @@
+#ifndef LINUX_FC0011_H_
+#define LINUX_FC0011_H_
+
+#include "dvb_frontend.h"
+
+
+/** struct fc0011_config - fc0011 hardware config
+ *
+ * @i2c_address: I2C bus address.
+ */
+struct fc0011_config {
+	u8 i2c_address;
+};
+
+/** enum fc0011_fe_callback_commands - Frontend callbacks
+ *
+ * @FC0011_FE_CALLBACK_POWER: Power on tuner hardware.
+ * @FC0011_FE_CALLBACK_RESET: Request a tuner reset.
+ */
+enum fc0011_fe_callback_commands {
+	FC0011_FE_CALLBACK_POWER,
+	FC0011_FE_CALLBACK_RESET,
+};
+
+#if defined(CONFIG_MEDIA_TUNER_FC0011) ||\
+    defined(CONFIG_MEDIA_TUNER_FC0011_MODULE)
+struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
+				   struct i2c_adapter *i2c,
+				   const struct fc0011_config *config);
+#else
+static inline
+struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
+				   struct i2c_adapter *i2c,
+				   const struct fc0011_config *config)
+{
+	dev_err(&i2c->dev, "fc0011 driver disabled in Kconfig\n");
+	return NULL;
+}
+#endif
+
+#endif /* LINUX_FC0011_H_ */
diff --git a/drivers/media/common/tuners/fc0012-priv.h b/drivers/media/common/tuners/fc0012-priv.h
new file mode 100644
index 000000000000..4577c917e616
--- /dev/null
+++ b/drivers/media/common/tuners/fc0012-priv.h
@@ -0,0 +1,43 @@
+/*
+ * Fitipower FC0012 tuner driver - private includes
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _FC0012_PRIV_H_
+#define _FC0012_PRIV_H_
+
+#define LOG_PREFIX "fc0012"
+
+#undef err
+#define err(f, arg...)  printk(KERN_ERR     LOG_PREFIX": " f "\n" , ## arg)
+#undef info
+#define info(f, arg...) printk(KERN_INFO    LOG_PREFIX": " f "\n" , ## arg)
+#undef warn
+#define warn(f, arg...) printk(KERN_WARNING LOG_PREFIX": " f "\n" , ## arg)
+
+struct fc0012_priv {
+	struct i2c_adapter *i2c;
+	u8 addr;
+	u8 dual_master;
+	u8 xtal_freq;
+
+	u32 frequency;
+	u32 bandwidth;
+};
+
+#endif
diff --git a/drivers/media/common/tuners/fc0012.c b/drivers/media/common/tuners/fc0012.c
new file mode 100644
index 000000000000..308135abd54c
--- /dev/null
+++ b/drivers/media/common/tuners/fc0012.c
@@ -0,0 +1,467 @@
+/*
+ * Fitipower FC0012 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "fc0012.h"
+#include "fc0012-priv.h"
+
+static int fc0012_writereg(struct fc0012_priv *priv, u8 reg, u8 val)
+{
+	u8 buf[2] = {reg, val};
+	struct i2c_msg msg = {
+		.addr = priv->addr, .flags = 0, .buf = buf, .len = 2
+	};
+
+	if (i2c_transfer(priv->i2c, &msg, 1) != 1) {
+		err("I2C write reg failed, reg: %02x, val: %02x", reg, val);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0012_readreg(struct fc0012_priv *priv, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2] = {
+		{ .addr = priv->addr, .flags = 0, .buf = &reg, .len = 1 },
+		{ .addr = priv->addr, .flags = I2C_M_RD, .buf = val, .len = 1 },
+	};
+
+	if (i2c_transfer(priv->i2c, msg, 2) != 2) {
+		err("I2C read reg failed, reg: %02x", reg);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0012_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+	return 0;
+}
+
+static int fc0012_init(struct dvb_frontend *fe)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	unsigned char reg[] = {
+		0x00,	/* dummy reg. 0 */
+		0x05,	/* reg. 0x01 */
+		0x10,	/* reg. 0x02 */
+		0x00,	/* reg. 0x03 */
+		0x00,	/* reg. 0x04 */
+		0x0f,	/* reg. 0x05: may also be 0x0a */
+		0x00,	/* reg. 0x06: divider 2, VCO slow */
+		0x00,	/* reg. 0x07: may also be 0x0f */
+		0xff,	/* reg. 0x08: AGC Clock divide by 256, AGC gain 1/256,
+			   Loop Bw 1/8 */
+		0x6e,	/* reg. 0x09: Disable LoopThrough, Enable LoopThrough: 0x6f */
+		0xb8,	/* reg. 0x0a: Disable LO Test Buffer */
+		0x82,	/* reg. 0x0b: Output Clock is same as clock frequency,
+			   may also be 0x83 */
+		0xfc,	/* reg. 0x0c: depending on AGC Up-Down mode, may need 0xf8 */
+		0x02,	/* reg. 0x0d: AGC Not Forcing & LNA Forcing, 0x02 for DVB-T */
+		0x00,	/* reg. 0x0e */
+		0x00,	/* reg. 0x0f */
+		0x00,	/* reg. 0x10: may also be 0x0d */
+		0x00,	/* reg. 0x11 */
+		0x1f,	/* reg. 0x12: Set to maximum gain */
+		0x08,	/* reg. 0x13: Set to Middle Gain: 0x08,
+			   Low Gain: 0x00, High Gain: 0x10, enable IX2: 0x80 */
+		0x00,	/* reg. 0x14 */
+		0x04,	/* reg. 0x15: Enable LNA COMPS */
+	};
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+	case FC_XTAL_28_8_MHZ:
+		reg[0x07] |= 0x20;
+		break;
+	case FC_XTAL_36_MHZ:
+	default:
+		break;
+	}
+
+	if (priv->dual_master)
+		reg[0x0c] |= 0x02;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	for (i = 1; i < sizeof(reg); i++) {
+		ret = fc0012_writereg(priv, i, reg[i]);
+		if (ret)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (ret)
+		err("fc0012_writereg failed: %d", ret);
+
+	return ret;
+}
+
+static int fc0012_sleep(struct dvb_frontend *fe)
+{
+	/* nothing to do here */
+	return 0;
+}
+
+static int fc0012_set_params(struct dvb_frontend *fe)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	u32 freq = p->frequency / 1000;
+	u32 delsys = p->delivery_system;
+	unsigned char reg[7], am, pm, multi, tmp;
+	unsigned long f_vco;
+	unsigned short xtal_freq_khz_2, xin, xdiv;
+	int vco_select = false;
+
+	if (fe->callback) {
+		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			FC_FE_CALLBACK_VHF_ENABLE, (freq > 300000 ? 0 : 1));
+		if (ret)
+			goto exit;
+	}
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+		xtal_freq_khz_2 = 27000 / 2;
+		break;
+	case FC_XTAL_36_MHZ:
+		xtal_freq_khz_2 = 36000 / 2;
+		break;
+	case FC_XTAL_28_8_MHZ:
+	default:
+		xtal_freq_khz_2 = 28800 / 2;
+		break;
+	}
+
+	/* select frequency divider and the frequency of VCO */
+	if (freq < 37084) {		/* freq * 96 < 3560000 */
+		multi = 96;
+		reg[5] = 0x82;
+		reg[6] = 0x00;
+	} else if (freq < 55625) {	/* freq * 64 < 3560000 */
+		multi = 64;
+		reg[5] = 0x82;
+		reg[6] = 0x02;
+	} else if (freq < 74167) {	/* freq * 48 < 3560000 */
+		multi = 48;
+		reg[5] = 0x42;
+		reg[6] = 0x00;
+	} else if (freq < 111250) {	/* freq * 32 < 3560000 */
+		multi = 32;
+		reg[5] = 0x42;
+		reg[6] = 0x02;
+	} else if (freq < 148334) {	/* freq * 24 < 3560000 */
+		multi = 24;
+		reg[5] = 0x22;
+		reg[6] = 0x00;
+	} else if (freq < 222500) {	/* freq * 16 < 3560000 */
+		multi = 16;
+		reg[5] = 0x22;
+		reg[6] = 0x02;
+	} else if (freq < 296667) {	/* freq * 12 < 3560000 */
+		multi = 12;
+		reg[5] = 0x12;
+		reg[6] = 0x00;
+	} else if (freq < 445000) {	/* freq * 8 < 3560000 */
+		multi = 8;
+		reg[5] = 0x12;
+		reg[6] = 0x02;
+	} else if (freq < 593334) {	/* freq * 6 < 3560000 */
+		multi = 6;
+		reg[5] = 0x0a;
+		reg[6] = 0x00;
+	} else {
+		multi = 4;
+		reg[5] = 0x0a;
+		reg[6] = 0x02;
+	}
+
+	f_vco = freq * multi;
+
+	if (f_vco >= 3060000) {
+		reg[6] |= 0x08;
+		vco_select = true;
+	}
+
+	if (freq >= 45000) {
+		/* From divided value (XDIV) determined the FA and FP value */
+		xdiv = (unsigned short)(f_vco / xtal_freq_khz_2);
+		if ((f_vco - xdiv * xtal_freq_khz_2) >= (xtal_freq_khz_2 / 2))
+			xdiv++;
+
+		pm = (unsigned char)(xdiv / 8);
+		am = (unsigned char)(xdiv - (8 * pm));
+
+		if (am < 2) {
+			reg[1] = am + 8;
+			reg[2] = pm - 1;
+		} else {
+			reg[1] = am;
+			reg[2] = pm;
+		}
+	} else {
+		/* fix for frequency less than 45 MHz */
+		reg[1] = 0x06;
+		reg[2] = 0x11;
+	}
+
+	/* fix clock out */
+	reg[6] |= 0x20;
+
+	/* From VCO frequency determines the XIN ( fractional part of Delta
+	   Sigma PLL) and divided value (XDIV) */
+	xin = (unsigned short)(f_vco - (f_vco / xtal_freq_khz_2) * xtal_freq_khz_2);
+	xin = (xin << 15) / xtal_freq_khz_2;
+	if (xin >= 16384)
+		xin += 32768;
+
+	reg[3] = xin >> 8;	/* xin with 9 bit resolution */
+	reg[4] = xin & 0xff;
+
+	if (delsys == SYS_DVBT) {
+		reg[6] &= 0x3f;	/* bits 6 and 7 describe the bandwidth */
+		switch (p->bandwidth_hz) {
+		case 6000000:
+			reg[6] |= 0x80;
+			break;
+		case 7000000:
+			reg[6] |= 0x40;
+			break;
+		case 8000000:
+		default:
+			break;
+		}
+	} else {
+		err("%s: modulation type not supported!", __func__);
+		return -EINVAL;
+	}
+
+	/* modified for Realtek demod */
+	reg[5] |= 0x07;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	for (i = 1; i <= 6; i++) {
+		ret = fc0012_writereg(priv, i, reg[i]);
+		if (ret)
+			goto exit;
+	}
+
+	/* VCO Calibration */
+	ret = fc0012_writereg(priv, 0x0e, 0x80);
+	if (!ret)
+		ret = fc0012_writereg(priv, 0x0e, 0x00);
+
+	/* VCO Re-Calibration if needed */
+	if (!ret)
+		ret = fc0012_writereg(priv, 0x0e, 0x00);
+
+	if (!ret) {
+		msleep(10);
+		ret = fc0012_readreg(priv, 0x0e, &tmp);
+	}
+	if (ret)
+		goto exit;
+
+	/* vco selection */
+	tmp &= 0x3f;
+
+	if (vco_select) {
+		if (tmp > 0x3c) {
+			reg[6] &= ~0x08;
+			ret = fc0012_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x00);
+		}
+	} else {
+		if (tmp < 0x02) {
+			reg[6] |= 0x08;
+			ret = fc0012_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x00);
+		}
+	}
+
+	priv->frequency = p->frequency;
+	priv->bandwidth = p->bandwidth_hz;
+
+exit:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static int fc0012_get_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	*frequency = priv->frequency;
+	return 0;
+}
+
+static int fc0012_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	/* CHECK: always ? */
+	*frequency = 0;
+	return 0;
+}
+
+static int fc0012_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	*bandwidth = priv->bandwidth;
+	return 0;
+}
+
+#define INPUT_ADC_LEVEL	-8
+
+static int fc0012_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	int ret;
+	unsigned char tmp;
+	int int_temp, lna_gain, int_lna, tot_agc_gain, power;
+	const int fc0012_lna_gain_table[] = {
+		/* low gain */
+		-63, -58, -99, -73,
+		-63, -65, -54, -60,
+		/* middle gain */
+		 71,  70,  68,  67,
+		 65,  63,  61,  58,
+		/* high gain */
+		197, 191, 188, 186,
+		184, 182, 181, 179,
+	};
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	ret = fc0012_writereg(priv, 0x12, 0x00);
+	if (ret)
+		goto err;
+
+	ret = fc0012_readreg(priv, 0x12, &tmp);
+	if (ret)
+		goto err;
+	int_temp = tmp;
+
+	ret = fc0012_readreg(priv, 0x13, &tmp);
+	if (ret)
+		goto err;
+	lna_gain = tmp & 0x1f;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (lna_gain < ARRAY_SIZE(fc0012_lna_gain_table)) {
+		int_lna = fc0012_lna_gain_table[lna_gain];
+		tot_agc_gain = (abs((int_temp >> 5) - 7) - 2 +
+				(int_temp & 0x1f)) * 2;
+		power = INPUT_ADC_LEVEL - tot_agc_gain - int_lna / 10;
+
+		if (power >= 45)
+			*strength = 255;	/* 100% */
+		else if (power < -95)
+			*strength = 0;
+		else
+			*strength = (power + 95) * 255 / 140;
+
+		*strength |= *strength << 8;
+	} else {
+		ret = -1;
+	}
+
+	goto exit;
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+exit:
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static const struct dvb_tuner_ops fc0012_tuner_ops = {
+	.info = {
+		.name           = "Fitipower FC0012",
+
+		.frequency_min  = 37000000,	/* estimate */
+		.frequency_max  = 862000000,	/* estimate */
+		.frequency_step = 0,
+	},
+
+	.release	= fc0012_release,
+
+	.init		= fc0012_init,
+	.sleep		= fc0012_sleep,
+
+	.set_params	= fc0012_set_params,
+
+	.get_frequency	= fc0012_get_frequency,
+	.get_if_frequency = fc0012_get_if_frequency,
+	.get_bandwidth	= fc0012_get_bandwidth,
+
+	.get_rf_strength = fc0012_get_rf_strength,
+};
+
+struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, u8 i2c_address, int dual_master,
+	enum fc001x_xtal_freq xtal_freq)
+{
+	struct fc0012_priv *priv = NULL;
+
+	priv = kzalloc(sizeof(struct fc0012_priv), GFP_KERNEL);
+	if (priv == NULL)
+		return NULL;
+
+	priv->i2c = i2c;
+	priv->dual_master = dual_master;
+	priv->addr = i2c_address;
+	priv->xtal_freq = xtal_freq;
+
+	info("Fitipower FC0012 successfully attached.");
+
+	fe->tuner_priv = priv;
+
+	memcpy(&fe->ops.tuner_ops, &fc0012_tuner_ops,
+		sizeof(struct dvb_tuner_ops));
+
+	return fe;
+}
+EXPORT_SYMBOL(fc0012_attach);
+
+MODULE_DESCRIPTION("Fitipower FC0012 silicon tuner driver");
+MODULE_AUTHOR("Hans-Frieder Vogt <hfvogt@gmx.net>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.6");
diff --git a/drivers/media/common/tuners/fc0012.h b/drivers/media/common/tuners/fc0012.h
new file mode 100644
index 000000000000..4dbd5efe8845
--- /dev/null
+++ b/drivers/media/common/tuners/fc0012.h
@@ -0,0 +1,44 @@
+/*
+ * Fitipower FC0012 tuner driver - include
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _FC0012_H_
+#define _FC0012_H_
+
+#include "dvb_frontend.h"
+#include "fc001x-common.h"
+
+#if defined(CONFIG_MEDIA_TUNER_FC0012) || \
+	(defined(CONFIG_MEDIA_TUNER_FC0012_MODULE) && defined(MODULE))
+extern struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq);
+#else
+static inline struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/common/tuners/fc0013-priv.h b/drivers/media/common/tuners/fc0013-priv.h
new file mode 100644
index 000000000000..bfd49dedea22
--- /dev/null
+++ b/drivers/media/common/tuners/fc0013-priv.h
@@ -0,0 +1,44 @@
+/*
+ * Fitipower FC0013 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#ifndef _FC0013_PRIV_H_
+#define _FC0013_PRIV_H_
+
+#define LOG_PREFIX "fc0013"
+
+#undef err
+#define err(f, arg...)  printk(KERN_ERR     LOG_PREFIX": " f "\n" , ## arg)
+#undef info
+#define info(f, arg...) printk(KERN_INFO    LOG_PREFIX": " f "\n" , ## arg)
+#undef warn
+#define warn(f, arg...) printk(KERN_WARNING LOG_PREFIX": " f "\n" , ## arg)
+
+struct fc0013_priv {
+	struct i2c_adapter *i2c;
+	u8 addr;
+	u8 dual_master;
+	u8 xtal_freq;
+
+	u32 frequency;
+	u32 bandwidth;
+};
+
+#endif
diff --git a/drivers/media/common/tuners/fc0013.c b/drivers/media/common/tuners/fc0013.c
new file mode 100644
index 000000000000..bd8f0f1e8f3b
--- /dev/null
+++ b/drivers/media/common/tuners/fc0013.c
@@ -0,0 +1,634 @@
+/*
+ * Fitipower FC0013 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ * partially based on driver code from Fitipower
+ * Copyright (C) 2010 Fitipower Integrated Technology Inc
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include "fc0013.h"
+#include "fc0013-priv.h"
+
+static int fc0013_writereg(struct fc0013_priv *priv, u8 reg, u8 val)
+{
+	u8 buf[2] = {reg, val};
+	struct i2c_msg msg = {
+		.addr = priv->addr, .flags = 0, .buf = buf, .len = 2
+	};
+
+	if (i2c_transfer(priv->i2c, &msg, 1) != 1) {
+		err("I2C write reg failed, reg: %02x, val: %02x", reg, val);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0013_readreg(struct fc0013_priv *priv, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2] = {
+		{ .addr = priv->addr, .flags = 0, .buf = &reg, .len = 1 },
+		{ .addr = priv->addr, .flags = I2C_M_RD, .buf = val, .len = 1 },
+	};
+
+	if (i2c_transfer(priv->i2c, msg, 2) != 2) {
+		err("I2C read reg failed, reg: %02x", reg);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0013_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+	return 0;
+}
+
+static int fc0013_init(struct dvb_frontend *fe)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	unsigned char reg[] = {
+		0x00,	/* reg. 0x00: dummy */
+		0x09,	/* reg. 0x01 */
+		0x16,	/* reg. 0x02 */
+		0x00,	/* reg. 0x03 */
+		0x00,	/* reg. 0x04 */
+		0x17,	/* reg. 0x05 */
+		0x02,	/* reg. 0x06 */
+		0x0a,	/* reg. 0x07: CHECK */
+		0xff,	/* reg. 0x08: AGC Clock divide by 256, AGC gain 1/256,
+			   Loop Bw 1/8 */
+		0x6f,	/* reg. 0x09: enable LoopThrough */
+		0xb8,	/* reg. 0x0a: Disable LO Test Buffer */
+		0x82,	/* reg. 0x0b: CHECK */
+		0xfc,	/* reg. 0x0c: depending on AGC Up-Down mode, may need 0xf8 */
+		0x01,	/* reg. 0x0d: AGC Not Forcing & LNA Forcing, may need 0x02 */
+		0x00,	/* reg. 0x0e */
+		0x00,	/* reg. 0x0f */
+		0x00,	/* reg. 0x10 */
+		0x00,	/* reg. 0x11 */
+		0x00,	/* reg. 0x12 */
+		0x00,	/* reg. 0x13 */
+		0x50,	/* reg. 0x14: DVB-t High Gain, UHF.
+			   Middle Gain: 0x48, Low Gain: 0x40 */
+		0x01,	/* reg. 0x15 */
+	};
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+	case FC_XTAL_28_8_MHZ:
+		reg[0x07] |= 0x20;
+		break;
+	case FC_XTAL_36_MHZ:
+	default:
+		break;
+	}
+
+	if (priv->dual_master)
+		reg[0x0c] |= 0x02;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	for (i = 1; i < sizeof(reg); i++) {
+		ret = fc0013_writereg(priv, i, reg[i]);
+		if (ret)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (ret)
+		err("fc0013_writereg failed: %d", ret);
+
+	return ret;
+}
+
+static int fc0013_sleep(struct dvb_frontend *fe)
+{
+	/* nothing to do here */
+	return 0;
+}
+
+int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int ret;
+	u8 rc_cal;
+	int val;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	/* push rc_cal value, get rc_cal value */
+	ret = fc0013_writereg(priv, 0x10, 0x00);
+	if (ret)
+		goto error_out;
+
+	/* get rc_cal value */
+	ret = fc0013_readreg(priv, 0x10, &rc_cal);
+	if (ret)
+		goto error_out;
+
+	rc_cal &= 0x0f;
+
+	val = (int)rc_cal + rc_val;
+
+	/* forcing rc_cal */
+	ret = fc0013_writereg(priv, 0x0d, 0x11);
+	if (ret)
+		goto error_out;
+
+	/* modify rc_cal value */
+	if (val > 15)
+		ret = fc0013_writereg(priv, 0x10, 0x0f);
+	else if (val < 0)
+		ret = fc0013_writereg(priv, 0x10, 0x00);
+	else
+		ret = fc0013_writereg(priv, 0x10, (u8)val);
+
+error_out:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	return ret;
+}
+EXPORT_SYMBOL(fc0013_rc_cal_add);
+
+int fc0013_rc_cal_reset(struct dvb_frontend *fe)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int ret;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	ret = fc0013_writereg(priv, 0x0d, 0x01);
+	if (!ret)
+		ret = fc0013_writereg(priv, 0x10, 0x00);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	return ret;
+}
+EXPORT_SYMBOL(fc0013_rc_cal_reset);
+
+static int fc0013_set_vhf_track(struct fc0013_priv *priv, u32 freq)
+{
+	int ret;
+	u8 tmp;
+
+	ret = fc0013_readreg(priv, 0x1d, &tmp);
+	if (ret)
+		goto error_out;
+	tmp &= 0xe3;
+	if (freq <= 177500) {		/* VHF Track: 7 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x1c);
+	} else if (freq <= 184500) {	/* VHF Track: 6 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x18);
+	} else if (freq <= 191500) {	/* VHF Track: 5 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x14);
+	} else if (freq <= 198500) {	/* VHF Track: 4 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x10);
+	} else if (freq <= 205500) {	/* VHF Track: 3 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x0c);
+	} else if (freq <= 219500) {	/* VHF Track: 2 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x08);
+	} else if (freq < 300000) {	/* VHF Track: 1 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x04);
+	} else {			/* UHF and GPS */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x1c);
+	}
+	if (ret)
+		goto error_out;
+error_out:
+	return ret;
+}
+
+static int fc0013_set_params(struct dvb_frontend *fe)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	u32 freq = p->frequency / 1000;
+	u32 delsys = p->delivery_system;
+	unsigned char reg[7], am, pm, multi, tmp;
+	unsigned long f_vco;
+	unsigned short xtal_freq_khz_2, xin, xdiv;
+	int vco_select = false;
+
+	if (fe->callback) {
+		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			FC_FE_CALLBACK_VHF_ENABLE, (freq > 300000 ? 0 : 1));
+		if (ret)
+			goto exit;
+	}
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+		xtal_freq_khz_2 = 27000 / 2;
+		break;
+	case FC_XTAL_36_MHZ:
+		xtal_freq_khz_2 = 36000 / 2;
+		break;
+	case FC_XTAL_28_8_MHZ:
+	default:
+		xtal_freq_khz_2 = 28800 / 2;
+		break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	/* set VHF track */
+	ret = fc0013_set_vhf_track(priv, freq);
+	if (ret)
+		goto exit;
+
+	if (freq < 300000) {
+		/* enable VHF filter */
+		ret = fc0013_readreg(priv, 0x07, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x07, tmp | 0x10);
+		if (ret)
+			goto exit;
+
+		/* disable UHF & disable GPS */
+		ret = fc0013_readreg(priv, 0x14, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x14, tmp & 0x1f);
+		if (ret)
+			goto exit;
+	} else if (freq <= 862000) {
+		/* disable VHF filter */
+		ret = fc0013_readreg(priv, 0x07, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x07, tmp & 0xef);
+		if (ret)
+			goto exit;
+
+		/* enable UHF & disable GPS */
+		ret = fc0013_readreg(priv, 0x14, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x14, (tmp & 0x1f) | 0x40);
+		if (ret)
+			goto exit;
+	} else {
+		/* disable VHF filter */
+		ret = fc0013_readreg(priv, 0x07, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x07, tmp & 0xef);
+		if (ret)
+			goto exit;
+
+		/* disable UHF & enable GPS */
+		ret = fc0013_readreg(priv, 0x14, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x14, (tmp & 0x1f) | 0x20);
+		if (ret)
+			goto exit;
+	}
+
+	/* select frequency divider and the frequency of VCO */
+	if (freq < 37084) {		/* freq * 96 < 3560000 */
+		multi = 96;
+		reg[5] = 0x82;
+		reg[6] = 0x00;
+	} else if (freq < 55625) {	/* freq * 64 < 3560000 */
+		multi = 64;
+		reg[5] = 0x02;
+		reg[6] = 0x02;
+	} else if (freq < 74167) {	/* freq * 48 < 3560000 */
+		multi = 48;
+		reg[5] = 0x42;
+		reg[6] = 0x00;
+	} else if (freq < 111250) {	/* freq * 32 < 3560000 */
+		multi = 32;
+		reg[5] = 0x82;
+		reg[6] = 0x02;
+	} else if (freq < 148334) {	/* freq * 24 < 3560000 */
+		multi = 24;
+		reg[5] = 0x22;
+		reg[6] = 0x00;
+	} else if (freq < 222500) {	/* freq * 16 < 3560000 */
+		multi = 16;
+		reg[5] = 0x42;
+		reg[6] = 0x02;
+	} else if (freq < 296667) {	/* freq * 12 < 3560000 */
+		multi = 12;
+		reg[5] = 0x12;
+		reg[6] = 0x00;
+	} else if (freq < 445000) {	/* freq * 8 < 3560000 */
+		multi = 8;
+		reg[5] = 0x22;
+		reg[6] = 0x02;
+	} else if (freq < 593334) {	/* freq * 6 < 3560000 */
+		multi = 6;
+		reg[5] = 0x0a;
+		reg[6] = 0x00;
+	} else if (freq < 950000) {	/* freq * 4 < 3800000 */
+		multi = 4;
+		reg[5] = 0x12;
+		reg[6] = 0x02;
+	} else {
+		multi = 2;
+		reg[5] = 0x0a;
+		reg[6] = 0x02;
+	}
+
+	f_vco = freq * multi;
+
+	if (f_vco >= 3060000) {
+		reg[6] |= 0x08;
+		vco_select = true;
+	}
+
+	if (freq >= 45000) {
+		/* From divided value (XDIV) determined the FA and FP value */
+		xdiv = (unsigned short)(f_vco / xtal_freq_khz_2);
+		if ((f_vco - xdiv * xtal_freq_khz_2) >= (xtal_freq_khz_2 / 2))
+			xdiv++;
+
+		pm = (unsigned char)(xdiv / 8);
+		am = (unsigned char)(xdiv - (8 * pm));
+
+		if (am < 2) {
+			reg[1] = am + 8;
+			reg[2] = pm - 1;
+		} else {
+			reg[1] = am;
+			reg[2] = pm;
+		}
+	} else {
+		/* fix for frequency less than 45 MHz */
+		reg[1] = 0x06;
+		reg[2] = 0x11;
+	}
+
+	/* fix clock out */
+	reg[6] |= 0x20;
+
+	/* From VCO frequency determines the XIN ( fractional part of Delta
+	   Sigma PLL) and divided value (XDIV) */
+	xin = (unsigned short)(f_vco - (f_vco / xtal_freq_khz_2) * xtal_freq_khz_2);
+	xin = (xin << 15) / xtal_freq_khz_2;
+	if (xin >= 16384)
+		xin += 32768;
+
+	reg[3] = xin >> 8;
+	reg[4] = xin & 0xff;
+
+	if (delsys == SYS_DVBT) {
+		reg[6] &= 0x3f; /* bits 6 and 7 describe the bandwidth */
+		switch (p->bandwidth_hz) {
+		case 6000000:
+			reg[6] |= 0x80;
+			break;
+		case 7000000:
+			reg[6] |= 0x40;
+			break;
+		case 8000000:
+		default:
+			break;
+		}
+	} else {
+		err("%s: modulation type not supported!", __func__);
+		return -EINVAL;
+	}
+
+	/* modified for Realtek demod */
+	reg[5] |= 0x07;
+
+	for (i = 1; i <= 6; i++) {
+		ret = fc0013_writereg(priv, i, reg[i]);
+		if (ret)
+			goto exit;
+	}
+
+	ret = fc0013_readreg(priv, 0x11, &tmp);
+	if (ret)
+		goto exit;
+	if (multi == 64)
+		ret = fc0013_writereg(priv, 0x11, tmp | 0x04);
+	else
+		ret = fc0013_writereg(priv, 0x11, tmp & 0xfb);
+	if (ret)
+		goto exit;
+
+	/* VCO Calibration */
+	ret = fc0013_writereg(priv, 0x0e, 0x80);
+	if (!ret)
+		ret = fc0013_writereg(priv, 0x0e, 0x00);
+
+	/* VCO Re-Calibration if needed */
+	if (!ret)
+		ret = fc0013_writereg(priv, 0x0e, 0x00);
+
+	if (!ret) {
+		msleep(10);
+		ret = fc0013_readreg(priv, 0x0e, &tmp);
+	}
+	if (ret)
+		goto exit;
+
+	/* vco selection */
+	tmp &= 0x3f;
+
+	if (vco_select) {
+		if (tmp > 0x3c) {
+			reg[6] &= ~0x08;
+			ret = fc0013_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x00);
+		}
+	} else {
+		if (tmp < 0x02) {
+			reg[6] |= 0x08;
+			ret = fc0013_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x00);
+		}
+	}
+
+	priv->frequency = p->frequency;
+	priv->bandwidth = p->bandwidth_hz;
+
+exit:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static int fc0013_get_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	*frequency = priv->frequency;
+	return 0;
+}
+
+static int fc0013_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	/* always ? */
+	*frequency = 0;
+	return 0;
+}
+
+static int fc0013_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	*bandwidth = priv->bandwidth;
+	return 0;
+}
+
+#define INPUT_ADC_LEVEL	-8
+
+static int fc0013_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int ret;
+	unsigned char tmp;
+	int int_temp, lna_gain, int_lna, tot_agc_gain, power;
+	const int fc0013_lna_gain_table[] = {
+		/* low gain */
+		-63, -58, -99, -73,
+		-63, -65, -54, -60,
+		/* middle gain */
+		 71,  70,  68,  67,
+		 65,  63,  61,  58,
+		/* high gain */
+		197, 191, 188, 186,
+		184, 182, 181, 179,
+	};
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	ret = fc0013_writereg(priv, 0x13, 0x00);
+	if (ret)
+		goto err;
+
+	ret = fc0013_readreg(priv, 0x13, &tmp);
+	if (ret)
+		goto err;
+	int_temp = tmp;
+
+	ret = fc0013_readreg(priv, 0x14, &tmp);
+	if (ret)
+		goto err;
+	lna_gain = tmp & 0x1f;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (lna_gain < ARRAY_SIZE(fc0013_lna_gain_table)) {
+		int_lna = fc0013_lna_gain_table[lna_gain];
+		tot_agc_gain = (abs((int_temp >> 5) - 7) - 2 +
+				(int_temp & 0x1f)) * 2;
+		power = INPUT_ADC_LEVEL - tot_agc_gain - int_lna / 10;
+
+		if (power >= 45)
+			*strength = 255;	/* 100% */
+		else if (power < -95)
+			*strength = 0;
+		else
+			*strength = (power + 95) * 255 / 140;
+
+		*strength |= *strength << 8;
+	} else {
+		ret = -1;
+	}
+
+	goto exit;
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+exit:
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static const struct dvb_tuner_ops fc0013_tuner_ops = {
+	.info = {
+		.name		= "Fitipower FC0013",
+
+		.frequency_min	= 37000000,	/* estimate */
+		.frequency_max	= 1680000000,	/* CHECK */
+		.frequency_step	= 0,
+	},
+
+	.release	= fc0013_release,
+
+	.init		= fc0013_init,
+	.sleep		= fc0013_sleep,
+
+	.set_params	= fc0013_set_params,
+
+	.get_frequency	= fc0013_get_frequency,
+	.get_if_frequency = fc0013_get_if_frequency,
+	.get_bandwidth	= fc0013_get_bandwidth,
+
+	.get_rf_strength = fc0013_get_rf_strength,
+};
+
+struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, u8 i2c_address, int dual_master,
+	enum fc001x_xtal_freq xtal_freq)
+{
+	struct fc0013_priv *priv = NULL;
+
+	priv = kzalloc(sizeof(struct fc0013_priv), GFP_KERNEL);
+	if (priv == NULL)
+		return NULL;
+
+	priv->i2c = i2c;
+	priv->dual_master = dual_master;
+	priv->addr = i2c_address;
+	priv->xtal_freq = xtal_freq;
+
+	info("Fitipower FC0013 successfully attached.");
+
+	fe->tuner_priv = priv;
+
+	memcpy(&fe->ops.tuner_ops, &fc0013_tuner_ops,
+		sizeof(struct dvb_tuner_ops));
+
+	return fe;
+}
+EXPORT_SYMBOL(fc0013_attach);
+
+MODULE_DESCRIPTION("Fitipower FC0013 silicon tuner driver");
+MODULE_AUTHOR("Hans-Frieder Vogt <hfvogt@gmx.net>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.2");
diff --git a/drivers/media/common/tuners/fc0013.h b/drivers/media/common/tuners/fc0013.h
new file mode 100644
index 000000000000..594efd64aeec
--- /dev/null
+++ b/drivers/media/common/tuners/fc0013.h
@@ -0,0 +1,57 @@
+/*
+ * Fitipower FC0013 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#ifndef _FC0013_H_
+#define _FC0013_H_
+
+#include "dvb_frontend.h"
+#include "fc001x-common.h"
+
+#if defined(CONFIG_MEDIA_TUNER_FC0013) || \
+	(defined(CONFIG_MEDIA_TUNER_FC0013_MODULE) && defined(MODULE))
+extern struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq);
+extern int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val);
+extern int fc0013_rc_cal_reset(struct dvb_frontend *fe);
+#else
+static inline struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val)
+{
+	return 0;
+}
+
+static inline int fc0013_rc_cal_reset(struct dvb_frontend *fe)
+{
+	return 0;
+}
+#endif
+
+#endif
diff --git a/drivers/media/common/tuners/fc001x-common.h b/drivers/media/common/tuners/fc001x-common.h
new file mode 100644
index 000000000000..718818156934
--- /dev/null
+++ b/drivers/media/common/tuners/fc001x-common.h
@@ -0,0 +1,39 @@
+/*
+ * Fitipower FC0012 & FC0013 tuner driver - common defines
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _FC001X_COMMON_H_
+#define _FC001X_COMMON_H_
+
+enum fc001x_xtal_freq {
+	FC_XTAL_27_MHZ,		/* 27000000 */
+	FC_XTAL_28_8_MHZ,	/* 28800000 */
+	FC_XTAL_36_MHZ,		/* 36000000 */
+};
+
+/*
+ * enum fc001x_fe_callback_commands - Frontend callbacks
+ *
+ * @FC_FE_CALLBACK_VHF_ENABLE: enable VHF or UHF
+ */
+enum fc001x_fe_callback_commands {
+	FC_FE_CALLBACK_VHF_ENABLE,
+};
+
+#endif
diff --git a/drivers/media/common/tuners/tua9001.c b/drivers/media/common/tuners/tua9001.c
new file mode 100644
index 000000000000..de2607084672
--- /dev/null
+++ b/drivers/media/common/tuners/tua9001.c
@@ -0,0 +1,215 @@
+/*
+ * Infineon TUA 9001 silicon tuner driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include "tua9001.h"
+#include "tua9001_priv.h"
+
+/* write register */
+static int tua9001_wr_reg(struct tua9001_priv *priv, u8 reg, u16 val)
+{
+	int ret;
+	u8 buf[3] = { reg, (val >> 8) & 0xff, (val >> 0) & 0xff };
+	struct i2c_msg msg[1] = {
+		{
+			.addr = priv->cfg->i2c_addr,
+			.flags = 0,
+			.len = sizeof(buf),
+			.buf = buf,
+		}
+	};
+
+	ret = i2c_transfer(priv->i2c, msg, 1);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		printk(KERN_WARNING "%s: I2C wr failed=%d reg=%02x\n",
+				__func__, ret, reg);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+static int tua9001_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+
+	return 0;
+}
+
+static int tua9001_init(struct dvb_frontend *fe)
+{
+	struct tua9001_priv *priv = fe->tuner_priv;
+	int ret = 0;
+	u8 i;
+	struct reg_val data[] = {
+		{ 0x1e, 0x6512 },
+		{ 0x25, 0xb888 },
+		{ 0x39, 0x5460 },
+		{ 0x3b, 0x00c0 },
+		{ 0x3a, 0xf000 },
+		{ 0x08, 0x0000 },
+		{ 0x32, 0x0030 },
+		{ 0x41, 0x703a },
+		{ 0x40, 0x1c78 },
+		{ 0x2c, 0x1c00 },
+		{ 0x36, 0xc013 },
+		{ 0x37, 0x6f18 },
+		{ 0x27, 0x0008 },
+		{ 0x2a, 0x0001 },
+		{ 0x34, 0x0a40 },
+	};
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open i2c-gate */
+
+	for (i = 0; i < ARRAY_SIZE(data); i++) {
+		ret = tua9001_wr_reg(priv, data[i].reg, data[i].val);
+		if (ret)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close i2c-gate */
+
+	if (ret < 0)
+		pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int tua9001_set_params(struct dvb_frontend *fe)
+{
+	struct tua9001_priv *priv = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret, i;
+	u16 val;
+	u32 frequency;
+	struct reg_val data[2];
+
+	pr_debug("%s: delivery_system=%d frequency=%d bandwidth_hz=%d\n",
+			__func__, c->delivery_system, c->frequency,
+			c->bandwidth_hz);
+
+	switch (c->delivery_system) {
+	case SYS_DVBT:
+		switch (c->bandwidth_hz) {
+		case 8000000:
+			val  = 0x0000;
+			break;
+		case 7000000:
+			val  = 0x1000;
+			break;
+		case 6000000:
+			val  = 0x2000;
+			break;
+		case 5000000:
+			val  = 0x3000;
+			break;
+		default:
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+
+	data[0].reg = 0x04;
+	data[0].val = val;
+
+	frequency = (c->frequency - 150000000);
+	frequency /= 100;
+	frequency *= 48;
+	frequency /= 10000;
+
+	data[1].reg = 0x1f;
+	data[1].val = frequency;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open i2c-gate */
+
+	for (i = 0; i < ARRAY_SIZE(data); i++) {
+		ret = tua9001_wr_reg(priv, data[i].reg, data[i].val);
+		if (ret < 0)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close i2c-gate */
+
+err:
+	if (ret < 0)
+		pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int tua9001_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	*frequency = 0; /* Zero-IF */
+
+	return 0;
+}
+
+static const struct dvb_tuner_ops tua9001_tuner_ops = {
+	.info = {
+		.name           = "Infineon TUA 9001",
+
+		.frequency_min  = 170000000,
+		.frequency_max  = 862000000,
+		.frequency_step = 0,
+	},
+
+	.release = tua9001_release,
+
+	.init = tua9001_init,
+	.set_params = tua9001_set_params,
+
+	.get_if_frequency = tua9001_get_if_frequency,
+};
+
+struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c, struct tua9001_config *cfg)
+{
+	struct tua9001_priv *priv = NULL;
+
+	priv = kzalloc(sizeof(struct tua9001_priv), GFP_KERNEL);
+	if (priv == NULL)
+		return NULL;
+
+	priv->cfg = cfg;
+	priv->i2c = i2c;
+
+	printk(KERN_INFO "Infineon TUA 9001 successfully attached.");
+
+	memcpy(&fe->ops.tuner_ops, &tua9001_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	fe->tuner_priv = priv;
+	return fe;
+}
+EXPORT_SYMBOL(tua9001_attach);
+
+MODULE_DESCRIPTION("Infineon TUA 9001 silicon tuner driver");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/tuners/tua9001.h b/drivers/media/common/tuners/tua9001.h
new file mode 100644
index 000000000000..38d6ae76b1d6
--- /dev/null
+++ b/drivers/media/common/tuners/tua9001.h
@@ -0,0 +1,46 @@
+/*
+ * Infineon TUA 9001 silicon tuner driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef TUA9001_H
+#define TUA9001_H
+
+#include "dvb_frontend.h"
+
+struct tua9001_config {
+	/*
+	 * I2C address
+	 */
+	u8 i2c_addr;
+};
+
+#if defined(CONFIG_MEDIA_TUNER_TUA9001) || \
+	(defined(CONFIG_MEDIA_TUNER_TUA9001_MODULE) && defined(MODULE))
+extern struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c, struct tua9001_config *cfg);
+#else
+static inline struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c, struct tua9001_config *cfg)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/common/tuners/tua9001_priv.h b/drivers/media/common/tuners/tua9001_priv.h
new file mode 100644
index 000000000000..73cc1ce0575c
--- /dev/null
+++ b/drivers/media/common/tuners/tua9001_priv.h
@@ -0,0 +1,34 @@
+/*
+ * Infineon TUA 9001 silicon tuner driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef TUA9001_PRIV_H
+#define TUA9001_PRIV_H
+
+struct reg_val {
+	u8 reg;
+	u16 val;
+};
+
+struct tua9001_priv {
+	struct tua9001_config *cfg;
+	struct i2c_adapter *i2c;
+};
+
+#endif
diff --git a/drivers/media/common/tuners/xc5000.c b/drivers/media/common/tuners/xc5000.c
index eab2ea424200..dcca42ca57be 100644
--- a/drivers/media/common/tuners/xc5000.c
+++ b/drivers/media/common/tuners/xc5000.c
@@ -54,7 +54,7 @@ struct xc5000_priv {
 	struct list_head hybrid_tuner_instance_list;
 
 	u32 if_khz;
-	u32 xtal_khz;
+	u16 xtal_khz;
 	u32 freq_hz;
 	u32 bandwidth;
 	u8  video_standard;
@@ -631,7 +631,10 @@ static int xc5000_fwupload(struct dvb_frontend *fe)
 		ret = xc_load_i2c_sequence(fe,  fw->data);
 		if (XC_RESULT_SUCCESS == ret)
 			ret = xc_set_xtal(fe);
-		printk(KERN_INFO "xc5000: firmware upload complete...\n");
+		if (XC_RESULT_SUCCESS == ret)
+			printk(KERN_INFO "xc5000: firmware upload complete...\n");
+		else
+			printk(KERN_ERR "xc5000: firmware upload failed...\n");
 	}
 
 out:
diff --git a/drivers/media/common/tuners/xc5000.h b/drivers/media/common/tuners/xc5000.h
index 39a73bf01406..b1a547494625 100644
--- a/drivers/media/common/tuners/xc5000.h
+++ b/drivers/media/common/tuners/xc5000.h
@@ -34,7 +34,7 @@ struct xc5000_config {
 	u8   i2c_address;
 	u32  if_khz;
 	u8   radio_input;
-	u32  xtal_khz;
+	u16  xtal_khz;
 
 	int chip_id;
 };
diff --git a/drivers/media/dvb/bt8xx/dst_ca.c b/drivers/media/dvb/bt8xx/dst_ca.c
index 48e48e8af55a..66f52f116b60 100644
--- a/drivers/media/dvb/bt8xx/dst_ca.c
+++ b/drivers/media/dvb/bt8xx/dst_ca.c
@@ -477,7 +477,6 @@ static int dst_check_ca_pmt(struct dst_state *state, struct ca_msg *p_ca_message
 static int ca_send_message(struct dst_state *state, struct ca_msg *p_ca_message, void __user *arg)
 {
 	int i = 0;
-	unsigned int ca_message_header_len;
 
 	u32 command = 0;
 	struct ca_msg *hw_buffer;
@@ -496,7 +495,6 @@ static int ca_send_message(struct dst_state *state, struct ca_msg *p_ca_message,
 
 
 	if (p_ca_message->msg) {
-		ca_message_header_len = p_ca_message->length;	/*	Restore it back when you are done	*/
 		/*	EN50221 tag	*/
 		command = 0;
 
diff --git a/drivers/media/dvb/ddbridge/ddbridge-core.c b/drivers/media/dvb/ddbridge/ddbridge-core.c
index d88c4aa7d24d..131b938e9e81 100644
--- a/drivers/media/dvb/ddbridge/ddbridge-core.c
+++ b/drivers/media/dvb/ddbridge/ddbridge-core.c
@@ -31,7 +31,6 @@
 #include <linux/pci.h>
 #include <linux/pci_ids.h>
 #include <linux/timer.h>
-#include <linux/version.h>
 #include <linux/i2c.h>
 #include <linux/swab.h>
 #include <linux/vmalloc.h>
@@ -1696,7 +1695,7 @@ static struct pci_driver ddb_pci_driver = {
 	.name        = "DDBridge",
 	.id_table    = ddb_id_tbl,
 	.probe       = ddb_probe,
-	.remove      = ddb_remove,
+	.remove      = __devexit_p(ddb_remove),
 };
 
 static __init int module_init_ddbridge(void)
diff --git a/drivers/media/dvb/dvb-core/dvb_demux.c b/drivers/media/dvb/dvb-core/dvb_demux.c
index faa3671b649e..d82469f842e2 100644
--- a/drivers/media/dvb/dvb-core/dvb_demux.c
+++ b/drivers/media/dvb/dvb-core/dvb_demux.c
@@ -568,6 +568,16 @@ void dvb_dmx_swfilter_204(struct dvb_demux *demux, const u8 *buf, size_t count)
 }
 EXPORT_SYMBOL(dvb_dmx_swfilter_204);
 
+void dvb_dmx_swfilter_raw(struct dvb_demux *demux, const u8 *buf, size_t count)
+{
+	spin_lock(&demux->lock);
+
+	demux->feed->cb.ts(buf, count, NULL, 0, &demux->feed->feed.ts, DMX_OK);
+
+	spin_unlock(&demux->lock);
+}
+EXPORT_SYMBOL(dvb_dmx_swfilter_raw);
+
 static struct dvb_demux_filter *dvb_dmx_filter_alloc(struct dvb_demux *demux)
 {
 	int i;
diff --git a/drivers/media/dvb/dvb-core/dvb_demux.h b/drivers/media/dvb/dvb-core/dvb_demux.h
index a7d876fd02dd..fa7188a253aa 100644
--- a/drivers/media/dvb/dvb-core/dvb_demux.h
+++ b/drivers/media/dvb/dvb-core/dvb_demux.h
@@ -145,5 +145,7 @@ void dvb_dmx_swfilter_packets(struct dvb_demux *dvbdmx, const u8 *buf,
 void dvb_dmx_swfilter(struct dvb_demux *demux, const u8 *buf, size_t count);
 void dvb_dmx_swfilter_204(struct dvb_demux *demux, const u8 *buf,
 			  size_t count);
+void dvb_dmx_swfilter_raw(struct dvb_demux *demux, const u8 *buf,
+			  size_t count);
 
 #endif /* _DVB_DEMUX_H_ */
diff --git a/drivers/media/dvb/dvb-core/dvb_frontend.c b/drivers/media/dvb/dvb-core/dvb_frontend.c
index cb888d835a89..aebcdf221dda 100644
--- a/drivers/media/dvb/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb/dvb-core/dvb_frontend.c
@@ -182,13 +182,13 @@ static enum dvbv3_emulation_type dvbv3_type(u32 delivery_system)
 	case SYS_DMBTH:
 		return DVBV3_OFDM;
 	case SYS_ATSC:
+	case SYS_ATSCMH:
 	case SYS_DVBC_ANNEX_B:
 		return DVBV3_ATSC;
 	case SYS_UNDEFINED:
 	case SYS_ISDBC:
 	case SYS_DVBH:
 	case SYS_DAB:
-	case SYS_ATSCMH:
 	default:
 		/*
 		 * Doesn't know how to emulate those types and/or
@@ -1030,6 +1030,25 @@ static struct dtv_cmds_h dtv_cmds[DTV_MAX_COMMAND + 1] = {
 	_DTV_CMD(DTV_HIERARCHY, 0, 0),
 
 	_DTV_CMD(DTV_ENUM_DELSYS, 0, 0),
+
+	_DTV_CMD(DTV_ATSCMH_PARADE_ID, 1, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_FRAME_ENSEMBLE, 1, 0),
+
+	_DTV_CMD(DTV_ATSCMH_FIC_VER, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_PARADE_ID, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_NOG, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_TNOG, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SGN, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_PRC, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_FRAME_MODE, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_FRAME_ENSEMBLE, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_CODE_MODE_PRI, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_CODE_MODE_SEC, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SCCC_BLOCK_MODE, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_A, 0, 0),
+	_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),
 };
 
 static void dtv_property_dump(struct dtv_property *tvp)
@@ -1121,6 +1140,8 @@ static int dtv_property_cache_sync(struct dvb_frontend *fe,
 	case DVBV3_ATSC:
 		dprintk("%s() Preparing ATSC req\n", __func__);
 		c->modulation = p->u.vsb.modulation;
+		if (c->delivery_system == SYS_ATSCMH)
+			break;
 		if ((c->modulation == VSB_8) || (c->modulation == VSB_16))
 			c->delivery_system = SYS_ATSC;
 		else
@@ -1367,6 +1388,54 @@ static int dtv_property_process_get(struct dvb_frontend *fe,
 	case DTV_DVBT2_PLP_ID:
 		tvp->u.data = c->dvbt2_plp_id;
 		break;
+
+	/* ATSC-MH */
+	case DTV_ATSCMH_FIC_VER:
+		tvp->u.data = fe->dtv_property_cache.atscmh_fic_ver;
+		break;
+	case DTV_ATSCMH_PARADE_ID:
+		tvp->u.data = fe->dtv_property_cache.atscmh_parade_id;
+		break;
+	case DTV_ATSCMH_NOG:
+		tvp->u.data = fe->dtv_property_cache.atscmh_nog;
+		break;
+	case DTV_ATSCMH_TNOG:
+		tvp->u.data = fe->dtv_property_cache.atscmh_tnog;
+		break;
+	case DTV_ATSCMH_SGN:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sgn;
+		break;
+	case DTV_ATSCMH_PRC:
+		tvp->u.data = fe->dtv_property_cache.atscmh_prc;
+		break;
+	case DTV_ATSCMH_RS_FRAME_MODE:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_frame_mode;
+		break;
+	case DTV_ATSCMH_RS_FRAME_ENSEMBLE:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_frame_ensemble;
+		break;
+	case DTV_ATSCMH_RS_CODE_MODE_PRI:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_code_mode_pri;
+		break;
+	case DTV_ATSCMH_RS_CODE_MODE_SEC:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_code_mode_sec;
+		break;
+	case DTV_ATSCMH_SCCC_BLOCK_MODE:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_block_mode;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_A:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_a;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_B:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_b;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_C:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_c;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_D:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_d;
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -1708,6 +1777,15 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
 	case DTV_DVBT2_PLP_ID:
 		c->dvbt2_plp_id = tvp->u.data;
 		break;
+
+	/* ATSC-MH */
+	case DTV_ATSCMH_PARADE_ID:
+		fe->dtv_property_cache.atscmh_parade_id = tvp->u.data;
+		break;
+	case DTV_ATSCMH_RS_FRAME_ENSEMBLE:
+		fe->dtv_property_cache.atscmh_rs_frame_ensemble = tvp->u.data;
+		break;
+
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/media/dvb/dvb-core/dvb_frontend.h b/drivers/media/dvb/dvb-core/dvb_frontend.h
index d63a8215fe03..e929d5697b87 100644
--- a/drivers/media/dvb/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb/dvb-core/dvb_frontend.h
@@ -372,6 +372,24 @@ struct dtv_frontend_properties {
 
 	/* DVB-T2 specifics */
 	u32                     dvbt2_plp_id;
+
+	/* ATSC-MH specifics */
+	u8			atscmh_fic_ver;
+	u8			atscmh_parade_id;
+	u8			atscmh_nog;
+	u8			atscmh_tnog;
+	u8			atscmh_sgn;
+	u8			atscmh_prc;
+
+	u8			atscmh_rs_frame_mode;
+	u8			atscmh_rs_frame_ensemble;
+	u8			atscmh_rs_code_mode_pri;
+	u8			atscmh_rs_code_mode_sec;
+	u8			atscmh_sccc_block_mode;
+	u8			atscmh_sccc_code_mode_a;
+	u8			atscmh_sccc_code_mode_b;
+	u8			atscmh_sccc_code_mode_c;
+	u8			atscmh_sccc_code_mode_d;
 };
 
 struct dvb_frontend {
diff --git a/drivers/media/dvb/dvb-usb/Kconfig b/drivers/media/dvb/dvb-usb/Kconfig
index 63bf45679f98..a26949336b3d 100644
--- a/drivers/media/dvb/dvb-usb/Kconfig
+++ b/drivers/media/dvb/dvb-usb/Kconfig
@@ -409,6 +409,7 @@ config DVB_USB_MXL111SF
 	tristate "MxL111SF DTV USB2.0 support"
 	depends on DVB_USB
 	select DVB_LGDT3305 if !DVB_FE_CUSTOMISE
+	select DVB_LG2160 if !DVB_FE_CUSTOMISE
 	select VIDEO_TVEEPROM
 	help
 	  Say Y here to support the MxL111SF USB2.0 DTV receiver.
@@ -422,3 +423,15 @@ config DVB_USB_RTL28XXU
 	select MEDIA_TUNER_MXL5005S if !MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to support the Realtek RTL28xxU DVB USB receiver.
+
+config DVB_USB_AF9035
+	tristate "Afatech AF9035 DVB-T USB2.0 support"
+	depends on DVB_USB
+	select DVB_AF9033
+	select MEDIA_TUNER_TUA9001 if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_FC0011 if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_MXL5007T if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_TDA18218 if !MEDIA_TUNER_CUSTOMISE
+	help
+	  Say Y here to support the Afatech AF9035 based DVB USB receiver.
+
diff --git a/drivers/media/dvb/dvb-usb/Makefile b/drivers/media/dvb/dvb-usb/Makefile
index b76acb5387e6..b667ac39a4e3 100644
--- a/drivers/media/dvb/dvb-usb/Makefile
+++ b/drivers/media/dvb/dvb-usb/Makefile
@@ -110,6 +110,9 @@ obj-$(CONFIG_DVB_USB_MXL111SF) += mxl111sf-tuner.o
 dvb-usb-rtl28xxu-objs = rtl28xxu.o
 obj-$(CONFIG_DVB_USB_RTL28XXU) += dvb-usb-rtl28xxu.o
 
+dvb-usb-af9035-objs = af9035.o
+obj-$(CONFIG_DVB_USB_AF9035) += dvb-usb-af9035.o
+
 ccflags-y += -I$(srctree)/drivers/media/dvb/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb/frontends/
 # due to tuner-xc3028
diff --git a/drivers/media/dvb/dvb-usb/af9015.c b/drivers/media/dvb/dvb-usb/af9015.c
index 7e70ea50ef26..677fed79b01e 100644
--- a/drivers/media/dvb/dvb-usb/af9015.c
+++ b/drivers/media/dvb/dvb-usb/af9015.c
@@ -244,8 +244,7 @@ static int af9015_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 	u8 uninitialized_var(mbox), addr_len;
 	struct req_t req;
 
-/* TODO: implement bus lock
-
+/*
 The bus lock is needed because there is two tuners both using same I2C-address.
 Due to that the only way to select correct tuner is use demodulator I2C-gate.
 
@@ -789,7 +788,7 @@ static void af9015_set_remote_config(struct usb_device *udev,
 	/* try to load remote based USB ID */
 	if (!props->rc.core.rc_codes)
 		props->rc.core.rc_codes = af9015_rc_setup_match(
-			(vid << 16) + pid, af9015_rc_setup_usbids);
+			(vid << 16) | pid, af9015_rc_setup_usbids);
 
 	/* try to load remote based USB iManufacturer string */
 	if (!props->rc.core.rc_codes && vid == USB_VID_AFATECH) {
@@ -1220,8 +1219,8 @@ static int af9015_af9013_frontend_attach(struct dvb_usb_adapter *adap)
 	}
 
 	/* attach demodulator */
-	adap->fe_adap[0].fe = dvb_attach(af9013_attach, &af9015_af9013_config[adap->id],
-		&adap->dev->i2c_adap);
+	adap->fe_adap[0].fe = dvb_attach(af9013_attach,
+		&af9015_af9013_config[adap->id], &adap->dev->i2c_adap);
 
 	/*
 	 * AF9015 firmware does not like if it gets interrupted by I2C adapter
@@ -1324,14 +1323,15 @@ static int af9015_tuner_attach(struct dvb_usb_adapter *adap)
 	switch (af9015_af9013_config[adap->id].tuner) {
 	case AF9013_TUNER_MT2060:
 	case AF9013_TUNER_MT2060_2:
-		ret = dvb_attach(mt2060_attach, adap->fe_adap[0].fe, &adap->dev->i2c_adap,
-			&af9015_mt2060_config,
+		ret = dvb_attach(mt2060_attach, adap->fe_adap[0].fe,
+			&adap->dev->i2c_adap, &af9015_mt2060_config,
 			af9015_config.mt2060_if1[adap->id])
 			== NULL ? -ENODEV : 0;
 		break;
 	case AF9013_TUNER_QT1010:
 	case AF9013_TUNER_QT1010A:
-		ret = dvb_attach(qt1010_attach, adap->fe_adap[0].fe, &adap->dev->i2c_adap,
+		ret = dvb_attach(qt1010_attach, adap->fe_adap[0].fe,
+			&adap->dev->i2c_adap,
 			&af9015_qt1010_config) == NULL ? -ENODEV : 0;
 		break;
 	case AF9013_TUNER_TDA18271:
@@ -1434,69 +1434,85 @@ enum af9015_usb_table_entry {
 };
 
 static struct usb_device_id af9015_usb_table[] = {
-	[AFATECH_9015] =
-		{USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9015)},
-	[AFATECH_9016] =
-		{USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9016)},
-	[WINFAST_DTV_GOLD] =
-		{USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV_DONGLE_GOLD)},
-	[PINNACLE_PCTV_71E] =
-		{USB_DEVICE(USB_VID_PINNACLE, USB_PID_PINNACLE_PCTV71E)},
-	[KWORLD_PLUSTV_399U] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U)},
-	[TINYTWIN] = {USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_TINYTWIN)},
-	[AZUREWAVE_TU700] =
-		{USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_AZUREWAVE_AD_TU700)},
-	[TERRATEC_AF9015] = {USB_DEVICE(USB_VID_TERRATEC,
+	[AFATECH_9015] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9015)},
+	[AFATECH_9016] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9016)},
+	[WINFAST_DTV_GOLD] = {
+		USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV_DONGLE_GOLD)},
+	[PINNACLE_PCTV_71E] = {
+		USB_DEVICE(USB_VID_PINNACLE, USB_PID_PINNACLE_PCTV71E)},
+	[KWORLD_PLUSTV_399U] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U)},
+	[TINYTWIN] = {
+		USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_TINYTWIN)},
+	[AZUREWAVE_TU700] = {
+		USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_AZUREWAVE_AD_TU700)},
+	[TERRATEC_AF9015] = {
+		USB_DEVICE(USB_VID_TERRATEC,
 				USB_PID_TERRATEC_CINERGY_T_USB_XE_REV2)},
-	[KWORLD_PLUSTV_PC160] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_2T)},
-	[AVERTV_VOLAR_X] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X)},
-	[XTENSIONS_380U] =
-		{USB_DEVICE(USB_VID_XTENSIONS, USB_PID_XTENSIONS_XD_380)},
-	[MSI_DIGIVOX_DUO] =
-		{USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGIVOX_DUO)},
-	[AVERTV_VOLAR_X_REV2] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X_2)},
-	[TELESTAR_STARSTICK_2] =
-		{USB_DEVICE(USB_VID_TELESTAR,  USB_PID_TELESTAR_STARSTICK_2)},
-	[AVERMEDIA_A309_USB] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A309)},
-	[MSI_DIGIVOX_MINI_III] =
-		{USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGI_VOX_MINI_III)},
-	[KWORLD_E396] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U)},
-	[KWORLD_E39B] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_2)},
-	[KWORLD_E395] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_3)},
-	[TREKSTOR_DVBT] = {USB_DEVICE(USB_VID_AFATECH, USB_PID_TREKSTOR_DVBT)},
-	[AVERTV_A850] = {USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850)},
-	[AVERTV_A805] = {USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A805)},
-	[CONCEPTRONIC_CTVDIGRCU] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_CONCEPTRONIC_CTVDIGRCU)},
-	[KWORLD_MC810] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_MC810)},
-	[GENIUS_TVGO_DVB_T03] =
-		{USB_DEVICE(USB_VID_KYE, USB_PID_GENIUS_TVGO_DVB_T03)},
-	[KWORLD_399U_2] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U_2)},
-	[KWORLD_PC160_T] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_T)},
-	[SVEON_STV20] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV20)},
-	[TINYTWIN_2] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_TINYTWIN_2)},
-	[WINFAST_DTV2000DS] =
-		{USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV2000DS)},
-	[KWORLD_UB383_T] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_UB383_T)},
-	[KWORLD_E39A] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_4)},
-	[AVERMEDIA_A815M] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A815M)},
-	[CINERGY_T_STICK_RC] = {USB_DEVICE(USB_VID_TERRATEC,
+	[KWORLD_PLUSTV_PC160] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_2T)},
+	[AVERTV_VOLAR_X] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X)},
+	[XTENSIONS_380U] = {
+		USB_DEVICE(USB_VID_XTENSIONS, USB_PID_XTENSIONS_XD_380)},
+	[MSI_DIGIVOX_DUO] = {
+		USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGIVOX_DUO)},
+	[AVERTV_VOLAR_X_REV2] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X_2)},
+	[TELESTAR_STARSTICK_2] = {
+		USB_DEVICE(USB_VID_TELESTAR,  USB_PID_TELESTAR_STARSTICK_2)},
+	[AVERMEDIA_A309_USB] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A309)},
+	[MSI_DIGIVOX_MINI_III] = {
+		USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGI_VOX_MINI_III)},
+	[KWORLD_E396] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U)},
+	[KWORLD_E39B] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_2)},
+	[KWORLD_E395] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_3)},
+	[TREKSTOR_DVBT] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_TREKSTOR_DVBT)},
+	[AVERTV_A850] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850)},
+	[AVERTV_A805] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A805)},
+	[CONCEPTRONIC_CTVDIGRCU] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_CONCEPTRONIC_CTVDIGRCU)},
+	[KWORLD_MC810] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_MC810)},
+	[GENIUS_TVGO_DVB_T03] = {
+		USB_DEVICE(USB_VID_KYE, USB_PID_GENIUS_TVGO_DVB_T03)},
+	[KWORLD_399U_2] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U_2)},
+	[KWORLD_PC160_T] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_T)},
+	[SVEON_STV20] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV20)},
+	[TINYTWIN_2] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_TINYTWIN_2)},
+	[WINFAST_DTV2000DS] = {
+		USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV2000DS)},
+	[KWORLD_UB383_T] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_UB383_T)},
+	[KWORLD_E39A] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_4)},
+	[AVERMEDIA_A815M] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A815M)},
+	[CINERGY_T_STICK_RC] = {
+		USB_DEVICE(USB_VID_TERRATEC,
 				USB_PID_TERRATEC_CINERGY_T_STICK_RC)},
-	[CINERGY_T_DUAL_RC] = {USB_DEVICE(USB_VID_TERRATEC,
+	[CINERGY_T_DUAL_RC] = {
+		USB_DEVICE(USB_VID_TERRATEC,
 				USB_PID_TERRATEC_CINERGY_T_STICK_DUAL_RC)},
-	[AVERTV_A850T] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850T)},
-	[TINYTWIN_3] = {USB_DEVICE(USB_VID_GTEK, USB_PID_TINYTWIN_3)},
-	[SVEON_STV22] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV22)},
+	[AVERTV_A850T] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850T)},
+	[TINYTWIN_3] = {
+		USB_DEVICE(USB_VID_GTEK, USB_PID_TINYTWIN_3)},
+	[SVEON_STV22] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV22)},
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, af9015_usb_table);
@@ -1516,43 +1532,44 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.caps = DVB_USB_ADAP_HAS_PID_FILTER |
-				DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
-
-				.pid_filter_count = 32,
-				.pid_filter       = af9015_pid_filter,
-				.pid_filter_ctrl  = af9015_pid_filter_ctrl,
-
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x84,
+				.num_frontends = 1,
+				.fe = {
+					{
+						.caps = DVB_USB_ADAP_HAS_PID_FILTER |
+							DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+
+						.pid_filter_count = 32,
+						.pid_filter       = af9015_pid_filter,
+						.pid_filter_ctrl  = af9015_pid_filter_ctrl,
+
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+						},
+					}
 				},
-			}},
 			},
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x85,
-					.u = {
-						.bulk = {
-							.buffersize =
-						TS_USB20_FRAME_SIZE,
-						}
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x85,
+							.u = {
+								.bulk = {
+									.buffersize = TS_USB20_FRAME_SIZE,
+								}
+							}
+						},
 					}
 				},
-			}},
 			}
 		},
 
@@ -1575,102 +1592,67 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 				.cold_ids = {
 					&af9015_usb_table[AFATECH_9015],
 					&af9015_usb_table[AFATECH_9016],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Leadtek WinFast DTV Dongle Gold",
 				.cold_ids = {
 					&af9015_usb_table[WINFAST_DTV_GOLD],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Pinnacle PCTV 71e",
 				.cold_ids = {
 					&af9015_usb_table[PINNACLE_PCTV_71E],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld PlusTV Dual DVB-T Stick " \
 					"(DVB-T 399U)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_PLUSTV_399U],
 					&af9015_usb_table[KWORLD_399U_2],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "DigitalNow TinyTwin DVB-T Receiver",
 				.cold_ids = {
 					&af9015_usb_table[TINYTWIN],
 					&af9015_usb_table[TINYTWIN_2],
 					&af9015_usb_table[TINYTWIN_3],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TwinHan AzureWave AD-TU700(704J)",
 				.cold_ids = {
 					&af9015_usb_table[AZUREWAVE_TU700],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TerraTec Cinergy T USB XE",
 				.cold_ids = {
 					&af9015_usb_table[TERRATEC_AF9015],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld PlusTV Dual DVB-T PCI " \
 					"(DVB-T PC160-2T)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_PLUSTV_PC160],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AVerMedia AVerTV DVB-T Volar X",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_VOLAR_X],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TerraTec Cinergy T Stick RC",
 				.cold_ids = {
 					&af9015_usb_table[CINERGY_T_STICK_RC],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TerraTec Cinergy T Stick Dual RC",
 				.cold_ids = {
 					&af9015_usb_table[CINERGY_T_DUAL_RC],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AverMedia AVerTV Red HD+ (A850T)",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_A850T],
-					NULL
 				},
-				.warm_ids = {NULL},
 			},
 		}
 	}, {
@@ -1686,43 +1668,44 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.caps = DVB_USB_ADAP_HAS_PID_FILTER |
-				DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
-
-				.pid_filter_count = 32,
-				.pid_filter       = af9015_pid_filter,
-				.pid_filter_ctrl  = af9015_pid_filter_ctrl,
-
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x84,
+				.num_frontends = 1,
+				.fe = {
+					{
+						.caps = DVB_USB_ADAP_HAS_PID_FILTER |
+							DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+
+						.pid_filter_count = 32,
+						.pid_filter       = af9015_pid_filter,
+						.pid_filter_ctrl  = af9015_pid_filter_ctrl,
+
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+						},
+					}
 				},
-			}},
 			},
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x85,
-					.u = {
-						.bulk = {
-							.buffersize =
-						TS_USB20_FRAME_SIZE,
-						}
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x85,
+							.u = {
+								.bulk = {
+									.buffersize = TS_USB20_FRAME_SIZE,
+								}
+							}
+						},
 					}
 				},
-			}},
 			}
 		},
 
@@ -1744,51 +1727,33 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 				.name = "Xtensions XD-380",
 				.cold_ids = {
 					&af9015_usb_table[XTENSIONS_380U],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "MSI DIGIVOX Duo",
 				.cold_ids = {
 					&af9015_usb_table[MSI_DIGIVOX_DUO],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Fujitsu-Siemens Slim Mobile USB DVB-T",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_VOLAR_X_REV2],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Telestar Starstick 2",
 				.cold_ids = {
 					&af9015_usb_table[TELESTAR_STARSTICK_2],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AVerMedia A309",
 				.cold_ids = {
 					&af9015_usb_table[AVERMEDIA_A309_USB],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "MSI Digi VOX mini III",
 				.cold_ids = {
 					&af9015_usb_table[MSI_DIGIVOX_MINI_III],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld USB DVB-T TV Stick II " \
 					"(VS-DVB-T 395U)",
 				.cold_ids = {
@@ -1796,34 +1761,23 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 					&af9015_usb_table[KWORLD_E39B],
 					&af9015_usb_table[KWORLD_E395],
 					&af9015_usb_table[KWORLD_E39A],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TrekStor DVB-T USB Stick",
 				.cold_ids = {
 					&af9015_usb_table[TREKSTOR_DVBT],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AverMedia AVerTV Volar Black HD " \
 					"(A850)",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_A850],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Sveon STV22 Dual USB DVB-T Tuner HDTV",
 				.cold_ids = {
 					&af9015_usb_table[SVEON_STV22],
-					NULL
 				},
-				.warm_ids = {NULL},
 			},
 		}
 	}, {
@@ -1839,43 +1793,44 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.caps = DVB_USB_ADAP_HAS_PID_FILTER |
-				DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
-
-				.pid_filter_count = 32,
-				.pid_filter       = af9015_pid_filter,
-				.pid_filter_ctrl  = af9015_pid_filter_ctrl,
-
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x84,
+				.num_frontends = 1,
+				.fe = {
+					{
+						.caps = DVB_USB_ADAP_HAS_PID_FILTER |
+							DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+
+						.pid_filter_count = 32,
+						.pid_filter       = af9015_pid_filter,
+						.pid_filter_ctrl  = af9015_pid_filter_ctrl,
+
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+						},
+					}
 				},
-			}},
 			},
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x85,
-					.u = {
-						.bulk = {
-							.buffersize =
-						TS_USB20_FRAME_SIZE,
-						}
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x85,
+							.u = {
+								.bulk = {
+									.buffersize = TS_USB20_FRAME_SIZE,
+								}
+							}
+						},
 					}
 				},
-			}},
 			}
 		},
 
@@ -1897,76 +1852,50 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 				.name = "AverMedia AVerTV Volar GPS 805 (A805)",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_A805],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Conceptronic USB2.0 DVB-T CTVDIGRCU " \
 					"V3.0",
 				.cold_ids = {
 					&af9015_usb_table[CONCEPTRONIC_CTVDIGRCU],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld Digial MC-810",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_MC810],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Genius TVGo DVB-T03",
 				.cold_ids = {
 					&af9015_usb_table[GENIUS_TVGO_DVB_T03],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld PlusTV DVB-T PCI Pro Card " \
 					"(DVB-T PC160-T)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_PC160_T],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Sveon STV20 Tuner USB DVB-T HDTV",
 				.cold_ids = {
 					&af9015_usb_table[SVEON_STV20],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Leadtek WinFast DTV2000DS",
 				.cold_ids = {
 					&af9015_usb_table[WINFAST_DTV2000DS],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld USB DVB-T Stick Mobile " \
 					"(UB383-T)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_UB383_T],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AverMedia AVerTV Volar M (A815Mac)",
 				.cold_ids = {
 					&af9015_usb_table[AVERMEDIA_A815M],
-					NULL
 				},
-				.warm_ids = {NULL},
 			},
 		}
 	},
@@ -2019,5 +1948,5 @@ static struct usb_driver af9015_usb_driver = {
 module_usb_driver(af9015_usb_driver);
 
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
-MODULE_DESCRIPTION("Driver for Afatech AF9015 DVB-T");
+MODULE_DESCRIPTION("Afatech AF9015 driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/dvb-usb/af9035.c b/drivers/media/dvb/dvb-usb/af9035.c
new file mode 100644
index 000000000000..e83b39d3993c
--- /dev/null
+++ b/drivers/media/dvb/dvb-usb/af9035.c
@@ -0,0 +1,1242 @@
+/*
+ * Afatech AF9035 DVB USB driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * Copyright (C) 2012 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include "af9035.h"
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+static DEFINE_MUTEX(af9035_usb_mutex);
+static struct dvb_usb_device_properties af9035_properties[2];
+static int af9035_properties_count = ARRAY_SIZE(af9035_properties);
+
+static u16 af9035_checksum(const u8 *buf, size_t len)
+{
+	size_t i;
+	u16 checksum = 0;
+
+	for (i = 1; i < len; i++) {
+		if (i % 2)
+			checksum += buf[i] << 8;
+		else
+			checksum += buf[i];
+	}
+	checksum = ~checksum;
+
+	return checksum;
+}
+
+static int af9035_ctrl_msg(struct usb_device *udev, struct usb_req *req)
+{
+#define BUF_LEN 64
+#define REQ_HDR_LEN 4 /* send header size */
+#define ACK_HDR_LEN 3 /* rece header size */
+#define CHECKSUM_LEN 2
+#define USB_TIMEOUT 2000
+
+	int ret, msg_len, act_len;
+	u8 buf[BUF_LEN];
+	static u8 seq; /* packet sequence number */
+	u16 checksum, tmp_checksum;
+
+	/* buffer overflow check */
+	if (req->wlen > (BUF_LEN - REQ_HDR_LEN - CHECKSUM_LEN) ||
+		req->rlen > (BUF_LEN - ACK_HDR_LEN - CHECKSUM_LEN)) {
+		pr_debug("%s: too much data wlen=%d rlen=%d\n", __func__,
+				req->wlen, req->rlen);
+		return -EINVAL;
+	}
+
+	if (mutex_lock_interruptible(&af9035_usb_mutex) < 0)
+		return -EAGAIN;
+
+	buf[0] = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN - 1;
+	buf[1] = req->mbox;
+	buf[2] = req->cmd;
+	buf[3] = seq++;
+	if (req->wlen)
+		memcpy(&buf[4], req->wbuf, req->wlen);
+
+	/* calc and add checksum */
+	checksum = af9035_checksum(buf, buf[0] - 1);
+	buf[buf[0] - 1] = (checksum >> 8);
+	buf[buf[0] - 0] = (checksum & 0xff);
+
+	msg_len = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN ;
+
+	/* send req */
+	ret = usb_bulk_msg(udev, usb_sndbulkpipe(udev, 0x02), buf, msg_len,
+			&act_len, USB_TIMEOUT);
+	if (ret < 0)
+		err("bulk message failed=%d (%d/%d)", ret, msg_len, act_len);
+	else
+		if (act_len != msg_len)
+			ret = -EIO; /* all data is not send */
+	if (ret < 0)
+		goto err_mutex_unlock;
+
+	/* no ack for those packets */
+	if (req->cmd == CMD_FW_DL)
+		goto exit_mutex_unlock;
+
+	/* receive ack and data if read req */
+	msg_len = ACK_HDR_LEN + req->rlen + CHECKSUM_LEN;
+	ret = usb_bulk_msg(udev, usb_rcvbulkpipe(udev, 0x81), buf, msg_len,
+			&act_len, USB_TIMEOUT);
+	if (ret < 0) {
+		err("recv bulk message failed=%d", ret);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	if (act_len != msg_len) {
+		err("recv bulk message truncated (%d != %d)", act_len, msg_len);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	/* verify checksum */
+	checksum = af9035_checksum(buf, act_len - 2);
+	tmp_checksum = (buf[act_len - 2] << 8) | buf[act_len - 1];
+	if (tmp_checksum != checksum) {
+		err("%s: command=%02x checksum mismatch (%04x != %04x)",
+		    __func__, req->cmd, tmp_checksum, checksum);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	/* check status */
+	if (buf[2]) {
+		pr_debug("%s: command=%02x failed fw error=%d\n", __func__,
+				req->cmd, buf[2]);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	/* read request, copy returned data to return buf */
+	if (req->rlen)
+		memcpy(req->rbuf, &buf[ACK_HDR_LEN], req->rlen);
+
+err_mutex_unlock:
+exit_mutex_unlock:
+	mutex_unlock(&af9035_usb_mutex);
+
+	return ret;
+}
+
+/* write multiple registers */
+static int af9035_wr_regs(struct dvb_usb_device *d, u32 reg, u8 *val, int len)
+{
+	u8 wbuf[6 + len];
+	u8 mbox = (reg >> 16) & 0xff;
+	struct usb_req req = { CMD_MEM_WR, mbox, sizeof(wbuf), wbuf, 0, NULL };
+
+	wbuf[0] = len;
+	wbuf[1] = 2;
+	wbuf[2] = 0;
+	wbuf[3] = 0;
+	wbuf[4] = (reg >> 8) & 0xff;
+	wbuf[5] = (reg >> 0) & 0xff;
+	memcpy(&wbuf[6], val, len);
+
+	return af9035_ctrl_msg(d->udev, &req);
+}
+
+/* read multiple registers */
+static int af9035_rd_regs(struct dvb_usb_device *d, u32 reg, u8 *val, int len)
+{
+	u8 wbuf[] = { len, 2, 0, 0, (reg >> 8) & 0xff, reg & 0xff };
+	u8 mbox = (reg >> 16) & 0xff;
+	struct usb_req req = { CMD_MEM_RD, mbox, sizeof(wbuf), wbuf, len, val };
+
+	return af9035_ctrl_msg(d->udev, &req);
+}
+
+/* write single register */
+static int af9035_wr_reg(struct dvb_usb_device *d, u32 reg, u8 val)
+{
+	return af9035_wr_regs(d, reg, &val, 1);
+}
+
+/* read single register */
+static int af9035_rd_reg(struct dvb_usb_device *d, u32 reg, u8 *val)
+{
+	return af9035_rd_regs(d, reg, val, 1);
+}
+
+/* write single register with mask */
+static int af9035_wr_reg_mask(struct dvb_usb_device *d, u32 reg, u8 val,
+		u8 mask)
+{
+	int ret;
+	u8 tmp;
+
+	/* no need for read if whole reg is written */
+	if (mask != 0xff) {
+		ret = af9035_rd_regs(d, reg, &tmp, 1);
+		if (ret)
+			return ret;
+
+		val &= mask;
+		tmp &= ~mask;
+		val |= tmp;
+	}
+
+	return af9035_wr_regs(d, reg, &val, 1);
+}
+
+static int af9035_i2c_master_xfer(struct i2c_adapter *adap,
+		struct i2c_msg msg[], int num)
+{
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+	struct state *state = d->priv;
+	int ret;
+
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+
+	/*
+	 * I2C sub header is 5 bytes long. Meaning of those bytes are:
+	 * 0: data len
+	 * 1: I2C addr << 1
+	 * 2: reg addr len
+	 *    byte 3 and 4 can be used as reg addr
+	 * 3: reg addr MSB
+	 *    used when reg addr len is set to 2
+	 * 4: reg addr LSB
+	 *    used when reg addr len is set to 1 or 2
+	 *
+	 * For the simplify we do not use register addr at all.
+	 * NOTE: As a firmware knows tuner type there is very small possibility
+	 * there could be some tuner I2C hacks done by firmware and this may
+	 * lead problems if firmware expects those bytes are used.
+	 */
+	if (num == 2 && !(msg[0].flags & I2C_M_RD) &&
+			(msg[1].flags & I2C_M_RD)) {
+		if (msg[0].len > 40 || msg[1].len > 40) {
+			/* TODO: correct limits > 40 */
+			ret = -EOPNOTSUPP;
+		} else if (msg[0].addr == state->af9033_config[0].i2c_addr) {
+			/* integrated demod */
+			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
+					msg[0].buf[2];
+			ret = af9035_rd_regs(d, reg, &msg[1].buf[0],
+					msg[1].len);
+		} else {
+			/* I2C */
+			u8 buf[5 + msg[0].len];
+			struct usb_req req = { CMD_I2C_RD, 0, sizeof(buf),
+					buf, msg[1].len, msg[1].buf };
+			buf[0] = msg[1].len;
+			buf[1] = msg[0].addr << 1;
+			buf[2] = 0x00; /* reg addr len */
+			buf[3] = 0x00; /* reg addr MSB */
+			buf[4] = 0x00; /* reg addr LSB */
+			memcpy(&buf[5], msg[0].buf, msg[0].len);
+			ret = af9035_ctrl_msg(d->udev, &req);
+		}
+	} else if (num == 1 && !(msg[0].flags & I2C_M_RD)) {
+		if (msg[0].len > 40) {
+			/* TODO: correct limits > 40 */
+			ret = -EOPNOTSUPP;
+		} else if (msg[0].addr == state->af9033_config[0].i2c_addr) {
+			/* integrated demod */
+			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
+					msg[0].buf[2];
+			ret = af9035_wr_regs(d, reg, &msg[0].buf[3],
+					msg[0].len - 3);
+		} else {
+			/* I2C */
+			u8 buf[5 + msg[0].len];
+			struct usb_req req = { CMD_I2C_WR, 0, sizeof(buf), buf,
+					0, NULL };
+			buf[0] = msg[0].len;
+			buf[1] = msg[0].addr << 1;
+			buf[2] = 0x00; /* reg addr len */
+			buf[3] = 0x00; /* reg addr MSB */
+			buf[4] = 0x00; /* reg addr LSB */
+			memcpy(&buf[5], msg[0].buf, msg[0].len);
+			ret = af9035_ctrl_msg(d->udev, &req);
+		}
+	} else {
+		/*
+		 * We support only two kind of I2C transactions:
+		 * 1) 1 x read + 1 x write
+		 * 2) 1 x write
+		 */
+		ret = -EOPNOTSUPP;
+	}
+
+	mutex_unlock(&d->i2c_mutex);
+
+	if (ret < 0)
+		return ret;
+	else
+		return num;
+}
+
+static u32 af9035_i2c_functionality(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C;
+}
+
+static struct i2c_algorithm af9035_i2c_algo = {
+	.master_xfer = af9035_i2c_master_xfer,
+	.functionality = af9035_i2c_functionality,
+};
+
+#define AF9035_POLL 250
+static int af9035_rc_query(struct dvb_usb_device *d)
+{
+	unsigned int key;
+	unsigned char b[4];
+	int ret;
+	struct usb_req req = { CMD_IR_GET, 0, 0, NULL, 4, b };
+
+	ret = af9035_ctrl_msg(d->udev, &req);
+	if (ret < 0)
+		goto err;
+
+	if ((b[2] + b[3]) == 0xff) {
+		if ((b[0] + b[1]) == 0xff) {
+			/* NEC */
+			key = b[0] << 8 | b[2];
+		} else {
+			/* ext. NEC */
+			key = b[0] << 16 | b[1] << 8 | b[2];
+		}
+	} else {
+		key = b[0] << 24 | b[1] << 16 | b[2] << 8 | b[3];
+	}
+
+	rc_keydown(d->rc_dev, key, 0);
+
+err:
+	/* ignore errors */
+	return 0;
+}
+
+static int af9035_init(struct dvb_usb_device *d)
+{
+	struct state *state = d->priv;
+	int ret, i;
+	u16 frame_size = 87 * 188 / 4;
+	u8  packet_size = 512 / 4;
+	struct reg_val_mask tab[] = {
+		{ 0x80f99d, 0x01, 0x01 },
+		{ 0x80f9a4, 0x01, 0x01 },
+		{ 0x00dd11, 0x00, 0x20 },
+		{ 0x00dd11, 0x00, 0x40 },
+		{ 0x00dd13, 0x00, 0x20 },
+		{ 0x00dd13, 0x00, 0x40 },
+		{ 0x00dd11, 0x20, 0x20 },
+		{ 0x00dd88, (frame_size >> 0) & 0xff, 0xff},
+		{ 0x00dd89, (frame_size >> 8) & 0xff, 0xff},
+		{ 0x00dd0c, packet_size, 0xff},
+		{ 0x00dd11, state->dual_mode << 6, 0x40 },
+		{ 0x00dd8a, (frame_size >> 0) & 0xff, 0xff},
+		{ 0x00dd8b, (frame_size >> 8) & 0xff, 0xff},
+		{ 0x00dd0d, packet_size, 0xff },
+		{ 0x80f9a3, 0x00, 0x01 },
+		{ 0x80f9cd, 0x00, 0x01 },
+		{ 0x80f99d, 0x00, 0x01 },
+		{ 0x80f9a4, 0x00, 0x01 },
+	};
+
+	pr_debug("%s: USB speed=%d frame_size=%04x packet_size=%02x\n",
+		__func__, d->udev->speed, frame_size, packet_size);
+
+	/* init endpoints */
+	for (i = 0; i < ARRAY_SIZE(tab); i++) {
+		ret = af9035_wr_reg_mask(d, tab[i].reg, tab[i].val,
+				tab[i].mask);
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_identify_state(struct usb_device *udev,
+		struct dvb_usb_device_properties *props,
+		struct dvb_usb_device_description **desc,
+		int *cold)
+{
+	int ret;
+	u8 wbuf[1] = { 1 };
+	u8 rbuf[4];
+	struct usb_req req = { CMD_FW_QUERYINFO, 0, sizeof(wbuf), wbuf,
+			sizeof(rbuf), rbuf };
+
+	ret = af9035_ctrl_msg(udev, &req);
+	if (ret < 0)
+		goto err;
+
+	pr_debug("%s: reply=%02x %02x %02x %02x\n", __func__,
+		rbuf[0], rbuf[1], rbuf[2], rbuf[3]);
+	if (rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])
+		*cold = 0;
+	else
+		*cold = 1;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_download_firmware(struct usb_device *udev,
+		const struct firmware *fw)
+{
+	int ret, i, j, len;
+	u8 wbuf[1];
+	u8 rbuf[4];
+	struct usb_req req = { 0, 0, 0, NULL, 0, NULL };
+	struct usb_req req_fw_dl = { CMD_FW_DL, 0, 0, wbuf, 0, NULL };
+	struct usb_req req_fw_ver = { CMD_FW_QUERYINFO, 0, 1, wbuf, 4, rbuf } ;
+	u8 hdr_core;
+	u16 hdr_addr, hdr_data_len, hdr_checksum;
+	#define MAX_DATA 58
+	#define HDR_SIZE 7
+
+	/*
+	 * Thanks to Daniel Glöckner <daniel-gl@gmx.net> about that info!
+	 *
+	 * byte 0: MCS 51 core
+	 *  There are two inside the AF9035 (1=Link and 2=OFDM) with separate
+	 *  address spaces
+	 * byte 1-2: Big endian destination address
+	 * byte 3-4: Big endian number of data bytes following the header
+	 * byte 5-6: Big endian header checksum, apparently ignored by the chip
+	 *  Calculated as ~(h[0]*256+h[1]+h[2]*256+h[3]+h[4]*256)
+	 */
+
+	for (i = fw->size; i > HDR_SIZE;) {
+		hdr_core = fw->data[fw->size - i + 0];
+		hdr_addr = fw->data[fw->size - i + 1] << 8;
+		hdr_addr |= fw->data[fw->size - i + 2] << 0;
+		hdr_data_len = fw->data[fw->size - i + 3] << 8;
+		hdr_data_len |= fw->data[fw->size - i + 4] << 0;
+		hdr_checksum = fw->data[fw->size - i + 5] << 8;
+		hdr_checksum |= fw->data[fw->size - i + 6] << 0;
+
+		pr_debug("%s: core=%d addr=%04x data_len=%d checksum=%04x\n",
+				__func__, hdr_core, hdr_addr, hdr_data_len,
+				hdr_checksum);
+
+		if (((hdr_core != 1) && (hdr_core != 2)) ||
+				(hdr_data_len > i)) {
+			pr_debug("%s: bad firmware\n", __func__);
+			break;
+		}
+
+		/* download begin packet */
+		req.cmd = CMD_FW_DL_BEGIN;
+		ret = af9035_ctrl_msg(udev, &req);
+		if (ret < 0)
+			goto err;
+
+		/* download firmware packet(s) */
+		for (j = HDR_SIZE + hdr_data_len; j > 0; j -= MAX_DATA) {
+			len = j;
+			if (len > MAX_DATA)
+				len = MAX_DATA;
+			req_fw_dl.wlen = len;
+			req_fw_dl.wbuf = (u8 *) &fw->data[fw->size - i +
+					HDR_SIZE + hdr_data_len - j];
+			ret = af9035_ctrl_msg(udev, &req_fw_dl);
+			if (ret < 0)
+				goto err;
+		}
+
+		/* download end packet */
+		req.cmd = CMD_FW_DL_END;
+		ret = af9035_ctrl_msg(udev, &req);
+		if (ret < 0)
+			goto err;
+
+		i -= hdr_data_len + HDR_SIZE;
+
+		pr_debug("%s: data uploaded=%zu\n", __func__, fw->size - i);
+	}
+
+	/* firmware loaded, request boot */
+	req.cmd = CMD_FW_BOOT;
+	ret = af9035_ctrl_msg(udev, &req);
+	if (ret < 0)
+		goto err;
+
+	/* ensure firmware starts */
+	wbuf[0] = 1;
+	ret = af9035_ctrl_msg(udev, &req_fw_ver);
+	if (ret < 0)
+		goto err;
+
+	if (!(rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])) {
+		info("firmware did not run");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	info("firmware version=%d.%d.%d.%d", rbuf[0], rbuf[1], rbuf[2],
+			rbuf[3]);
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_download_firmware_it9135(struct usb_device *udev,
+		const struct firmware *fw)
+{
+	int ret, i, i_prev;
+	u8 wbuf[1];
+	u8 rbuf[4];
+	struct usb_req req = { 0, 0, 0, NULL, 0, NULL };
+	struct usb_req req_fw_dl = { CMD_FW_SCATTER_WR, 0, 0, NULL, 0, NULL };
+	struct usb_req req_fw_ver = { CMD_FW_QUERYINFO, 0, 1, wbuf, 4, rbuf } ;
+	#define HDR_SIZE 7
+
+	/*
+	 * There seems to be following firmware header. Meaning of bytes 0-3
+	 * is unknown.
+	 *
+	 * 0: 3
+	 * 1: 0, 1
+	 * 2: 0
+	 * 3: 1, 2, 3
+	 * 4: addr MSB
+	 * 5: addr LSB
+	 * 6: count of data bytes ?
+	 */
+
+	for (i = HDR_SIZE, i_prev = 0; i <= fw->size; i++) {
+		if (i == fw->size ||
+				(fw->data[i + 0] == 0x03 &&
+				(fw->data[i + 1] == 0x00 ||
+				fw->data[i + 1] == 0x01) &&
+				fw->data[i + 2] == 0x00)) {
+			req_fw_dl.wlen = i - i_prev;
+			req_fw_dl.wbuf = (u8 *) &fw->data[i_prev];
+			i_prev = i;
+			ret = af9035_ctrl_msg(udev, &req_fw_dl);
+			if (ret < 0)
+				goto err;
+
+			pr_debug("%s: data uploaded=%d\n", __func__, i);
+		}
+	}
+
+	/* firmware loaded, request boot */
+	req.cmd = CMD_FW_BOOT;
+	ret = af9035_ctrl_msg(udev, &req);
+	if (ret < 0)
+		goto err;
+
+	/* ensure firmware starts */
+	wbuf[0] = 1;
+	ret = af9035_ctrl_msg(udev, &req_fw_ver);
+	if (ret < 0)
+		goto err;
+
+	if (!(rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])) {
+		info("firmware did not run");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	info("firmware version=%d.%d.%d.%d", rbuf[0], rbuf[1], rbuf[2],
+			rbuf[3]);
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+/* abuse that callback as there is no better one for reading eeprom */
+static int af9035_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
+{
+	struct state *state = d->priv;
+	int ret, i, eeprom_shift = 0;
+	u8 tmp;
+	u16 tmp16;
+
+	/* check if there is dual tuners */
+	ret = af9035_rd_reg(d, EEPROM_DUAL_MODE, &tmp);
+	if (ret < 0)
+		goto err;
+
+	state->dual_mode = tmp;
+	pr_debug("%s: dual mode=%d\n", __func__, state->dual_mode);
+
+	for (i = 0; i < af9035_properties[0].num_adapters; i++) {
+		/* tuner */
+		ret = af9035_rd_reg(d, EEPROM_1_TUNER_ID + eeprom_shift, &tmp);
+		if (ret < 0)
+			goto err;
+
+		state->af9033_config[i].tuner = tmp;
+		pr_debug("%s: [%d]tuner=%02x\n", __func__, i, tmp);
+
+		switch (tmp) {
+		case AF9033_TUNER_TUA9001:
+		case AF9033_TUNER_FC0011:
+		case AF9033_TUNER_MXL5007T:
+		case AF9033_TUNER_TDA18218:
+			state->af9033_config[i].spec_inv = 1;
+			break;
+		default:
+			warn("tuner ID=%02x not supported, please report!",
+				tmp);
+		};
+
+		/* tuner IF frequency */
+		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_L + eeprom_shift, &tmp);
+		if (ret < 0)
+			goto err;
+
+		tmp16 = tmp;
+
+		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_H + eeprom_shift, &tmp);
+		if (ret < 0)
+			goto err;
+
+		tmp16 |= tmp << 8;
+
+		pr_debug("%s: [%d]IF=%d\n", __func__, i, tmp16);
+
+		eeprom_shift = 0x10; /* shift for the 2nd tuner params */
+	}
+
+	/* get demod clock */
+	ret = af9035_rd_reg(d, 0x00d800, &tmp);
+	if (ret < 0)
+		goto err;
+
+	tmp = (tmp >> 0) & 0x0f;
+
+	for (i = 0; i < af9035_properties[0].num_adapters; i++)
+		state->af9033_config[i].clock = clock_lut[tmp];
+
+	ret = af9035_rd_reg(d, EEPROM_IR_MODE, &tmp);
+	if (ret < 0)
+		goto err;
+	pr_debug("%s: ir_mode=%02x\n", __func__, tmp);
+
+	/* don't activate rc if in HID mode or if not available */
+	if (tmp == 5) {
+		ret = af9035_rd_reg(d, EEPROM_IR_TYPE, &tmp);
+		if (ret < 0)
+			goto err;
+		pr_debug("%s: ir_type=%02x\n", __func__, tmp);
+
+		switch (tmp) {
+		case 0: /* NEC */
+		default:
+			d->props.rc.core.protocol = RC_TYPE_NEC;
+			d->props.rc.core.allowed_protos = RC_TYPE_NEC;
+			break;
+		case 1: /* RC6 */
+			d->props.rc.core.protocol = RC_TYPE_RC6;
+			d->props.rc.core.allowed_protos = RC_TYPE_RC6;
+			break;
+		}
+		d->props.rc.core.rc_query = af9035_rc_query;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+/* abuse that callback as there is no better one for reading eeprom */
+static int af9035_read_mac_address_it9135(struct dvb_usb_device *d, u8 mac[6])
+{
+	struct state *state = d->priv;
+	int ret, i;
+	u8 tmp;
+
+	state->dual_mode = false;
+
+	/* get demod clock */
+	ret = af9035_rd_reg(d, 0x00d800, &tmp);
+	if (ret < 0)
+		goto err;
+
+	tmp = (tmp >> 0) & 0x0f;
+
+	for (i = 0; i < af9035_properties[0].num_adapters; i++)
+		state->af9033_config[i].clock = clock_lut_it9135[tmp];
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_fc0011_tuner_callback(struct dvb_usb_device *d,
+		int cmd, int arg)
+{
+	int ret;
+
+	switch (cmd) {
+	case FC0011_FE_CALLBACK_POWER:
+		/* Tuner enable */
+		ret = af9035_wr_reg_mask(d, 0xd8eb, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0xd8ec, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0xd8ed, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		/* LED */
+		ret = af9035_wr_reg_mask(d, 0xd8d0, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0xd8d1, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 50000);
+		break;
+	case FC0011_FE_CALLBACK_RESET:
+		ret = af9035_wr_reg(d, 0xd8e9, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg(d, 0xd8e8, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg(d, 0xd8e7, 1);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 20000);
+
+		ret = af9035_wr_reg(d, 0xd8e7, 0);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 20000);
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_tuner_callback(struct dvb_usb_device *d, int cmd, int arg)
+{
+	struct state *state = d->priv;
+
+	switch (state->af9033_config[0].tuner) {
+	case AF9033_TUNER_FC0011:
+		return af9035_fc0011_tuner_callback(d, cmd, arg);
+	default:
+		break;
+	}
+
+	return -ENODEV;
+}
+
+static int af9035_frontend_callback(void *adapter_priv, int component,
+				    int cmd, int arg)
+{
+	struct i2c_adapter *adap = adapter_priv;
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+
+	switch (component) {
+	case DVB_FRONTEND_COMPONENT_TUNER:
+		return af9035_tuner_callback(d, cmd, arg);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap->dev->priv;
+	int ret;
+
+	if (!state->af9033_config[adap->id].tuner) {
+		/* unsupported tuner */
+		ret = -ENODEV;
+		goto err;
+	}
+
+	if (adap->id == 0) {
+		state->af9033_config[0].ts_mode = AF9033_TS_MODE_USB;
+		state->af9033_config[1].ts_mode = AF9033_TS_MODE_SERIAL;
+
+		ret = af9035_wr_reg(adap->dev, 0x00417f,
+				state->af9033_config[1].i2c_addr);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg(adap->dev, 0x00d81a,
+				state->dual_mode);
+		if (ret < 0)
+			goto err;
+	}
+
+	/* attach demodulator */
+	adap->fe_adap[0].fe = dvb_attach(af9033_attach,
+			&state->af9033_config[adap->id], &adap->dev->i2c_adap);
+	if (adap->fe_adap[0].fe == NULL) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	/* disable I2C-gate */
+	adap->fe_adap[0].fe->ops.i2c_gate_ctrl = NULL;
+	adap->fe_adap[0].fe->callback = af9035_frontend_callback;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static struct tua9001_config af9035_tua9001_config = {
+	.i2c_addr = 0x60,
+};
+
+static const struct fc0011_config af9035_fc0011_config = {
+	.i2c_address = 0x60,
+};
+
+static struct mxl5007t_config af9035_mxl5007t_config = {
+	.xtal_freq_hz = MxL_XTAL_24_MHZ,
+	.if_freq_hz = MxL_IF_4_57_MHZ,
+	.invert_if = 0,
+	.loop_thru_enable = 0,
+	.clk_out_enable = 0,
+	.clk_out_amp = MxL_CLKOUT_AMP_0_94V,
+};
+
+static struct tda18218_config af9035_tda18218_config = {
+	.i2c_address = 0x60,
+	.i2c_wr_max = 21,
+};
+
+static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap->dev->priv;
+	int ret;
+	struct dvb_frontend *fe;
+
+	switch (state->af9033_config[adap->id].tuner) {
+	case AF9033_TUNER_TUA9001:
+		/* AF9035 gpiot3 = TUA9001 RESETN
+		   AF9035 gpiot2 = TUA9001 RXEN */
+
+		/* configure gpiot2 and gpiot2 as output */
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8ec, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8ed, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e8, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e9, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* reset tuner */
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e7, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(2000, 20000);
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e7, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* activate tuner RX */
+		/* TODO: use callback for TUA9001 RXEN */
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8eb, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* attach tuner */
+		fe = dvb_attach(tua9001_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, &af9035_tua9001_config);
+		break;
+	case AF9033_TUNER_FC0011:
+		fe = dvb_attach(fc0011_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, &af9035_fc0011_config);
+		break;
+	case AF9033_TUNER_MXL5007T:
+		ret = af9035_wr_reg(adap->dev, 0x00d8e0, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8e1, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8df, 0);
+		if (ret < 0)
+			goto err;
+
+		msleep(30);
+
+		ret = af9035_wr_reg(adap->dev, 0x00d8df, 1);
+		if (ret < 0)
+			goto err;
+
+		msleep(300);
+
+		ret = af9035_wr_reg(adap->dev, 0x00d8c0, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8c1, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8bf, 0);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8b4, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8b5, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8b3, 1);
+		if (ret < 0)
+			goto err;
+
+		/* attach tuner */
+		fe = dvb_attach(mxl5007t_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, 0x60, &af9035_mxl5007t_config);
+		break;
+	case AF9033_TUNER_TDA18218:
+		/* attach tuner */
+		fe = dvb_attach(tda18218_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, &af9035_tda18218_config);
+		break;
+	default:
+		fe = NULL;
+	}
+
+	if (fe == NULL) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+enum af9035_id_entry {
+	AF9035_15A4_9035,
+	AF9035_15A4_1000,
+	AF9035_15A4_1001,
+	AF9035_15A4_1002,
+	AF9035_15A4_1003,
+	AF9035_0CCD_0093,
+	AF9035_07CA_A835,
+	AF9035_07CA_B835,
+	AF9035_07CA_1867,
+	AF9035_07CA_A867,
+	AF9035_07CA_0825,
+};
+
+static struct usb_device_id af9035_id[] = {
+	[AF9035_15A4_9035] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_9035)},
+	[AF9035_15A4_1000] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1000)},
+	[AF9035_15A4_1001] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1001)},
+	[AF9035_15A4_1002] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1002)},
+	[AF9035_15A4_1003] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1003)},
+	[AF9035_0CCD_0093] = {
+		USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_T_STICK)},
+	[AF9035_07CA_A835] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A835)},
+	[AF9035_07CA_B835] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_B835)},
+	[AF9035_07CA_1867] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_1867)},
+	[AF9035_07CA_A867] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A867)},
+	[AF9035_07CA_0825] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_TWINSTAR)},
+	{},
+};
+
+MODULE_DEVICE_TABLE(usb, af9035_id);
+
+static struct dvb_usb_device_properties af9035_properties[] = {
+	{
+		.caps = DVB_USB_IS_AN_I2C_ADAPTER,
+
+		.usb_ctrl = DEVICE_SPECIFIC,
+		.download_firmware = af9035_download_firmware,
+		.firmware = "dvb-usb-af9035-02.fw",
+		.no_reconnect = 1,
+
+		.size_of_priv = sizeof(struct state),
+
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9035_frontend_attach,
+						.tuner_attach = af9035_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+							.u = {
+								.bulk = {
+									.buffersize = (87 * 188),
+								}
+							}
+						}
+					}
+				}
+			}
+		},
+
+		.identify_state = af9035_identify_state,
+		.read_mac_address = af9035_read_mac_address,
+
+		.i2c_algo = &af9035_i2c_algo,
+
+		.rc.core = {
+			.protocol       = RC_TYPE_UNKNOWN,
+			.module_name    = "af9035",
+			.rc_query       = NULL,
+			.rc_interval    = AF9035_POLL,
+			.allowed_protos = RC_TYPE_UNKNOWN,
+			.rc_codes       = RC_MAP_EMPTY,
+		},
+		.num_device_descs = 5,
+		.devices = {
+			{
+				.name = "Afatech AF9035 reference design",
+				.cold_ids = {
+					&af9035_id[AF9035_15A4_9035],
+					&af9035_id[AF9035_15A4_1000],
+					&af9035_id[AF9035_15A4_1001],
+					&af9035_id[AF9035_15A4_1002],
+					&af9035_id[AF9035_15A4_1003],
+				},
+			}, {
+				.name = "TerraTec Cinergy T Stick",
+				.cold_ids = {
+					&af9035_id[AF9035_0CCD_0093],
+				},
+			}, {
+				.name = "AVerMedia AVerTV Volar HD/PRO (A835)",
+				.cold_ids = {
+					&af9035_id[AF9035_07CA_A835],
+					&af9035_id[AF9035_07CA_B835],
+				},
+			}, {
+				.name = "AVerMedia HD Volar (A867)",
+				.cold_ids = {
+					&af9035_id[AF9035_07CA_1867],
+					&af9035_id[AF9035_07CA_A867],
+				},
+			}, {
+				.name = "AVerMedia Twinstar (A825)",
+				.cold_ids = {
+					&af9035_id[AF9035_07CA_0825],
+				},
+			},
+		}
+	},
+	{
+		.caps = DVB_USB_IS_AN_I2C_ADAPTER,
+
+		.usb_ctrl = DEVICE_SPECIFIC,
+		.download_firmware = af9035_download_firmware_it9135,
+		.firmware = "dvb-usb-it9135-01.fw",
+		.no_reconnect = 1,
+
+		.size_of_priv = sizeof(struct state),
+
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9035_frontend_attach,
+						.tuner_attach = af9035_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+							.u = {
+								.bulk = {
+									.buffersize = (87 * 188),
+								}
+							}
+						}
+					}
+				}
+			}
+		},
+
+		.identify_state = af9035_identify_state,
+		.read_mac_address = af9035_read_mac_address_it9135,
+
+		.i2c_algo = &af9035_i2c_algo,
+
+		.num_device_descs = 0, /* disabled as no support for IT9135 */
+		.devices = {
+			{
+				.name = "ITE Tech. IT9135 reference design",
+			},
+		}
+	},
+};
+
+static int af9035_usb_probe(struct usb_interface *intf,
+			    const struct usb_device_id *id)
+{
+	int ret, i;
+	struct dvb_usb_device *d = NULL;
+	struct usb_device *udev;
+	bool found;
+
+	pr_debug("%s: interface=%d\n", __func__,
+			intf->cur_altsetting->desc.bInterfaceNumber);
+
+	/* interface 0 is used by DVB-T receiver and
+	   interface 1 is for remote controller (HID) */
+	if (intf->cur_altsetting->desc.bInterfaceNumber != 0)
+		return 0;
+
+	/* Dynamic USB ID support. Replaces first device ID with current one. */
+	udev = interface_to_usbdev(intf);
+
+	for (i = 0, found = false; i < ARRAY_SIZE(af9035_id) - 1; i++) {
+		if (af9035_id[i].idVendor ==
+				le16_to_cpu(udev->descriptor.idVendor) &&
+				af9035_id[i].idProduct ==
+				le16_to_cpu(udev->descriptor.idProduct)) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		pr_debug("%s: using dynamic ID %04x:%04x\n", __func__,
+				le16_to_cpu(udev->descriptor.idVendor),
+				le16_to_cpu(udev->descriptor.idProduct));
+		af9035_properties[0].devices[0].cold_ids[0]->idVendor =
+				le16_to_cpu(udev->descriptor.idVendor);
+		af9035_properties[0].devices[0].cold_ids[0]->idProduct =
+				le16_to_cpu(udev->descriptor.idProduct);
+	}
+
+
+	for (i = 0; i < af9035_properties_count; i++) {
+		ret = dvb_usb_device_init(intf, &af9035_properties[i],
+				THIS_MODULE, &d, adapter_nr);
+
+		if (ret == -ENODEV)
+			continue;
+		else
+			break;
+	}
+
+	if (ret < 0)
+		goto err;
+
+	if (d) {
+		ret = af9035_init(d);
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+/* usb specific object needed to register this driver with the usb subsystem */
+static struct usb_driver af9035_usb_driver = {
+	.name = "dvb_usb_af9035",
+	.probe = af9035_usb_probe,
+	.disconnect = dvb_usb_device_exit,
+	.id_table = af9035_id,
+};
+
+module_usb_driver(af9035_usb_driver);
+
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_DESCRIPTION("Afatech AF9035 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/dvb-usb/af9035.h b/drivers/media/dvb/dvb-usb/af9035.h
new file mode 100644
index 000000000000..481a1a43dd2a
--- /dev/null
+++ b/drivers/media/dvb/dvb-usb/af9035.h
@@ -0,0 +1,113 @@
+/*
+ * Afatech AF9035 DVB USB driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * Copyright (C) 2012 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef AF9035_H
+#define AF9035_H
+
+/* prefix for dvb-usb log writings */
+#define DVB_USB_LOG_PREFIX "af9035"
+
+#include "dvb-usb.h"
+#include "af9033.h"
+#include "tua9001.h"
+#include "fc0011.h"
+#include "mxl5007t.h"
+#include "tda18218.h"
+
+struct reg_val {
+	u32 reg;
+	u8  val;
+};
+
+struct reg_val_mask {
+	u32 reg;
+	u8  val;
+	u8  mask;
+};
+
+struct usb_req {
+	u8  cmd;
+	u8  mbox;
+	u8  wlen;
+	u8  *wbuf;
+	u8  rlen;
+	u8  *rbuf;
+};
+
+struct state {
+	bool dual_mode;
+
+	struct af9033_config af9033_config[2];
+};
+
+u32 clock_lut[] = {
+	20480000, /*      FPGA */
+	16384000, /* 16.38 MHz */
+	20480000, /* 20.48 MHz */
+	36000000, /* 36.00 MHz */
+	30000000, /* 30.00 MHz */
+	26000000, /* 26.00 MHz */
+	28000000, /* 28.00 MHz */
+	32000000, /* 32.00 MHz */
+	34000000, /* 34.00 MHz */
+	24000000, /* 24.00 MHz */
+	22000000, /* 22.00 MHz */
+	12000000, /* 12.00 MHz */
+};
+
+u32 clock_lut_it9135[] = {
+	12000000, /* 12.00 MHz */
+	20480000, /* 20.48 MHz */
+	36000000, /* 36.00 MHz */
+	30000000, /* 30.00 MHz */
+	26000000, /* 26.00 MHz */
+	28000000, /* 28.00 MHz */
+	32000000, /* 32.00 MHz */
+	34000000, /* 34.00 MHz */
+	24000000, /* 24.00 MHz */
+	22000000, /* 22.00 MHz */
+};
+
+/* EEPROM locations */
+#define EEPROM_IR_MODE            0x430d
+#define EEPROM_DUAL_MODE          0x4326
+#define EEPROM_IR_TYPE            0x4329
+#define EEPROM_1_IFFREQ_L         0x432d
+#define EEPROM_1_IFFREQ_H         0x432e
+#define EEPROM_1_TUNER_ID         0x4331
+#define EEPROM_2_IFFREQ_L         0x433d
+#define EEPROM_2_IFFREQ_H         0x433e
+#define EEPROM_2_TUNER_ID         0x4341
+
+/* USB commands */
+#define CMD_MEM_RD                  0x00
+#define CMD_MEM_WR                  0x01
+#define CMD_I2C_RD                  0x02
+#define CMD_I2C_WR                  0x03
+#define CMD_IR_GET                  0x18
+#define CMD_FW_DL                   0x21
+#define CMD_FW_QUERYINFO            0x22
+#define CMD_FW_BOOT                 0x23
+#define CMD_FW_DL_BEGIN             0x24
+#define CMD_FW_DL_END               0x25
+#define CMD_FW_SCATTER_WR           0x29
+
+#endif
diff --git a/drivers/media/dvb/dvb-usb/dib0700_core.c b/drivers/media/dvb/dvb-usb/dib0700_core.c
index 02290c60f72f..7e9e00fae04e 100644
--- a/drivers/media/dvb/dvb-usb/dib0700_core.c
+++ b/drivers/media/dvb/dvb-usb/dib0700_core.c
@@ -32,7 +32,7 @@ int dib0700_get_version(struct dvb_usb_device *d, u32 *hwversion,
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	ret = usb_control_msg(d->udev, usb_rcvctrlpipe(d->udev, 0),
@@ -118,7 +118,7 @@ int dib0700_set_gpio(struct dvb_usb_device *d, enum dib07x0_gpios gpio, u8 gpio_
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_GPIO;
@@ -139,7 +139,7 @@ static int dib0700_set_usb_xfer_len(struct dvb_usb_device *d, u16 nb_ts_packets)
 	if (st->fw_version >= 0x10201) {
 		if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 			err("could not acquire lock");
-			return 0;
+			return -EINTR;
 		}
 
 		st->buf[0] = REQUEST_SET_USB_XFER_LEN;
@@ -178,7 +178,7 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 	/* Ensure nobody else hits the i2c bus while we're sending our
 	   sequence of messages, (such as the remote control thread) */
 	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
-		return -EAGAIN;
+		return -EINTR;
 
 	for (i = 0; i < num; i++) {
 		if (i == 0) {
@@ -228,7 +228,8 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 			/* Write request */
 			if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 				err("could not acquire lock");
-				return 0;
+				mutex_unlock(&d->i2c_mutex);
+				return -EINTR;
 			}
 			st->buf[0] = REQUEST_NEW_I2C_WRITE;
 			st->buf[1] = msg[i].addr << 1;
@@ -271,10 +272,11 @@ static int dib0700_i2c_xfer_legacy(struct i2c_adapter *adap,
 	int i,len;
 
 	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
-		return -EAGAIN;
+		return -EINTR;
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		mutex_unlock(&d->i2c_mutex);
+		return -EINTR;
 	}
 
 	for (i = 0; i < num; i++) {
@@ -369,7 +371,7 @@ static int dib0700_set_clock(struct dvb_usb_device *d, u8 en_pll,
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_CLOCK;
@@ -401,7 +403,7 @@ int dib0700_set_i2c_speed(struct dvb_usb_device *d, u16 scl_kHz)
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_I2C_PARAM;
@@ -561,7 +563,7 @@ int dib0700_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 
 	if (mutex_lock_interruptible(&adap->dev->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_ENABLE_VIDEO;
@@ -611,7 +613,7 @@ int dib0700_change_protocol(struct rc_dev *rc, u64 rc_type)
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_RC;
diff --git a/drivers/media/dvb/dvb-usb/dib0700_devices.c b/drivers/media/dvb/dvb-usb/dib0700_devices.c
index f9e966aa26e7..510001da6e83 100644
--- a/drivers/media/dvb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/dvb/dvb-usb/dib0700_devices.c
@@ -3569,6 +3569,7 @@ struct usb_device_id dib0700_usb_id_table[] = {
 	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE7090E) },
 	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE7790E) },
 /* 80 */{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE8096P) },
+	{ USB_DEVICE(USB_VID_ELGATO,	USB_PID_ELGATO_EYETV_DTT_2) },
 	{ 0 }		/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, dib0700_usb_id_table);
@@ -3832,7 +3833,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 			},
 		},
 
-		.num_device_descs = 11,
+		.num_device_descs = 12,
 		.devices = {
 			{   "DiBcom STK7070P reference design",
 				{ &dib0700_usb_id_table[15], NULL },
@@ -3878,6 +3879,10 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 				{ &dib0700_usb_id_table[50], NULL },
 				{ NULL },
 			},
+			{   "Elgato EyeTV DTT rev. 2",
+				{ &dib0700_usb_id_table[81], NULL },
+				{ NULL },
+			},
 		},
 
 		.rc.core = {
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
index 397d8f232731..7a6160bf54ba 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
@@ -76,6 +76,11 @@
 #define USB_PID_AFATECH_AF9005				0x9020
 #define USB_PID_AFATECH_AF9015_9015			0x9015
 #define USB_PID_AFATECH_AF9015_9016			0x9016
+#define USB_PID_AFATECH_AF9035_1000			0x1000
+#define USB_PID_AFATECH_AF9035_1001			0x1001
+#define USB_PID_AFATECH_AF9035_1002			0x1002
+#define USB_PID_AFATECH_AF9035_1003			0x1003
+#define USB_PID_AFATECH_AF9035_9035			0x9035
 #define USB_PID_TREKSTOR_DVBT				0x901b
 #define USB_VID_ALINK_DTU				0xf170
 #define USB_PID_ANSONIC_DVBT_USB			0x6000
@@ -152,6 +157,7 @@
 #define USB_PID_KWORLD_VSTREAM_WARM			0x17df
 #define USB_PID_TERRATEC_CINERGY_T_USB_XE		0x0055
 #define USB_PID_TERRATEC_CINERGY_T_USB_XE_REV2		0x0069
+#define USB_PID_TERRATEC_CINERGY_T_STICK		0x0093
 #define USB_PID_TERRATEC_CINERGY_T_STICK_RC		0x0097
 #define USB_PID_TERRATEC_CINERGY_T_STICK_DUAL_RC	0x0099
 #define USB_PID_TWINHAN_VP7041_COLD			0x3201
@@ -221,6 +227,11 @@
 #define USB_PID_AVERMEDIA_A850T				0x850b
 #define USB_PID_AVERMEDIA_A805				0xa805
 #define USB_PID_AVERMEDIA_A815M				0x815a
+#define USB_PID_AVERMEDIA_A835				0xa835
+#define USB_PID_AVERMEDIA_B835				0xb835
+#define USB_PID_AVERMEDIA_1867				0x1867
+#define USB_PID_AVERMEDIA_A867				0xa867
+#define USB_PID_AVERMEDIA_TWINSTAR			0x0825
 #define USB_PID_TECHNOTREND_CONNECT_S2400               0x3006
 #define USB_PID_TECHNOTREND_CONNECT_CT3650		0x300d
 #define USB_PID_TERRATEC_CINERGY_DT_XS_DIVERSITY	0x005a
@@ -327,6 +338,7 @@
 #define USB_PID_MYGICA_D689				0xd811
 #define USB_PID_ELGATO_EYETV_DIVERSITY			0x0011
 #define USB_PID_ELGATO_EYETV_DTT			0x0021
+#define USB_PID_ELGATO_EYETV_DTT_2			0x003f
 #define USB_PID_ELGATO_EYETV_DTT_Dlx			0x0020
 #define USB_PID_ELGATO_EYETV_SAT			0x002a
 #define USB_PID_DVB_T_USB_STICK_HIGH_SPEED_COLD		0x5000
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-urb.c b/drivers/media/dvb/dvb-usb/dvb-usb-urb.c
index 53a5c30b51b2..5c8f651344fc 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-urb.c
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-urb.c
@@ -80,6 +80,14 @@ static void dvb_usb_data_complete_204(struct usb_data_stream *stream, u8 *buffer
 		dvb_dmx_swfilter_204(&adap->demux, buffer, length);
 }
 
+static void dvb_usb_data_complete_raw(struct usb_data_stream *stream,
+				      u8 *buffer, size_t length)
+{
+	struct dvb_usb_adapter *adap = stream->user_priv;
+	if (adap->feedcount > 0 && adap->state & DVB_USB_ADAP_STATE_DVB)
+		dvb_dmx_swfilter_raw(&adap->demux, buffer, length);
+}
+
 int dvb_usb_adapter_stream_init(struct dvb_usb_adapter *adap)
 {
 	int i, ret = 0;
@@ -90,6 +98,10 @@ int dvb_usb_adapter_stream_init(struct dvb_usb_adapter *adap)
 			adap->fe_adap[i].stream.complete =
 				dvb_usb_data_complete_204;
 		else
+		if (adap->props.fe[i].caps & DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD)
+			adap->fe_adap[i].stream.complete =
+				dvb_usb_data_complete_raw;
+		else
 		adap->fe_adap[i].stream.complete  = dvb_usb_data_complete;
 		adap->fe_adap[i].stream.user_priv = adap;
 		ret = usb_urb_init(&adap->fe_adap[i].stream,
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb.h b/drivers/media/dvb/dvb-usb/dvb-usb.h
index 6d7d13f9ce68..99f94409efa1 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb.h
@@ -141,6 +141,7 @@ struct dvb_usb_adapter_fe_properties {
 #define DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF 0x02
 #define DVB_USB_ADAP_NEED_PID_FILTERING           0x04
 #define DVB_USB_ADAP_RECEIVES_204_BYTE_TS         0x08
+#define DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD         0x10
 	int caps;
 	int pid_filter_count;
 
@@ -156,7 +157,7 @@ struct dvb_usb_adapter_fe_properties {
 	int size_of_priv;
 };
 
-#define MAX_NO_OF_FE_PER_ADAP 2
+#define MAX_NO_OF_FE_PER_ADAP 3
 struct dvb_usb_adapter_properties {
 	int size_of_priv;
 
diff --git a/drivers/media/dvb/dvb-usb/dw2102.c b/drivers/media/dvb/dvb-usb/dw2102.c
index 451c5a7adfb2..9382895b1b88 100644
--- a/drivers/media/dvb/dvb-usb/dw2102.c
+++ b/drivers/media/dvb/dvb-usb/dw2102.c
@@ -148,7 +148,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int i = 0, ret = 0;
+	int i = 0;
 	u8 buf6[] = {0x2c, 0x05, 0xc0, 0, 0, 0, 0};
 	u16 value;
 
@@ -162,7 +162,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		/* read stv0299 register */
 		value = msg[0].buf[0];/* register */
 		for (i = 0; i < msg[1].len; i++) {
-			ret = dw210x_op_rw(d->udev, 0xb5, value + i, 0,
+			dw210x_op_rw(d->udev, 0xb5, value + i, 0,
 					buf6, 2, DW210X_READ_MSG);
 			msg[1].buf[i] = buf6[0];
 		}
@@ -174,7 +174,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			buf6[0] = 0x2a;
 			buf6[1] = msg[0].buf[0];
 			buf6[2] = msg[0].buf[1];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					buf6, 3, DW210X_WRITE_MSG);
 			break;
 		case 0x60:
@@ -187,17 +187,17 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				buf6[4] = msg[0].buf[1];
 				buf6[5] = msg[0].buf[2];
 				buf6[6] = msg[0].buf[3];
-				ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+				dw210x_op_rw(d->udev, 0xb2, 0, 0,
 						buf6, 7, DW210X_WRITE_MSG);
 			} else {
 			/* read from tuner */
-				ret = dw210x_op_rw(d->udev, 0xb5, 0, 0,
+				dw210x_op_rw(d->udev, 0xb5, 0, 0,
 						buf6, 1, DW210X_READ_MSG);
 				msg[0].buf[0] = buf6[0];
 			}
 			break;
 		case (DW2102_RC_QUERY):
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					buf6, 2, DW210X_READ_MSG);
 			msg[0].buf[0] = buf6[0];
 			msg[0].buf[1] = buf6[1];
@@ -205,7 +205,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		case (DW2102_VOLTAGE_CTRL):
 			buf6[0] = 0x30;
 			buf6[1] = msg[0].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					buf6, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -221,7 +221,6 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 						struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0;
 	u8 buf6[] = {0, 0, 0, 0, 0, 0, 0};
 
 	if (!d)
@@ -235,10 +234,10 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 		buf6[0] = msg[0].addr << 1;
 		buf6[1] = msg[0].len;
 		buf6[2] = msg[0].buf[0];
-		ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+		dw210x_op_rw(d->udev, 0xc2, 0, 0,
 				buf6, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* read si2109 register */
-		ret = dw210x_op_rw(d->udev, 0xc3, 0xd0, 0,
+		dw210x_op_rw(d->udev, 0xc3, 0xd0, 0,
 				buf6, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, buf6 + 2, msg[1].len);
 
@@ -250,11 +249,11 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 			buf6[0] = msg[0].addr << 1;
 			buf6[1] = msg[0].len;
 			memcpy(buf6 + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0, buf6,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0, buf6,
 					msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		case(DW2102_RC_QUERY):
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					buf6, 2, DW210X_READ_MSG);
 			msg[0].buf[0] = buf6[0];
 			msg[0].buf[1] = buf6[1];
@@ -262,7 +261,7 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 		case(DW2102_VOLTAGE_CTRL):
 			buf6[0] = 0x30;
 			buf6[1] = msg[0].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					buf6, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -276,7 +275,6 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0;
 
 	if (!d)
 		return -ENODEV;
@@ -291,10 +289,10 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 		obuf[0] = msg[0].addr << 1;
 		obuf[1] = msg[0].len;
 		obuf[2] = msg[0].buf[0];
-		ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+		dw210x_op_rw(d->udev, 0xc2, 0, 0,
 				obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* second read registers */
-		ret = dw210x_op_rw(d->udev, 0xc3, 0xd1 , 0,
+		dw210x_op_rw(d->udev, 0xc3, 0xd1 , 0,
 				ibuf, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, ibuf + 2, msg[1].len);
 
@@ -308,7 +306,7 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			obuf[0] = msg[0].addr << 1;
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0,
 					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -318,13 +316,13 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			obuf[0] = msg[0].addr << 1;
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0,
 					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 2, DW210X_READ_MSG);
 			memcpy(msg[0].buf, ibuf , 2);
 			break;
@@ -333,7 +331,7 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			u8 obuf[2];
 			obuf[0] = 0x30;
 			obuf[1] = msg[0].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -349,7 +347,6 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0;
 	int len, i, j;
 
 	if (!d)
@@ -361,7 +358,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 		switch (msg[j].addr) {
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 2, DW210X_READ_MSG);
 			memcpy(msg[j].buf, ibuf , 2);
 			break;
@@ -370,7 +367,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 			u8 obuf[2];
 			obuf[0] = 0x30;
 			obuf[1] = msg[j].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -382,7 +379,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 			if (msg[j].flags == I2C_M_RD) {
 				/* read registers */
 				u8  ibuf[msg[j].len + 2];
-				ret = dw210x_op_rw(d->udev, 0xc3,
+				dw210x_op_rw(d->udev, 0xc3,
 						(msg[j].addr << 1) + 1, 0,
 						ibuf, msg[j].len + 2,
 						DW210X_READ_MSG);
@@ -402,7 +399,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				do {
 					memcpy(obuf + 3, msg[j].buf + i,
 							(len > 16 ? 16 : len));
-					ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+					dw210x_op_rw(d->udev, 0xc2, 0, 0,
 						obuf, (len > 16 ? 16 : len) + 3,
 						DW210X_WRITE_MSG);
 					i += 16;
@@ -414,7 +411,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				obuf[0] = msg[j].addr << 1;
 				obuf[1] = msg[j].len;
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+				dw210x_op_rw(d->udev, 0xc2, 0, 0,
 						obuf, msg[j].len + 2,
 						DW210X_WRITE_MSG);
 			}
@@ -432,7 +429,7 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 								int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0, i;
+	int i;
 
 	if (!d)
 		return -ENODEV;
@@ -447,10 +444,10 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		obuf[0] = msg[0].addr << 1;
 		obuf[1] = msg[0].len;
 		obuf[2] = msg[0].buf[0];
-		ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+		dw210x_op_rw(d->udev, 0xc2, 0, 0,
 				obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* second read registers */
-		ret = dw210x_op_rw(d->udev, 0xc3, 0x19 , 0,
+		dw210x_op_rw(d->udev, 0xc3, 0x19 , 0,
 				ibuf, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, ibuf + 2, msg[1].len);
 
@@ -465,13 +462,13 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			obuf[0] = msg[0].addr << 1;
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0,
 					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 2, DW210X_READ_MSG);
 			memcpy(msg[0].buf, ibuf , 2);
 			break;
@@ -496,7 +493,6 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	struct usb_device *udev;
-	int ret = 0;
 	int len, i, j;
 
 	if (!d)
@@ -509,7 +505,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		switch (msg[j].addr) {
 		case (DW2102_RC_QUERY): {
 			u8 ibuf[5];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 5, DW210X_READ_MSG);
 			memcpy(msg[j].buf, ibuf + 3, 2);
 			break;
@@ -519,11 +515,11 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 
 			obuf[0] = 1;
 			obuf[1] = msg[j].buf[1];/* off-on */
-			ret = dw210x_op_rw(d->udev, 0x8a, 0, 0,
+			dw210x_op_rw(d->udev, 0x8a, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			obuf[0] = 3;
 			obuf[1] = msg[j].buf[0];/* 13v-18v */
-			ret = dw210x_op_rw(d->udev, 0x8a, 0, 0,
+			dw210x_op_rw(d->udev, 0x8a, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -532,7 +528,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 
 			obuf[0] = 5;
 			obuf[1] = msg[j].buf[0];
-			ret = dw210x_op_rw(d->udev, 0x8a, 0, 0,
+			dw210x_op_rw(d->udev, 0x8a, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -545,7 +541,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			if (msg[j].flags == I2C_M_RD) {
 				/* read registers */
 				u8 ibuf[msg[j].len];
-				ret = dw210x_op_rw(d->udev, 0x91, 0, 0,
+				dw210x_op_rw(d->udev, 0x91, 0, 0,
 						ibuf, msg[j].len,
 						DW210X_READ_MSG);
 				memcpy(msg[j].buf, ibuf, msg[j].len);
@@ -563,7 +559,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				do {
 					memcpy(obuf + 3, msg[j].buf + i,
 							(len > 16 ? 16 : len));
-					ret = dw210x_op_rw(d->udev, 0x80, 0, 0,
+					dw210x_op_rw(d->udev, 0x80, 0, 0,
 						obuf, (len > 16 ? 16 : len) + 3,
 						DW210X_WRITE_MSG);
 					i += 16;
@@ -575,7 +571,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[0] = msg[j + 1].len;
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev,
+				dw210x_op_rw(d->udev,
 						udev->descriptor.idProduct ==
 						0x7500 ? 0x92 : 0x90, 0, 0,
 						obuf, msg[j].len + 2,
@@ -587,7 +583,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[0] = msg[j].len + 1;
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev, 0x80, 0, 0,
+				dw210x_op_rw(d->udev, 0x80, 0, 0,
 						obuf, msg[j].len + 2,
 						DW210X_WRITE_MSG);
 				break;
diff --git a/drivers/media/dvb/dvb-usb/it913x.c b/drivers/media/dvb/dvb-usb/it913x.c
index 482d249ca7f3..6244fe9d1a3a 100644
--- a/drivers/media/dvb/dvb-usb/it913x.c
+++ b/drivers/media/dvb/dvb-usb/it913x.c
@@ -81,7 +81,7 @@ static int it913x_bulk_write(struct usb_device *dev,
 	for (i = 0; i < IT913X_RETRY; i++) {
 		ret = usb_bulk_msg(dev, usb_sndbulkpipe(dev, pipe),
 				snd, len , &actual_l, IT913X_SND_TIMEOUT);
-		if (ret == 0 || ret != -EBUSY || ret != -ETIMEDOUT)
+		if (ret != -EBUSY && ret != -ETIMEDOUT)
 			break;
 	}
 
@@ -99,7 +99,7 @@ static int it913x_bulk_read(struct usb_device *dev,
 	for (i = 0; i < IT913X_RETRY; i++) {
 		ret = usb_bulk_msg(dev, usb_rcvbulkpipe(dev, pipe),
 				 rev, len , &actual_l, IT913X_RCV_TIMEOUT);
-		if (ret == 0 || ret != -EBUSY || ret != -ETIMEDOUT)
+		if (ret != -EBUSY && ret != -ETIMEDOUT)
 			break;
 	}
 
diff --git a/drivers/media/dvb/dvb-usb/lmedm04.c b/drivers/media/dvb/dvb-usb/lmedm04.c
index 5dde06d066ff..25d1031460f8 100644
--- a/drivers/media/dvb/dvb-usb/lmedm04.c
+++ b/drivers/media/dvb/dvb-usb/lmedm04.c
@@ -373,7 +373,7 @@ static int lme2510_pid_filter_ctrl(struct dvb_usb_adapter *adap, int onoff)
 	struct lme2510_state *st = adap->dev->priv;
 	static u8 clear_pid_reg[] = LME_ALL_PIDS;
 	static u8 rbuf[1];
-	int ret;
+	int ret = 0;
 
 	deb_info(1, "PID Clearing Filter");
 
@@ -1205,14 +1205,13 @@ static int lme2510_probe(struct usb_interface *intf,
 		const struct usb_device_id *id)
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
-	int ret = 0;
 
 	usb_reset_configuration(udev);
 
 	usb_set_interface(udev, intf->cur_altsetting->desc.bInterfaceNumber, 1);
 
 	if (udev->speed != USB_SPEED_HIGH) {
-		ret = usb_reset_device(udev);
+		usb_reset_device(udev);
 		info("DEV Failed to connect in HIGH SPEED mode");
 		return -ENODEV;
 	}
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c b/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c
index 72db6eef4b9c..74da5bb1ce99 100644
--- a/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c
+++ b/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c
@@ -284,6 +284,7 @@ static int mxl111sf_tuner_set_params(struct dvb_frontend *fe)
 
 	switch (delsys) {
 	case SYS_ATSC:
+	case SYS_ATSCMH:
 		bw = 0; /* ATSC */
 		break;
 	case SYS_DVBC_ANNEX_B:
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf.c b/drivers/media/dvb/dvb-usb/mxl111sf.c
index 81305de2fea5..cd842798f5af 100644
--- a/drivers/media/dvb/dvb-usb/mxl111sf.c
+++ b/drivers/media/dvb/dvb-usb/mxl111sf.c
@@ -21,6 +21,7 @@
 #include "mxl111sf-tuner.h"
 
 #include "lgdt3305.h"
+#include "lg2160.h"
 
 int dvb_usb_mxl111sf_debug;
 module_param_named(debug, dvb_usb_mxl111sf_debug, int, 0644);
@@ -31,6 +32,10 @@ int dvb_usb_mxl111sf_isoc;
 module_param_named(isoc, dvb_usb_mxl111sf_isoc, int, 0644);
 MODULE_PARM_DESC(isoc, "enable usb isoc xfer (0=bulk, 1=isoc).");
 
+int dvb_usb_mxl111sf_spi;
+module_param_named(spi, dvb_usb_mxl111sf_spi, int, 0644);
+MODULE_PARM_DESC(spi, "use spi rather than tp for data xfer (0=tp, 1=spi).");
+
 #define ANT_PATH_AUTO 0
 #define ANT_PATH_EXTERNAL 1
 #define ANT_PATH_INTERNAL 2
@@ -340,7 +345,6 @@ static int mxl111sf_ep6_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 	struct mxl111sf_state *state = d->priv;
 	struct mxl111sf_adap_state *adap_state = adap->fe_adap[adap->active_fe].priv;
 	int ret = 0;
-	u8 tmp;
 
 	deb_info("%s(%d)\n", __func__, onoff);
 
@@ -361,6 +365,33 @@ static int mxl111sf_ep6_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 	return ret;
 }
 
+static int mxl111sf_ep5_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int ret = 0;
+
+	deb_info("%s(%d)\n", __func__, onoff);
+
+	if (onoff) {
+		ret = mxl111sf_enable_usb_output(state);
+		mxl_fail(ret);
+
+		ret = mxl111sf_init_i2s_port(state, 200);
+		mxl_fail(ret);
+		ret = mxl111sf_config_i2s(state, 0, 15);
+		mxl_fail(ret);
+	} else {
+		ret = mxl111sf_disable_i2s_port(state);
+		mxl_fail(ret);
+	}
+	if (state->chip_rev > MXL111SF_V6)
+		ret = mxl111sf_config_spi(state, onoff);
+	mxl_fail(ret);
+
+	return ret;
+}
+
 static int mxl111sf_ep4_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 {
 	struct dvb_usb_device *d = adap->dev;
@@ -453,6 +484,255 @@ fail:
 	return ret;
 }
 
+static struct lg2160_config hauppauge_lg2160_config = {
+	.lg_chip            = LG2160,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+};
+
+static int mxl111sf_lg2160_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int fe_id = adap->num_frontends_initialized;
+	struct mxl111sf_adap_state *adap_state = adap->fe_adap[fe_id].priv;
+	int ret;
+
+	deb_adv("%s()\n", __func__);
+
+	/* save a pointer to the dvb_usb_device in device state */
+	state->d = d;
+	adap_state->alt_mode = (dvb_usb_mxl111sf_isoc) ? 2 : 1;
+	state->alt_mode = adap_state->alt_mode;
+
+	if (usb_set_interface(adap->dev->udev, 0, state->alt_mode) < 0)
+		err("set interface failed");
+
+	state->gpio_mode = MXL111SF_GPIO_MOD_MH;
+	adap_state->gpio_mode = state->gpio_mode;
+	adap_state->device_mode = MXL_TUNER_MODE;
+	adap_state->ep6_clockphase = 1;
+
+	ret = mxl1x1sf_soft_reset(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_init_tuner_demod(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_enable_usb_output(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl1x1sf_top_master_ctrl(state, 1);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_init_port_expander(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_gpio_mode_switch(state, state->gpio_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = get_chip_info(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	adap->fe_adap[fe_id].fe = dvb_attach(lg2160_attach,
+			      &hauppauge_lg2160_config,
+			      &adap->dev->i2c_adap);
+	if (adap->fe_adap[fe_id].fe) {
+		adap_state->fe_init = adap->fe_adap[fe_id].fe->ops.init;
+		adap->fe_adap[fe_id].fe->ops.init = mxl111sf_adap_fe_init;
+		adap_state->fe_sleep = adap->fe_adap[fe_id].fe->ops.sleep;
+		adap->fe_adap[fe_id].fe->ops.sleep = mxl111sf_adap_fe_sleep;
+		return 0;
+	}
+	ret = -EIO;
+fail:
+	return ret;
+}
+
+static struct lg2160_config hauppauge_lg2161_1019_config = {
+	.lg_chip            = LG2161_1019,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 2, /* LG2161_OIF_SPI_MAS */
+};
+
+static struct lg2160_config hauppauge_lg2161_1040_config = {
+	.lg_chip            = LG2161_1040,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 4, /* LG2161_OIF_SPI_MAS */
+};
+
+static int mxl111sf_lg2161_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int fe_id = adap->num_frontends_initialized;
+	struct mxl111sf_adap_state *adap_state = adap->fe_adap[fe_id].priv;
+	int ret;
+
+	deb_adv("%s()\n", __func__);
+
+	/* save a pointer to the dvb_usb_device in device state */
+	state->d = d;
+	adap_state->alt_mode = (dvb_usb_mxl111sf_isoc) ? 2 : 1;
+	state->alt_mode = adap_state->alt_mode;
+
+	if (usb_set_interface(adap->dev->udev, 0, state->alt_mode) < 0)
+		err("set interface failed");
+
+	state->gpio_mode = MXL111SF_GPIO_MOD_MH;
+	adap_state->gpio_mode = state->gpio_mode;
+	adap_state->device_mode = MXL_TUNER_MODE;
+	adap_state->ep6_clockphase = 1;
+
+	ret = mxl1x1sf_soft_reset(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_init_tuner_demod(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_enable_usb_output(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl1x1sf_top_master_ctrl(state, 1);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_init_port_expander(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_gpio_mode_switch(state, state->gpio_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = get_chip_info(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	adap->fe_adap[fe_id].fe = dvb_attach(lg2160_attach,
+			      (MXL111SF_V8_200 == state->chip_rev) ?
+			      &hauppauge_lg2161_1040_config :
+			      &hauppauge_lg2161_1019_config,
+			      &adap->dev->i2c_adap);
+	if (adap->fe_adap[fe_id].fe) {
+		adap_state->fe_init = adap->fe_adap[fe_id].fe->ops.init;
+		adap->fe_adap[fe_id].fe->ops.init = mxl111sf_adap_fe_init;
+		adap_state->fe_sleep = adap->fe_adap[fe_id].fe->ops.sleep;
+		adap->fe_adap[fe_id].fe->ops.sleep = mxl111sf_adap_fe_sleep;
+		return 0;
+	}
+	ret = -EIO;
+fail:
+	return ret;
+}
+
+static struct lg2160_config hauppauge_lg2161_1019_ep6_config = {
+	.lg_chip            = LG2161_1019,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 1, /* LG2161_OIF_SERIAL_TS */
+};
+
+static struct lg2160_config hauppauge_lg2161_1040_ep6_config = {
+	.lg_chip            = LG2161_1040,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 7, /* LG2161_OIF_SERIAL_TS */
+};
+
+static int mxl111sf_lg2161_ep6_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int fe_id = adap->num_frontends_initialized;
+	struct mxl111sf_adap_state *adap_state = adap->fe_adap[fe_id].priv;
+	int ret;
+
+	deb_adv("%s()\n", __func__);
+
+	/* save a pointer to the dvb_usb_device in device state */
+	state->d = d;
+	adap_state->alt_mode = (dvb_usb_mxl111sf_isoc) ? 2 : 1;
+	state->alt_mode = adap_state->alt_mode;
+
+	if (usb_set_interface(adap->dev->udev, 0, state->alt_mode) < 0)
+		err("set interface failed");
+
+	state->gpio_mode = MXL111SF_GPIO_MOD_MH;
+	adap_state->gpio_mode = state->gpio_mode;
+	adap_state->device_mode = MXL_TUNER_MODE;
+	adap_state->ep6_clockphase = 0;
+
+	ret = mxl1x1sf_soft_reset(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_init_tuner_demod(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_enable_usb_output(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl1x1sf_top_master_ctrl(state, 1);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_init_port_expander(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_gpio_mode_switch(state, state->gpio_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = get_chip_info(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	adap->fe_adap[fe_id].fe = dvb_attach(lg2160_attach,
+			      (MXL111SF_V8_200 == state->chip_rev) ?
+			      &hauppauge_lg2161_1040_ep6_config :
+			      &hauppauge_lg2161_1019_ep6_config,
+			      &adap->dev->i2c_adap);
+	if (adap->fe_adap[fe_id].fe) {
+		adap_state->fe_init = adap->fe_adap[fe_id].fe->ops.init;
+		adap->fe_adap[fe_id].fe->ops.init = mxl111sf_adap_fe_init;
+		adap_state->fe_sleep = adap->fe_adap[fe_id].fe->ops.sleep;
+		adap->fe_adap[fe_id].fe->ops.sleep = mxl111sf_adap_fe_sleep;
+		return 0;
+	}
+	ret = -EIO;
+fail:
+	return ret;
+}
+
 static struct mxl111sf_demod_config mxl_demod_config = {
 	.read_reg        = mxl111sf_read_reg,
 	.write_reg       = mxl111sf_write_reg,
@@ -650,6 +930,18 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties;
 static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties;
 static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties;
 static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mh_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mh_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_isoc_properties;
 
 static int mxl111sf_probe(struct usb_interface *intf,
 			  const struct usb_device_id *id)
@@ -664,12 +956,50 @@ static int mxl111sf_probe(struct usb_interface *intf,
 				       THIS_MODULE, &d, adapter_nr) ||
 	      0 == dvb_usb_device_init(intf,
 				       &mxl111sf_atsc_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_atsc_mh_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mh_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      ((dvb_usb_mxl111sf_spi) &&
+	       (0 == dvb_usb_device_init(intf,
+					 &mxl111sf_mercury_spi_isoc_properties,
+					 THIS_MODULE, &d, adapter_nr) ||
+		0 == dvb_usb_device_init(intf,
+					 &mxl111sf_mercury_mh_spi_isoc_properties,
+					 THIS_MODULE, &d, adapter_nr))) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_tp_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_mh_tp_isoc_properties,
 				       THIS_MODULE, &d, adapter_nr))) ||
 	    0 == dvb_usb_device_init(intf,
 				     &mxl111sf_dvbt_bulk_properties,
 				     THIS_MODULE, &d, adapter_nr) ||
 	    0 == dvb_usb_device_init(intf,
 				     &mxl111sf_atsc_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_atsc_mh_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_mh_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    ((dvb_usb_mxl111sf_spi) &&
+	     (0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_spi_bulk_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_mh_spi_bulk_properties,
+				       THIS_MODULE, &d, adapter_nr))) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_mercury_tp_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_mercury_mh_tp_bulk_properties,
 				     THIS_MODULE, &d, adapter_nr) || 0) {
 
 		struct mxl111sf_state *state = d->priv;
@@ -787,6 +1117,36 @@ MODULE_DEVICE_TABLE(usb, mxl111sf_table);
 		}					\
 	}
 
+#define MXL111SF_EP5_BULK_STREAMING_CONFIG		\
+	.size_of_priv = sizeof(struct mxl111sf_adap_state), \
+	.streaming_ctrl = mxl111sf_ep5_streaming_ctrl,	\
+	.stream = {					\
+		.type = USB_BULK,			\
+		.count = 5,				\
+		.endpoint = 0x05,			\
+		.u = {					\
+			.bulk = {			\
+				.buffersize = 8192,	\
+			}				\
+		}					\
+	}
+
+#define MXL111SF_EP5_ISOC_STREAMING_CONFIG		\
+	.size_of_priv = sizeof(struct mxl111sf_adap_state), \
+	.streaming_ctrl = mxl111sf_ep5_streaming_ctrl,	\
+	.stream = {					\
+		.type = USB_ISOC,			\
+		.count = 5,				\
+		.endpoint = 0x05,			\
+		.u = {					\
+			.isoc = {			\
+				.framesperurb = 96,	\
+				.framesize = 200,	\
+				.interval = 1,		\
+			}				\
+		}					\
+	}
+
 #define MXL111SF_EP6_BULK_STREAMING_CONFIG		\
 	.size_of_priv = sizeof(struct mxl111sf_adap_state), \
 	.streaming_ctrl = mxl111sf_ep6_streaming_ctrl,	\
@@ -848,7 +1208,7 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties = {
 		} },
 		},
 	},
-	.num_device_descs = 4,
+	.num_device_descs = 3,
 	.devices = {
 		{   "Hauppauge 126xxx DVBT (bulk)",
 			{ NULL },
@@ -866,11 +1226,6 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties = {
 			  &mxl111sf_table[24], &mxl111sf_table[26],
 			  NULL },
 		},
-		{   "Hauppauge 126xxx (tp-bulk)",
-			{ NULL },
-			{ &mxl111sf_table[28], &mxl111sf_table[30],
-			  NULL },
-		},
 	}
 };
 
@@ -890,7 +1245,7 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties = {
 		} },
 		},
 	},
-	.num_device_descs = 4,
+	.num_device_descs = 3,
 	.devices = {
 		{   "Hauppauge 126xxx DVBT (isoc)",
 			{ NULL },
@@ -908,11 +1263,6 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties = {
 			  &mxl111sf_table[24], &mxl111sf_table[26],
 			  NULL },
 		},
-		{   "Hauppauge 126xxx (tp-isoc)",
-			{ NULL },
-			{ &mxl111sf_table[28], &mxl111sf_table[30],
-			  NULL },
-		},
 	}
 };
 
@@ -923,33 +1273,159 @@ static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
 	.adapter = {
 		{
 		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
-		.num_frontends = 2,
+		.num_frontends = 1,
 		.fe = {{
 			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
 			.tuner_attach     = mxl111sf_attach_tuner,
 
 			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		}},
 		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge 126xxx ATSC (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[1], &mxl111sf_table[5],
+			  NULL },
+		},
+		{   "Hauppauge 117xxx ATSC (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[12],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
 		{
-			.frontend_attach  = mxl111sf_attach_demod,
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 1,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
 			.tuner_attach     = mxl111sf_attach_tuner,
 
-			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
 		}},
 		},
 	},
-	.num_device_descs = 6,
+	.num_device_descs = 2,
 	.devices = {
-		{   "Hauppauge 126xxx ATSC (bulk)",
+		{   "Hauppauge 126xxx ATSC (isoc)",
 			{ NULL },
 			{ &mxl111sf_table[1], &mxl111sf_table[5],
 			  NULL },
 		},
-		{   "Hauppauge 117xxx ATSC (bulk)",
+		{   "Hauppauge 117xxx ATSC (isoc)",
 			{ NULL },
 			{ &mxl111sf_table[12],
 			  NULL },
 		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mh_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 1,
+		.fe = {{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "HCW 126xxx (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[2], &mxl111sf_table[6],
+			  NULL },
+		},
+		{   "HCW 117xxx (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[13],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mh_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 1,
+		.fe = {{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "HCW 126xxx (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[2], &mxl111sf_table[6],
+			  NULL },
+		},
+		{   "HCW 117xxx (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[13],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
 		{   "Hauppauge 126xxx ATSC+ (bulk)",
 			{ NULL },
 			{ &mxl111sf_table[0], &mxl111sf_table[3],
@@ -963,13 +1439,96 @@ static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
 			  &mxl111sf_table[32], &mxl111sf_table[33],
 			  NULL },
 		},
-		{   "Hauppauge Mercury (tp-bulk)",
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge 126xxx ATSC+ (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[0], &mxl111sf_table[3],
+			  &mxl111sf_table[7], &mxl111sf_table[9],
+			  &mxl111sf_table[10], NULL },
+		},
+		{   "Hauppauge 117xxx ATSC+ (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[11], &mxl111sf_table[14],
+			  &mxl111sf_table[16], &mxl111sf_table[17],
+			  &mxl111sf_table[32], &mxl111sf_table[33],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge Mercury (spi-bulk)",
 			{ NULL },
 			{ &mxl111sf_table[19], &mxl111sf_table[21],
 			  &mxl111sf_table[23], &mxl111sf_table[25],
-			  &mxl111sf_table[27], NULL },
+			  NULL },
 		},
-		{   "Hauppauge WinTV-Aero-M",
+		{   "Hauppauge WinTV-Aero-M (spi-bulk)",
 			{ NULL },
 			{ &mxl111sf_table[29], &mxl111sf_table[31],
 			  NULL },
@@ -977,14 +1536,14 @@ static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
 	}
 };
 
-static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_isoc_properties = {
 	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
 
 	.num_adapters = 1,
 	.adapter = {
 		{
 		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
-		.num_frontends = 2,
+		.num_frontends = 3,
 		.fe = {{
 			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
 			.tuner_attach     = mxl111sf_attach_tuner,
@@ -996,34 +1555,111 @@ static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
 			.tuner_attach     = mxl111sf_attach_tuner,
 
 			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
 		}},
 		},
 	},
-	.num_device_descs = 6,
+	.num_device_descs = 2,
 	.devices = {
-		{   "Hauppauge 126xxx ATSC (isoc)",
+		{   "Hauppauge Mercury (spi-isoc)",
 			{ NULL },
-			{ &mxl111sf_table[1], &mxl111sf_table[5],
+			{ &mxl111sf_table[19], &mxl111sf_table[21],
+			  &mxl111sf_table[23], &mxl111sf_table[25],
 			  NULL },
 		},
-		{   "Hauppauge 117xxx ATSC (isoc)",
+		{   "Hauppauge WinTV-Aero-M (spi-isoc)",
 			{ NULL },
-			{ &mxl111sf_table[12],
+			{ &mxl111sf_table[29], &mxl111sf_table[31],
 			  NULL },
 		},
-		{   "Hauppauge 126xxx ATSC+ (isoc)",
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge Mercury (tp-bulk)",
 			{ NULL },
-			{ &mxl111sf_table[0], &mxl111sf_table[3],
-			  &mxl111sf_table[7], &mxl111sf_table[9],
-			  &mxl111sf_table[10], NULL },
+			{ &mxl111sf_table[19], &mxl111sf_table[21],
+			  &mxl111sf_table[23], &mxl111sf_table[25],
+			  &mxl111sf_table[27], NULL },
 		},
-		{   "Hauppauge 117xxx ATSC+ (isoc)",
+		{   "Hauppauge WinTV-Aero-M",
 			{ NULL },
-			{ &mxl111sf_table[11], &mxl111sf_table[14],
-			  &mxl111sf_table[16], &mxl111sf_table[17],
-			  &mxl111sf_table[32], &mxl111sf_table[33],
+			{ &mxl111sf_table[29], &mxl111sf_table[31],
 			  NULL },
 		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
 		{   "Hauppauge Mercury (tp-isoc)",
 			{ NULL },
 			{ &mxl111sf_table[19], &mxl111sf_table[21],
@@ -1038,6 +1674,146 @@ static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
 	}
 };
 
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (tp-bulk)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (tp-isoc)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (spi-bulk)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (spi-isoc)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
 static struct usb_driver mxl111sf_driver = {
 	.name		= "dvb_usb_mxl111sf",
 	.probe		= mxl111sf_probe,
diff --git a/drivers/media/dvb/dvb-usb/rtl28xxu.c b/drivers/media/dvb/dvb-usb/rtl28xxu.c
index 8f4736a10fc8..41e1f5537f44 100644
--- a/drivers/media/dvb/dvb-usb/rtl28xxu.c
+++ b/drivers/media/dvb/dvb-usb/rtl28xxu.c
@@ -322,6 +322,9 @@ static int rtl2831u_frontend_attach(struct dvb_usb_adapter *adap)
 	 * since there is some demod params needed to set according to tuner.
 	 */
 
+	/* demod needs some time to wake up */
+	msleep(20);
+
 	/* open demod I2C gate */
 	ret = rtl28xxu_ctrl_msg(adap->dev, &req_gate);
 	if (ret)
@@ -909,6 +912,8 @@ static int rtl28xxu_probe(struct usb_interface *intf,
 	int ret, i;
 	int properties_count = ARRAY_SIZE(rtl28xxu_properties);
 	struct dvb_usb_device *d;
+	struct usb_device *udev;
+	bool found;
 
 	deb_info("%s: interface=%d\n", __func__,
 		intf->cur_altsetting->desc.bInterfaceNumber);
@@ -916,6 +921,29 @@ static int rtl28xxu_probe(struct usb_interface *intf,
 	if (intf->cur_altsetting->desc.bInterfaceNumber != 0)
 		return 0;
 
+	/* Dynamic USB ID support. Replaces first device ID with current one .*/
+	udev = interface_to_usbdev(intf);
+
+	for (i = 0, found = false; i < ARRAY_SIZE(rtl28xxu_table) - 1; i++) {
+		if (rtl28xxu_table[i].idVendor ==
+				le16_to_cpu(udev->descriptor.idVendor) &&
+				rtl28xxu_table[i].idProduct ==
+				le16_to_cpu(udev->descriptor.idProduct)) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		deb_info("%s: using dynamic ID %04x:%04x\n", __func__,
+				le16_to_cpu(udev->descriptor.idVendor),
+				le16_to_cpu(udev->descriptor.idProduct));
+		rtl28xxu_properties[0].devices[0].warm_ids[0]->idVendor =
+				le16_to_cpu(udev->descriptor.idVendor);
+		rtl28xxu_properties[0].devices[0].warm_ids[0]->idProduct =
+				le16_to_cpu(udev->descriptor.idProduct);
+	}
+
 	for (i = 0; i < properties_count; i++) {
 		ret = dvb_usb_device_init(intf, &rtl28xxu_properties[i],
 				THIS_MODULE, &d, adapter_nr);
diff --git a/drivers/media/dvb/frontends/Kconfig b/drivers/media/dvb/frontends/Kconfig
index 21246707fbfb..b98ebb264e29 100644
--- a/drivers/media/dvb/frontends/Kconfig
+++ b/drivers/media/dvb/frontends/Kconfig
@@ -531,6 +531,14 @@ config DVB_LGDT3305
 	  An ATSC 8VSB and QAM64/256 tuner module. Say Y when you want
 	  to support this frontend.
 
+config DVB_LG2160
+	tristate "LG Electronics LG216x based"
+	depends on DVB_CORE && I2C
+	default m if DVB_FE_CUSTOMISE
+	help
+	  An ATSC/MH demodulator module. Say Y when you want
+	  to support this frontend.
+
 config DVB_S5H1409
 	tristate "Samsung S5H1409 based"
 	depends on DVB_CORE && I2C
@@ -540,12 +548,26 @@ config DVB_S5H1409
 	  to support this frontend.
 
 config DVB_AU8522
-	tristate "Auvitek AU8522 based"
-	depends on DVB_CORE && I2C && VIDEO_V4L2
+	depends on I2C
+	tristate
+
+config DVB_AU8522_DTV
+	tristate "Auvitek AU8522 based DTV demod"
+	depends on DVB_CORE && I2C
+	select DVB_AU8522
 	default m if DVB_FE_CUSTOMISE
 	help
-	  An ATSC 8VSB and QAM64/256 tuner module. Say Y when you want
-	  to support this frontend.
+	  An ATSC 8VSB, QAM64/256 & NTSC demodulator module. Say Y when
+	  you want to enable DTV demodulation support for this frontend.
+
+config DVB_AU8522_V4L
+	tristate "Auvitek AU8522 based ATV demod"
+	depends on VIDEO_V4L2 && I2C
+	select DVB_AU8522
+	default m if DVB_FE_CUSTOMISE
+	help
+	  An ATSC 8VSB, QAM64/256 & NTSC demodulator module. Say Y when
+	  you want to enable ATV demodulation support for this frontend.
 
 config DVB_S5H1411
 	tristate "Samsung S5H1411 based"
@@ -713,6 +735,11 @@ config DVB_M88RS2000
 	  A DVB-S tuner module.
 	  Say Y when you want to support this frontend.
 
+config DVB_AF9033
+	tristate "Afatech AF9033 DVB-T demodulator"
+	depends on DVB_CORE && I2C
+	default m if DVB_FE_CUSTOMISE
+
 comment "Tools to develop new frontends"
 
 config DVB_DUMMY_FE
diff --git a/drivers/media/dvb/frontends/Makefile b/drivers/media/dvb/frontends/Makefile
index 86fa808bf589..cd1ac2fd5774 100644
--- a/drivers/media/dvb/frontends/Makefile
+++ b/drivers/media/dvb/frontends/Makefile
@@ -7,7 +7,6 @@ ccflags-y += -I$(srctree)/drivers/media/common/tuners/
 
 stb0899-objs = stb0899_drv.o stb0899_algo.o
 stv0900-objs = stv0900_core.o stv0900_sw.o
-au8522-objs = au8522_dig.o au8522_decoder.o
 drxd-objs = drxd_firm.o drxd_hard.o
 cxd2820r-objs = cxd2820r_core.o cxd2820r_c.o cxd2820r_t.o cxd2820r_t2.o
 drxk-objs := drxk_hard.o
@@ -50,6 +49,7 @@ obj-$(CONFIG_DVB_BCM3510) += bcm3510.o
 obj-$(CONFIG_DVB_S5H1420) += s5h1420.o
 obj-$(CONFIG_DVB_LGDT330X) += lgdt330x.o
 obj-$(CONFIG_DVB_LGDT3305) += lgdt3305.o
+obj-$(CONFIG_DVB_LG2160) += lg2160.o
 obj-$(CONFIG_DVB_CX24123) += cx24123.o
 obj-$(CONFIG_DVB_LNBP21) += lnbp21.o
 obj-$(CONFIG_DVB_LNBP22) += lnbp22.o
@@ -63,7 +63,9 @@ obj-$(CONFIG_DVB_TUNER_DIB0090) += dib0090.o
 obj-$(CONFIG_DVB_TUA6100) += tua6100.o
 obj-$(CONFIG_DVB_S5H1409) += s5h1409.o
 obj-$(CONFIG_DVB_TUNER_ITD1000) += itd1000.o
-obj-$(CONFIG_DVB_AU8522) += au8522.o
+obj-$(CONFIG_DVB_AU8522) += au8522_common.o
+obj-$(CONFIG_DVB_AU8522_DTV) += au8522_dig.o
+obj-$(CONFIG_DVB_AU8522_V4L) += au8522_decoder.o
 obj-$(CONFIG_DVB_TDA10048) += tda10048.o
 obj-$(CONFIG_DVB_TUNER_CX24113) += cx24113.o
 obj-$(CONFIG_DVB_S5H1411) += s5h1411.o
@@ -98,4 +100,5 @@ obj-$(CONFIG_DVB_A8293) += a8293.o
 obj-$(CONFIG_DVB_TDA10071) += tda10071.o
 obj-$(CONFIG_DVB_RTL2830) += rtl2830.o
 obj-$(CONFIG_DVB_M88RS2000) += m88rs2000.o
+obj-$(CONFIG_DVB_AF9033) += af9033.o
 
diff --git a/drivers/media/dvb/frontends/af9013.c b/drivers/media/dvb/frontends/af9013.c
index 6bcbcf543b38..5bc570d77846 100644
--- a/drivers/media/dvb/frontends/af9013.c
+++ b/drivers/media/dvb/frontends/af9013.c
@@ -514,7 +514,6 @@ err:
 
 static void af9013_statistics_work(struct work_struct *work)
 {
-	int ret;
 	struct af9013_state *state = container_of(work,
 		struct af9013_state, statistics_work.work);
 	unsigned int next_msec;
@@ -530,27 +529,27 @@ static void af9013_statistics_work(struct work_struct *work)
 	default:
 		state->statistics_step = 0;
 	case 0:
-		ret = af9013_statistics_signal_strength(&state->fe);
+		af9013_statistics_signal_strength(&state->fe);
 		state->statistics_step++;
 		next_msec = 300;
 		break;
 	case 1:
-		ret = af9013_statistics_snr_start(&state->fe);
+		af9013_statistics_snr_start(&state->fe);
 		state->statistics_step++;
 		next_msec = 200;
 		break;
 	case 2:
-		ret = af9013_statistics_ber_unc_start(&state->fe);
+		af9013_statistics_ber_unc_start(&state->fe);
 		state->statistics_step++;
 		next_msec = 1000;
 		break;
 	case 3:
-		ret = af9013_statistics_snr_result(&state->fe);
+		af9013_statistics_snr_result(&state->fe);
 		state->statistics_step++;
 		next_msec = 400;
 		break;
 	case 4:
-		ret = af9013_statistics_ber_unc_result(&state->fe);
+		af9013_statistics_ber_unc_result(&state->fe);
 		state->statistics_step++;
 		next_msec = 100;
 		break;
@@ -558,8 +557,6 @@ static void af9013_statistics_work(struct work_struct *work)
 
 	schedule_delayed_work(&state->statistics_work,
 		msecs_to_jiffies(next_msec));
-
-	return;
 }
 
 static int af9013_get_tune_settings(struct dvb_frontend *fe,
diff --git a/drivers/media/dvb/frontends/af9033.c b/drivers/media/dvb/frontends/af9033.c
new file mode 100644
index 000000000000..a38998286260
--- /dev/null
+++ b/drivers/media/dvb/frontends/af9033.c
@@ -0,0 +1,980 @@
+/*
+ * Afatech AF9033 demodulator driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * Copyright (C) 2012 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include "af9033_priv.h"
+
+struct af9033_state {
+	struct i2c_adapter *i2c;
+	struct dvb_frontend fe;
+	struct af9033_config cfg;
+
+	u32 bandwidth_hz;
+	bool ts_mode_parallel;
+	bool ts_mode_serial;
+
+	u32 ber;
+	u32 ucb;
+	unsigned long last_stat_check;
+};
+
+/* write multiple registers */
+static int af9033_wr_regs(struct af9033_state *state, u32 reg, const u8 *val,
+		int len)
+{
+	int ret;
+	u8 buf[3 + len];
+	struct i2c_msg msg[1] = {
+		{
+			.addr = state->cfg.i2c_addr,
+			.flags = 0,
+			.len = sizeof(buf),
+			.buf = buf,
+		}
+	};
+
+	buf[0] = (reg >> 16) & 0xff;
+	buf[1] = (reg >>  8) & 0xff;
+	buf[2] = (reg >>  0) & 0xff;
+	memcpy(&buf[3], val, len);
+
+	ret = i2c_transfer(state->i2c, msg, 1);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		printk(KERN_WARNING "%s: i2c wr failed=%d reg=%06x len=%d\n",
+				__func__, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+/* read multiple registers */
+static int af9033_rd_regs(struct af9033_state *state, u32 reg, u8 *val, int len)
+{
+	int ret;
+	u8 buf[3] = { (reg >> 16) & 0xff, (reg >> 8) & 0xff,
+			(reg >> 0) & 0xff };
+	struct i2c_msg msg[2] = {
+		{
+			.addr = state->cfg.i2c_addr,
+			.flags = 0,
+			.len = sizeof(buf),
+			.buf = buf
+		}, {
+			.addr = state->cfg.i2c_addr,
+			.flags = I2C_M_RD,
+			.len = len,
+			.buf = val
+		}
+	};
+
+	ret = i2c_transfer(state->i2c, msg, 2);
+	if (ret == 2) {
+		ret = 0;
+	} else {
+		printk(KERN_WARNING "%s: i2c rd failed=%d reg=%06x len=%d\n",
+				__func__, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+
+/* write single register */
+static int af9033_wr_reg(struct af9033_state *state, u32 reg, u8 val)
+{
+	return af9033_wr_regs(state, reg, &val, 1);
+}
+
+/* read single register */
+static int af9033_rd_reg(struct af9033_state *state, u32 reg, u8 *val)
+{
+	return af9033_rd_regs(state, reg, val, 1);
+}
+
+/* write single register with mask */
+static int af9033_wr_reg_mask(struct af9033_state *state, u32 reg, u8 val,
+		u8 mask)
+{
+	int ret;
+	u8 tmp;
+
+	/* no need for read if whole reg is written */
+	if (mask != 0xff) {
+		ret = af9033_rd_regs(state, reg, &tmp, 1);
+		if (ret)
+			return ret;
+
+		val &= mask;
+		tmp &= ~mask;
+		val |= tmp;
+	}
+
+	return af9033_wr_regs(state, reg, &val, 1);
+}
+
+/* read single register with mask */
+static int af9033_rd_reg_mask(struct af9033_state *state, u32 reg, u8 *val,
+		u8 mask)
+{
+	int ret, i;
+	u8 tmp;
+
+	ret = af9033_rd_regs(state, reg, &tmp, 1);
+	if (ret)
+		return ret;
+
+	tmp &= mask;
+
+	/* find position of the first bit */
+	for (i = 0; i < 8; i++) {
+		if ((mask >> i) & 0x01)
+			break;
+	}
+	*val = tmp >> i;
+
+	return 0;
+}
+
+static u32 af9033_div(u32 a, u32 b, u32 x)
+{
+	u32 r = 0, c = 0, i;
+
+	pr_debug("%s: a=%d b=%d x=%d\n", __func__, a, b, x);
+
+	if (a > b) {
+		c = a / b;
+		a = a - c * b;
+	}
+
+	for (i = 0; i < x; i++) {
+		if (a >= b) {
+			r += 1;
+			a -= b;
+		}
+		a <<= 1;
+		r <<= 1;
+	}
+	r = (c << (u32)x) + r;
+
+	pr_debug("%s: a=%d b=%d x=%d r=%d r=%x\n", __func__, a, b, x, r, r);
+
+	return r;
+}
+
+static void af9033_release(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+
+	kfree(state);
+}
+
+static int af9033_init(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret, i, len;
+	const struct reg_val *init;
+	u8 buf[4];
+	u32 adc_cw, clock_cw;
+	struct reg_val_mask tab[] = {
+		{ 0x80fb24, 0x00, 0x08 },
+		{ 0x80004c, 0x00, 0xff },
+		{ 0x00f641, state->cfg.tuner, 0xff },
+		{ 0x80f5ca, 0x01, 0x01 },
+		{ 0x80f715, 0x01, 0x01 },
+		{ 0x00f41f, 0x04, 0x04 },
+		{ 0x00f41a, 0x01, 0x01 },
+		{ 0x80f731, 0x00, 0x01 },
+		{ 0x00d91e, 0x00, 0x01 },
+		{ 0x00d919, 0x00, 0x01 },
+		{ 0x80f732, 0x00, 0x01 },
+		{ 0x00d91f, 0x00, 0x01 },
+		{ 0x00d91a, 0x00, 0x01 },
+		{ 0x80f730, 0x00, 0x01 },
+		{ 0x80f778, 0x00, 0xff },
+		{ 0x80f73c, 0x01, 0x01 },
+		{ 0x80f776, 0x00, 0x01 },
+		{ 0x00d8fd, 0x01, 0xff },
+		{ 0x00d830, 0x01, 0xff },
+		{ 0x00d831, 0x00, 0xff },
+		{ 0x00d832, 0x00, 0xff },
+		{ 0x80f985, state->ts_mode_serial, 0x01 },
+		{ 0x80f986, state->ts_mode_parallel, 0x01 },
+		{ 0x00d827, 0x00, 0xff },
+		{ 0x00d829, 0x00, 0xff },
+	};
+
+	/* program clock control */
+	clock_cw = af9033_div(state->cfg.clock, 1000000ul, 19ul);
+	buf[0] = (clock_cw >>  0) & 0xff;
+	buf[1] = (clock_cw >>  8) & 0xff;
+	buf[2] = (clock_cw >> 16) & 0xff;
+	buf[3] = (clock_cw >> 24) & 0xff;
+
+	pr_debug("%s: clock=%d clock_cw=%08x\n", __func__, state->cfg.clock,
+			clock_cw);
+
+	ret = af9033_wr_regs(state, 0x800025, buf, 4);
+	if (ret < 0)
+		goto err;
+
+	/* program ADC control */
+	for (i = 0; i < ARRAY_SIZE(clock_adc_lut); i++) {
+		if (clock_adc_lut[i].clock == state->cfg.clock)
+			break;
+	}
+
+	adc_cw = af9033_div(clock_adc_lut[i].adc, 1000000ul, 19ul);
+	buf[0] = (adc_cw >>  0) & 0xff;
+	buf[1] = (adc_cw >>  8) & 0xff;
+	buf[2] = (adc_cw >> 16) & 0xff;
+
+	pr_debug("%s: adc=%d adc_cw=%06x\n", __func__, clock_adc_lut[i].adc,
+			adc_cw);
+
+	ret = af9033_wr_regs(state, 0x80f1cd, buf, 3);
+	if (ret < 0)
+		goto err;
+
+	/* program register table */
+	for (i = 0; i < ARRAY_SIZE(tab); i++) {
+		ret = af9033_wr_reg_mask(state, tab[i].reg, tab[i].val,
+				tab[i].mask);
+		if (ret < 0)
+			goto err;
+	}
+
+	/* settings for TS interface */
+	if (state->cfg.ts_mode == AF9033_TS_MODE_USB) {
+		ret = af9033_wr_reg_mask(state, 0x80f9a5, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9033_wr_reg_mask(state, 0x80f9b5, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+	} else {
+		ret = af9033_wr_reg_mask(state, 0x80f990, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9033_wr_reg_mask(state, 0x80f9b5, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+	}
+
+	/* load OFSM settings */
+	pr_debug("%s: load ofsm settings\n", __func__);
+	len = ARRAY_SIZE(ofsm_init);
+	init = ofsm_init;
+	for (i = 0; i < len; i++) {
+		ret = af9033_wr_reg(state, init[i].reg, init[i].val);
+		if (ret < 0)
+			goto err;
+	}
+
+	/* load tuner specific settings */
+	pr_debug("%s: load tuner specific settings\n",
+			__func__);
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_TUA9001:
+		len = ARRAY_SIZE(tuner_init_tua9001);
+		init = tuner_init_tua9001;
+		break;
+	case AF9033_TUNER_FC0011:
+		len = ARRAY_SIZE(tuner_init_fc0011);
+		init = tuner_init_fc0011;
+		break;
+	case AF9033_TUNER_MXL5007T:
+		len = ARRAY_SIZE(tuner_init_mxl5007t);
+		init = tuner_init_mxl5007t;
+		break;
+	case AF9033_TUNER_TDA18218:
+		len = ARRAY_SIZE(tuner_init_tda18218);
+		init = tuner_init_tda18218;
+		break;
+	default:
+		pr_debug("%s: unsupported tuner ID=%d\n", __func__,
+				state->cfg.tuner);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	for (i = 0; i < len; i++) {
+		ret = af9033_wr_reg(state, init[i].reg, init[i].val);
+		if (ret < 0)
+			goto err;
+	}
+
+	state->bandwidth_hz = 0; /* force to program all parameters */
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_sleep(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret, i;
+	u8 tmp;
+
+	ret = af9033_wr_reg(state, 0x80004c, 1);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800000, 0);
+	if (ret < 0)
+		goto err;
+
+	for (i = 100, tmp = 1; i && tmp; i--) {
+		ret = af9033_rd_reg(state, 0x80004c, &tmp);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(200, 10000);
+	}
+
+	pr_debug("%s: loop=%d\n", __func__, i);
+
+	if (i == 0) {
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	ret = af9033_wr_reg_mask(state, 0x80fb24, 0x08, 0x08);
+	if (ret < 0)
+		goto err;
+
+	/* prevent current leak (?) */
+	if (state->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
+		/* enable parallel TS */
+		ret = af9033_wr_reg_mask(state, 0x00d917, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9033_wr_reg_mask(state, 0x00d916, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_get_tune_settings(struct dvb_frontend *fe,
+		struct dvb_frontend_tune_settings *fesettings)
+{
+	fesettings->min_delay_ms = 800;
+	fesettings->step_size = 0;
+	fesettings->max_drift = 0;
+
+	return 0;
+}
+
+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;
+	u8 tmp, buf[3], bandwidth_reg_val;
+	u32 if_frequency, freq_cw, adc_freq;
+
+	pr_debug("%s: frequency=%d bandwidth_hz=%d\n", __func__, c->frequency,
+			c->bandwidth_hz);
+
+	/* check bandwidth */
+	switch (c->bandwidth_hz) {
+	case 6000000:
+		bandwidth_reg_val = 0x00;
+		break;
+	case 7000000:
+		bandwidth_reg_val = 0x01;
+		break;
+	case 8000000:
+		bandwidth_reg_val = 0x02;
+		break;
+	default:
+		pr_debug("%s: invalid bandwidth_hz\n", __func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* program tuner */
+	if (fe->ops.tuner_ops.set_params)
+		fe->ops.tuner_ops.set_params(fe);
+
+	/* program CFOE coefficients */
+	if (c->bandwidth_hz != state->bandwidth_hz) {
+		for (i = 0; i < ARRAY_SIZE(coeff_lut); i++) {
+			if (coeff_lut[i].clock == state->cfg.clock &&
+				coeff_lut[i].bandwidth_hz == c->bandwidth_hz) {
+				break;
+			}
+		}
+		ret =  af9033_wr_regs(state, 0x800001,
+				coeff_lut[i].val, sizeof(coeff_lut[i].val));
+	}
+
+	/* program frequency control */
+	if (c->bandwidth_hz != state->bandwidth_hz) {
+		spec_inv = state->cfg.spec_inv ? -1 : 1;
+
+		for (i = 0; i < ARRAY_SIZE(clock_adc_lut); i++) {
+			if (clock_adc_lut[i].clock == state->cfg.clock)
+				break;
+		}
+		adc_freq = clock_adc_lut[i].adc;
+
+		/* get used IF frequency */
+		if (fe->ops.tuner_ops.get_if_frequency)
+			fe->ops.tuner_ops.get_if_frequency(fe, &if_frequency);
+		else
+			if_frequency = 0;
+
+		while (if_frequency > (adc_freq / 2))
+			if_frequency -= adc_freq;
+
+		if (if_frequency >= 0)
+			spec_inv *= -1;
+		else
+			if_frequency *= -1;
+
+		freq_cw = af9033_div(if_frequency, adc_freq, 23ul);
+
+		if (spec_inv == -1)
+			freq_cw *= -1;
+
+		/* get adc multiplies */
+		ret = af9033_rd_reg(state, 0x800045, &tmp);
+		if (ret < 0)
+			goto err;
+
+		if (tmp == 1)
+			freq_cw /= 2;
+
+		buf[0] = (freq_cw >>  0) & 0xff;
+		buf[1] = (freq_cw >>  8) & 0xff;
+		buf[2] = (freq_cw >> 16) & 0x7f;
+		ret = af9033_wr_regs(state, 0x800029, buf, 3);
+		if (ret < 0)
+			goto err;
+
+		state->bandwidth_hz = c->bandwidth_hz;
+	}
+
+	ret = af9033_wr_reg_mask(state, 0x80f904, bandwidth_reg_val, 0x03);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800040, 0x00);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800047, 0x00);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg_mask(state, 0x80f999, 0x00, 0x01);
+	if (ret < 0)
+		goto err;
+
+	if (c->frequency <= 230000000)
+		tmp = 0x00; /* VHF */
+	else
+		tmp = 0x01; /* UHF */
+
+	ret = af9033_wr_reg(state, 0x80004b, tmp);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800000, 0x00);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_get_frontend(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	u8 buf[8];
+
+	pr_debug("%s\n", __func__);
+
+	/* read all needed registers */
+	ret = af9033_rd_regs(state, 0x80f900, buf, sizeof(buf));
+	if (ret < 0)
+		goto err;
+
+	switch ((buf[0] >> 0) & 3) {
+	case 0:
+		c->transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+	case 1:
+		c->transmission_mode = TRANSMISSION_MODE_8K;
+		break;
+	}
+
+	switch ((buf[1] >> 0) & 3) {
+	case 0:
+		c->guard_interval = GUARD_INTERVAL_1_32;
+		break;
+	case 1:
+		c->guard_interval = GUARD_INTERVAL_1_16;
+		break;
+	case 2:
+		c->guard_interval = GUARD_INTERVAL_1_8;
+		break;
+	case 3:
+		c->guard_interval = GUARD_INTERVAL_1_4;
+		break;
+	}
+
+	switch ((buf[2] >> 0) & 7) {
+	case 0:
+		c->hierarchy = HIERARCHY_NONE;
+		break;
+	case 1:
+		c->hierarchy = HIERARCHY_1;
+		break;
+	case 2:
+		c->hierarchy = HIERARCHY_2;
+		break;
+	case 3:
+		c->hierarchy = HIERARCHY_4;
+		break;
+	}
+
+	switch ((buf[3] >> 0) & 3) {
+	case 0:
+		c->modulation = QPSK;
+		break;
+	case 1:
+		c->modulation = QAM_16;
+		break;
+	case 2:
+		c->modulation = QAM_64;
+		break;
+	}
+
+	switch ((buf[4] >> 0) & 3) {
+	case 0:
+		c->bandwidth_hz = 6000000;
+		break;
+	case 1:
+		c->bandwidth_hz = 7000000;
+		break;
+	case 2:
+		c->bandwidth_hz = 8000000;
+		break;
+	}
+
+	switch ((buf[6] >> 0) & 7) {
+	case 0:
+		c->code_rate_HP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_HP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_HP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_HP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_HP = FEC_7_8;
+		break;
+	case 5:
+		c->code_rate_HP = FEC_NONE;
+		break;
+	}
+
+	switch ((buf[7] >> 0) & 7) {
+	case 0:
+		c->code_rate_LP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_LP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_LP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_LP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_LP = FEC_7_8;
+		break;
+	case 5:
+		c->code_rate_LP = FEC_NONE;
+		break;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+	u8 tmp;
+
+	*status = 0;
+
+	/* radio channel status, 0=no result, 1=has signal, 2=no signal */
+	ret = af9033_rd_reg(state, 0x800047, &tmp);
+	if (ret < 0)
+		goto err;
+
+	/* has signal */
+	if (tmp == 0x01)
+		*status |= FE_HAS_SIGNAL;
+
+	if (tmp != 0x02) {
+		/* TPS lock */
+		ret = af9033_rd_reg_mask(state, 0x80f5a9, &tmp, 0x01);
+		if (ret < 0)
+			goto err;
+
+		if (tmp)
+			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
+					FE_HAS_VITERBI;
+
+		/* full lock */
+		ret = af9033_rd_reg_mask(state, 0x80f999, &tmp, 0x01);
+		if (ret < 0)
+			goto err;
+
+		if (tmp)
+			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
+					FE_HAS_VITERBI | FE_HAS_SYNC |
+					FE_HAS_LOCK;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret, i, len;
+	u8 buf[3], tmp;
+	u32 snr_val;
+	const struct val_snr *uninitialized_var(snr_lut);
+
+	/* read value */
+	ret = af9033_rd_regs(state, 0x80002c, buf, 3);
+	if (ret < 0)
+		goto err;
+
+	snr_val = (buf[2] << 16) | (buf[1] << 8) | buf[0];
+
+	/* read current modulation */
+	ret = af9033_rd_reg(state, 0x80f903, &tmp);
+	if (ret < 0)
+		goto err;
+
+	switch ((tmp >> 0) & 3) {
+	case 0:
+		len = ARRAY_SIZE(qpsk_snr_lut);
+		snr_lut = qpsk_snr_lut;
+		break;
+	case 1:
+		len = ARRAY_SIZE(qam16_snr_lut);
+		snr_lut = qam16_snr_lut;
+		break;
+	case 2:
+		len = ARRAY_SIZE(qam64_snr_lut);
+		snr_lut = qam64_snr_lut;
+		break;
+	default:
+		goto err;
+	}
+
+	for (i = 0; i < len; i++) {
+		tmp = snr_lut[i].snr;
+
+		if (snr_val < snr_lut[i].val)
+			break;
+	}
+
+	*snr = tmp * 10; /* dB/10 */
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+	u8 strength2;
+
+	/* read signal strength of 0-100 scale */
+	ret = af9033_rd_reg(state, 0x800048, &strength2);
+	if (ret < 0)
+		goto err;
+
+	/* scale value to 0x0000-0xffff */
+	*strength = strength2 * 0xffff / 100;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_update_ch_stat(struct af9033_state *state)
+{
+	int ret = 0;
+	u32 err_cnt, bit_cnt;
+	u16 abort_cnt;
+	u8 buf[7];
+
+	/* only update data every half second */
+	if (time_after(jiffies, state->last_stat_check + msecs_to_jiffies(500))) {
+		ret = af9033_rd_regs(state, 0x800032, buf, sizeof(buf));
+		if (ret < 0)
+			goto err;
+		/* in 8 byte packets? */
+		abort_cnt = (buf[1] << 8) + buf[0];
+		/* in bits */
+		err_cnt = (buf[4] << 16) + (buf[3] << 8) + buf[2];
+		/* in 8 byte packets? always(?) 0x2710 = 10000 */
+		bit_cnt = (buf[6] << 8) + buf[5];
+
+		if (bit_cnt < abort_cnt) {
+			abort_cnt = 1000;
+			state->ber = 0xffffffff;
+		} else {
+			/* 8 byte packets, that have not been rejected already */
+			bit_cnt -= (u32)abort_cnt;
+			if (bit_cnt == 0) {
+				state->ber = 0xffffffff;
+			} else {
+				err_cnt -= (u32)abort_cnt * 8 * 8;
+				bit_cnt *= 8 * 8;
+				state->ber = err_cnt * (0xffffffff / bit_cnt);
+			}
+		}
+		state->ucb += abort_cnt;
+		state->last_stat_check = jiffies;
+	}
+
+	return 0;
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int af9033_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+
+	ret = af9033_update_ch_stat(state);
+	if (ret < 0)
+		return ret;
+
+	*ber = state->ber;
+
+	return 0;
+}
+
+static int af9033_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+
+	ret = af9033_update_ch_stat(state);
+	if (ret < 0)
+		return ret;
+
+	*ucblocks = state->ucb;
+
+	return 0;
+}
+
+static int af9033_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+
+	pr_debug("%s: enable=%d\n", __func__, enable);
+
+	ret = af9033_wr_reg_mask(state, 0x00fa04, enable, 0x01);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static struct dvb_frontend_ops af9033_ops;
+
+struct dvb_frontend *af9033_attach(const struct af9033_config *config,
+		struct i2c_adapter *i2c)
+{
+	int ret;
+	struct af9033_state *state;
+	u8 buf[8];
+
+	pr_debug("%s:\n", __func__);
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct af9033_state), GFP_KERNEL);
+	if (state == NULL)
+		goto err;
+
+	/* setup the state */
+	state->i2c = i2c;
+	memcpy(&state->cfg, config, sizeof(struct af9033_config));
+
+	if (state->cfg.clock != 12000000) {
+		printk(KERN_INFO "af9033: unsupported clock=%d, only " \
+				"12000000 Hz is supported currently\n",
+				state->cfg.clock);
+		goto err;
+	}
+
+	/* firmware version */
+	ret = af9033_rd_regs(state, 0x0083e9, &buf[0], 4);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_rd_regs(state, 0x804191, &buf[4], 4);
+	if (ret < 0)
+		goto err;
+
+	printk(KERN_INFO "af9033: firmware version: LINK=%d.%d.%d.%d " \
+			"OFDM=%d.%d.%d.%d\n", buf[0], buf[1], buf[2], buf[3],
+			buf[4], buf[5], buf[6], buf[7]);
+
+	/* configure internal TS mode */
+	switch (state->cfg.ts_mode) {
+	case AF9033_TS_MODE_PARALLEL:
+		state->ts_mode_parallel = true;
+		break;
+	case AF9033_TS_MODE_SERIAL:
+		state->ts_mode_serial = true;
+		break;
+	case AF9033_TS_MODE_USB:
+		/* usb mode for AF9035 */
+	default:
+		break;
+	}
+
+	/* create dvb_frontend */
+	memcpy(&state->fe.ops, &af9033_ops, sizeof(struct dvb_frontend_ops));
+	state->fe.demodulator_priv = state;
+
+	return &state->fe;
+
+err:
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(af9033_attach);
+
+static struct dvb_frontend_ops af9033_ops = {
+	.delsys = { SYS_DVBT },
+	.info = {
+		.name = "Afatech AF9033 (DVB-T)",
+		.frequency_min = 174000000,
+		.frequency_max = 862000000,
+		.frequency_stepsize = 250000,
+		.frequency_tolerance = 0,
+		.caps =	FE_CAN_FEC_1_2 |
+			FE_CAN_FEC_2_3 |
+			FE_CAN_FEC_3_4 |
+			FE_CAN_FEC_5_6 |
+			FE_CAN_FEC_7_8 |
+			FE_CAN_FEC_AUTO |
+			FE_CAN_QPSK |
+			FE_CAN_QAM_16 |
+			FE_CAN_QAM_64 |
+			FE_CAN_QAM_AUTO |
+			FE_CAN_TRANSMISSION_MODE_AUTO |
+			FE_CAN_GUARD_INTERVAL_AUTO |
+			FE_CAN_HIERARCHY_AUTO |
+			FE_CAN_RECOVER |
+			FE_CAN_MUTE_TS
+	},
+
+	.release = af9033_release,
+
+	.init = af9033_init,
+	.sleep = af9033_sleep,
+
+	.get_tune_settings = af9033_get_tune_settings,
+	.set_frontend = af9033_set_frontend,
+	.get_frontend = af9033_get_frontend,
+
+	.read_status = af9033_read_status,
+	.read_snr = af9033_read_snr,
+	.read_signal_strength = af9033_read_signal_strength,
+	.read_ber = af9033_read_ber,
+	.read_ucblocks = af9033_read_ucblocks,
+
+	.i2c_gate_ctrl = af9033_i2c_gate_ctrl,
+};
+
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_DESCRIPTION("Afatech AF9033 DVB-T demodulator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/frontends/af9033.h b/drivers/media/dvb/frontends/af9033.h
new file mode 100644
index 000000000000..9e302c3f0f7d
--- /dev/null
+++ b/drivers/media/dvb/frontends/af9033.h
@@ -0,0 +1,75 @@
+/*
+ * Afatech AF9033 demodulator driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * Copyright (C) 2012 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef AF9033_H
+#define AF9033_H
+
+struct af9033_config {
+	/*
+	 * I2C address
+	 */
+	u8 i2c_addr;
+
+	/*
+	 * clock Hz
+	 * 12000000, 22000000, 24000000, 34000000, 32000000, 28000000, 26000000,
+	 * 30000000, 36000000, 20480000, 16384000
+	 */
+	u32 clock;
+
+	/*
+	 * tuner
+	 */
+#define AF9033_TUNER_TUA9001     0x27 /* Infineon TUA 9001 */
+#define AF9033_TUNER_FC0011      0x28 /* Fitipower FC0011 */
+#define AF9033_TUNER_MXL5007T    0xa0 /* MaxLinear MxL5007T */
+#define AF9033_TUNER_TDA18218    0xa1 /* NXP TDA 18218HN */
+	u8 tuner;
+
+	/*
+	 * TS settings
+	 */
+#define AF9033_TS_MODE_USB       0
+#define AF9033_TS_MODE_PARALLEL  1
+#define AF9033_TS_MODE_SERIAL    2
+	u8 ts_mode:2;
+
+	/*
+	 * input spectrum inversion
+	 */
+	bool spec_inv;
+};
+
+
+#if defined(CONFIG_DVB_AF9033) || \
+	(defined(CONFIG_DVB_AF9033_MODULE) && defined(MODULE))
+extern struct dvb_frontend *af9033_attach(const struct af9033_config *config,
+	struct i2c_adapter *i2c);
+#else
+static inline struct dvb_frontend *af9033_attach(
+	const struct af9033_config *config, struct i2c_adapter *i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif /* AF9033_H */
diff --git a/drivers/media/dvb/frontends/af9033_priv.h b/drivers/media/dvb/frontends/af9033_priv.h
new file mode 100644
index 000000000000..0b783b9ed75e
--- /dev/null
+++ b/drivers/media/dvb/frontends/af9033_priv.h
@@ -0,0 +1,470 @@
+/*
+ * Afatech AF9033 demodulator driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * Copyright (C) 2012 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef AF9033_PRIV_H
+#define AF9033_PRIV_H
+
+#include "dvb_frontend.h"
+#include "af9033.h"
+
+struct reg_val {
+	u32 reg;
+	u8  val;
+};
+
+struct reg_val_mask {
+	u32 reg;
+	u8  val;
+	u8  mask;
+};
+
+struct coeff {
+	u32 clock;
+	u32 bandwidth_hz;
+	u8 val[36];
+};
+
+struct clock_adc {
+	u32 clock;
+	u32 adc;
+};
+
+struct val_snr {
+	u32 val;
+	u8 snr;
+};
+
+/* Xtal clock vs. ADC clock lookup table */
+static const struct clock_adc clock_adc_lut[] = {
+	{ 16384000, 20480000 },
+	{ 20480000, 20480000 },
+	{ 36000000, 20250000 },
+	{ 30000000, 20156250 },
+	{ 26000000, 20583333 },
+	{ 28000000, 20416667 },
+	{ 32000000, 20500000 },
+	{ 34000000, 20187500 },
+	{ 24000000, 20500000 },
+	{ 22000000, 20625000 },
+	{ 12000000, 20250000 },
+};
+
+/* pre-calculated coeff lookup table */
+static const struct coeff coeff_lut[] = {
+	/* 12.000 MHz */
+	{ 12000000, 8000000, {
+		0x01, 0xce, 0x55, 0xc9, 0x00, 0xe7, 0x2a, 0xe4, 0x00, 0x73,
+		0x99, 0x0f, 0x00, 0x73, 0x95, 0x72, 0x00, 0x73, 0x91, 0xd5,
+		0x00, 0x39, 0xca, 0xb9, 0x00, 0xe7, 0x2a, 0xe4, 0x00, 0x73,
+		0x95, 0x72, 0x37, 0x02, 0xce, 0x01 }
+	},
+	{ 12000000, 7000000, {
+		0x01, 0x94, 0x8b, 0x10, 0x00, 0xca, 0x45, 0x88, 0x00, 0x65,
+		0x25, 0xed, 0x00, 0x65, 0x22, 0xc4, 0x00, 0x65, 0x1f, 0x9b,
+		0x00, 0x32, 0x91, 0x62, 0x00, 0xca, 0x45, 0x88, 0x00, 0x65,
+		0x22, 0xc4, 0x88, 0x02, 0x95, 0x01 }
+	},
+	{ 12000000, 6000000, {
+		0x01, 0x5a, 0xc0, 0x56, 0x00, 0xad, 0x60, 0x2b, 0x00, 0x56,
+		0xb2, 0xcb, 0x00, 0x56, 0xb0, 0x15, 0x00, 0x56, 0xad, 0x60,
+		0x00, 0x2b, 0x58, 0x0b, 0x00, 0xad, 0x60, 0x2b, 0x00, 0x56,
+		0xb0, 0x15, 0xf4, 0x02, 0x5b, 0x01 }
+	},
+};
+
+/* QPSK SNR lookup table */
+static const struct val_snr qpsk_snr_lut[] = {
+	{ 0x0b4771,  0 },
+	{ 0x0c1aed,  1 },
+	{ 0x0d0d27,  2 },
+	{ 0x0e4d19,  3 },
+	{ 0x0e5da8,  4 },
+	{ 0x107097,  5 },
+	{ 0x116975,  6 },
+	{ 0x1252d9,  7 },
+	{ 0x131fa4,  8 },
+	{ 0x13d5e1,  9 },
+	{ 0x148e53, 10 },
+	{ 0x15358b, 11 },
+	{ 0x15dd29, 12 },
+	{ 0x168112, 13 },
+	{ 0x170b61, 14 },
+	{ 0x17a532, 15 },
+	{ 0x180f94, 16 },
+	{ 0x186ed2, 17 },
+	{ 0x18b271, 18 },
+	{ 0x18e118, 19 },
+	{ 0x18ff4b, 20 },
+	{ 0x190af1, 21 },
+	{ 0x191451, 22 },
+	{ 0xffffff, 23 },
+};
+
+/* QAM16 SNR lookup table */
+static const struct val_snr qam16_snr_lut[] = {
+	{ 0x04f0d5,  0 },
+	{ 0x05387a,  1 },
+	{ 0x0573a4,  2 },
+	{ 0x05a99e,  3 },
+	{ 0x05cc80,  4 },
+	{ 0x05eb62,  5 },
+	{ 0x05fecf,  6 },
+	{ 0x060b80,  7 },
+	{ 0x062501,  8 },
+	{ 0x064865,  9 },
+	{ 0x069604, 10 },
+	{ 0x06f356, 11 },
+	{ 0x07706a, 12 },
+	{ 0x0804d3, 13 },
+	{ 0x089d1a, 14 },
+	{ 0x093e3d, 15 },
+	{ 0x09e35d, 16 },
+	{ 0x0a7c3c, 17 },
+	{ 0x0afaf8, 18 },
+	{ 0x0b719d, 19 },
+	{ 0x0bda6a, 20 },
+	{ 0x0c0c75, 21 },
+	{ 0x0c3f7d, 22 },
+	{ 0x0c5e62, 23 },
+	{ 0x0c6c31, 24 },
+	{ 0x0c7925, 25 },
+	{ 0xffffff, 26 },
+};
+
+/* QAM64 SNR lookup table */
+static const struct val_snr qam64_snr_lut[] = {
+	{ 0x0256d0,  0 },
+	{ 0x027a65,  1 },
+	{ 0x029873,  2 },
+	{ 0x02b7fe,  3 },
+	{ 0x02cf1e,  4 },
+	{ 0x02e234,  5 },
+	{ 0x02f409,  6 },
+	{ 0x030046,  7 },
+	{ 0x030844,  8 },
+	{ 0x030a02,  9 },
+	{ 0x030cde, 10 },
+	{ 0x031031, 11 },
+	{ 0x03144c, 12 },
+	{ 0x0315dd, 13 },
+	{ 0x031920, 14 },
+	{ 0x0322d0, 15 },
+	{ 0x0339fc, 16 },
+	{ 0x0364a1, 17 },
+	{ 0x038bcc, 18 },
+	{ 0x03c7d3, 19 },
+	{ 0x0408cc, 20 },
+	{ 0x043bed, 21 },
+	{ 0x048061, 22 },
+	{ 0x04be95, 23 },
+	{ 0x04fa7d, 24 },
+	{ 0x052405, 25 },
+	{ 0x05570d, 26 },
+	{ 0x059feb, 27 },
+	{ 0x05bf38, 28 },
+	{ 0xffffff, 29 },
+};
+
+static const struct reg_val ofsm_init[] = {
+	{ 0x800051, 0x01 },
+	{ 0x800070, 0x0a },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800099, 0x01 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000ab, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000c0, 0x05 },
+	{ 0x8000c4, 0x19 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5df, 0xfb },
+	{ 0x80f5e0, 0x00 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f5f8, 0x01 },
+	{ 0x80f5fd, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+/* Infineon TUA 9001 tuner init
+   AF9033_TUNER_TUA9001    = 0x27 */
+static const struct reg_val tuner_init_tua9001[] = {
+	{ 0x800046, 0x27 },
+	{ 0x800057, 0x00 },
+	{ 0x800058, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x80006d, 0x00 },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800074, 0x01 },
+	{ 0x800075, 0x03 },
+	{ 0x800076, 0x02 },
+	{ 0x800077, 0x00 },
+	{ 0x800078, 0x01 },
+	{ 0x800079, 0x00 },
+	{ 0x80007a, 0x7e },
+	{ 0x80007b, 0x3e },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x01 },
+	{ 0x800095, 0x02 },
+	{ 0x800096, 0x01 },
+	{ 0x800098, 0x0a },
+	{ 0x80009b, 0x05 },
+	{ 0x80009c, 0x80 },
+	{ 0x8000b3, 0x00 },
+	{ 0x8000c1, 0x01 },
+	{ 0x8000c2, 0x00 },
+	{ 0x80f007, 0x00 },
+	{ 0x80f01f, 0x82 },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x82 },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f047, 0x00 },
+	{ 0x80f054, 0x00 },
+	{ 0x80f055, 0x00 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f1e6, 0x00 },
+};
+
+/* Fitipower fc0011 tuner init
+   AF9033_TUNER_FC0011    = 0x28 */
+static const struct reg_val tuner_init_fc0011[] = {
+	{ 0x800046, AF9033_TUNER_FC0011 },
+	{ 0x800057, 0x00 },
+	{ 0x800058, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0xa5 },
+	{ 0x80006e, 0x01 },
+	{ 0x800071, 0x0A },
+	{ 0x800072, 0x02 },
+	{ 0x800074, 0x01 },
+	{ 0x800079, 0x01 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x80009b, 0x2D },
+	{ 0x80009c, 0x60 },
+	{ 0x80009d, 0x23 },
+	{ 0x8000a4, 0x50 },
+	{ 0x8000ad, 0x50 },
+	{ 0x8000b3, 0x01 },
+	{ 0x8000b7, 0x88 },
+	{ 0x8000b8, 0xa6 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x01 },
+	{ 0x8000c7, 0x69 },
+	{ 0x80F007, 0x00 },
+	{ 0x80F00A, 0x1B },
+	{ 0x80F00B, 0x1B },
+	{ 0x80F00C, 0x1B },
+	{ 0x80F00D, 0x1B },
+	{ 0x80F00E, 0xFF },
+	{ 0x80F00F, 0x01 },
+	{ 0x80F010, 0x00 },
+	{ 0x80F011, 0x02 },
+	{ 0x80F012, 0xFF },
+	{ 0x80F013, 0x01 },
+	{ 0x80F014, 0x00 },
+	{ 0x80F015, 0x02 },
+	{ 0x80F01B, 0xEF },
+	{ 0x80F01C, 0x01 },
+	{ 0x80F01D, 0x0f },
+	{ 0x80F01E, 0x02 },
+	{ 0x80F01F, 0x6E },
+	{ 0x80F020, 0x00 },
+	{ 0x80F025, 0xDE },
+	{ 0x80F026, 0x00 },
+	{ 0x80F027, 0x0A },
+	{ 0x80F028, 0x03 },
+	{ 0x80F029, 0x6E },
+	{ 0x80F02A, 0x00 },
+	{ 0x80F047, 0x00 },
+	{ 0x80F054, 0x00 },
+	{ 0x80F055, 0x00 },
+	{ 0x80F077, 0x01 },
+	{ 0x80F1E6, 0x00 },
+};
+
+/* MaxLinear MxL5007T tuner init
+   AF9033_TUNER_MXL5007T    = 0xa0 */
+static const struct reg_val tuner_init_mxl5007t[] = {
+	{ 0x800046, 0x1b },
+	{ 0x800057, 0x01 },
+	{ 0x800058, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x96 },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800074, 0x01 },
+	{ 0x800079, 0x01 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x8000b3, 0x01 },
+	{ 0x8000c1, 0x01 },
+	{ 0x8000c2, 0x00 },
+	{ 0x80f007, 0x00 },
+	{ 0x80f00c, 0x19 },
+	{ 0x80f00d, 0x1a },
+	{ 0x80f012, 0xda },
+	{ 0x80f013, 0x00 },
+	{ 0x80f014, 0x00 },
+	{ 0x80f015, 0x02 },
+	{ 0x80f01f, 0x82 },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x82 },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f077, 0x02 },
+	{ 0x80f1e6, 0x00 },
+};
+
+/* NXP TDA 18218HN tuner init
+   AF9033_TUNER_TDA18218    = 0xa1 */
+static const struct reg_val tuner_init_tda18218[] = {
+	{0x800046, 0xa1},
+	{0x800057, 0x01},
+	{0x800058, 0x01},
+	{0x80005f, 0x00},
+	{0x800060, 0x00},
+	{0x800071, 0x05},
+	{0x800072, 0x02},
+	{0x800074, 0x01},
+	{0x800079, 0x01},
+	{0x800093, 0x00},
+	{0x800094, 0x00},
+	{0x800095, 0x00},
+	{0x800096, 0x00},
+	{0x8000b3, 0x01},
+	{0x8000c3, 0x01},
+	{0x8000c4, 0x00},
+	{0x80f007, 0x00},
+	{0x80f00c, 0x19},
+	{0x80f00d, 0x1a},
+	{0x80f012, 0xda},
+	{0x80f013, 0x00},
+	{0x80f014, 0x00},
+	{0x80f015, 0x02},
+	{0x80f01f, 0x82},
+	{0x80f020, 0x00},
+	{0x80f029, 0x82},
+	{0x80f02a, 0x00},
+	{0x80f077, 0x02},
+	{0x80f1e6, 0x00},
+};
+
+#endif /* AF9033_PRIV_H */
+
diff --git a/drivers/media/dvb/frontends/au8522_common.c b/drivers/media/dvb/frontends/au8522_common.c
new file mode 100644
index 000000000000..5cfe151ee394
--- /dev/null
+++ b/drivers/media/dvb/frontends/au8522_common.c
@@ -0,0 +1,259 @@
+/*
+    Auvitek AU8522 QAM/8VSB demodulator driver
+
+    Copyright (C) 2008 Steven Toth <stoth@linuxtv.org>
+    Copyright (C) 2008 Devin Heitmueller <dheitmueller@linuxtv.org>
+    Copyright (C) 2005-2008 Auvitek International, Ltd.
+    Copyright (C) 2012 Michael Krufky <mkrufky@linuxtv.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include <linux/i2c.h>
+#include "dvb_frontend.h"
+#include "au8522_priv.h"
+
+MODULE_LICENSE("GPL");
+
+static int debug;
+
+#define dprintk(arg...)\
+  do { if (debug)\
+	 printk(arg);\
+  } while (0)
+
+/* Despite the name "hybrid_tuner", the framework works just as well for
+   hybrid demodulators as well... */
+static LIST_HEAD(hybrid_tuner_instance_list);
+static DEFINE_MUTEX(au8522_list_mutex);
+
+/* 16 bit registers, 8 bit values */
+int au8522_writereg(struct au8522_state *state, u16 reg, u8 data)
+{
+	int ret;
+	u8 buf[] = { (reg >> 8) | 0x80, reg & 0xff, data };
+
+	struct i2c_msg msg = { .addr = state->config->demod_address,
+			       .flags = 0, .buf = buf, .len = 3 };
+
+	ret = i2c_transfer(state->i2c, &msg, 1);
+
+	if (ret != 1)
+		printk("%s: writereg error (reg == 0x%02x, val == 0x%04x, "
+		       "ret == %i)\n", __func__, reg, data, ret);
+
+	return (ret != 1) ? -1 : 0;
+}
+EXPORT_SYMBOL(au8522_writereg);
+
+u8 au8522_readreg(struct au8522_state *state, u16 reg)
+{
+	int ret;
+	u8 b0[] = { (reg >> 8) | 0x40, reg & 0xff };
+	u8 b1[] = { 0 };
+
+	struct i2c_msg msg[] = {
+		{ .addr = state->config->demod_address, .flags = 0,
+		  .buf = b0, .len = 2 },
+		{ .addr = state->config->demod_address, .flags = I2C_M_RD,
+		  .buf = b1, .len = 1 } };
+
+	ret = i2c_transfer(state->i2c, msg, 2);
+
+	if (ret != 2)
+		printk(KERN_ERR "%s: readreg error (ret == %i)\n",
+		       __func__, ret);
+	return b1[0];
+}
+EXPORT_SYMBOL(au8522_readreg);
+
+int au8522_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct au8522_state *state = fe->demodulator_priv;
+
+	dprintk("%s(%d)\n", __func__, enable);
+
+	if (state->operational_mode == AU8522_ANALOG_MODE) {
+		/* We're being asked to manage the gate even though we're
+		   not in digital mode.  This can occur if we get switched
+		   over to analog mode before the dvb_frontend kernel thread
+		   has completely shutdown */
+		return 0;
+	}
+
+	if (enable)
+		return au8522_writereg(state, 0x106, 1);
+	else
+		return au8522_writereg(state, 0x106, 0);
+}
+EXPORT_SYMBOL(au8522_i2c_gate_ctrl);
+
+/* Reset the demod hardware and reset all of the configuration registers
+   to a default state. */
+int au8522_get_state(struct au8522_state **state, struct i2c_adapter *i2c,
+		     u8 client_address)
+{
+	int ret;
+
+	mutex_lock(&au8522_list_mutex);
+	ret = hybrid_tuner_request_state(struct au8522_state, (*state),
+					 hybrid_tuner_instance_list,
+					 i2c, client_address, "au8522");
+	mutex_unlock(&au8522_list_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL(au8522_get_state);
+
+void au8522_release_state(struct au8522_state *state)
+{
+	mutex_lock(&au8522_list_mutex);
+	if (state != NULL)
+		hybrid_tuner_release_state(state);
+	mutex_unlock(&au8522_list_mutex);
+}
+EXPORT_SYMBOL(au8522_release_state);
+
+static int au8522_led_gpio_enable(struct au8522_state *state, int onoff)
+{
+	struct au8522_led_config *led_config = state->config->led_cfg;
+	u8 val;
+
+	/* bail out if we can't control an LED */
+	if (!led_config || !led_config->gpio_output ||
+	    !led_config->gpio_output_enable || !led_config->gpio_output_disable)
+		return 0;
+
+	val = au8522_readreg(state, 0x4000 |
+			     (led_config->gpio_output & ~0xc000));
+	if (onoff) {
+		/* enable GPIO output */
+		val &= ~((led_config->gpio_output_enable >> 8) & 0xff);
+		val |=  (led_config->gpio_output_enable & 0xff);
+	} else {
+		/* disable GPIO output */
+		val &= ~((led_config->gpio_output_disable >> 8) & 0xff);
+		val |=  (led_config->gpio_output_disable & 0xff);
+	}
+	return au8522_writereg(state, 0x8000 |
+			       (led_config->gpio_output & ~0xc000), val);
+}
+
+/* led = 0 | off
+ * led = 1 | signal ok
+ * led = 2 | signal strong
+ * led < 0 | only light led if leds are currently off
+ */
+int au8522_led_ctrl(struct au8522_state *state, int led)
+{
+	struct au8522_led_config *led_config = state->config->led_cfg;
+	int i, ret = 0;
+
+	/* bail out if we can't control an LED */
+	if (!led_config || !led_config->gpio_leds ||
+	    !led_config->num_led_states || !led_config->led_states)
+		return 0;
+
+	if (led < 0) {
+		/* if LED is already lit, then leave it as-is */
+		if (state->led_state)
+			return 0;
+		else
+			led *= -1;
+	}
+
+	/* toggle LED if changing state */
+	if (state->led_state != led) {
+		u8 val;
+
+		dprintk("%s: %d\n", __func__, led);
+
+		au8522_led_gpio_enable(state, 1);
+
+		val = au8522_readreg(state, 0x4000 |
+				     (led_config->gpio_leds & ~0xc000));
+
+		/* start with all leds off */
+		for (i = 0; i < led_config->num_led_states; i++)
+			val &= ~led_config->led_states[i];
+
+		/* set selected LED state */
+		if (led < led_config->num_led_states)
+			val |= led_config->led_states[led];
+		else if (led_config->num_led_states)
+			val |=
+			led_config->led_states[led_config->num_led_states - 1];
+
+		ret = au8522_writereg(state, 0x8000 |
+				      (led_config->gpio_leds & ~0xc000), val);
+		if (ret < 0)
+			return ret;
+
+		state->led_state = led;
+
+		if (led == 0)
+			au8522_led_gpio_enable(state, 0);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(au8522_led_ctrl);
+
+int au8522_init(struct dvb_frontend *fe)
+{
+	struct au8522_state *state = fe->demodulator_priv;
+	dprintk("%s()\n", __func__);
+
+	state->operational_mode = AU8522_DIGITAL_MODE;
+
+	/* Clear out any state associated with the digital side of the
+	   chip, so that when it gets powered back up it won't think
+	   that it is already tuned */
+	state->current_frequency = 0;
+
+	au8522_writereg(state, 0xa4, 1 << 5);
+
+	au8522_i2c_gate_ctrl(fe, 1);
+
+	return 0;
+}
+EXPORT_SYMBOL(au8522_init);
+
+int au8522_sleep(struct dvb_frontend *fe)
+{
+	struct au8522_state *state = fe->demodulator_priv;
+	dprintk("%s()\n", __func__);
+
+	/* Only power down if the digital side is currently using the chip */
+	if (state->operational_mode == AU8522_ANALOG_MODE) {
+		/* We're not in one of the expected power modes, which means
+		   that the DVB thread is probably telling us to go to sleep
+		   even though the analog frontend has already started using
+		   the chip.  So ignore the request */
+		return 0;
+	}
+
+	/* turn off led */
+	au8522_led_ctrl(state, 0);
+
+	/* Power down the chip */
+	au8522_writereg(state, 0xa4, 1 << 5);
+
+	state->current_frequency = 0;
+
+	return 0;
+}
+EXPORT_SYMBOL(au8522_sleep);
diff --git a/drivers/media/dvb/frontends/au8522_dig.c b/drivers/media/dvb/frontends/au8522_dig.c
index 25f650934c73..5fc70d6cd04f 100644
--- a/drivers/media/dvb/frontends/au8522_dig.c
+++ b/drivers/media/dvb/frontends/au8522_dig.c
@@ -30,74 +30,11 @@
 
 static int debug;
 
-/* Despite the name "hybrid_tuner", the framework works just as well for
-   hybrid demodulators as well... */
-static LIST_HEAD(hybrid_tuner_instance_list);
-static DEFINE_MUTEX(au8522_list_mutex);
-
 #define dprintk(arg...)\
 	do { if (debug)\
 		printk(arg);\
 	} while (0)
 
-/* 16 bit registers, 8 bit values */
-int au8522_writereg(struct au8522_state *state, u16 reg, u8 data)
-{
-	int ret;
-	u8 buf[] = { (reg >> 8) | 0x80, reg & 0xff, data };
-
-	struct i2c_msg msg = { .addr = state->config->demod_address,
-			       .flags = 0, .buf = buf, .len = 3 };
-
-	ret = i2c_transfer(state->i2c, &msg, 1);
-
-	if (ret != 1)
-		printk("%s: writereg error (reg == 0x%02x, val == 0x%04x, "
-		       "ret == %i)\n", __func__, reg, data, ret);
-
-	return (ret != 1) ? -1 : 0;
-}
-
-u8 au8522_readreg(struct au8522_state *state, u16 reg)
-{
-	int ret;
-	u8 b0[] = { (reg >> 8) | 0x40, reg & 0xff };
-	u8 b1[] = { 0 };
-
-	struct i2c_msg msg[] = {
-		{ .addr = state->config->demod_address, .flags = 0,
-		  .buf = b0, .len = 2 },
-		{ .addr = state->config->demod_address, .flags = I2C_M_RD,
-		  .buf = b1, .len = 1 } };
-
-	ret = i2c_transfer(state->i2c, msg, 2);
-
-	if (ret != 2)
-		printk(KERN_ERR "%s: readreg error (ret == %i)\n",
-		       __func__, ret);
-	return b1[0];
-}
-
-static int au8522_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
-{
-	struct au8522_state *state = fe->demodulator_priv;
-
-	dprintk("%s(%d)\n", __func__, enable);
-
-	if (state->operational_mode == AU8522_ANALOG_MODE) {
-		/* We're being asked to manage the gate even though we're
-		   not in digital mode.  This can occur if we get switched
-		   over to analog mode before the dvb_frontend kernel thread
-		   has completely shutdown */
-		return 0;
-	}
-
-	if (enable)
-		return au8522_writereg(state, 0x106, 1);
-	else
-		return au8522_writereg(state, 0x106, 0);
-}
-
 struct mse2snr_tab {
 	u16 val;
 	u16 data;
@@ -609,136 +546,6 @@ static int au8522_set_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-/* Reset the demod hardware and reset all of the configuration registers
-   to a default state. */
-int au8522_init(struct dvb_frontend *fe)
-{
-	struct au8522_state *state = fe->demodulator_priv;
-	dprintk("%s()\n", __func__);
-
-	state->operational_mode = AU8522_DIGITAL_MODE;
-
-	/* Clear out any state associated with the digital side of the
-	   chip, so that when it gets powered back up it won't think
-	   that it is already tuned */
-	state->current_frequency = 0;
-
-	au8522_writereg(state, 0xa4, 1 << 5);
-
-	au8522_i2c_gate_ctrl(fe, 1);
-
-	return 0;
-}
-
-static int au8522_led_gpio_enable(struct au8522_state *state, int onoff)
-{
-	struct au8522_led_config *led_config = state->config->led_cfg;
-	u8 val;
-
-	/* bail out if we can't control an LED */
-	if (!led_config || !led_config->gpio_output ||
-	    !led_config->gpio_output_enable || !led_config->gpio_output_disable)
-		return 0;
-
-	val = au8522_readreg(state, 0x4000 |
-			     (led_config->gpio_output & ~0xc000));
-	if (onoff) {
-		/* enable GPIO output */
-		val &= ~((led_config->gpio_output_enable >> 8) & 0xff);
-		val |=  (led_config->gpio_output_enable & 0xff);
-	} else {
-		/* disable GPIO output */
-		val &= ~((led_config->gpio_output_disable >> 8) & 0xff);
-		val |=  (led_config->gpio_output_disable & 0xff);
-	}
-	return au8522_writereg(state, 0x8000 |
-			       (led_config->gpio_output & ~0xc000), val);
-}
-
-/* led = 0 | off
- * led = 1 | signal ok
- * led = 2 | signal strong
- * led < 0 | only light led if leds are currently off
- */
-static int au8522_led_ctrl(struct au8522_state *state, int led)
-{
-	struct au8522_led_config *led_config = state->config->led_cfg;
-	int i, ret = 0;
-
-	/* bail out if we can't control an LED */
-	if (!led_config || !led_config->gpio_leds ||
-	    !led_config->num_led_states || !led_config->led_states)
-		return 0;
-
-	if (led < 0) {
-		/* if LED is already lit, then leave it as-is */
-		if (state->led_state)
-			return 0;
-		else
-			led *= -1;
-	}
-
-	/* toggle LED if changing state */
-	if (state->led_state != led) {
-		u8 val;
-
-		dprintk("%s: %d\n", __func__, led);
-
-		au8522_led_gpio_enable(state, 1);
-
-		val = au8522_readreg(state, 0x4000 |
-				     (led_config->gpio_leds & ~0xc000));
-
-		/* start with all leds off */
-		for (i = 0; i < led_config->num_led_states; i++)
-			val &= ~led_config->led_states[i];
-
-		/* set selected LED state */
-		if (led < led_config->num_led_states)
-			val |= led_config->led_states[led];
-		else if (led_config->num_led_states)
-			val |=
-			led_config->led_states[led_config->num_led_states - 1];
-
-		ret = au8522_writereg(state, 0x8000 |
-				      (led_config->gpio_leds & ~0xc000), val);
-		if (ret < 0)
-			return ret;
-
-		state->led_state = led;
-
-		if (led == 0)
-			au8522_led_gpio_enable(state, 0);
-	}
-
-	return 0;
-}
-
-int au8522_sleep(struct dvb_frontend *fe)
-{
-	struct au8522_state *state = fe->demodulator_priv;
-	dprintk("%s()\n", __func__);
-
-	/* Only power down if the digital side is currently using the chip */
-	if (state->operational_mode == AU8522_ANALOG_MODE) {
-		/* We're not in one of the expected power modes, which means
-		   that the DVB thread is probably telling us to go to sleep
-		   even though the analog frontend has already started using
-		   the chip.  So ignore the request */
-		return 0;
-	}
-
-	/* turn off led */
-	au8522_led_ctrl(state, 0);
-
-	/* Power down the chip */
-	au8522_writereg(state, 0xa4, 1 << 5);
-
-	state->current_frequency = 0;
-
-	return 0;
-}
-
 static int au8522_read_status(struct dvb_frontend *fe, fe_status_t *status)
 {
 	struct au8522_state *state = fe->demodulator_priv;
@@ -931,28 +738,6 @@ static int au8522_get_tune_settings(struct dvb_frontend *fe,
 
 static struct dvb_frontend_ops au8522_ops;
 
-int au8522_get_state(struct au8522_state **state, struct i2c_adapter *i2c,
-		     u8 client_address)
-{
-	int ret;
-
-	mutex_lock(&au8522_list_mutex);
-	ret = hybrid_tuner_request_state(struct au8522_state, (*state),
-					 hybrid_tuner_instance_list,
-					 i2c, client_address, "au8522");
-	mutex_unlock(&au8522_list_mutex);
-
-	return ret;
-}
-
-void au8522_release_state(struct au8522_state *state)
-{
-	mutex_lock(&au8522_list_mutex);
-	if (state != NULL)
-		hybrid_tuner_release_state(state);
-	mutex_unlock(&au8522_list_mutex);
-}
-
 
 static void au8522_release(struct dvb_frontend *fe)
 {
diff --git a/drivers/media/dvb/frontends/au8522_priv.h b/drivers/media/dvb/frontends/au8522_priv.h
index 751e17d692a9..6e4a438732b5 100644
--- a/drivers/media/dvb/frontends/au8522_priv.h
+++ b/drivers/media/dvb/frontends/au8522_priv.h
@@ -81,6 +81,8 @@ int au8522_sleep(struct dvb_frontend *fe);
 int au8522_get_state(struct au8522_state **state, struct i2c_adapter *i2c,
 		     u8 client_address);
 void au8522_release_state(struct au8522_state *state);
+int au8522_i2c_gate_ctrl(struct dvb_frontend *fe, int enable);
+int au8522_led_ctrl(struct au8522_state *state, int led);
 
 /* REGISTERS */
 #define AU8522_INPUT_CONTROL_REG081H			0x081
diff --git a/drivers/media/dvb/frontends/cx24110.c b/drivers/media/dvb/frontends/cx24110.c
index 5101f10f2d7a..98ecaf0900d6 100644
--- a/drivers/media/dvb/frontends/cx24110.c
+++ b/drivers/media/dvb/frontends/cx24110.c
@@ -512,14 +512,13 @@ static int cx24110_read_snr(struct dvb_frontend* fe, u16* snr)
 static int cx24110_read_ucblocks(struct dvb_frontend* fe, u32* ucblocks)
 {
 	struct cx24110_state *state = fe->demodulator_priv;
-	u32 lastbyer;
 
 	if(cx24110_readreg(state,0x10)&0x40) {
 		/* the RS error counter has finished one counting window */
 		cx24110_writereg(state,0x10,0x60); /* select the byer reg */
-		lastbyer=cx24110_readreg(state,0x12)|
-			(cx24110_readreg(state,0x13)<<8)|
-			(cx24110_readreg(state,0x14)<<16);
+		cx24110_readreg(state, 0x12) |
+			(cx24110_readreg(state, 0x13) << 8) |
+			(cx24110_readreg(state, 0x14) << 16);
 		cx24110_writereg(state,0x10,0x70); /* select the bler reg */
 		state->lastbler=cx24110_readreg(state,0x12)|
 			(cx24110_readreg(state,0x13)<<8)|
diff --git a/drivers/media/dvb/frontends/cxd2820r_core.c b/drivers/media/dvb/frontends/cxd2820r_core.c
index 5c7c2aaf9bf5..3bba37d74f57 100644
--- a/drivers/media/dvb/frontends/cxd2820r_core.c
+++ b/drivers/media/dvb/frontends/cxd2820r_core.c
@@ -526,12 +526,12 @@ static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 		if (ret)
 			goto error;
 
-		if (status & FE_HAS_SIGNAL)
+		if (status & FE_HAS_LOCK)
 			break;
 	}
 
 	/* check if we have a valid signal */
-	if (status) {
+	if (status & FE_HAS_LOCK) {
 		priv->last_tune_failed = 0;
 		return DVBFE_ALGO_SEARCH_SUCCESS;
 	} else {
diff --git a/drivers/media/dvb/frontends/dib7000p.c b/drivers/media/dvb/frontends/dib7000p.c
index 5ceadc285b3a..3e1eefada0e8 100644
--- a/drivers/media/dvb/frontends/dib7000p.c
+++ b/drivers/media/dvb/frontends/dib7000p.c
@@ -2396,11 +2396,6 @@ struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr,
 	   more common) */
 	st->i2c_master.gated_tuner_i2c_adap.dev.parent = i2c_adap->dev.parent;
 
-	/* FIXME: make sure the dev.parent field is initialized, or else
-	   request_firmware() will hit an OOPS (this should be moved somewhere
-	   more common) */
-	st->i2c_master.gated_tuner_i2c_adap.dev.parent = i2c_adap->dev.parent;
-
 	dibx000_init_i2c_master(&st->i2c_master, DIB7000P, st->i2c_adap, st->i2c_addr);
 
 	/* init 7090 tuner adapter */
diff --git a/drivers/media/dvb/frontends/dib9000.c b/drivers/media/dvb/frontends/dib9000.c
index 80848b4c15d4..6201c59a78dd 100644
--- a/drivers/media/dvb/frontends/dib9000.c
+++ b/drivers/media/dvb/frontends/dib9000.c
@@ -31,13 +31,6 @@ struct i2c_device {
 	u8 *i2c_write_buffer;
 };
 
-/* lock */
-#define DIB_LOCK struct mutex
-#define DibAcquireLock(lock) mutex_lock_interruptible(lock)
-#define DibReleaseLock(lock) mutex_unlock(lock)
-#define DibInitLock(lock) mutex_init(lock)
-#define DibFreeLock(lock)
-
 struct dib9000_pid_ctrl {
 #define DIB9000_PID_FILTER_CTRL 0
 #define DIB9000_PID_FILTER      1
@@ -82,11 +75,11 @@ struct dib9000_state {
 			} fe_mm[18];
 			u8 memcmd;
 
-			DIB_LOCK mbx_if_lock;	/* to protect read/write operations */
-			DIB_LOCK mbx_lock;	/* to protect the whole mailbox handling */
+			struct mutex mbx_if_lock;	/* to protect read/write operations */
+			struct mutex mbx_lock;	/* to protect the whole mailbox handling */
 
-			DIB_LOCK mem_lock;	/* to protect the memory accesses */
-			DIB_LOCK mem_mbx_lock;	/* to protect the memory-based mailbox */
+			struct mutex mem_lock;	/* to protect the memory accesses */
+			struct mutex mem_mbx_lock;	/* to protect the memory-based mailbox */
 
 #define MBX_MAX_WORDS (256 - 200 - 2)
 #define DIB9000_MSG_CACHE_SIZE 2
@@ -108,7 +101,7 @@ struct dib9000_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[255];
 	u8 i2c_read_buffer[255];
-	DIB_LOCK demod_lock;
+	struct mutex demod_lock;
 	u8 get_frontend_internal;
 	struct dib9000_pid_ctrl pid_ctrl[10];
 	s8 pid_ctrl_index; /* -1: empty list; -2: do not use the list */
@@ -446,13 +439,13 @@ static int dib9000_risc_mem_read(struct dib9000_state *state, u8 cmd, u8 * b, u1
 	if (!state->platform.risc.fw_is_running)
 		return -EIO;
 
-	if (DibAcquireLock(&state->platform.risc.mem_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
 	dib9000_risc_mem_setup(state, cmd | 0x80);
 	dib9000_risc_mem_read_chunks(state, b, len);
-	DibReleaseLock(&state->platform.risc.mem_lock);
+	mutex_unlock(&state->platform.risc.mem_lock);
 	return 0;
 }
 
@@ -462,13 +455,13 @@ static int dib9000_risc_mem_write(struct dib9000_state *state, u8 cmd, const u8
 	if (!state->platform.risc.fw_is_running)
 		return -EIO;
 
-	if (DibAcquireLock(&state->platform.risc.mem_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
 	dib9000_risc_mem_setup(state, cmd);
 	dib9000_risc_mem_write_chunks(state, b, m->size);
-	DibReleaseLock(&state->platform.risc.mem_lock);
+	mutex_unlock(&state->platform.risc.mem_lock);
 	return 0;
 }
 
@@ -537,7 +530,7 @@ static int dib9000_mbx_send_attr(struct dib9000_state *state, u8 id, u16 * data,
 	if (!state->platform.risc.fw_is_running)
 		return -EINVAL;
 
-	if (DibAcquireLock(&state->platform.risc.mbx_if_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mbx_if_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -584,7 +577,7 @@ static int dib9000_mbx_send_attr(struct dib9000_state *state, u8 id, u16 * data,
 	ret = (u8) dib9000_write_word_attr(state, 1043, 1 << 14, attr);
 
 out:
-	DibReleaseLock(&state->platform.risc.mbx_if_lock);
+	mutex_unlock(&state->platform.risc.mbx_if_lock);
 
 	return ret;
 }
@@ -602,7 +595,7 @@ static u8 dib9000_mbx_read(struct dib9000_state *state, u16 * data, u8 risc_id,
 	if (!state->platform.risc.fw_is_running)
 		return 0;
 
-	if (DibAcquireLock(&state->platform.risc.mbx_if_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mbx_if_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
@@ -643,7 +636,7 @@ static u8 dib9000_mbx_read(struct dib9000_state *state, u16 * data, u8 risc_id,
 	/* Update register nb_mes_in_TX */
 	dib9000_write_word_attr(state, 1028 + mc_base, 1 << 14, attr);
 
-	DibReleaseLock(&state->platform.risc.mbx_if_lock);
+	mutex_unlock(&state->platform.risc.mbx_if_lock);
 
 	return size + 1;
 }
@@ -708,12 +701,11 @@ static u8 dib9000_mbx_count(struct dib9000_state *state, u8 risc_id, u16 attr)
 static int dib9000_mbx_process(struct dib9000_state *state, u16 attr)
 {
 	int ret = 0;
-	u16 tmp;
 
 	if (!state->platform.risc.fw_is_running)
 		return -1;
 
-	if (DibAcquireLock(&state->platform.risc.mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return -1;
 	}
@@ -721,10 +713,10 @@ static int dib9000_mbx_process(struct dib9000_state *state, u16 attr)
 	if (dib9000_mbx_count(state, 1, attr))	/* 1=RiscB */
 		ret = dib9000_mbx_fetch_to_cache(state, attr);
 
-	tmp = dib9000_read_word_attr(state, 1229, attr);	/* Clear the IRQ */
+	dib9000_read_word_attr(state, 1229, attr);	/* Clear the IRQ */
 /*      if (tmp) */
 /*              dprintk( "cleared IRQ: %x", tmp); */
-	DibReleaseLock(&state->platform.risc.mbx_lock);
+	mutex_unlock(&state->platform.risc.mbx_lock);
 
 	return ret;
 }
@@ -1193,7 +1185,7 @@ static int dib9000_fw_get_channel(struct dvb_frontend *fe)
 	struct dibDVBTChannel *ch;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -1323,7 +1315,7 @@ static int dib9000_fw_get_channel(struct dvb_frontend *fe)
 	}
 
 error:
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 	return ret;
 }
 
@@ -1678,7 +1670,7 @@ static int dib9000_fw_component_bus_xfer(struct i2c_adapter *i2c_adap, struct i2
 		p[12] = 0;
 	}
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
@@ -1692,7 +1684,7 @@ static int dib9000_fw_component_bus_xfer(struct i2c_adapter *i2c_adap, struct i2
 
 	/* do the transaction */
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_COMPONENT_ACCESS) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		return 0;
 	}
 
@@ -1700,7 +1692,7 @@ static int dib9000_fw_component_bus_xfer(struct i2c_adapter *i2c_adap, struct i2
 	if ((num > 1) && (msg[1].flags & I2C_M_RD))
 		dib9000_risc_mem_read(state, FE_MM_RW_COMPONENT_ACCESS_BUFFER, msg[1].buf, msg[1].len);
 
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	return num;
 }
@@ -1789,7 +1781,7 @@ int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
 		return 0;
 	}
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -1799,7 +1791,7 @@ int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
 
 	dprintk("PID filter enabled %d", onoff);
 	ret = dib9000_write_word(state, 294 + 1, val);
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 
 }
@@ -1824,14 +1816,14 @@ int dib9000_fw_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
 		return 0;
 	}
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
 	dprintk("Index %x, PID %d, OnOff %d", id, pid, onoff);
 	ret = dib9000_write_word(state, 300 + 1 + id,
 			onoff ? (1 << 13) | pid : 0);
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 EXPORT_SYMBOL(dib9000_fw_pid_filter);
@@ -1851,11 +1843,6 @@ static void dib9000_release(struct dvb_frontend *demod)
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (st->fe[index_frontend] != NULL); index_frontend++)
 		dvb_frontend_detach(st->fe[index_frontend]);
 
-	DibFreeLock(&state->platform.risc.mbx_if_lock);
-	DibFreeLock(&state->platform.risc.mbx_lock);
-	DibFreeLock(&state->platform.risc.mem_lock);
-	DibFreeLock(&state->platform.risc.mem_mbx_lock);
-	DibFreeLock(&state->demod_lock);
 	dibx000_exit_i2c_master(&st->i2c_master);
 
 	i2c_del_adapter(&st->tuner_adap);
@@ -1875,7 +1862,7 @@ static int dib9000_sleep(struct dvb_frontend *fe)
 	u8 index_frontend;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -1887,7 +1874,7 @@ static int dib9000_sleep(struct dvb_frontend *fe)
 	ret = dib9000_mbx_send(state, OUT_MSG_FE_SLEEP, NULL, 0);
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -1905,7 +1892,7 @@ static int dib9000_get_frontend(struct dvb_frontend *fe)
 	int ret = 0;
 
 	if (state->get_frontend_internal == 0) {
-		if (DibAcquireLock(&state->demod_lock) < 0) {
+		if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 			dprintk("could not get the lock");
 			return -EINTR;
 		}
@@ -1964,7 +1951,7 @@ static int dib9000_get_frontend(struct dvb_frontend *fe)
 
 return_value:
 	if (state->get_frontend_internal == 0)
-		DibReleaseLock(&state->demod_lock);
+		mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2012,7 +1999,7 @@ static int dib9000_set_frontend(struct dvb_frontend *fe)
 	}
 
 	state->pid_ctrl_index = -1; /* postpone the pid filtering cmd */
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
@@ -2081,7 +2068,7 @@ static int dib9000_set_frontend(struct dvb_frontend *fe)
 	/* check the tune result */
 	if (exit_condition == 1) {	/* tune failed */
 		dprintk("tune failed");
-		DibReleaseLock(&state->demod_lock);
+		mutex_unlock(&state->demod_lock);
 		/* tune failed; put all the pid filtering cmd to junk */
 		state->pid_ctrl_index = -1;
 		return 0;
@@ -2137,7 +2124,7 @@ static int dib9000_set_frontend(struct dvb_frontend *fe)
 	/* turn off the diversity for the last frontend */
 	dib9000_fw_set_diversity_in(state->fe[index_frontend - 1], 0);
 
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	if (state->pid_ctrl_index >= 0) {
 		u8 index_pid_filter_cmd;
 		u8 pid_ctrl_index = state->pid_ctrl_index;
@@ -2175,7 +2162,7 @@ static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 	u8 index_frontend;
 	u16 lock = 0, lock_slave = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -2197,7 +2184,7 @@ static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 	if ((lock & 0x0008) || (lock_slave & 0x0008))
 		*stat |= FE_HAS_LOCK;
 
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 
 	return 0;
 }
@@ -2208,30 +2195,30 @@ static int dib9000_read_ber(struct dvb_frontend *fe, u32 * ber)
 	u16 *c;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		ret = -EINTR;
 		goto error;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		ret = -EIO;
 		goto error;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR,
 			state->i2c_read_buffer, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	c = (u16 *)state->i2c_read_buffer;
 
 	*ber = c[10] << 16 | c[11];
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2243,7 +2230,7 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 	u16 val;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -2256,18 +2243,18 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 			*strength += val;
 	}
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		ret = -EINTR;
 		goto error;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		ret = -EIO;
 		goto error;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	val = 65535 - c[4];
 	if (val > 65535 - *strength)
@@ -2276,7 +2263,7 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 		*strength += val;
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2287,16 +2274,16 @@ static u32 dib9000_get_snr(struct dvb_frontend *fe)
 	u32 n, s, exp;
 	u16 val;
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		return 0;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	val = c[7];
 	n = (val >> 4) & 0xff;
@@ -2326,7 +2313,7 @@ static int dib9000_read_snr(struct dvb_frontend *fe, u16 * snr)
 	u8 index_frontend;
 	u32 snr_master;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -2340,7 +2327,7 @@ static int dib9000_read_snr(struct dvb_frontend *fe, u16 * snr)
 	} else
 		*snr = 0;
 
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 
 	return 0;
 }
@@ -2351,27 +2338,27 @@ static int dib9000_read_unc_blocks(struct dvb_frontend *fe, u32 * unc)
 	u16 *c = (u16 *)state->i2c_read_buffer;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		ret = -EINTR;
 		goto error;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		ret = -EIO;
 		goto error;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	*unc = c[12];
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2514,11 +2501,11 @@ struct dvb_frontend *dib9000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, c
 	st->gpio_val = DIB9000_GPIO_DEFAULT_VALUES;
 	st->gpio_pwm_pos = DIB9000_GPIO_DEFAULT_PWM_POS;
 
-	DibInitLock(&st->platform.risc.mbx_if_lock);
-	DibInitLock(&st->platform.risc.mbx_lock);
-	DibInitLock(&st->platform.risc.mem_lock);
-	DibInitLock(&st->platform.risc.mem_mbx_lock);
-	DibInitLock(&st->demod_lock);
+	mutex_init(&st->platform.risc.mbx_if_lock);
+	mutex_init(&st->platform.risc.mbx_lock);
+	mutex_init(&st->platform.risc.mem_lock);
+	mutex_init(&st->platform.risc.mem_mbx_lock);
+	mutex_init(&st->demod_lock);
 	st->get_frontend_internal = 0;
 
 	st->pid_ctrl_index = -2;
diff --git a/drivers/media/dvb/frontends/drxd.h b/drivers/media/dvb/frontends/drxd.h
index 34398738f9bc..216c8c3702f8 100644
--- a/drivers/media/dvb/frontends/drxd.h
+++ b/drivers/media/dvb/frontends/drxd.h
@@ -51,9 +51,23 @@ struct drxd_config {
 	 s16(*osc_deviation) (void *priv, s16 dev, int flag);
 };
 
+#if defined(CONFIG_DVB_DRXD) || \
+			(defined(CONFIG_DVB_DRXD_MODULE) && defined(MODULE))
 extern
 struct dvb_frontend *drxd_attach(const struct drxd_config *config,
 				 void *priv, struct i2c_adapter *i2c,
 				 struct device *dev);
+#else
+static inline
+struct dvb_frontend *drxd_attach(const struct drxd_config *config,
+				 void *priv, struct i2c_adapter *i2c,
+				 struct device *dev)
+{
+	printk(KERN_INFO "%s: not probed - driver disabled by Kconfig\n",
+	       __func__);
+	return NULL;
+}
+#endif
+
 extern int drxd_config_i2c(struct dvb_frontend *, int);
 #endif
diff --git a/drivers/media/dvb/frontends/drxk_hard.c b/drivers/media/dvb/frontends/drxk_hard.c
index a414b1f2b6a5..60b868faeacf 100644
--- a/drivers/media/dvb/frontends/drxk_hard.c
+++ b/drivers/media/dvb/frontends/drxk_hard.c
@@ -1380,20 +1380,20 @@ static int DownloadMicrocode(struct drxk_state *state,
 			     const u8 pMCImage[], u32 Length)
 {
 	const u8 *pSrc = pMCImage;
-	u16 Flags;
-	u16 Drain;
 	u32 Address;
 	u16 nBlocks;
 	u16 BlockSize;
-	u16 BlockCRC;
 	u32 offset = 0;
 	u32 i;
 	int status = 0;
 
 	dprintk(1, "\n");
 
-	/* down the drain (we don care about MAGIC_WORD) */
+	/* down the drain (we don't care about MAGIC_WORD) */
+#if 0
+	/* For future reference */
 	Drain = (pSrc[0] << 8) | pSrc[1];
+#endif
 	pSrc += sizeof(u16);
 	offset += sizeof(u16);
 	nBlocks = (pSrc[0] << 8) | pSrc[1];
@@ -1410,11 +1410,17 @@ static int DownloadMicrocode(struct drxk_state *state,
 		pSrc += sizeof(u16);
 		offset += sizeof(u16);
 
+#if 0
+		/* For future reference */
 		Flags = (pSrc[0] << 8) | pSrc[1];
+#endif
 		pSrc += sizeof(u16);
 		offset += sizeof(u16);
 
+#if 0
+		/* For future reference */
 		BlockCRC = (pSrc[0] << 8) | pSrc[1];
+#endif
 		pSrc += sizeof(u16);
 		offset += sizeof(u16);
 
@@ -5829,7 +5835,7 @@ static int WriteGPIO(struct drxk_state *state)
 		}
 		if (state->UIO_mask & 0x0002) { /* UIO-2 */
 			/* write to io pad configuration register - output mode */
-			status = write16(state, SIO_PDR_SMA_TX_CFG__A, state->m_GPIOCfg);
+			status = write16(state, SIO_PDR_SMA_RX_CFG__A, state->m_GPIOCfg);
 			if (status < 0)
 				goto error;
 
@@ -5848,7 +5854,7 @@ static int WriteGPIO(struct drxk_state *state)
 		}
 		if (state->UIO_mask & 0x0004) { /* UIO-3 */
 			/* write to io pad configuration register - output mode */
-			status = write16(state, SIO_PDR_SMA_TX_CFG__A, state->m_GPIOCfg);
+			status = write16(state, SIO_PDR_GPIO_CFG__A, state->m_GPIOCfg);
 			if (status < 0)
 				goto error;
 
diff --git a/drivers/media/dvb/frontends/drxk_map.h b/drivers/media/dvb/frontends/drxk_map.h
index 9b11a8328869..23e16c12f234 100644
--- a/drivers/media/dvb/frontends/drxk_map.h
+++ b/drivers/media/dvb/frontends/drxk_map.h
@@ -432,6 +432,7 @@
 #define  SIO_PDR_UIO_OUT_LO__A                                             0x7F0016
 #define  SIO_PDR_OHW_CFG__A                                                0x7F001F
 #define    SIO_PDR_OHW_CFG_FREF_SEL__M                                     0x3
+#define  SIO_PDR_GPIO_CFG__A                                               0x7F0021
 #define  SIO_PDR_MSTRT_CFG__A                                              0x7F0025
 #define  SIO_PDR_MERR_CFG__A                                               0x7F0026
 #define  SIO_PDR_MCLK_CFG__A                                               0x7F0028
@@ -446,4 +447,5 @@
 #define  SIO_PDR_MD5_CFG__A                                                0x7F0030
 #define  SIO_PDR_MD6_CFG__A                                                0x7F0031
 #define  SIO_PDR_MD7_CFG__A                                                0x7F0032
+#define  SIO_PDR_SMA_RX_CFG__A                                             0x7F0037
 #define  SIO_PDR_SMA_TX_CFG__A                                             0x7F0038
diff --git a/drivers/media/dvb/frontends/ds3000.c b/drivers/media/dvb/frontends/ds3000.c
index af65d013db11..4c8ac2657c4a 100644
--- a/drivers/media/dvb/frontends/ds3000.c
+++ b/drivers/media/dvb/frontends/ds3000.c
@@ -1114,7 +1114,10 @@ static int ds3000_set_frontend(struct dvb_frontend *fe)
 			ds3000_writereg(state,
 				ds3000_dvbs2_init_tab[i],
 				ds3000_dvbs2_init_tab[i + 1]);
-		ds3000_writereg(state, 0xfe, 0x98);
+		if (c->symbol_rate >= 30000000)
+			ds3000_writereg(state, 0xfe, 0x54);
+		else
+			ds3000_writereg(state, 0xfe, 0x98);
 		break;
 	default:
 		return 1;
diff --git a/drivers/media/dvb/frontends/it913x-fe.c b/drivers/media/dvb/frontends/it913x-fe.c
index 84df03c29179..708cbf197913 100644
--- a/drivers/media/dvb/frontends/it913x-fe.c
+++ b/drivers/media/dvb/frontends/it913x-fe.c
@@ -633,10 +633,9 @@ static int it913x_fe_read_snr(struct dvb_frontend *fe, u16 *snr)
 static int it913x_fe_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
 	struct it913x_fe_state *state = fe->demodulator_priv;
-	int ret;
 	u8 reg[5];
 	/* Read Aborted Packets and Pre-Viterbi error rate 5 bytes */
-	ret = it913x_read_reg(state, RSD_ABORT_PKT_LSB, reg, sizeof(reg));
+	it913x_read_reg(state, RSD_ABORT_PKT_LSB, reg, sizeof(reg));
 	state->ucblocks += (u32)(reg[1] << 8) | reg[0];
 	*ber = (u32)(reg[4] << 16) | (reg[3] << 8) | reg[2];
 	return 0;
@@ -658,10 +657,9 @@ static int it913x_fe_get_frontend(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct it913x_fe_state *state = fe->demodulator_priv;
-	int ret;
 	u8 reg[8];
 
-	ret = it913x_read_reg(state, REG_TPSD_TX_MODE, reg, sizeof(reg));
+	it913x_read_reg(state, REG_TPSD_TX_MODE, reg, sizeof(reg));
 
 	if (reg[3] < 3)
 		p->modulation = fe_con[reg[3]];
@@ -691,25 +689,25 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct it913x_fe_state *state = fe->demodulator_priv;
-	int ret, i;
+	int i;
 	u8 empty_ch, last_ch;
 
 	state->it913x_status = 0;
 
 	/* Set bw*/
-	ret = it913x_fe_select_bw(state, p->bandwidth_hz,
+	it913x_fe_select_bw(state, p->bandwidth_hz,
 		state->adcFrequency);
 
 	/* Training Mode Off */
-	ret = it913x_write_reg(state, PRO_LINK, TRAINING_MODE, 0x0);
+	it913x_write_reg(state, PRO_LINK, TRAINING_MODE, 0x0);
 
 	/* Clear Empty Channel */
-	ret = it913x_write_reg(state, PRO_DMOD, EMPTY_CHANNEL_STATUS, 0x0);
+	it913x_write_reg(state, PRO_DMOD, EMPTY_CHANNEL_STATUS, 0x0);
 
 	/* Clear bits */
-	ret = it913x_write_reg(state, PRO_DMOD, MP2IF_SYNC_LK, 0x0);
+	it913x_write_reg(state, PRO_DMOD, MP2IF_SYNC_LK, 0x0);
 	/* LED on */
-	ret = it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x1);
+	it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x1);
 	/* Select Band*/
 	if ((p->frequency >= 51000000) && (p->frequency <= 230000000))
 		i = 0;
@@ -720,7 +718,7 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 	else
 		return -EOPNOTSUPP;
 
-	ret = it913x_write_reg(state, PRO_DMOD, FREE_BAND, i);
+	it913x_write_reg(state, PRO_DMOD, FREE_BAND, i);
 
 	deb_info("Frontend Set Tuner Type %02x", state->tuner_type);
 	switch (state->tuner_type) {
@@ -730,7 +728,7 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 	case IT9135_60:
 	case IT9135_61:
 	case IT9135_62:
-		ret = it9137_set_tuner(state,
+		it9137_set_tuner(state,
 			p->bandwidth_hz, p->frequency);
 		break;
 	default:
@@ -742,9 +740,9 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 		break;
 	}
 	/* LED off */
-	ret = it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x0);
+	it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x0);
 	/* Trigger ofsm */
-	ret = it913x_write_reg(state, PRO_DMOD, TRIGGER_OFSM, 0x0);
+	it913x_write_reg(state, PRO_DMOD, TRIGGER_OFSM, 0x0);
 	last_ch = 2;
 	for (i = 0; i < 40; ++i) {
 		empty_ch = it913x_read_reg_u8(state, EMPTY_CHANNEL_STATUS);
diff --git a/drivers/media/dvb/frontends/lg2160.c b/drivers/media/dvb/frontends/lg2160.c
new file mode 100644
index 000000000000..a3ab1a5b6597
--- /dev/null
+++ b/drivers/media/dvb/frontends/lg2160.c
@@ -0,0 +1,1468 @@
+/*
+ *    Support for LG2160 - ATSC/MH
+ *
+ *    Copyright (C) 2010 Michael Krufky <mkrufky@linuxtv.org>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/jiffies.h>
+#include <linux/dvb/frontend.h>
+#include "lg2160.h"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "set debug level (info=1, reg=2 (or-able))");
+
+#define DBG_INFO 1
+#define DBG_REG  2
+
+#define lg_printk(kern, fmt, arg...)					\
+	printk(kern "%s: " fmt, __func__, ##arg)
+
+#define lg_info(fmt, arg...)	printk(KERN_INFO "lg2160: " fmt, ##arg)
+#define lg_warn(fmt, arg...)	lg_printk(KERN_WARNING,       fmt, ##arg)
+#define lg_err(fmt, arg...)	lg_printk(KERN_ERR,           fmt, ##arg)
+#define lg_dbg(fmt, arg...) if (debug & DBG_INFO)			\
+				lg_printk(KERN_DEBUG,         fmt, ##arg)
+#define lg_reg(fmt, arg...) if (debug & DBG_REG)			\
+				lg_printk(KERN_DEBUG,         fmt, ##arg)
+
+#define lg_fail(ret)							\
+({									\
+	int __ret;							\
+	__ret = (ret < 0);						\
+	if (__ret)							\
+		lg_err("error %d on line %d\n",	ret, __LINE__);		\
+	__ret;								\
+})
+
+struct lg216x_state {
+	struct i2c_adapter *i2c_adap;
+	const struct lg2160_config *cfg;
+
+	struct dvb_frontend frontend;
+
+	u32 current_frequency;
+	u8 parade_id;
+	u8 fic_ver;
+	unsigned int last_reset;
+};
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_write_reg(struct lg216x_state *state, u16 reg, u8 val)
+{
+	int ret;
+	u8 buf[] = { reg >> 8, reg & 0xff, val };
+	struct i2c_msg msg = {
+		.addr = state->cfg->i2c_addr, .flags = 0,
+		.buf = buf, .len = 3,
+	};
+
+	lg_reg("reg: 0x%04x, val: 0x%02x\n", reg, val);
+
+	ret = i2c_transfer(state->i2c_adap, &msg, 1);
+
+	if (ret != 1) {
+		lg_err("error (addr %02x %02x <- %02x, err = %i)\n",
+		       msg.buf[0], msg.buf[1], msg.buf[2], ret);
+		if (ret < 0)
+			return ret;
+		else
+			return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int lg216x_read_reg(struct lg216x_state *state, u16 reg, u8 *val)
+{
+	int ret;
+	u8 reg_buf[] = { reg >> 8, reg & 0xff };
+	struct i2c_msg msg[] = {
+		{ .addr = state->cfg->i2c_addr,
+		  .flags = 0, .buf = reg_buf, .len = 2 },
+		{ .addr = state->cfg->i2c_addr,
+		  .flags = I2C_M_RD, .buf = val, .len = 1 },
+	};
+
+	lg_reg("reg: 0x%04x\n", reg);
+
+	ret = i2c_transfer(state->i2c_adap, msg, 2);
+
+	if (ret != 2) {
+		lg_err("error (addr %02x reg %04x error (ret == %i)\n",
+		       state->cfg->i2c_addr, reg, ret);
+		if (ret < 0)
+			return ret;
+		else
+			return -EREMOTEIO;
+	}
+	return 0;
+}
+
+struct lg216x_reg {
+	u16 reg;
+	u8 val;
+};
+
+static int lg216x_write_regs(struct lg216x_state *state,
+			     struct lg216x_reg *regs, int len)
+{
+	int i, ret;
+
+	lg_reg("writing %d registers...\n", len);
+
+	for (i = 0; i < len - 1; i++) {
+		ret = lg216x_write_reg(state, regs[i].reg, regs[i].val);
+		if (lg_fail(ret))
+			return ret;
+	}
+	return 0;
+}
+
+static int lg216x_set_reg_bit(struct lg216x_state *state,
+			      u16 reg, int bit, int onoff)
+{
+	u8 val;
+	int ret;
+
+	lg_reg("reg: 0x%04x, bit: %d, level: %d\n", reg, bit, onoff);
+
+	ret = lg216x_read_reg(state, reg, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= ~(1 << bit);
+	val |= (onoff & 1) << bit;
+
+	ret = lg216x_write_reg(state, reg, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	if (state->cfg->deny_i2c_rptr)
+		return 0;
+
+	lg_dbg("(%d)\n", enable);
+
+	ret = lg216x_set_reg_bit(state, 0x0000, 0, enable ? 0 : 1);
+
+	msleep(1);
+
+	return ret;
+}
+
+static int lg216x_soft_reset(struct lg216x_state *state)
+{
+	int ret;
+
+	lg_dbg("\n");
+
+	ret = lg216x_write_reg(state, 0x0002, 0x00);
+	if (lg_fail(ret))
+		goto fail;
+
+	msleep(20);
+	ret = lg216x_write_reg(state, 0x0002, 0x01);
+	if (lg_fail(ret))
+		goto fail;
+
+	state->last_reset = jiffies_to_msecs(jiffies);
+fail:
+	return ret;
+}
+
+static int lg216x_initialize(struct lg216x_state *state)
+{
+	int ret;
+
+	static struct lg216x_reg lg2160_init[] = {
+#if 0
+		{ .reg = 0x0015, .val = 0xe6 },
+#else
+		{ .reg = 0x0015, .val = 0xf7 },
+		{ .reg = 0x001b, .val = 0x52 },
+		{ .reg = 0x0208, .val = 0x00 },
+		{ .reg = 0x0209, .val = 0x82 },
+		{ .reg = 0x0210, .val = 0xf9 },
+		{ .reg = 0x020a, .val = 0x00 },
+		{ .reg = 0x020b, .val = 0x82 },
+		{ .reg = 0x020d, .val = 0x28 },
+		{ .reg = 0x020f, .val = 0x14 },
+#endif
+	};
+
+	static struct lg216x_reg lg2161_init[] = {
+		{ .reg = 0x0000, .val = 0x41 },
+		{ .reg = 0x0001, .val = 0xfb },
+		{ .reg = 0x0216, .val = 0x00 },
+		{ .reg = 0x0219, .val = 0x00 },
+		{ .reg = 0x021b, .val = 0x55 },
+		{ .reg = 0x0606, .val = 0x0a },
+	};
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_write_regs(state,
+					lg2160_init, ARRAY_SIZE(lg2160_init));
+		break;
+	case LG2161:
+		ret = lg216x_write_regs(state,
+					lg2161_init, ARRAY_SIZE(lg2161_init));
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_soft_reset(state);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_set_if(struct lg216x_state *state)
+{
+	u8 val;
+	int ret;
+
+	lg_dbg("%d KHz\n", state->cfg->if_khz);
+
+	ret = lg216x_read_reg(state, 0x0132, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfb;
+	val |= (0 == state->cfg->if_khz) ? 0x04 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0132, val);
+	lg_fail(ret);
+
+	/* if NOT zero IF, 6 MHz is the default */
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg2160_agc_fix(struct lg216x_state *state,
+			  int if_agc_fix, int rf_agc_fix)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0100, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xf3;
+	val |= (if_agc_fix) ? 0x08 : 0x00;
+	val |= (rf_agc_fix) ? 0x04 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0100, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+#if 0
+static int lg2160_agc_freeze(struct lg216x_state *state,
+			     int if_agc_freeze, int rf_agc_freeze)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0100, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xcf;
+	val |= (if_agc_freeze) ? 0x20 : 0x00;
+	val |= (rf_agc_freeze) ? 0x10 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0100, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+#endif
+
+static int lg2160_agc_polarity(struct lg216x_state *state,
+			       int if_agc_polarity, int rf_agc_polarity)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0100, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfc;
+	val |= (if_agc_polarity) ? 0x02 : 0x00;
+	val |= (rf_agc_polarity) ? 0x01 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0100, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2160_tuner_pwr_save_polarity(struct lg216x_state *state,
+					  int polarity)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0008, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfe;
+	val |= (polarity) ? 0x01 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0008, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2160_spectrum_polarity(struct lg216x_state *state,
+				    int inverted)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0132, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfd;
+	val |= (inverted) ? 0x02 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0132, val);
+	lg_fail(ret);
+fail:
+	return lg216x_soft_reset(state);
+}
+
+static int lg2160_tuner_pwr_save(struct lg216x_state *state, int onoff)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0007, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xbf;
+	val |= (onoff) ? 0x40 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0007, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg216x_set_parade(struct lg216x_state *state, int id)
+{
+	int ret;
+
+	ret = lg216x_write_reg(state, 0x013e, id & 0x7f);
+	if (lg_fail(ret))
+		goto fail;
+
+	state->parade_id = id & 0x7f;
+fail:
+	return ret;
+}
+
+static int lg216x_set_ensemble(struct lg216x_state *state, int id)
+{
+	int ret;
+	u16 reg;
+	u8 val;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		reg = 0x0400;
+		break;
+	case LG2161:
+	default:
+		reg = 0x0500;
+		break;
+	}
+
+	ret = lg216x_read_reg(state, reg, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfe;
+	val |= (id) ? 0x01 : 0x00;
+
+	ret = lg216x_write_reg(state, reg, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2160_set_spi_clock(struct lg216x_state *state)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0014, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xf3;
+	val |= (state->cfg->spi_clock << 2);
+
+	ret = lg216x_write_reg(state, 0x0014, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2161_set_output_interface(struct lg216x_state *state)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0014, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= ~0x07;
+	val |= state->cfg->output_if; /* FIXME: needs sanity check */
+
+	ret = lg216x_write_reg(state, 0x0014, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg216x_enable_fic(struct lg216x_state *state, int onoff)
+{
+	int ret;
+
+	ret = lg216x_write_reg(state, 0x0017, 0x23);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_write_reg(state, 0x0016, 0xfc);
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_write_reg(state, 0x0016,
+				       0xfc | ((onoff) ? 0x02 : 0x00));
+		break;
+	case LG2161:
+		ret = lg216x_write_reg(state, 0x0016, (onoff) ? 0x10 : 0x00);
+		break;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_initialize(state);
+	if (lg_fail(ret))
+		goto fail;
+
+	if (onoff) {
+		ret = lg216x_write_reg(state, 0x0017, 0x03);
+		lg_fail(ret);
+	}
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_get_fic_version(struct lg216x_state *state, u8 *ficver)
+{
+	u8 val;
+	int ret;
+
+	*ficver = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0128, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*ficver = (val >> 3) & 0x1f;
+fail:
+	return ret;
+}
+
+#if 0
+static int lg2160_get_parade_id(struct lg216x_state *state, u8 *id)
+{
+	u8 val;
+	int ret;
+
+	*id = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0123, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*id = val & 0x7f;
+fail:
+	return ret;
+}
+#endif
+
+static int lg216x_get_nog(struct lg216x_state *state, u8 *nog)
+{
+	u8 val;
+	int ret;
+
+	*nog = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0124, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*nog = ((val >> 4) & 0x07) + 1;
+fail:
+	return ret;
+}
+
+static int lg216x_get_tnog(struct lg216x_state *state, u8 *tnog)
+{
+	u8 val;
+	int ret;
+
+	*tnog = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0125, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*tnog = val & 0x1f;
+fail:
+	return ret;
+}
+
+static int lg216x_get_sgn(struct lg216x_state *state, u8 *sgn)
+{
+	u8 val;
+	int ret;
+
+	*sgn = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0124, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*sgn = val & 0x0f;
+fail:
+	return ret;
+}
+
+static int lg216x_get_prc(struct lg216x_state *state, u8 *prc)
+{
+	u8 val;
+	int ret;
+
+	*prc = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0125, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*prc = ((val >> 5) & 0x07) + 1;
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_get_rs_frame_mode(struct lg216x_state *state,
+				    enum atscmh_rs_frame_mode *rs_framemode)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0410, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0513, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	switch ((val >> 4) & 0x03) {
+#if 1
+	default:
+#endif
+	case 0x00:
+		*rs_framemode = ATSCMH_RSFRAME_PRI_ONLY;
+		break;
+	case 0x01:
+		*rs_framemode = ATSCMH_RSFRAME_PRI_SEC;
+		break;
+#if 0
+	default:
+		*rs_framemode = ATSCMH_RSFRAME_RES;
+		break;
+#endif
+	}
+fail:
+	return ret;
+}
+
+static
+int lg216x_get_rs_frame_ensemble(struct lg216x_state *state,
+				 enum atscmh_rs_frame_ensemble *rs_frame_ens)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0400, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0500, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0x01;
+	*rs_frame_ens = (enum atscmh_rs_frame_ensemble) val;
+fail:
+	return ret;
+}
+
+static int lg216x_get_rs_code_mode(struct lg216x_state *state,
+				   enum atscmh_rs_code_mode *rs_code_pri,
+				   enum atscmh_rs_code_mode *rs_code_sec)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0410, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0513, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	*rs_code_pri = (enum atscmh_rs_code_mode) ((val >> 2) & 0x03);
+	*rs_code_sec = (enum atscmh_rs_code_mode) (val & 0x03);
+fail:
+	return ret;
+}
+
+static int lg216x_get_sccc_block_mode(struct lg216x_state *state,
+				      enum atscmh_sccc_block_mode *sccc_block)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0315, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0511, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (val & 0x03) {
+	case 0x00:
+		*sccc_block = ATSCMH_SCCC_BLK_SEP;
+		break;
+	case 0x01:
+		*sccc_block = ATSCMH_SCCC_BLK_COMB;
+		break;
+	default:
+		*sccc_block = ATSCMH_SCCC_BLK_RES;
+		break;
+	}
+fail:
+	return ret;
+}
+
+static int lg216x_get_sccc_code_mode(struct lg216x_state *state,
+				     enum atscmh_sccc_code_mode *mode_a,
+				     enum atscmh_sccc_code_mode *mode_b,
+				     enum atscmh_sccc_code_mode *mode_c,
+				     enum atscmh_sccc_code_mode *mode_d)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0316, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0512, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	switch ((val >> 6) & 0x03) {
+	case 0x00:
+		*mode_a = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_a = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_a = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+
+	switch ((val >> 4) & 0x03) {
+	case 0x00:
+		*mode_b = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_b = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_b = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+
+	switch ((val >> 2) & 0x03) {
+	case 0x00:
+		*mode_c = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_c = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_c = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+
+	switch (val & 0x03) {
+	case 0x00:
+		*mode_d = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_d = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_d = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+#if 0
+static int lg216x_read_fic_err_count(struct lg216x_state *state, u8 *err)
+{
+	u8 fic_err;
+	int ret;
+
+	*err = 0;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0012, &fic_err);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x001e, &fic_err);
+		break;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = fic_err;
+fail:
+	return ret;
+}
+
+static int lg2160_read_crc_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 crc_err1, crc_err2;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0411, &crc_err1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0412, &crc_err2);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)(((crc_err2 & 0x0f) << 8) | crc_err1);
+fail:
+	return ret;
+}
+
+static int lg2161_read_crc_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 crc_err;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0612, &crc_err);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)crc_err;
+fail:
+	return ret;
+}
+
+static int lg216x_read_crc_err_count(struct lg216x_state *state, u16 *err)
+{
+	int ret;
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_read_crc_err_count(state, err);
+		break;
+	case LG2161:
+		ret = lg2161_read_crc_err_count(state, err);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int lg2160_read_rs_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 rs_err1, rs_err2;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0413, &rs_err1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0414, &rs_err2);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)(((rs_err2 & 0x0f) << 8) | rs_err1);
+fail:
+	return ret;
+}
+
+static int lg2161_read_rs_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 rs_err1, rs_err2;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0613, &rs_err1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0614, &rs_err2);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)((rs_err1 << 8) | rs_err2);
+fail:
+	return ret;
+}
+
+static int lg216x_read_rs_err_count(struct lg216x_state *state, u16 *err)
+{
+	int ret;
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_read_rs_err_count(state, err);
+		break;
+	case LG2161:
+		ret = lg2161_read_rs_err_count(state, err);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+#endif
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_get_frontend(struct dvb_frontend *fe)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	lg_dbg("\n");
+
+	fe->dtv_property_cache.modulation = VSB_8;
+	fe->dtv_property_cache.frequency = state->current_frequency;
+	fe->dtv_property_cache.delivery_system = SYS_ATSCMH;
+
+	ret = lg216x_get_fic_version(state,
+				     &fe->dtv_property_cache.atscmh_fic_ver);
+	if (lg_fail(ret))
+		goto fail;
+	if (state->fic_ver != fe->dtv_property_cache.atscmh_fic_ver) {
+		state->fic_ver = fe->dtv_property_cache.atscmh_fic_ver;
+
+#if 0
+		ret = lg2160_get_parade_id(state,
+				&fe->dtv_property_cache.atscmh_parade_id);
+		if (lg_fail(ret))
+			goto fail;
+/* #else */
+		fe->dtv_property_cache.atscmh_parade_id = state->parade_id;
+#endif
+		ret = lg216x_get_nog(state,
+				     &fe->dtv_property_cache.atscmh_nog);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_tnog(state,
+				      &fe->dtv_property_cache.atscmh_tnog);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_sgn(state,
+				     &fe->dtv_property_cache.atscmh_sgn);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_prc(state,
+				     &fe->dtv_property_cache.atscmh_prc);
+		if (lg_fail(ret))
+			goto fail;
+
+		ret = lg216x_get_rs_frame_mode(state,
+			(enum atscmh_rs_frame_mode *)
+			&fe->dtv_property_cache.atscmh_rs_frame_mode);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_rs_frame_ensemble(state,
+			(enum atscmh_rs_frame_ensemble *)
+			&fe->dtv_property_cache.atscmh_rs_frame_ensemble);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_rs_code_mode(state,
+			(enum atscmh_rs_code_mode *)
+			&fe->dtv_property_cache.atscmh_rs_code_mode_pri,
+			(enum atscmh_rs_code_mode *)
+			&fe->dtv_property_cache.atscmh_rs_code_mode_sec);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_sccc_block_mode(state,
+			(enum atscmh_sccc_block_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_block_mode);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_sccc_code_mode(state,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_a,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_b,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_c,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_d);
+		if (lg_fail(ret))
+			goto fail;
+	}
+#if 0
+	ret = lg216x_read_fic_err_count(state,
+				(u8 *)&fe->dtv_property_cache.atscmh_fic_err);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg216x_read_crc_err_count(state,
+				&fe->dtv_property_cache.atscmh_crc_err);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg216x_read_rs_err_count(state,
+				&fe->dtv_property_cache.atscmh_rs_err);
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		if (((fe->dtv_property_cache.atscmh_rs_err >= 240) &&
+		     (fe->dtv_property_cache.atscmh_crc_err >= 240)) &&
+		    ((jiffies_to_msecs(jiffies) - state->last_reset) > 6000))
+			ret = lg216x_soft_reset(state);
+		break;
+	case LG2161:
+		/* no fix needed here (as far as we know) */
+		ret = 0;
+		break;
+	}
+	lg_fail(ret);
+#endif
+fail:
+	return ret;
+}
+
+static int lg216x_get_property(struct dvb_frontend *fe,
+			       struct dtv_property *tvp)
+{
+	return (DTV_ATSCMH_FIC_VER == tvp->cmd) ?
+		lg216x_get_frontend(fe) : 0;
+}
+
+
+static int lg2160_set_frontend(struct dvb_frontend *fe)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	lg_dbg("(%d)\n", fe->dtv_property_cache.frequency);
+
+	if (fe->ops.tuner_ops.set_params) {
+		ret = fe->ops.tuner_ops.set_params(fe);
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 0);
+		if (lg_fail(ret))
+			goto fail;
+		state->current_frequency = fe->dtv_property_cache.frequency;
+	}
+
+	ret = lg2160_agc_fix(state, 0, 0);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg2160_agc_polarity(state, 0, 0);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg2160_tuner_pwr_save_polarity(state, 1);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg216x_set_if(state);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg2160_spectrum_polarity(state, state->cfg->spectral_inversion);
+	if (lg_fail(ret))
+		goto fail;
+
+	/* be tuned before this point */
+	ret = lg216x_soft_reset(state);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg2160_tuner_pwr_save(state, 0);
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_set_spi_clock(state);
+		if (lg_fail(ret))
+			goto fail;
+		break;
+	case LG2161:
+		ret = lg2161_set_output_interface(state);
+		if (lg_fail(ret))
+			goto fail;
+		break;
+	}
+
+	ret = lg216x_set_parade(state, fe->dtv_property_cache.atscmh_parade_id);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_set_ensemble(state,
+			fe->dtv_property_cache.atscmh_rs_frame_ensemble);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_initialize(state);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_enable_fic(state, 1);
+	lg_fail(ret);
+
+	lg216x_get_frontend(fe);
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg2160_read_lock_status(struct lg216x_state *state,
+				   int *acq_lock, int *sync_lock)
+{
+	u8 val;
+	int ret;
+
+	*acq_lock = 0;
+	*sync_lock = 0;
+
+	ret = lg216x_read_reg(state, 0x011b, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*sync_lock = (val & 0x20) ? 0 : 1;
+	*acq_lock  = (val & 0x40) ? 0 : 1;
+fail:
+	return ret;
+}
+
+#ifdef USE_LG2161_LOCK_BITS
+static int lg2161_read_lock_status(struct lg216x_state *state,
+				   int *acq_lock, int *sync_lock)
+{
+	u8 val;
+	int ret;
+
+	*acq_lock = 0;
+	*sync_lock = 0;
+
+	ret = lg216x_read_reg(state, 0x0304, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*sync_lock = (val & 0x80) ? 0 : 1;
+
+	ret = lg216x_read_reg(state, 0x011b, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*acq_lock  = (val & 0x40) ? 0 : 1;
+fail:
+	return ret;
+}
+#endif
+
+static int lg216x_read_lock_status(struct lg216x_state *state,
+				   int *acq_lock, int *sync_lock)
+{
+#ifdef USE_LG2161_LOCK_BITS
+	int ret;
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_read_lock_status(state, acq_lock, sync_lock);
+		break;
+	case LG2161:
+		ret = lg2161_read_lock_status(state, acq_lock, sync_lock);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+#else
+	return lg2160_read_lock_status(state, acq_lock, sync_lock);
+#endif
+}
+
+static int lg216x_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret, acq_lock, sync_lock;
+
+	*status = 0;
+
+	ret = lg216x_read_lock_status(state, &acq_lock, &sync_lock);
+	if (lg_fail(ret))
+		goto fail;
+
+	lg_dbg("%s%s\n",
+	       acq_lock  ? "SIGNALEXIST " : "",
+	       sync_lock ? "SYNCLOCK"     : "");
+
+	if (acq_lock)
+		*status |= FE_HAS_SIGNAL;
+	if (sync_lock)
+		*status |= FE_HAS_SYNC;
+
+	if (*status)
+		*status |= FE_HAS_CARRIER | FE_HAS_VITERBI | FE_HAS_LOCK;
+
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg2160_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	u8 snr1, snr2;
+	int ret;
+
+	*snr = 0;
+
+	ret = lg216x_read_reg(state, 0x0202, &snr1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0203, &snr2);
+	if (lg_fail(ret))
+		goto fail;
+
+	if ((snr1 == 0xba) || (snr2 == 0xdf))
+		*snr = 0;
+	else
+#if 1
+	*snr =  ((snr1 >> 4) * 100) + ((snr1 & 0x0f) * 10) + (snr2 >> 4);
+#else /* BCD */
+	*snr =  (snr2 | (snr1 << 8));
+#endif
+fail:
+	return ret;
+}
+
+static int lg2161_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	u8 snr1, snr2;
+	int ret;
+
+	*snr = 0;
+
+	ret = lg216x_read_reg(state, 0x0302, &snr1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0303, &snr2);
+	if (lg_fail(ret))
+		goto fail;
+
+	if ((snr1 == 0xba) || (snr2 == 0xfd))
+		*snr = 0;
+	else
+
+	*snr =  ((snr1 >> 4) * 100) + ((snr1 & 0x0f) * 10) + (snr2 & 0x0f);
+fail:
+	return ret;
+}
+
+static int lg216x_read_signal_strength(struct dvb_frontend *fe,
+				       u16 *strength)
+{
+#if 0
+	/* borrowed from lgdt330x.c
+	 *
+	 * Calculate strength from SNR up to 35dB
+	 * Even though the SNR can go higher than 35dB,
+	 * there is some comfort factor in having a range of
+	 * strong signals that can show at 100%
+	 */
+	struct lg216x_state *state = fe->demodulator_priv;
+	u16 snr;
+	int ret;
+#endif
+	*strength = 0;
+#if 0
+	ret = fe->ops.read_snr(fe, &snr);
+	if (lg_fail(ret))
+		goto fail;
+	/* Rather than use the 8.8 value snr, use state->snr which is 8.24 */
+	/* scale the range 0 - 35*2^24 into 0 - 65535 */
+	if (state->snr >= 8960 * 0x10000)
+		*strength = 0xffff;
+	else
+		*strength = state->snr / 8960;
+fail:
+	return ret;
+#else
+	return 0;
+#endif
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+#if 0
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	ret = lg216x_read_rs_err_count(state,
+				       &fe->dtv_property_cache.atscmh_rs_err);
+	if (lg_fail(ret))
+		goto fail;
+
+	*ucblocks = fe->dtv_property_cache.atscmh_rs_err;
+fail:
+#else
+	*ucblocks = 0;
+#endif
+	return 0;
+}
+
+static int lg216x_get_tune_settings(struct dvb_frontend *fe,
+				    struct dvb_frontend_tune_settings
+				    *fe_tune_settings)
+{
+	fe_tune_settings->min_delay_ms = 500;
+	lg_dbg("\n");
+	return 0;
+}
+
+static void lg216x_release(struct dvb_frontend *fe)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	lg_dbg("\n");
+	kfree(state);
+}
+
+static struct dvb_frontend_ops lg2160_ops = {
+	.delsys = { SYS_ATSCMH },
+	.info = {
+		.name = "LG Electronics LG2160 ATSC/MH Frontend",
+		.frequency_min      = 54000000,
+		.frequency_max      = 858000000,
+		.frequency_stepsize = 62500,
+	},
+	.i2c_gate_ctrl        = lg216x_i2c_gate_ctrl,
+#if 0
+	.init                 = lg216x_init,
+	.sleep                = lg216x_sleep,
+#endif
+	.get_property         = lg216x_get_property,
+
+	.set_frontend         = lg2160_set_frontend,
+	.get_frontend         = lg216x_get_frontend,
+	.get_tune_settings    = lg216x_get_tune_settings,
+	.read_status          = lg216x_read_status,
+#if 0
+	.read_ber             = lg216x_read_ber,
+#endif
+	.read_signal_strength = lg216x_read_signal_strength,
+	.read_snr             = lg2160_read_snr,
+	.read_ucblocks        = lg216x_read_ucblocks,
+	.release              = lg216x_release,
+};
+
+static struct dvb_frontend_ops lg2161_ops = {
+	.delsys = { SYS_ATSCMH },
+	.info = {
+		.name = "LG Electronics LG2161 ATSC/MH Frontend",
+		.frequency_min      = 54000000,
+		.frequency_max      = 858000000,
+		.frequency_stepsize = 62500,
+	},
+	.i2c_gate_ctrl        = lg216x_i2c_gate_ctrl,
+#if 0
+	.init                 = lg216x_init,
+	.sleep                = lg216x_sleep,
+#endif
+	.get_property         = lg216x_get_property,
+
+	.set_frontend         = lg2160_set_frontend,
+	.get_frontend         = lg216x_get_frontend,
+	.get_tune_settings    = lg216x_get_tune_settings,
+	.read_status          = lg216x_read_status,
+#if 0
+	.read_ber             = lg216x_read_ber,
+#endif
+	.read_signal_strength = lg216x_read_signal_strength,
+	.read_snr             = lg2161_read_snr,
+	.read_ucblocks        = lg216x_read_ucblocks,
+	.release              = lg216x_release,
+};
+
+struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
+				   struct i2c_adapter *i2c_adap)
+{
+	struct lg216x_state *state = NULL;
+
+	lg_dbg("(%d-%04x)\n",
+	       i2c_adap ? i2c_adapter_id(i2c_adap) : 0,
+	       config ? config->i2c_addr : 0);
+
+	state = kzalloc(sizeof(struct lg216x_state), GFP_KERNEL);
+	if (state == NULL)
+		goto fail;
+
+	state->cfg = config;
+	state->i2c_adap = i2c_adap;
+	state->fic_ver = 0xff;
+	state->parade_id = 0xff;
+
+	switch (config->lg_chip) {
+	default:
+		lg_warn("invalid chip requested, defaulting to LG2160");
+		/* fall-thru */
+	case LG2160:
+		memcpy(&state->frontend.ops, &lg2160_ops,
+		       sizeof(struct dvb_frontend_ops));
+		break;
+	case LG2161:
+		memcpy(&state->frontend.ops, &lg2161_ops,
+		       sizeof(struct dvb_frontend_ops));
+		break;
+	}
+
+	state->frontend.demodulator_priv = state;
+	state->current_frequency = -1;
+	/* parade 1 by default */
+	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);
+
+MODULE_DESCRIPTION("LG Electronics LG216x ATSC/MH Demodulator Driver");
+MODULE_AUTHOR("Michael Krufky <mkrufky@linuxtv.org>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.3");
+
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/dvb/frontends/lg2160.h b/drivers/media/dvb/frontends/lg2160.h
new file mode 100644
index 000000000000..9e2c0f41199a
--- /dev/null
+++ b/drivers/media/dvb/frontends/lg2160.h
@@ -0,0 +1,84 @@
+/*
+ *    Support for LG2160 - ATSC/MH
+ *
+ *    Copyright (C) 2010 Michael Krufky <mkrufky@linuxtv.org>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#ifndef _LG2160_H_
+#define _LG2160_H_
+
+#include <linux/i2c.h>
+#include "dvb_frontend.h"
+
+enum lg_chip_type {
+	LG2160 = 0,
+	LG2161 = 1,
+};
+
+#define LG2161_1019 LG2161
+#define LG2161_1040 LG2161
+
+enum lg2160_spi_clock {
+	LG2160_SPI_3_125_MHZ = 0,
+	LG2160_SPI_6_25_MHZ = 1,
+	LG2160_SPI_12_5_MHZ = 2,
+};
+
+#if 0
+enum lg2161_oif {
+	LG2161_OIF_EBI2_SLA  = 1,
+	LG2161_OIF_SDIO_SLA  = 2,
+	LG2161_OIF_SPI_SLA   = 3,
+	LG2161_OIF_SPI_MAS   = 4,
+	LG2161_OIF_SERIAL_TS = 7,
+};
+#endif
+
+struct lg2160_config {
+	u8 i2c_addr;
+
+	/* user defined IF frequency in KHz */
+	u16 if_khz;
+
+	/* disable i2c repeater - 0:repeater enabled 1:repeater disabled */
+	int deny_i2c_rptr:1;
+
+	/* spectral inversion - 0:disabled 1:enabled */
+	int spectral_inversion:1;
+
+	unsigned int output_if;
+	enum lg2160_spi_clock spi_clock;
+	enum lg_chip_type lg_chip;
+};
+
+#if defined(CONFIG_DVB_LG2160) || (defined(CONFIG_DVB_LG2160_MODULE) && \
+				     defined(MODULE))
+extern
+struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
+				     struct i2c_adapter *i2c_adap);
+#else
+static inline
+struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
+				     struct i2c_adapter *i2c_adap)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif /* CONFIG_DVB_LG2160 */
+
+#endif /* _LG2160_H_ */
diff --git a/drivers/media/dvb/frontends/lgs8gxx.c b/drivers/media/dvb/frontends/lgs8gxx.c
index 4de1d3520cd2..568363a10a31 100644
--- a/drivers/media/dvb/frontends/lgs8gxx.c
+++ b/drivers/media/dvb/frontends/lgs8gxx.c
@@ -262,7 +262,6 @@ static int lgs8gxx_set_mode_auto(struct lgs8gxx_state *priv)
 
 static int lgs8gxx_set_mode_manual(struct lgs8gxx_state *priv)
 {
-	int ret = 0;
 	u8 t;
 
 	if (priv->config->prod == LGS8GXX_PROD_LGS8G75) {
@@ -296,7 +295,7 @@ static int lgs8gxx_set_mode_manual(struct lgs8gxx_state *priv)
 	if (priv->config->prod == LGS8GXX_PROD_LGS8913)
 		lgs8gxx_write_reg(priv, 0xC1, 0);
 
-	ret = lgs8gxx_read_reg(priv, 0xC5, &t);
+	lgs8gxx_read_reg(priv, 0xC5, &t);
 	t = (t & 0xE0) | 0x06;
 	lgs8gxx_write_reg(priv, 0xC5, t);
 
diff --git a/drivers/media/dvb/frontends/m88rs2000.c b/drivers/media/dvb/frontends/m88rs2000.c
index 045ee5a6f7ae..312588e84dae 100644
--- a/drivers/media/dvb/frontends/m88rs2000.c
+++ b/drivers/media/dvb/frontends/m88rs2000.c
@@ -416,9 +416,25 @@ static int m88rs2000_tab_set(struct m88rs2000_state *state,
 
 static int m88rs2000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
 {
-	deb_info("%s: %s\n", __func__,
-		volt == SEC_VOLTAGE_13 ? "SEC_VOLTAGE_13" :
-		volt == SEC_VOLTAGE_18 ? "SEC_VOLTAGE_18" : "??");
+	struct m88rs2000_state *state = fe->demodulator_priv;
+	u8 data;
+
+	data = m88rs2000_demod_read(state, 0xb2);
+	data |= 0x03; /* bit0 V/H, bit1 off/on */
+
+	switch (volt) {
+	case SEC_VOLTAGE_18:
+		data &= ~0x03;
+		break;
+	case SEC_VOLTAGE_13:
+		data &= ~0x03;
+		data |= 0x01;
+		break;
+	case SEC_VOLTAGE_OFF:
+		break;
+	}
+
+	m88rs2000_demod_write(state, 0xb2, data);
 
 	return 0;
 }
@@ -654,7 +670,6 @@ static int m88rs2000_set_tuner(struct dvb_frontend *fe, u16 *offset)
 static int m88rs2000_set_fec(struct m88rs2000_state *state,
 		fe_code_rate_t fec)
 {
-	int ret;
 	u16 fec_set;
 	switch (fec) {
 	/* This is not confirmed kept for reference */
@@ -677,7 +692,7 @@ static int m88rs2000_set_fec(struct m88rs2000_state *state,
 	default:
 		fec_set = 0x08;
 	}
-	ret = m88rs2000_demod_write(state, 0x76, fec_set);
+	m88rs2000_demod_write(state, 0x76, fec_set);
 
 	return 0;
 }
@@ -772,13 +787,13 @@ static int m88rs2000_set_frontend(struct dvb_frontend *fe)
 		return -ENODEV;
 
 	for (i = 0; i < 25; i++) {
-		u8 reg = m88rs2000_demod_read(state, 0x8c);
+		reg = m88rs2000_demod_read(state, 0x8c);
 		if ((reg & 0x7) == 0x7) {
 			status = FE_HAS_LOCK;
 			break;
 		}
 		state->no_lock_count++;
-		if (state->no_lock_count > 15) {
+		if (state->no_lock_count == 15) {
 			reg = m88rs2000_demod_read(state, 0x70);
 			reg ^= 0x4;
 			m88rs2000_demod_write(state, 0x70, reg);
diff --git a/drivers/media/dvb/frontends/rtl2830.c b/drivers/media/dvb/frontends/rtl2830.c
index 45196c5b0736..93612ebac519 100644
--- a/drivers/media/dvb/frontends/rtl2830.c
+++ b/drivers/media/dvb/frontends/rtl2830.c
@@ -374,6 +374,118 @@ err:
 	return ret;
 }
 
+static int rtl2830_get_frontend(struct dvb_frontend *fe)
+{
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	u8 buf[3];
+
+	if (priv->sleeping)
+		return 0;
+
+	ret = rtl2830_rd_regs(priv, 0x33c, buf, 2);
+	if (ret)
+		goto err;
+
+	ret = rtl2830_rd_reg(priv, 0x351, &buf[2]);
+	if (ret)
+		goto err;
+
+	dbg("%s: TPS=%02x %02x %02x", __func__, buf[0], buf[1], buf[2]);
+
+	switch ((buf[0] >> 2) & 3) {
+	case 0:
+		c->modulation = QPSK;
+		break;
+	case 1:
+		c->modulation = QAM_16;
+		break;
+	case 2:
+		c->modulation = QAM_64;
+		break;
+	}
+
+	switch ((buf[2] >> 2) & 1) {
+	case 0:
+		c->transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+	case 1:
+		c->transmission_mode = TRANSMISSION_MODE_8K;
+	}
+
+	switch ((buf[2] >> 0) & 3) {
+	case 0:
+		c->guard_interval = GUARD_INTERVAL_1_32;
+		break;
+	case 1:
+		c->guard_interval = GUARD_INTERVAL_1_16;
+		break;
+	case 2:
+		c->guard_interval = GUARD_INTERVAL_1_8;
+		break;
+	case 3:
+		c->guard_interval = GUARD_INTERVAL_1_4;
+		break;
+	}
+
+	switch ((buf[0] >> 4) & 7) {
+	case 0:
+		c->hierarchy = HIERARCHY_NONE;
+		break;
+	case 1:
+		c->hierarchy = HIERARCHY_1;
+		break;
+	case 2:
+		c->hierarchy = HIERARCHY_2;
+		break;
+	case 3:
+		c->hierarchy = HIERARCHY_4;
+		break;
+	}
+
+	switch ((buf[1] >> 3) & 7) {
+	case 0:
+		c->code_rate_HP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_HP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_HP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_HP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_HP = FEC_7_8;
+		break;
+	}
+
+	switch ((buf[1] >> 0) & 7) {
+	case 0:
+		c->code_rate_LP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_LP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_LP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_LP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_LP = FEC_7_8;
+		break;
+	}
+
+	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
+}
+
 static int rtl2830_read_status(struct dvb_frontend *fe, fe_status_t *status)
 {
 	struct rtl2830_priv *priv = fe->demodulator_priv;
@@ -404,14 +516,72 @@ err:
 
 static int rtl2830_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
-	*snr = 0;
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	int ret, hierarchy, constellation;
+	u8 buf[2], tmp;
+	u16 tmp16;
+#define CONSTELLATION_NUM 3
+#define HIERARCHY_NUM 4
+	static const u32 snr_constant[CONSTELLATION_NUM][HIERARCHY_NUM] = {
+		{ 70705899, 70705899, 70705899, 70705899 },
+		{ 82433173, 82433173, 87483115, 94445660 },
+		{ 92888734, 92888734, 95487525, 99770748 },
+	};
+
+	if (priv->sleeping)
+		return 0;
+
+	/* reports SNR in resolution of 0.1 dB */
+
+	ret = rtl2830_rd_reg(priv, 0x33c, &tmp);
+	if (ret)
+		goto err;
+
+	constellation = (tmp >> 2) & 0x03; /* [3:2] */
+	if (constellation > CONSTELLATION_NUM - 1)
+		goto err;
+
+	hierarchy = (tmp >> 4) & 0x07; /* [6:4] */
+	if (hierarchy > HIERARCHY_NUM - 1)
+		goto err;
+
+	ret = rtl2830_rd_regs(priv, 0x40c, buf, 2);
+	if (ret)
+		goto err;
+
+	tmp16 = buf[0] << 8 | buf[1];
+
+	if (tmp16)
+		*snr = (snr_constant[constellation][hierarchy] -
+				intlog10(tmp16)) / ((1 << 24) / 100);
+	else
+		*snr = 0;
+
 	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
 }
 
 static int rtl2830_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
-	*ber = 0;
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	int ret;
+	u8 buf[2];
+
+	if (priv->sleeping)
+		return 0;
+
+	ret = rtl2830_rd_regs(priv, 0x34e, buf, 2);
+	if (ret)
+		goto err;
+
+	*ber = buf[0] << 8 | buf[1];
+
 	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
 }
 
 static int rtl2830_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
@@ -422,8 +592,32 @@ static int rtl2830_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 
 static int rtl2830_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 {
-	*strength = 0;
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	int ret;
+	u8 buf[2];
+	u16 if_agc_raw, if_agc;
+
+	if (priv->sleeping)
+		return 0;
+
+	ret = rtl2830_rd_regs(priv, 0x359, buf, 2);
+	if (ret)
+		goto err;
+
+	if_agc_raw = (buf[0] << 8 | buf[1]) & 0x3fff;
+
+	if (if_agc_raw & (1 << 9))
+		if_agc = -(~(if_agc_raw - 1) & 0x1ff);
+	else
+		if_agc = if_agc_raw;
+
+	*strength = (u8) (55 - if_agc / 182);
+	*strength |= *strength << 8;
+
 	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
 }
 
 static struct dvb_frontend_ops rtl2830_ops;
@@ -549,6 +743,7 @@ static struct dvb_frontend_ops rtl2830_ops = {
 	.get_tune_settings = rtl2830_get_tune_settings,
 
 	.set_frontend = rtl2830_set_frontend,
+	.get_frontend = rtl2830_get_frontend,
 
 	.read_status = rtl2830_read_status,
 	.read_snr = rtl2830_read_snr,
diff --git a/drivers/media/dvb/frontends/rtl2830_priv.h b/drivers/media/dvb/frontends/rtl2830_priv.h
index 4a464761b5b8..9b20557ccf6c 100644
--- a/drivers/media/dvb/frontends/rtl2830_priv.h
+++ b/drivers/media/dvb/frontends/rtl2830_priv.h
@@ -22,6 +22,7 @@
 #define RTL2830_PRIV_H
 
 #include "dvb_frontend.h"
+#include "dvb_math.h"
 #include "rtl2830.h"
 
 #define LOG_PREFIX "rtl2830"
diff --git a/drivers/media/dvb/frontends/stb0899_drv.c b/drivers/media/dvb/frontends/stb0899_drv.c
index dd08f4ac64a8..8b0dc74a3298 100644
--- a/drivers/media/dvb/frontends/stb0899_drv.c
+++ b/drivers/media/dvb/frontends/stb0899_drv.c
@@ -637,11 +637,9 @@ static void stb0899_init_calc(struct stb0899_state *state)
 	struct stb0899_internal *internal = &state->internal;
 	int master_clk;
 	u8 agc[2];
-	u8 agc1cn;
 	u32 reg;
 
 	/* Read registers (in burst mode)	*/
-	agc1cn = stb0899_read_reg(state, STB0899_AGC1CN);
 	stb0899_read_regs(state, STB0899_AGC1REF, agc, 2); /* AGC1R and AGC2O	*/
 
 	/* Initial calculations	*/
@@ -823,15 +821,12 @@ static int stb0899_send_diseqc_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t
 
 static int stb0899_diseqc_init(struct stb0899_state *state)
 {
-	struct dvb_diseqc_master_cmd tx_data;
 /*
 	struct dvb_diseqc_slave_reply rx_data;
 */
-	u8 f22_tx, f22_rx, reg;
+	u8 f22_tx, reg;
 
 	u32 mclk, tx_freq = 22000;/* count = 0, i; */
-	tx_data.msg[0] = 0xe2;
-	tx_data.msg_len = 3;
 	reg = stb0899_read_reg(state, STB0899_DISCNTRL2);
 	STB0899_SETFIELD_VAL(ONECHIP_TRX, reg, 0);
 	stb0899_write_reg(state, STB0899_DISCNTRL2, reg);
@@ -849,7 +844,6 @@ static int stb0899_diseqc_init(struct stb0899_state *state)
 	f22_tx = mclk / (tx_freq * 32);
 	stb0899_write_reg(state, STB0899_DISF22, f22_tx); /* DiSEqC Tx freq	*/
 	state->rx_freq = 20000;
-	f22_rx = mclk / (state->rx_freq * 32);
 
 	return 0;
 }
diff --git a/drivers/media/dvb/frontends/stb6100.c b/drivers/media/dvb/frontends/stb6100.c
index def88abb30bf..2e93e65d2cdb 100644
--- a/drivers/media/dvb/frontends/stb6100.c
+++ b/drivers/media/dvb/frontends/stb6100.c
@@ -158,7 +158,6 @@ static int stb6100_read_regs(struct stb6100_state *state, u8 regs[])
 static int stb6100_read_reg(struct stb6100_state *state, u8 reg)
 {
 	u8 regs[STB6100_NUMREGS];
-	int rc;
 
 	struct i2c_msg msg = {
 		.addr	= state->config->tuner_address + reg,
@@ -167,7 +166,7 @@ static int stb6100_read_reg(struct stb6100_state *state, u8 reg)
 		.len	= 1
 	};
 
-	rc = i2c_transfer(state->i2c, &msg, 1);
+	i2c_transfer(state->i2c, &msg, 1);
 
 	if (unlikely(reg >= STB6100_NUMREGS)) {
 		dprintk(verbose, FE_ERROR, 1, "Invalid register offset 0x%x", reg);
diff --git a/drivers/media/dvb/frontends/stv0297.c b/drivers/media/dvb/frontends/stv0297.c
index 85c157a1fe5e..d40f226160ef 100644
--- a/drivers/media/dvb/frontends/stv0297.c
+++ b/drivers/media/dvb/frontends/stv0297.c
@@ -414,7 +414,6 @@ static int stv0297_set_frontend(struct dvb_frontend *fe)
 	int delay;
 	int sweeprate;
 	int carrieroffset;
-	unsigned long starttime;
 	unsigned long timeout;
 	fe_spectral_inversion_t inversion;
 
@@ -543,7 +542,6 @@ static int stv0297_set_frontend(struct dvb_frontend *fe)
 	stv0297_writereg_mask(state, 0x43, 0x10, 0x10);
 
 	/* wait for WGAGC lock */
-	starttime = jiffies;
 	timeout = jiffies + msecs_to_jiffies(2000);
 	while (time_before(jiffies, timeout)) {
 		msleep(10);
diff --git a/drivers/media/dvb/frontends/stv0900_sw.c b/drivers/media/dvb/frontends/stv0900_sw.c
index ba0709b2d433..4af20780fb9c 100644
--- a/drivers/media/dvb/frontends/stv0900_sw.c
+++ b/drivers/media/dvb/frontends/stv0900_sw.c
@@ -835,7 +835,6 @@ static void stv0900_track_optimization(struct dvb_frontend *fe)
 		blind_tun_sw = 0,
 		modulation;
 
-	enum fe_stv0900_rolloff rolloff;
 	enum fe_stv0900_modcode foundModcod;
 
 	dprintk("%s\n", __func__);
@@ -940,7 +939,6 @@ static void stv0900_track_optimization(struct dvb_frontend *fe)
 
 	freq1 = stv0900_read_reg(intp, CFR2);
 	freq0 = stv0900_read_reg(intp, CFR1);
-	rolloff = stv0900_get_bits(intp, ROLLOFF_STATUS);
 	if (intp->srch_algo[demod] == STV0900_BLIND_SEARCH) {
 		stv0900_write_reg(intp, SFRSTEP, 0x00);
 		stv0900_write_bits(intp, SCAN_ENABLE, 0);
diff --git a/drivers/media/dvb/frontends/stv090x.c b/drivers/media/dvb/frontends/stv090x.c
index 4aef1877ed42..d79e69f65cbb 100644
--- a/drivers/media/dvb/frontends/stv090x.c
+++ b/drivers/media/dvb/frontends/stv090x.c
@@ -2842,7 +2842,6 @@ static int stv090x_optimize_track(struct stv090x_state *state)
 {
 	struct dvb_frontend *fe = &state->frontend;
 
-	enum stv090x_rolloff rolloff;
 	enum stv090x_modcod modcod;
 
 	s32 srate, pilots, aclc, f_1, f_0, i = 0, blind_tune = 0;
@@ -2966,7 +2965,6 @@ static int stv090x_optimize_track(struct stv090x_state *state)
 	f_1 = STV090x_READ_DEMOD(state, CFR2);
 	f_0 = STV090x_READ_DEMOD(state, CFR1);
 	reg = STV090x_READ_DEMOD(state, TMGOBS);
-	rolloff = STV090x_GETFIELD_Px(reg, ROLLOFF_STATUS_FIELD);
 
 	if (state->algo == STV090x_BLIND_SEARCH) {
 		STV090x_WRITE_DEMOD(state, SFRSTEP, 0x00);
diff --git a/drivers/media/dvb/frontends/zl10353.c b/drivers/media/dvb/frontends/zl10353.c
index ac7237891374..82946cd517f5 100644
--- a/drivers/media/dvb/frontends/zl10353.c
+++ b/drivers/media/dvb/frontends/zl10353.c
@@ -525,7 +525,7 @@ static int zl10353_read_snr(struct dvb_frontend *fe, u16 *snr)
 		zl10353_dump_regs(fe);
 
 	_snr = zl10353_read_register(state, SNR);
-	*snr = (_snr << 8) | _snr;
+	*snr = 10 * _snr / 8;
 
 	return 0;
 }
@@ -559,7 +559,6 @@ static int zl10353_init(struct dvb_frontend *fe)
 {
 	struct zl10353_state *state = fe->demodulator_priv;
 	u8 zl10353_reset_attach[6] = { 0x50, 0x03, 0x64, 0x46, 0x15, 0x0F };
-	int rc = 0;
 
 	if (debug_regs)
 		zl10353_dump_regs(fe);
@@ -573,7 +572,7 @@ static int zl10353_init(struct dvb_frontend *fe)
 	/* Do a "hard" reset if not already done */
 	if (zl10353_read_register(state, 0x50) != zl10353_reset_attach[1] ||
 	    zl10353_read_register(state, 0x51) != zl10353_reset_attach[2]) {
-		rc = zl10353_write(fe, zl10353_reset_attach,
+		zl10353_write(fe, zl10353_reset_attach,
 				   sizeof(zl10353_reset_attach));
 		if (debug_regs)
 			zl10353_dump_regs(fe);
diff --git a/drivers/media/dvb/mantis/hopper_cards.c b/drivers/media/dvb/mantis/hopper_cards.c
index 71622f65c037..cc0251e01077 100644
--- a/drivers/media/dvb/mantis/hopper_cards.c
+++ b/drivers/media/dvb/mantis/hopper_cards.c
@@ -65,7 +65,7 @@ static int devs;
 
 static irqreturn_t hopper_irq_handler(int irq, void *dev_id)
 {
-	u32 stat = 0, mask = 0, lstat = 0;
+	u32 stat = 0, mask = 0;
 	u32 rst_stat = 0, rst_mask = 0;
 
 	struct mantis_pci *mantis;
@@ -80,7 +80,6 @@ static irqreturn_t hopper_irq_handler(int irq, void *dev_id)
 
 	stat = mmread(MANTIS_INT_STAT);
 	mask = mmread(MANTIS_INT_MASK);
-	lstat = stat & ~MANTIS_INT_RISCSTAT;
 	if (!(stat & mask))
 		return IRQ_NONE;
 
diff --git a/drivers/media/dvb/mantis/mantis_cards.c b/drivers/media/dvb/mantis/mantis_cards.c
index c2bb90b3e529..095cf3a994e2 100644
--- a/drivers/media/dvb/mantis/mantis_cards.c
+++ b/drivers/media/dvb/mantis/mantis_cards.c
@@ -73,7 +73,7 @@ static char *label[10] = {
 
 static irqreturn_t mantis_irq_handler(int irq, void *dev_id)
 {
-	u32 stat = 0, mask = 0, lstat = 0;
+	u32 stat = 0, mask = 0;
 	u32 rst_stat = 0, rst_mask = 0;
 
 	struct mantis_pci *mantis;
@@ -88,7 +88,6 @@ static irqreturn_t mantis_irq_handler(int irq, void *dev_id)
 
 	stat = mmread(MANTIS_INT_STAT);
 	mask = mmread(MANTIS_INT_MASK);
-	lstat = stat & ~MANTIS_INT_RISCSTAT;
 	if (!(stat & mask))
 		return IRQ_NONE;
 
diff --git a/drivers/media/dvb/mantis/mantis_dma.c b/drivers/media/dvb/mantis/mantis_dma.c
index c61ca7d3daea..566c407175a4 100644
--- a/drivers/media/dvb/mantis/mantis_dma.c
+++ b/drivers/media/dvb/mantis/mantis_dma.c
@@ -199,10 +199,6 @@ void mantis_dma_start(struct mantis_pci *mantis)
 
 void mantis_dma_stop(struct mantis_pci *mantis)
 {
-	u32 stat = 0, mask = 0;
-
-	stat = mmread(MANTIS_INT_STAT);
-	mask = mmread(MANTIS_INT_MASK);
 	dprintk(MANTIS_DEBUG, 1, "Mantis Stop DMA engine");
 
 	mmwrite((mmread(MANTIS_GPIF_ADDR) & (~(MANTIS_GPIF_HIFRDWRN))), MANTIS_GPIF_ADDR);
diff --git a/drivers/media/dvb/mantis/mantis_evm.c b/drivers/media/dvb/mantis/mantis_evm.c
index 36f2256ebb0e..71ce52875c38 100644
--- a/drivers/media/dvb/mantis/mantis_evm.c
+++ b/drivers/media/dvb/mantis/mantis_evm.c
@@ -41,10 +41,9 @@ static void mantis_hifevm_work(struct work_struct *work)
 	struct mantis_ca *ca = container_of(work, struct mantis_ca, hif_evm_work);
 	struct mantis_pci *mantis = ca->ca_priv;
 
-	u32 gpif_stat, gpif_mask;
+	u32 gpif_stat;
 
 	gpif_stat = mmread(MANTIS_GPIF_STATUS);
-	gpif_mask = mmread(MANTIS_GPIF_IRQCFG);
 
 	if (gpif_stat & MANTIS_GPIF_DETSTAT) {
 		if (gpif_stat & MANTIS_CARD_PLUGIN) {
diff --git a/drivers/media/dvb/ngene/ngene-core.c b/drivers/media/dvb/ngene/ngene-core.c
index f129a9303f80..39857384af10 100644
--- a/drivers/media/dvb/ngene/ngene-core.c
+++ b/drivers/media/dvb/ngene/ngene-core.c
@@ -1409,10 +1409,8 @@ static int ngene_start(struct ngene *dev)
 	if (stat < 0)
 		goto fail;
 
-	if (!stat)
-		return stat;
+	return 0;
 
-	/* otherwise error: fall through */
 fail:
 	ngwritel(0, NGENE_INT_ENABLE);
 	free_irq(dev->pci_dev->irq, dev);
diff --git a/drivers/media/dvb/pluto2/pluto2.c b/drivers/media/dvb/pluto2/pluto2.c
index e1f20c236989..f148b19a206a 100644
--- a/drivers/media/dvb/pluto2/pluto2.c
+++ b/drivers/media/dvb/pluto2/pluto2.c
@@ -481,14 +481,6 @@ static int lg_tdtpe001p_tuner_set_params(struct dvb_frontend *fe)
 	if (p->bandwidth_hz == 8000000)
 		buf[3] |= 0x08;
 
-	if (sizeof(buf) == 6) {
-		buf[4] = buf[2];
-		buf[4] &= ~0x1c;
-		buf[4] |=  0x18;
-
-		buf[5] = (0 << 7) | (2 << 4);
-	}
-
 	msg.addr = I2C_ADDR_TUA6034 >> 1;
 	msg.flags = 0;
 	msg.buf = buf;
diff --git a/drivers/media/dvb/siano/smssdio.c b/drivers/media/dvb/siano/smssdio.c
index 91f8c8291e2b..d6f3f100699a 100644
--- a/drivers/media/dvb/siano/smssdio.c
+++ b/drivers/media/dvb/siano/smssdio.c
@@ -114,7 +114,7 @@ out:
 
 static void smssdio_interrupt(struct sdio_func *func)
 {
-	int ret, isr;
+	int ret;
 
 	struct smssdio_device *smsdev;
 	struct smscore_buffer_t *cb;
@@ -127,7 +127,7 @@ static void smssdio_interrupt(struct sdio_func *func)
 	 * The interrupt register has no defined meaning. It is just
 	 * a way of turning of the level triggered interrupt.
 	 */
-	isr = sdio_readb(func, SMSSDIO_INT, &ret);
+	(void)sdio_readb(func, SMSSDIO_INT, &ret);
 	if (ret) {
 		sms_err("Unable to read interrupt register!\n");
 		return;
diff --git a/drivers/media/dvb/siano/smsusb.c b/drivers/media/dvb/siano/smsusb.c
index b1fe5137df09..63c004a25e0b 100644
--- a/drivers/media/dvb/siano/smsusb.c
+++ b/drivers/media/dvb/siano/smsusb.c
@@ -542,6 +542,8 @@ static const struct usb_device_id smsusb_id_table[] __devinitconst = {
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
 	{ USB_DEVICE(0x2040, 0xc090),
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
+	{ USB_DEVICE(0x2040, 0xc0a0),
+		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
 	{ } /* Terminating entry */
 	};
 
diff --git a/drivers/media/dvb/ttpci/av7110_v4l.c b/drivers/media/dvb/ttpci/av7110_v4l.c
index ee8ee1d481fa..1b2d15140a1d 100644
--- a/drivers/media/dvb/ttpci/av7110_v4l.c
+++ b/drivers/media/dvb/ttpci/av7110_v4l.c
@@ -107,7 +107,7 @@ static struct v4l2_input inputs[4] = {
 		.index		= 1,
 		.name		= "Television",
 		.type		= V4L2_INPUT_TYPE_TUNER,
-		.audioset	= 2,
+		.audioset	= 1,
 		.tuner		= 0,
 		.std		= V4L2_STD_PAL_BG|V4L2_STD_NTSC_M,
 		.status		= 0,
@@ -494,7 +494,7 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	dprintk(2, "VIDIOC_S_INPUT: %d\n", input);
 
 	if (!av7110->analog_tuner_flags)
-		return 0;
+		return input ? -EINVAL : 0;
 
 	if (input >= 4)
 		return -EINVAL;
@@ -503,19 +503,38 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	return av7110_dvb_c_switch(fh);
 }
 
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	dprintk(2, "VIDIOC_G_AUDIO: %d\n", a->index);
+	if (a->index != 0)
+		return -EINVAL;
+	*a = msp3400_v4l2_audio;
+	return 0;
+}
+
 static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
 	dprintk(2, "VIDIOC_G_AUDIO: %d\n", a->index);
 	if (a->index != 0)
 		return -EINVAL;
-	memcpy(a, &msp3400_v4l2_audio, sizeof(struct v4l2_audio));
+	if (av7110->current_input >= 2)
+		return -EINVAL;
+	*a = msp3400_v4l2_audio;
 	return 0;
 }
 
 static int vidioc_s_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
 	dprintk(2, "VIDIOC_S_AUDIO: %d\n", a->index);
-	return 0;
+	if (av7110->current_input >= 2)
+		return -EINVAL;
+	return a->index ? -EINVAL : 0;
 }
 
 static int vidioc_g_sliced_vbi_cap(struct file *file, void *fh,
@@ -802,26 +821,39 @@ int av7110_init_v4l(struct av7110 *av7110)
 		ERR("cannot init capture device. skipping\n");
 		return -ENODEV;
 	}
-	vv_data->ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data->ops.vidioc_g_input = vidioc_g_input;
-	vv_data->ops.vidioc_s_input = vidioc_s_input;
-	vv_data->ops.vidioc_g_tuner = vidioc_g_tuner;
-	vv_data->ops.vidioc_s_tuner = vidioc_s_tuner;
-	vv_data->ops.vidioc_g_frequency = vidioc_g_frequency;
-	vv_data->ops.vidioc_s_frequency = vidioc_s_frequency;
-	vv_data->ops.vidioc_g_audio = vidioc_g_audio;
-	vv_data->ops.vidioc_s_audio = vidioc_s_audio;
-	vv_data->ops.vidioc_g_sliced_vbi_cap = vidioc_g_sliced_vbi_cap;
-	vv_data->ops.vidioc_g_fmt_sliced_vbi_out = vidioc_g_fmt_sliced_vbi_out;
-	vv_data->ops.vidioc_s_fmt_sliced_vbi_out = vidioc_s_fmt_sliced_vbi_out;
+	vv_data->vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data->vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data->vid_ops.vidioc_s_input = vidioc_s_input;
+	vv_data->vid_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data->vid_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data->vid_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data->vid_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data->vid_ops.vidioc_enumaudio = vidioc_enumaudio;
+	vv_data->vid_ops.vidioc_g_audio = vidioc_g_audio;
+	vv_data->vid_ops.vidioc_s_audio = vidioc_s_audio;
+	vv_data->vid_ops.vidioc_g_fmt_vbi_cap = NULL;
+
+	vv_data->vbi_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data->vbi_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data->vbi_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data->vbi_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data->vbi_ops.vidioc_g_fmt_vbi_cap = NULL;
+	vv_data->vbi_ops.vidioc_g_sliced_vbi_cap = vidioc_g_sliced_vbi_cap;
+	vv_data->vbi_ops.vidioc_g_fmt_sliced_vbi_out = vidioc_g_fmt_sliced_vbi_out;
+	vv_data->vbi_ops.vidioc_s_fmt_sliced_vbi_out = vidioc_s_fmt_sliced_vbi_out;
+
+	if (FW_VERSION(av7110->arm_app) < 0x2623)
+		vv_data->capabilities &= ~V4L2_CAP_SLICED_VBI_OUTPUT;
 
 	if (saa7146_register_device(&av7110->v4l_dev, dev, "av7110", VFL_TYPE_GRABBER)) {
 		ERR("cannot register capture device. skipping\n");
 		saa7146_vv_release(dev);
 		return -ENODEV;
 	}
-	if (saa7146_register_device(&av7110->vbi_dev, dev, "av7110", VFL_TYPE_VBI))
-		ERR("cannot register vbi v4l2 device. skipping\n");
+	if (FW_VERSION(av7110->arm_app) >= 0x2623) {
+		if (saa7146_register_device(&av7110->vbi_dev, dev, "av7110", VFL_TYPE_VBI))
+			ERR("cannot register vbi v4l2 device. skipping\n");
+	}
 	return 0;
 }
 
@@ -905,7 +937,7 @@ static int std_callback(struct saa7146_dev* dev, struct saa7146_standard *std)
 static struct saa7146_ext_vv av7110_vv_data_st = {
 	.inputs		= 1,
 	.audios		= 1,
-	.capabilities	= V4L2_CAP_SLICED_VBI_OUTPUT,
+	.capabilities	= V4L2_CAP_SLICED_VBI_OUTPUT | V4L2_CAP_AUDIO,
 	.flags		= 0,
 
 	.stds		= &standard[0],
@@ -920,7 +952,7 @@ static struct saa7146_ext_vv av7110_vv_data_st = {
 static struct saa7146_ext_vv av7110_vv_data_c = {
 	.inputs		= 1,
 	.audios		= 1,
-	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_SLICED_VBI_OUTPUT,
+	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_SLICED_VBI_OUTPUT | V4L2_CAP_AUDIO,
 	.flags		= SAA7146_USE_PORT_B_FOR_VBI,
 
 	.stds		= &standard[0],
diff --git a/drivers/media/dvb/ttpci/budget-av.c b/drivers/media/dvb/ttpci/budget-av.c
index 8b32e282bf5d..12ddb53c58dc 100644
--- a/drivers/media/dvb/ttpci/budget-av.c
+++ b/drivers/media/dvb/ttpci/budget-av.c
@@ -1483,9 +1483,9 @@ static int budget_av_attach(struct saa7146_dev *dev, struct saa7146_pci_extensio
 			ERR("cannot init vv subsystem\n");
 			return err;
 		}
-		vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-		vv_data.ops.vidioc_g_input = vidioc_g_input;
-		vv_data.ops.vidioc_s_input = vidioc_s_input;
+		vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+		vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+		vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
 
 		if ((err = saa7146_register_device(&budget_av->vd, dev, "knc1", VFL_TYPE_GRABBER))) {
 			/* fixme: proper cleanup here */
diff --git a/drivers/media/media-entity.c b/drivers/media/media-entity.c
index 056138f63c7d..e1cd13283407 100644
--- a/drivers/media/media-entity.c
+++ b/drivers/media/media-entity.c
@@ -214,23 +214,76 @@ EXPORT_SYMBOL_GPL(media_entity_graph_walk_next);
  * pipeline pointer must be identical for all nested calls to
  * media_entity_pipeline_start().
  */
-void media_entity_pipeline_start(struct media_entity *entity,
-				 struct media_pipeline *pipe)
+__must_check int media_entity_pipeline_start(struct media_entity *entity,
+					     struct media_pipeline *pipe)
 {
 	struct media_device *mdev = entity->parent;
 	struct media_entity_graph graph;
+	struct media_entity *entity_err = entity;
+	int ret;
 
 	mutex_lock(&mdev->graph_mutex);
 
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
+		unsigned int i;
+
 		entity->stream_count++;
 		WARN_ON(entity->pipe && entity->pipe != pipe);
 		entity->pipe = pipe;
+
+		/* Already streaming --- no need to check. */
+		if (entity->stream_count > 1)
+			continue;
+
+		if (!entity->ops || !entity->ops->link_validate)
+			continue;
+
+		for (i = 0; i < entity->num_links; i++) {
+			struct media_link *link = &entity->links[i];
+
+			/* Is this pad part of an enabled link? */
+			if (!(link->flags & MEDIA_LNK_FL_ENABLED))
+				continue;
+
+			/* Are we the sink or not? */
+			if (link->sink->entity != entity)
+				continue;
+
+			ret = entity->ops->link_validate(link);
+			if (ret < 0 && ret != -ENOIOCTLCMD)
+				goto error;
+		}
 	}
 
 	mutex_unlock(&mdev->graph_mutex);
+
+	return 0;
+
+error:
+	/*
+	 * Link validation on graph failed. We revert what we did and
+	 * return the error.
+	 */
+	media_entity_graph_walk_start(&graph, entity_err);
+
+	while ((entity_err = media_entity_graph_walk_next(&graph))) {
+		entity_err->stream_count--;
+		if (entity_err->stream_count == 0)
+			entity_err->pipe = NULL;
+
+		/*
+		 * We haven't increased stream_count further than this
+		 * so we quit here.
+		 */
+		if (entity_err == entity)
+			break;
+	}
+
+	mutex_unlock(&mdev->graph_mutex);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(media_entity_pipeline_start);
 
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 8db2d7f4b52a..c257da13d766 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -320,7 +320,7 @@ config RADIO_MIROPCM20
 	  module will be called radio-miropcm20.
 
 config RADIO_SF16FMI
-	tristate "SF16-FMI/SF16-FMP Radio"
+	tristate "SF16-FMI/SF16-FMP/SF16-FMD Radio"
 	depends on ISA && VIDEO_V4L2
 	---help---
 	  Choose Y here if you have one of these FM radio cards.
@@ -329,7 +329,7 @@ config RADIO_SF16FMI
 	  module will be called radio-sf16fmi.
 
 config RADIO_SF16FMR2
-	tristate "SF16FMR2 Radio"
+	tristate "SF16-FMR2/SF16-FMD2 Radio"
 	depends on ISA && VIDEO_V4L2 && SND
 	---help---
 	  Choose Y here if you have one of these FM radio cards.
diff --git a/drivers/media/radio/dsbr100.c b/drivers/media/radio/dsbr100.c
index f36905b63645..63b112b555b2 100644
--- a/drivers/media/radio/dsbr100.c
+++ b/drivers/media/radio/dsbr100.c
@@ -1,92 +1,37 @@
 /* A driver for the D-Link DSB-R100 USB radio and Gemtek USB Radio 21.
- The device plugs into both the USB and an analog audio input, so this thing
- only deals with initialisation and frequency setting, the
- audio data has to be handled by a sound driver.
-
- Major issue: I can't find out where the device reports the signal
- strength, and indeed the windows software appearantly just looks
- at the stereo indicator as well.  So, scanning will only find
- stereo stations.  Sad, but I can't help it.
-
- Also, the windows program sends oodles of messages over to the
- device, and I couldn't figure out their meaning.  My suspicion
- is that they don't have any:-)
-
- You might find some interesting stuff about this module at
- http://unimut.fsk.uni-heidelberg.de/unimut/demi/dsbr
-
- Copyright (c) 2000 Markus Demleitner <msdemlei@cl.uni-heidelberg.de>
-
- This program is free software; you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation; either version 2 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program; if not, write to the Free Software
- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
-
- History:
-
- Version 0.46:
-	Removed usb_dsbr100_open/close calls and radio->users counter. Also,
-	radio->muted changed to radio->status and suspend/resume calls updated.
-
- Version 0.45:
-	Converted to v4l2_device.
-
- Version 0.44:
-	Add suspend/resume functions, fix unplug of device,
-	a lot of cleanups and fixes by Alexey Klimov <klimov.linux@gmail.com>
-
- Version 0.43:
-	Oliver Neukum: avoided DMA coherency issue
-
- Version 0.42:
-	Converted dsbr100 to use video_ioctl2
-	by Douglas Landgraf <dougsland@gmail.com>
-
- Version 0.41-ac1:
-	Alan Cox: Some cleanups and fixes
-
- Version 0.41:
-	Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@infradead.org>
-
- Version 0.40:
-	Markus: Updates for 2.6.x kernels, code layout changes, name sanitizing
-
- Version 0.30:
-	Markus: Updates for 2.5.x kernel and more ISO compliant source
-
- Version 0.25:
-	PSL and Markus: Cleanup, radio now doesn't stop on device close
-
- Version 0.24:
-	Markus: Hope I got these silly VIDEO_TUNER_LOW issues finally
-	right.  Some minor cleanup, improved standalone compilation
-
- Version 0.23:
-	Markus: Sign extension bug fixed by declaring transfer_buffer unsigned
-
- Version 0.22:
-	Markus: Some (brown bag) cleanup in what VIDIOCSTUNER returns,
-	thanks to Mike Cox for pointing the problem out.
-
- Version 0.21:
-	Markus: Minor cleanup, warnings if something goes wrong, lame attempt
-	to adhere to Documentation/CodingStyle
-
- Version 0.2:
-	Brad Hards <bradh@dynamite.com.au>: Fixes to make it work as non-module
-	Markus: Copyright clarification
-
- Version 0.01: Markus: initial release
-
+ * The device plugs into both the USB and an analog audio input, so this thing
+ * only deals with initialisation and frequency setting, the
+ * audio data has to be handled by a sound driver.
+ *
+ * Major issue: I can't find out where the device reports the signal
+ * strength, and indeed the windows software appearantly just looks
+ * at the stereo indicator as well.  So, scanning will only find
+ * stereo stations.  Sad, but I can't help it.
+ *
+ * Also, the windows program sends oodles of messages over to the
+ * device, and I couldn't figure out their meaning.  My suspicion
+ * is that they don't have any:-)
+ *
+ * You might find some interesting stuff about this module at
+ * http://unimut.fsk.uni-heidelberg.de/unimut/demi/dsbr
+ *
+ * Fully tested with the Keene USB FM Transmitter and the v4l2-compliance tool.
+ *
+ * Copyright (c) 2000 Markus Demleitner <msdemlei@cl.uni-heidelberg.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */
 
 #include <linux/kernel.h>
@@ -95,17 +40,19 @@
 #include <linux/slab.h>
 #include <linux/input.h>
 #include <linux/videodev2.h>
+#include <linux/usb.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
-#include <linux/usb.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 
 /*
  * Version Information
  */
-#define DRIVER_VERSION "0.4.7"
-
-#define DRIVER_AUTHOR "Markus Demleitner <msdemlei@tucana.harvard.edu>"
-#define DRIVER_DESC "D-Link DSB-R100 USB FM radio driver"
+MODULE_AUTHOR("Markus Demleitner <msdemlei@tucana.harvard.edu>");
+MODULE_DESCRIPTION("D-Link DSB-R100 USB FM radio driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.1.0");
 
 #define DSB100_VENDOR 0x04b4
 #define DSB100_PRODUCT 0x1002
@@ -122,19 +69,8 @@ devices, that would be 76 and 91.  */
 #define FREQ_MAX 108.0
 #define FREQ_MUL 16000
 
-/* defines for radio->status */
-#define STARTED	0
-#define STOPPED	1
-
 #define v4l2_dev_to_radio(d) container_of(d, struct dsbr100_device, v4l2_dev)
 
-static int usb_dsbr100_probe(struct usb_interface *intf,
-			     const struct usb_device_id *id);
-static void usb_dsbr100_disconnect(struct usb_interface *intf);
-static int usb_dsbr100_suspend(struct usb_interface *intf,
-						pm_message_t message);
-static int usb_dsbr100_resume(struct usb_interface *intf);
-
 static int radio_nr = -1;
 module_param(radio_nr, int, 0);
 
@@ -143,179 +79,92 @@ struct dsbr100_device {
 	struct usb_device *usbdev;
 	struct video_device videodev;
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 
 	u8 *transfer_buffer;
 	struct mutex v4l2_lock;
 	int curfreq;
-	int stereo;
-	int status;
-};
-
-static struct usb_device_id usb_dsbr100_device_table [] = {
-	{ USB_DEVICE(DSB100_VENDOR, DSB100_PRODUCT) },
-	{ }						/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE (usb, usb_dsbr100_device_table);
-
-/* USB subsystem interface */
-static struct usb_driver usb_dsbr100_driver = {
-	.name			= "dsbr100",
-	.probe			= usb_dsbr100_probe,
-	.disconnect		= usb_dsbr100_disconnect,
-	.id_table		= usb_dsbr100_device_table,
-	.suspend		= usb_dsbr100_suspend,
-	.resume			= usb_dsbr100_resume,
-	.reset_resume		= usb_dsbr100_resume,
-	.supports_autosuspend	= 0,
+	bool stereo;
+	bool muted;
 };
 
 /* Low-level device interface begins here */
 
-/* switch on radio */
-static int dsbr100_start(struct dsbr100_device *radio)
+/* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
+static int dsbr100_setfreq(struct dsbr100_device *radio, unsigned freq)
 {
-	int retval;
-	int request;
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x00, 0xC7, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
+	unsigned f = (freq / 16 * 80) / 1000 + 856;
+	int retval = 0;
+
+	if (!radio->muted) {
+		retval = usb_control_msg(radio->usbdev,
+				usb_rcvctrlpipe(radio->usbdev, 0),
+				DSB100_TUNE,
+				USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+				(f >> 8) & 0x00ff, f & 0xff,
+				radio->transfer_buffer, 8, 300);
+		if (retval >= 0)
+			mdelay(1);
 	}
 
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		DSB100_ONOFF,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x01, 0x00, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = DSB100_ONOFF;
-		goto usb_control_msg_failed;
+	if (retval >= 0) {
+		radio->curfreq = freq;
+		return 0;
 	}
-
-	radio->status = STARTED;
-	return (radio->transfer_buffer)[0];
-
-usb_control_msg_failed:
 	dev_err(&radio->usbdev->dev,
 		"%s - usb_control_msg returned %i, request %i\n",
-			__func__, retval, request);
+			__func__, retval, DSB100_TUNE);
 	return retval;
-
 }
 
-/* switch off radio */
-static int dsbr100_stop(struct dsbr100_device *radio)
+/* switch on radio */
+static int dsbr100_start(struct dsbr100_device *radio)
 {
-	int retval;
-	int request;
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x16, 0x1C, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
-	}
-
-	retval = usb_control_msg(radio->usbdev,
+	int retval = usb_control_msg(radio->usbdev,
 		usb_rcvctrlpipe(radio->usbdev, 0),
 		DSB100_ONOFF,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x00, 0x00, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = DSB100_ONOFF;
-		goto usb_control_msg_failed;
-	}
-
-	radio->status = STOPPED;
-	return (radio->transfer_buffer)[0];
+		0x01, 0x00, radio->transfer_buffer, 8, 300);
 
-usb_control_msg_failed:
+	if (retval >= 0)
+		return dsbr100_setfreq(radio, radio->curfreq);
 	dev_err(&radio->usbdev->dev,
 		"%s - usb_control_msg returned %i, request %i\n",
-			__func__, retval, request);
+			__func__, retval, DSB100_ONOFF);
 	return retval;
 
 }
 
-/* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
-static int dsbr100_setfreq(struct dsbr100_device *radio)
+/* switch off radio */
+static int dsbr100_stop(struct dsbr100_device *radio)
 {
-	int retval;
-	int request;
-	int freq = (radio->curfreq / 16 * 80) / 1000 + 856;
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		DSB100_TUNE,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		(freq >> 8) & 0x00ff, freq & 0xff,
-		radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = DSB100_TUNE;
-		goto usb_control_msg_failed;
-	}
-
-	retval = usb_control_msg(radio->usbdev,
+	int retval = usb_control_msg(radio->usbdev,
 		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
+		DSB100_ONOFF,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x96, 0xB7, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
-	}
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE |  USB_DIR_IN,
-		0x00, 0x24, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
-	}
-
-	radio->stereo = !((radio->transfer_buffer)[0] & 0x01);
-	return (radio->transfer_buffer)[0];
+		0x00, 0x00, radio->transfer_buffer, 8, 300);
 
-usb_control_msg_failed:
-	radio->stereo = -1;
+	if (retval >= 0)
+		return 0;
 	dev_err(&radio->usbdev->dev,
 		"%s - usb_control_msg returned %i, request %i\n",
-			__func__, retval, request);
+			__func__, retval, DSB100_ONOFF);
 	return retval;
+
 }
 
 /* return the device status.  This is, in effect, just whether it
 sees a stereo signal or not.  Pity. */
 static void dsbr100_getstat(struct dsbr100_device *radio)
 {
-	int retval;
-
-	retval = usb_control_msg(radio->usbdev,
+	int retval = usb_control_msg(radio->usbdev,
 		usb_rcvctrlpipe(radio->usbdev, 0),
 		USB_REQ_GET_STATUS,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x00 , 0x24, radio->transfer_buffer, 8, 300);
+		0x00, 0x24, radio->transfer_buffer, 8, 300);
 
 	if (retval < 0) {
-		radio->stereo = -1;
+		radio->stereo = false;
 		dev_err(&radio->usbdev->dev,
 			"%s - usb_control_msg returned %i, request %i\n",
 				__func__, retval, USB_REQ_GET_STATUS);
@@ -332,7 +181,8 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strlcpy(v->driver, "dsbr100", sizeof(v->driver));
 	strlcpy(v->card, "D-Link R-100 USB FM Radio", sizeof(v->card));
 	usb_make_path(radio->usbdev, v->bus_info, sizeof(v->bus_info));
-	v->capabilities = V4L2_CAP_TUNER;
+	v->device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -349,13 +199,11 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 	v->type = V4L2_TUNER_RADIO;
 	v->rangelow = FREQ_MIN * FREQ_MUL;
 	v->rangehigh = FREQ_MAX * FREQ_MUL;
-	v->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
-	v->capability = V4L2_TUNER_CAP_LOW;
-	if(radio->stereo)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
-	else
-		v->audmode = V4L2_TUNER_MODE_MONO;
-	v->signal = 0xffff;     /* We can't get the signal strength */
+	v->rxsubchans = radio->stereo ? V4L2_TUNER_SUB_STEREO :
+		V4L2_TUNER_SUB_MONO;
+	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
+	v->audmode = V4L2_TUNER_MODE_STEREO;
+	v->signal = radio->stereo ? 0xffff : 0;     /* We can't get the signal strength */
 	return 0;
 }
 
@@ -369,14 +217,12 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
 	struct dsbr100_device *radio = video_drvdata(file);
-	int retval;
 
-	radio->curfreq = f->frequency;
+	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
+		return -EINVAL;
 
-	retval = dsbr100_setfreq(radio);
-	if (retval < 0)
-		dev_warn(&radio->usbdev->dev, "Set frequency failed\n");
-	return 0;
+	return dsbr100_setfreq(radio, clamp_t(unsigned, f->frequency,
+			FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL));
 }
 
 static int vidioc_g_frequency(struct file *file, void *priv,
@@ -384,90 +230,26 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 {
 	struct dsbr100_device *radio = video_drvdata(file);
 
+	if (f->tuner)
+		return -EINVAL;
 	f->type = V4L2_TUNER_RADIO;
 	f->frequency = radio->curfreq;
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-	}
-
-	return -EINVAL;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
+static int usb_dsbr100_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct dsbr100_device *radio = video_drvdata(file);
+	struct dsbr100_device *radio =
+		container_of(ctrl->handler, struct dsbr100_device, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = radio->status;
-		return 0;
+		radio->muted = ctrl->val;
+		return radio->muted ? dsbr100_stop(radio) : dsbr100_start(radio);
 	}
 	return -EINVAL;
 }
 
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct dsbr100_device *radio = video_drvdata(file);
-	int retval;
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value) {
-			retval = dsbr100_stop(radio);
-			if (retval < 0) {
-				dev_warn(&radio->usbdev->dev,
-					 "Radio did not respond properly\n");
-				return -EBUSY;
-			}
-		} else {
-			retval = dsbr100_start(radio);
-			if (retval < 0) {
-				dev_warn(&radio->usbdev->dev,
-					 "Radio did not respond properly\n");
-				return -EBUSY;
-			}
-		}
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int vidioc_g_audio(struct file *file, void *priv,
-				struct v4l2_audio *a)
-{
-	if (a->index > 1)
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-	a->capability = V4L2_AUDCAP_STEREO;
-	return 0;
-}
-
-static int vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-static int vidioc_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	return i ? -EINVAL : 0;
-}
-
-static int vidioc_s_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	return a->index ? -EINVAL : 0;
-}
 
 /* USB subsystem interface begins here */
 
@@ -481,8 +263,17 @@ static void usb_dsbr100_disconnect(struct usb_interface *intf)
 {
 	struct dsbr100_device *radio = usb_get_intfdata(intf);
 
-	v4l2_device_get(&radio->v4l2_dev);
 	mutex_lock(&radio->v4l2_lock);
+	/*
+	 * Disconnect is also called on unload, and in that case we need to
+	 * mute the device. This call will silently fail if it is called
+	 * after a physical disconnect.
+	 */
+	usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		DSB100_ONOFF,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		0x00, 0x00, radio->transfer_buffer, 8, 300);
 	usb_set_intfdata(intf, NULL);
 	video_unregister_device(&radio->videodev);
 	v4l2_device_disconnect(&radio->v4l2_dev);
@@ -495,25 +286,13 @@ static void usb_dsbr100_disconnect(struct usb_interface *intf)
 static int usb_dsbr100_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct dsbr100_device *radio = usb_get_intfdata(intf);
-	int retval;
 
 	mutex_lock(&radio->v4l2_lock);
-	if (radio->status == STARTED) {
-		retval = dsbr100_stop(radio);
-		if (retval < 0)
-			dev_warn(&intf->dev, "dsbr100_stop failed\n");
-
-		/* After dsbr100_stop() status set to STOPPED.
-		 * If we want driver to start radio on resume
-		 * we set status equal to STARTED.
-		 * On resume we will check status and run radio if needed.
-		 */
-		radio->status = STARTED;
-	}
+	if (!radio->muted && dsbr100_stop(radio) < 0)
+		dev_warn(&intf->dev, "dsbr100_stop failed\n");
 	mutex_unlock(&radio->v4l2_lock);
 
 	dev_info(&intf->dev, "going into suspend..\n");
-
 	return 0;
 }
 
@@ -521,18 +300,13 @@ static int usb_dsbr100_suspend(struct usb_interface *intf, pm_message_t message)
 static int usb_dsbr100_resume(struct usb_interface *intf)
 {
 	struct dsbr100_device *radio = usb_get_intfdata(intf);
-	int retval;
 
 	mutex_lock(&radio->v4l2_lock);
-	if (radio->status == STARTED) {
-		retval = dsbr100_start(radio);
-		if (retval < 0)
-			dev_warn(&intf->dev, "dsbr100_start failed\n");
-	}
+	if (!radio->muted && dsbr100_start(radio) < 0)
+		dev_warn(&intf->dev, "dsbr100_start failed\n");
 	mutex_unlock(&radio->v4l2_lock);
 
 	dev_info(&intf->dev, "coming out of suspend..\n");
-
 	return 0;
 }
 
@@ -541,15 +315,23 @@ static void usb_dsbr100_release(struct v4l2_device *v4l2_dev)
 {
 	struct dsbr100_device *radio = v4l2_dev_to_radio(v4l2_dev);
 
+	v4l2_ctrl_handler_free(&radio->hdl);
 	v4l2_device_unregister(&radio->v4l2_dev);
 	kfree(radio->transfer_buffer);
 	kfree(radio);
 }
 
+static const struct v4l2_ctrl_ops usb_dsbr100_ctrl_ops = {
+	.s_ctrl = usb_dsbr100_s_ctrl,
+};
+
 /* File system interface */
 static const struct v4l2_file_operations usb_dsbr100_fops = {
 	.owner		= THIS_MODULE,
 	.unlocked_ioctl	= video_ioctl2,
+	.open           = v4l2_fh_open,
+	.release        = v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 };
 
 static const struct v4l2_ioctl_ops usb_dsbr100_ioctl_ops = {
@@ -558,13 +340,9 @@ static const struct v4l2_ioctl_ops usb_dsbr100_ioctl_ops = {
 	.vidioc_s_tuner     = vidioc_s_tuner,
 	.vidioc_g_frequency = vidioc_g_frequency,
 	.vidioc_s_frequency = vidioc_s_frequency,
-	.vidioc_queryctrl   = vidioc_queryctrl,
-	.vidioc_g_ctrl      = vidioc_g_ctrl,
-	.vidioc_s_ctrl      = vidioc_s_ctrl,
-	.vidioc_g_audio     = vidioc_g_audio,
-	.vidioc_s_audio     = vidioc_s_audio,
-	.vidioc_g_input     = vidioc_g_input,
-	.vidioc_s_input     = vidioc_s_input,
+	.vidioc_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 /* check if the device is present and register with v4l and usb if it is */
@@ -593,11 +371,17 @@ static int usb_dsbr100_probe(struct usb_interface *intf,
 	retval = v4l2_device_register(&intf->dev, v4l2_dev);
 	if (retval < 0) {
 		v4l2_err(v4l2_dev, "couldn't register v4l2_device\n");
-		kfree(radio->transfer_buffer);
-		kfree(radio);
-		return retval;
+		goto err_reg_dev;
 	}
 
+	v4l2_ctrl_handler_init(&radio->hdl, 1);
+	v4l2_ctrl_new_std(&radio->hdl, &usb_dsbr100_ctrl_ops,
+			  V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	if (radio->hdl.error) {
+		retval = radio->hdl.error;
+		v4l2_err(v4l2_dev, "couldn't register control\n");
+		goto err_reg_ctrl;
+	}
 	mutex_init(&radio->v4l2_lock);
 	strlcpy(radio->videodev.name, v4l2_dev->name, sizeof(radio->videodev.name));
 	radio->videodev.v4l2_dev = v4l2_dev;
@@ -605,28 +389,46 @@ static int usb_dsbr100_probe(struct usb_interface *intf,
 	radio->videodev.ioctl_ops = &usb_dsbr100_ioctl_ops;
 	radio->videodev.release = video_device_release_empty;
 	radio->videodev.lock = &radio->v4l2_lock;
+	radio->videodev.ctrl_handler = &radio->hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->videodev.flags);
 
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->curfreq = FREQ_MIN * FREQ_MUL;
-	radio->status = STOPPED;
+	radio->muted = true;
 
 	video_set_drvdata(&radio->videodev, radio);
+	usb_set_intfdata(intf, radio);
 
 	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO, radio_nr);
-	if (retval < 0) {
-		v4l2_err(v4l2_dev, "couldn't register video device\n");
-		v4l2_device_unregister(v4l2_dev);
-		kfree(radio->transfer_buffer);
-		kfree(radio);
-		return -EIO;
-	}
-	usb_set_intfdata(intf, radio);
-	return 0;
+	if (retval == 0)
+		return 0;
+	v4l2_err(v4l2_dev, "couldn't register video device\n");
+
+err_reg_ctrl:
+	v4l2_ctrl_handler_free(&radio->hdl);
+	v4l2_device_unregister(v4l2_dev);
+err_reg_dev:
+	kfree(radio->transfer_buffer);
+	kfree(radio);
+	return retval;
 }
 
-module_usb_driver(usb_dsbr100_driver);
+static struct usb_device_id usb_dsbr100_device_table[] = {
+	{ USB_DEVICE(DSB100_VENDOR, DSB100_PRODUCT) },
+	{ }						/* Terminating entry */
+};
 
-MODULE_AUTHOR( DRIVER_AUTHOR );
-MODULE_DESCRIPTION( DRIVER_DESC );
-MODULE_LICENSE("GPL");
-MODULE_VERSION(DRIVER_VERSION);
+MODULE_DEVICE_TABLE(usb, usb_dsbr100_device_table);
+
+/* USB subsystem interface */
+static struct usb_driver usb_dsbr100_driver = {
+	.name			= "dsbr100",
+	.probe			= usb_dsbr100_probe,
+	.disconnect		= usb_dsbr100_disconnect,
+	.id_table		= usb_dsbr100_device_table,
+	.suspend		= usb_dsbr100_suspend,
+	.resume			= usb_dsbr100_resume,
+	.reset_resume		= usb_dsbr100_resume,
+};
+
+module_usb_driver(usb_dsbr100_driver);
diff --git a/drivers/media/radio/radio-gemtek.c b/drivers/media/radio/radio-gemtek.c
index 2e639ce6f256..235c0e349820 100644
--- a/drivers/media/radio/radio-gemtek.c
+++ b/drivers/media/radio/radio-gemtek.c
@@ -29,6 +29,7 @@
 #include <linux/videodev2.h>	/* kernel radio structs		*/
 #include <linux/mutex.h>
 #include <linux/io.h>		/* outb, outb_p			*/
+#include <linux/pnp.h>
 #include <linux/slab.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
@@ -283,6 +284,16 @@ static const struct radio_isa_ops gemtek_ops = {
 
 static const int gemtek_ioports[] = { 0x20c, 0x30c, 0x24c, 0x34c, 0x248, 0x28c };
 
+#ifdef CONFIG_PNP
+static struct pnp_device_id gemtek_pnp_devices[] = {
+	/* AOpen FX-3D/Pro Radio */
+	{.id = "ADS7183", .driver_data = 0},
+	{.id = ""}
+};
+
+MODULE_DEVICE_TABLE(pnp, gemtek_pnp_devices);
+#endif
+
 static struct radio_isa_driver gemtek_driver = {
 	.driver = {
 		.match		= radio_isa_match,
@@ -292,6 +303,14 @@ static struct radio_isa_driver gemtek_driver = {
 			.name	= "radio-gemtek",
 		},
 	},
+#ifdef CONFIG_PNP
+	.pnp_driver = {
+		.name		= "radio-gemtek",
+		.id_table	= gemtek_pnp_devices,
+		.probe		= radio_isa_pnp_probe,
+		.remove		= radio_isa_pnp_remove,
+	},
+#endif
 	.io_params = io,
 	.radio_nr_params = radio_nr,
 	.io_ports = gemtek_ioports,
@@ -305,12 +324,18 @@ static struct radio_isa_driver gemtek_driver = {
 static int __init gemtek_init(void)
 {
 	gemtek_driver.probe = probe;
+#ifdef CONFIG_PNP
+	pnp_register_driver(&gemtek_driver.pnp_driver);
+#endif
 	return isa_register_driver(&gemtek_driver.driver, GEMTEK_MAX);
 }
 
 static void __exit gemtek_exit(void)
 {
 	hardmute = 1;	/* Turn off PLL */
+#ifdef CONFIG_PNP
+	pnp_unregister_driver(&gemtek_driver.pnp_driver);
+#endif
 	isa_unregister_driver(&gemtek_driver.driver);
 }
 
diff --git a/drivers/media/radio/radio-isa.c b/drivers/media/radio/radio-isa.c
index 06f906351fad..3c0067de4324 100644
--- a/drivers/media/radio/radio-isa.c
+++ b/drivers/media/radio/radio-isa.c
@@ -150,14 +150,6 @@ static int radio_isa_log_status(struct file *file, void *priv)
 	return 0;
 }
 
-static int radio_isa_subscribe_event(struct v4l2_fh *fh,
-				struct v4l2_event_subscription *sub)
-{
-	if (sub->type == V4L2_EVENT_CTRL)
-		return v4l2_event_subscribe(fh, sub, 0);
-	return -EINVAL;
-}
-
 static const struct v4l2_ctrl_ops radio_isa_ctrl_ops = {
 	.s_ctrl = radio_isa_s_ctrl,
 };
@@ -177,7 +169,7 @@ static const struct v4l2_ioctl_ops radio_isa_ioctl_ops = {
 	.vidioc_g_frequency = radio_isa_g_frequency,
 	.vidioc_s_frequency = radio_isa_s_frequency,
 	.vidioc_log_status  = radio_isa_log_status,
-	.vidioc_subscribe_event   = radio_isa_subscribe_event,
+	.vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
@@ -199,56 +191,31 @@ static bool radio_isa_valid_io(const struct radio_isa_driver *drv, int io)
 	return false;
 }
 
-int radio_isa_probe(struct device *pdev, unsigned int dev)
+struct radio_isa_card *radio_isa_alloc(struct radio_isa_driver *drv,
+				struct device *pdev)
 {
-	struct radio_isa_driver *drv = pdev->platform_data;
-	const struct radio_isa_ops *ops = drv->ops;
 	struct v4l2_device *v4l2_dev;
-	struct radio_isa_card *isa;
-	int res;
+	struct radio_isa_card *isa = drv->ops->alloc();
+	if (!isa)
+		return NULL;
 
-	isa = drv->ops->alloc();
-	if (isa == NULL)
-		return -ENOMEM;
 	dev_set_drvdata(pdev, isa);
 	isa->drv = drv;
-	isa->io = drv->io_params[dev];
 	v4l2_dev = &isa->v4l2_dev;
 	strlcpy(v4l2_dev->name, dev_name(pdev), sizeof(v4l2_dev->name));
 
-	if (drv->probe && ops->probe) {
-		int i;
-
-		for (i = 0; i < drv->num_of_io_ports; ++i) {
-			int io = drv->io_ports[i];
-
-			if (request_region(io, drv->region_size, v4l2_dev->name)) {
-				bool found = ops->probe(isa, io);
-
-				release_region(io, drv->region_size);
-				if (found) {
-					isa->io = io;
-					break;
-				}
-			}
-		}
-	}
-
-	if (!radio_isa_valid_io(drv, isa->io)) {
-		int i;
+	return isa;
+}
 
-		if (isa->io < 0)
-			return -ENODEV;
-		v4l2_err(v4l2_dev, "you must set an I/O address with io=0x%03x",
-				drv->io_ports[0]);
-		for (i = 1; i < drv->num_of_io_ports; i++)
-			printk(KERN_CONT "/0x%03x", drv->io_ports[i]);
-		printk(KERN_CONT ".\n");
-		kfree(isa);
-		return -EINVAL;
-	}
+int radio_isa_common_probe(struct radio_isa_card *isa, struct device *pdev,
+				int radio_nr, unsigned region_size)
+{
+	const struct radio_isa_driver *drv = isa->drv;
+	const struct radio_isa_ops *ops = drv->ops;
+	struct v4l2_device *v4l2_dev = &isa->v4l2_dev;
+	int res;
 
-	if (!request_region(isa->io, drv->region_size, v4l2_dev->name)) {
+	if (!request_region(isa->io, region_size, v4l2_dev->name)) {
 		v4l2_err(v4l2_dev, "port 0x%x already in use\n", isa->io);
 		kfree(isa);
 		return -EBUSY;
@@ -299,42 +266,126 @@ int radio_isa_probe(struct device *pdev, unsigned int dev)
 		res = ops->s_stereo(isa, isa->stereo);
 	if (res < 0) {
 		v4l2_err(v4l2_dev, "Could not setup card\n");
-		goto err_node_reg;
+		goto err_hdl;
 	}
-	res = video_register_device(&isa->vdev, VFL_TYPE_RADIO,
-					drv->radio_nr_params[dev]);
+	res = video_register_device(&isa->vdev, VFL_TYPE_RADIO, radio_nr);
+
 	if (res < 0) {
 		v4l2_err(v4l2_dev, "Could not register device node\n");
-		goto err_node_reg;
+		goto err_hdl;
 	}
 
 	v4l2_info(v4l2_dev, "Initialized radio card %s on port 0x%03x\n",
 			drv->card, isa->io);
 	return 0;
 
-err_node_reg:
-	v4l2_ctrl_handler_free(&isa->hdl);
 err_hdl:
-	v4l2_device_unregister(&isa->v4l2_dev);
+	v4l2_ctrl_handler_free(&isa->hdl);
 err_dev_reg:
-	release_region(isa->io, drv->region_size);
+	release_region(isa->io, region_size);
 	kfree(isa);
 	return res;
 }
-EXPORT_SYMBOL_GPL(radio_isa_probe);
 
-int radio_isa_remove(struct device *pdev, unsigned int dev)
+int radio_isa_common_remove(struct radio_isa_card *isa, unsigned region_size)
 {
-	struct radio_isa_card *isa = dev_get_drvdata(pdev);
 	const struct radio_isa_ops *ops = isa->drv->ops;
 
 	ops->s_mute_volume(isa, true, isa->volume ? isa->volume->cur.val : 0);
 	video_unregister_device(&isa->vdev);
 	v4l2_ctrl_handler_free(&isa->hdl);
 	v4l2_device_unregister(&isa->v4l2_dev);
-	release_region(isa->io, isa->drv->region_size);
+	release_region(isa->io, region_size);
 	v4l2_info(&isa->v4l2_dev, "Removed radio card %s\n", isa->drv->card);
 	kfree(isa);
 	return 0;
 }
+
+int radio_isa_probe(struct device *pdev, unsigned int dev)
+{
+	struct radio_isa_driver *drv = pdev->platform_data;
+	const struct radio_isa_ops *ops = drv->ops;
+	struct v4l2_device *v4l2_dev;
+	struct radio_isa_card *isa;
+
+	isa = radio_isa_alloc(drv, pdev);
+	if (!isa)
+		return -ENOMEM;
+	isa->io = drv->io_params[dev];
+	v4l2_dev = &isa->v4l2_dev;
+
+	if (drv->probe && ops->probe) {
+		int i;
+
+		for (i = 0; i < drv->num_of_io_ports; ++i) {
+			int io = drv->io_ports[i];
+
+			if (request_region(io, drv->region_size, v4l2_dev->name)) {
+				bool found = ops->probe(isa, io);
+
+				release_region(io, drv->region_size);
+				if (found) {
+					isa->io = io;
+					break;
+				}
+			}
+		}
+	}
+
+	if (!radio_isa_valid_io(drv, isa->io)) {
+		int i;
+
+		if (isa->io < 0)
+			return -ENODEV;
+		v4l2_err(v4l2_dev, "you must set an I/O address with io=0x%03x",
+				drv->io_ports[0]);
+		for (i = 1; i < drv->num_of_io_ports; i++)
+			printk(KERN_CONT "/0x%03x", drv->io_ports[i]);
+		printk(KERN_CONT ".\n");
+		kfree(isa);
+		return -EINVAL;
+	}
+
+	return radio_isa_common_probe(isa, pdev, drv->radio_nr_params[dev],
+					drv->region_size);
+}
+EXPORT_SYMBOL_GPL(radio_isa_probe);
+
+int radio_isa_remove(struct device *pdev, unsigned int dev)
+{
+	struct radio_isa_card *isa = dev_get_drvdata(pdev);
+
+	return radio_isa_common_remove(isa, isa->drv->region_size);
+}
 EXPORT_SYMBOL_GPL(radio_isa_remove);
+
+#ifdef CONFIG_PNP
+int radio_isa_pnp_probe(struct pnp_dev *dev, const struct pnp_device_id *dev_id)
+{
+	struct pnp_driver *pnp_drv = to_pnp_driver(dev->dev.driver);
+	struct radio_isa_driver *drv = container_of(pnp_drv,
+					struct radio_isa_driver, pnp_driver);
+	struct radio_isa_card *isa;
+
+	if (!pnp_port_valid(dev, 0))
+		return -ENODEV;
+
+	isa = radio_isa_alloc(drv, &dev->dev);
+	if (!isa)
+		return -ENOMEM;
+
+	isa->io = pnp_port_start(dev, 0);
+
+	return radio_isa_common_probe(isa, &dev->dev, drv->radio_nr_params[0],
+					pnp_port_len(dev, 0));
+}
+EXPORT_SYMBOL_GPL(radio_isa_pnp_probe);
+
+void radio_isa_pnp_remove(struct pnp_dev *dev)
+{
+	struct radio_isa_card *isa = dev_get_drvdata(&dev->dev);
+
+	radio_isa_common_remove(isa, pnp_port_len(dev, 0));
+}
+EXPORT_SYMBOL_GPL(radio_isa_pnp_remove);
+#endif
diff --git a/drivers/media/radio/radio-isa.h b/drivers/media/radio/radio-isa.h
index 8a0ea84d86de..ba4c01f1bd0c 100644
--- a/drivers/media/radio/radio-isa.h
+++ b/drivers/media/radio/radio-isa.h
@@ -24,6 +24,7 @@
 #define _RADIO_ISA_H_
 
 #include <linux/isa.h>
+#include <linux/pnp.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
@@ -76,6 +77,9 @@ struct radio_isa_ops {
 /* Top level structure needed to instantiate the cards */
 struct radio_isa_driver {
 	struct isa_driver driver;
+#ifdef CONFIG_PNP
+	struct pnp_driver pnp_driver;
+#endif
 	const struct radio_isa_ops *ops;
 	/* The module_param_array with the specified I/O ports */
 	int *io_params;
@@ -101,5 +105,10 @@ struct radio_isa_driver {
 int radio_isa_match(struct device *pdev, unsigned int dev);
 int radio_isa_probe(struct device *pdev, unsigned int dev);
 int radio_isa_remove(struct device *pdev, unsigned int dev);
+#ifdef CONFIG_PNP
+int radio_isa_pnp_probe(struct pnp_dev *dev,
+			const struct pnp_device_id *dev_id);
+void radio_isa_pnp_remove(struct pnp_dev *dev);
+#endif
 
 #endif
diff --git a/drivers/media/radio/radio-keene.c b/drivers/media/radio/radio-keene.c
index 55bd1d2937c8..79adf3e654e5 100644
--- a/drivers/media/radio/radio-keene.c
+++ b/drivers/media/radio/radio-keene.c
@@ -28,7 +28,6 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
 #include <linux/usb.h>
-#include <linux/version.h>
 #include <linux/mutex.h>
 
 /* driver and module definitions */
@@ -149,7 +148,6 @@ static void usb_keene_disconnect(struct usb_interface *intf)
 {
 	struct keene_device *radio = to_keene_dev(usb_get_intfdata(intf));
 
-	v4l2_device_get(&radio->v4l2_dev);
 	mutex_lock(&radio->lock);
 	usb_set_intfdata(intf, NULL);
 	video_unregister_device(&radio->vdev);
@@ -158,6 +156,23 @@ static void usb_keene_disconnect(struct usb_interface *intf)
 	v4l2_device_put(&radio->v4l2_dev);
 }
 
+static int usb_keene_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct keene_device *radio = to_keene_dev(usb_get_intfdata(intf));
+
+	return keene_cmd_main(radio, 0, false);
+}
+
+static int usb_keene_resume(struct usb_interface *intf)
+{
+	struct keene_device *radio = to_keene_dev(usb_get_intfdata(intf));
+
+	mdelay(50);
+	keene_cmd_set(radio);
+	keene_cmd_main(radio, radio->curfreq, true);
+	return 0;
+}
+
 static int vidioc_querycap(struct file *file, void *priv,
 					struct v4l2_capability *v)
 {
@@ -256,18 +271,6 @@ static int keene_s_ctrl(struct v4l2_ctrl *ctrl)
 	return -EINVAL;
 }
 
-static int vidioc_subscribe_event(struct v4l2_fh *fh,
-				struct v4l2_event_subscription *sub)
-{
-	switch (sub->type) {
-	case V4L2_EVENT_CTRL:
-		return v4l2_event_subscribe(fh, sub, 0);
-	default:
-		return -EINVAL;
-	}
-}
-
-
 /* File system interface */
 static const struct v4l2_file_operations usb_keene_fops = {
 	.owner		= THIS_MODULE,
@@ -288,7 +291,7 @@ static const struct v4l2_ioctl_ops usb_keene_ioctl_ops = {
 	.vidioc_g_frequency = vidioc_g_frequency,
 	.vidioc_s_frequency = vidioc_s_frequency,
 	.vidioc_log_status = v4l2_ctrl_log_status,
-	.vidioc_subscribe_event = vidioc_subscribe_event,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
@@ -404,6 +407,9 @@ static struct usb_driver usb_keene_driver = {
 	.probe			= usb_keene_probe,
 	.disconnect		= usb_keene_disconnect,
 	.id_table		= usb_keene_device_table,
+	.suspend		= usb_keene_suspend,
+	.resume			= usb_keene_resume,
+	.reset_resume		= usb_keene_resume,
 };
 
 static int __init keene_init(void)
diff --git a/drivers/media/radio/radio-mr800.c b/drivers/media/radio/radio-mr800.c
index a860a72a58ec..94cb6bc690f5 100644
--- a/drivers/media/radio/radio-mr800.c
+++ b/drivers/media/radio/radio-mr800.c
@@ -62,6 +62,8 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <linux/usb.h>
 #include <linux/mutex.h>
 
@@ -101,12 +103,17 @@ devices, that would be 76 and 91.  */
  * List isn't full and will be updated with implementation of new functions
  */
 #define AMRADIO_SET_FREQ	0xa4
+#define AMRADIO_GET_READY_FLAG	0xa5
+#define AMRADIO_GET_SIGNAL	0xa7
+#define AMRADIO_GET_FREQ	0xa8
+#define AMRADIO_SET_SEARCH_UP	0xa9
+#define AMRADIO_SET_SEARCH_DOWN	0xaa
 #define AMRADIO_SET_MUTE	0xab
+#define AMRADIO_SET_RIGHT_MUTE	0xac
+#define AMRADIO_SET_LEFT_MUTE	0xad
 #define AMRADIO_SET_MONO	0xae
-
-/* Comfortable defines for amradio_set_mute */
-#define AMRADIO_START		0x00
-#define AMRADIO_STOP		0x01
+#define AMRADIO_SET_SEARCH_LVL	0xb0
+#define AMRADIO_STOP_SEARCH	0xb1
 
 /* Comfortable defines for amradio_set_stereo */
 #define WANT_STEREO		0x00
@@ -117,29 +124,20 @@ static int radio_nr = -1;
 module_param(radio_nr, int, 0);
 MODULE_PARM_DESC(radio_nr, "Radio Nr");
 
-static int usb_amradio_probe(struct usb_interface *intf,
-			     const struct usb_device_id *id);
-static void usb_amradio_disconnect(struct usb_interface *intf);
-static int usb_amradio_open(struct file *file);
-static int usb_amradio_close(struct file *file);
-static int usb_amradio_suspend(struct usb_interface *intf,
-				pm_message_t message);
-static int usb_amradio_resume(struct usb_interface *intf);
-
 /* Data for one (physical) device */
 struct amradio_device {
 	/* reference to USB and video device */
 	struct usb_device *usbdev;
 	struct usb_interface *intf;
-	struct video_device videodev;
+	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 
-	unsigned char *buffer;
+	u8 *buffer;
 	struct mutex lock;	/* buffer locking */
 	int curfreq;
 	int stereo;
 	int muted;
-	int initialized;
 };
 
 static inline struct amradio_device *to_amradio_dev(struct v4l2_device *v4l2_dev)
@@ -147,29 +145,8 @@ static inline struct amradio_device *to_amradio_dev(struct v4l2_device *v4l2_dev
 	return container_of(v4l2_dev, struct amradio_device, v4l2_dev);
 }
 
-/* USB Device ID List */
-static struct usb_device_id usb_amradio_device_table[] = {
-	{USB_DEVICE_AND_INTERFACE_INFO(USB_AMRADIO_VENDOR, USB_AMRADIO_PRODUCT,
-							USB_CLASS_HID, 0, 0) },
-	{ }						/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(usb, usb_amradio_device_table);
-
-/* USB subsystem interface */
-static struct usb_driver usb_amradio_driver = {
-	.name			= MR800_DRIVER_NAME,
-	.probe			= usb_amradio_probe,
-	.disconnect		= usb_amradio_disconnect,
-	.suspend		= usb_amradio_suspend,
-	.resume			= usb_amradio_resume,
-	.reset_resume		= usb_amradio_resume,
-	.id_table		= usb_amradio_device_table,
-	.supports_autosuspend	= 1,
-};
-
-/* switch on/off the radio. Send 8 bytes to device */
-static int amradio_set_mute(struct amradio_device *radio, char argument)
+static int amradio_send_cmd(struct amradio_device *radio, u8 cmd, u8 arg,
+		u8 *extra, u8 extralen, bool reply)
 {
 	int retval;
 	int size;
@@ -177,99 +154,92 @@ static int amradio_set_mute(struct amradio_device *radio, char argument)
 	radio->buffer[0] = 0x00;
 	radio->buffer[1] = 0x55;
 	radio->buffer[2] = 0xaa;
-	radio->buffer[3] = 0x00;
-	radio->buffer[4] = AMRADIO_SET_MUTE;
-	radio->buffer[5] = argument;
+	radio->buffer[3] = extralen;
+	radio->buffer[4] = cmd;
+	radio->buffer[5] = arg;
 	radio->buffer[6] = 0x00;
-	radio->buffer[7] = 0x00;
+	radio->buffer[7] = extra || reply ? 8 : 0;
 
 	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
+		radio->buffer, BUFFER_LENGTH, &size, USB_TIMEOUT);
 
 	if (retval < 0 || size != BUFFER_LENGTH) {
-		amradio_dev_warn(&radio->videodev.dev, "set mute failed\n");
-		return retval;
+		if (video_is_registered(&radio->vdev))
+			amradio_dev_warn(&radio->vdev.dev,
+					"cmd %02x failed\n", cmd);
+		return retval ? retval : -EIO;
 	}
+	if (!extra && !reply)
+		return 0;
 
-	radio->muted = argument;
+	if (extra) {
+		memcpy(radio->buffer, extra, extralen);
+		memset(radio->buffer + extralen, 0, 8 - extralen);
+		retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
+			radio->buffer, BUFFER_LENGTH, &size, USB_TIMEOUT);
+	} else {
+		memset(radio->buffer, 0, 8);
+		retval = usb_bulk_msg(radio->usbdev, usb_rcvbulkpipe(radio->usbdev, 0x81),
+			radio->buffer, BUFFER_LENGTH, &size, USB_TIMEOUT);
+	}
+	if (retval == 0 && size == BUFFER_LENGTH)
+		return 0;
+	if (video_is_registered(&radio->vdev) && cmd != AMRADIO_GET_READY_FLAG)
+		amradio_dev_warn(&radio->vdev.dev, "follow-up to cmd %02x failed\n", cmd);
+	return retval ? retval : -EIO;
+}
 
-	return retval;
+/* switch on/off the radio. Send 8 bytes to device */
+static int amradio_set_mute(struct amradio_device *radio, bool mute)
+{
+	int ret = amradio_send_cmd(radio,
+			AMRADIO_SET_MUTE, mute, NULL, 0, false);
+
+	if (!ret)
+		radio->muted = mute;
+	return ret;
 }
 
 /* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
-static int amradio_setfreq(struct amradio_device *radio, int freq)
+static int amradio_set_freq(struct amradio_device *radio, int freq)
 {
-	int retval;
-	int size;
 	unsigned short freq_send = 0x10 + (freq >> 3) / 25;
-
-	radio->buffer[0] = 0x00;
-	radio->buffer[1] = 0x55;
-	radio->buffer[2] = 0xaa;
-	radio->buffer[3] = 0x03;
-	radio->buffer[4] = AMRADIO_SET_FREQ;
-	radio->buffer[5] = 0x00;
-	radio->buffer[6] = 0x00;
-	radio->buffer[7] = 0x08;
-
-	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
-
-	if (retval < 0 || size != BUFFER_LENGTH)
-		goto out_err;
+	u8 buf[3];
+	int retval;
 
 	/* frequency is calculated from freq_send and placed in first 2 bytes */
-	radio->buffer[0] = (freq_send >> 8) & 0xff;
-	radio->buffer[1] = freq_send & 0xff;
-	radio->buffer[2] = 0x01;
-	radio->buffer[3] = 0x00;
-	radio->buffer[4] = 0x00;
-	/* 5 and 6 bytes of buffer already = 0x00 */
-	radio->buffer[7] = 0x00;
-
-	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
-
-	if (retval < 0 || size != BUFFER_LENGTH)
-		goto out_err;
+	buf[0] = (freq_send >> 8) & 0xff;
+	buf[1] = freq_send & 0xff;
+	buf[2] = 0x01;
 
+	retval = amradio_send_cmd(radio, AMRADIO_SET_FREQ, 0, buf, 3, false);
+	if (retval)
+		return retval;
 	radio->curfreq = freq;
-	goto out;
-
-out_err:
-	amradio_dev_warn(&radio->videodev.dev, "set frequency failed\n");
-out:
-	return retval;
+	msleep(40);
+	return 0;
 }
 
-static int amradio_set_stereo(struct amradio_device *radio, char argument)
+static int amradio_set_stereo(struct amradio_device *radio, bool stereo)
 {
-	int retval;
-	int size;
+	int ret = amradio_send_cmd(radio,
+			AMRADIO_SET_MONO, !stereo, NULL, 0, false);
 
-	radio->buffer[0] = 0x00;
-	radio->buffer[1] = 0x55;
-	radio->buffer[2] = 0xaa;
-	radio->buffer[3] = 0x00;
-	radio->buffer[4] = AMRADIO_SET_MONO;
-	radio->buffer[5] = argument;
-	radio->buffer[6] = 0x00;
-	radio->buffer[7] = 0x00;
-
-	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
-
-	if (retval < 0 || size != BUFFER_LENGTH) {
-		amradio_dev_warn(&radio->videodev.dev, "set stereo failed\n");
-		return retval;
-	}
+	if (!ret)
+		radio->stereo = stereo;
+	return ret;
+}
 
-	if (argument == WANT_STEREO)
-		radio->stereo = 1;
-	else
-		radio->stereo = 0;
+static int amradio_get_stat(struct amradio_device *radio, bool *is_stereo, u32 *signal)
+{
+	int ret = amradio_send_cmd(radio,
+			AMRADIO_GET_SIGNAL, 0, NULL, 0, true);
 
-	return retval;
+	if (ret)
+		return ret;
+	*is_stereo = radio->buffer[2] >> 7;
+	*signal = (radio->buffer[3] & 0xf0) << 8;
+	return 0;
 }
 
 /* Handle unplugging the device.
@@ -282,25 +252,26 @@ static void usb_amradio_disconnect(struct usb_interface *intf)
 	struct amradio_device *radio = to_amradio_dev(usb_get_intfdata(intf));
 
 	mutex_lock(&radio->lock);
-	/* increase the device node's refcount */
-	get_device(&radio->videodev.dev);
+	video_unregister_device(&radio->vdev);
+	amradio_set_mute(radio, true);
+	usb_set_intfdata(intf, NULL);
 	v4l2_device_disconnect(&radio->v4l2_dev);
-	video_unregister_device(&radio->videodev);
 	mutex_unlock(&radio->lock);
-	/* decrease the device node's refcount, allowing it to be released */
-	put_device(&radio->videodev.dev);
+	v4l2_device_put(&radio->v4l2_dev);
 }
 
 /* vidioc_querycap - query device capabilities */
 static int vidioc_querycap(struct file *file, void *priv,
 					struct v4l2_capability *v)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
 
 	strlcpy(v->driver, "radio-mr800", sizeof(v->driver));
 	strlcpy(v->card, "AverMedia MR 800 USB FM Radio", sizeof(v->card));
 	usb_make_path(radio->usbdev, v->bus_info, sizeof(v->bus_info));
-	v->capabilities = V4L2_CAP_TUNER;
+	v->device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER |
+					V4L2_CAP_HW_FREQ_SEEK;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -308,44 +279,34 @@ static int vidioc_querycap(struct file *file, void *priv,
 static int vidioc_g_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
+	bool is_stereo = false;
 	int retval;
 
 	if (v->index > 0)
 		return -EINVAL;
 
-/* TODO: Add function which look is signal stereo or not
- * 	amradio_getstat(radio);
- */
-
-/* we call amradio_set_stereo to set radio->stereo
- * Honestly, amradio_getstat should cover this in future and
- * amradio_set_stereo shouldn't be here
- */
-	retval = amradio_set_stereo(radio, WANT_STEREO);
+	v->signal = 0;
+	retval = amradio_get_stat(radio, &is_stereo, &v->signal);
+	if (retval)
+		return retval;
 
 	strcpy(v->name, "FM");
 	v->type = V4L2_TUNER_RADIO;
 	v->rangelow = FREQ_MIN * FREQ_MUL;
 	v->rangehigh = FREQ_MAX * FREQ_MUL;
-	v->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
-	v->capability = V4L2_TUNER_CAP_LOW;
-	if (radio->stereo)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
-	else
-		v->audmode = V4L2_TUNER_MODE_MONO;
-	v->signal = 0xffff;     /* Can't get the signal strength, sad.. */
-	v->afc = 0; /* Don't know what is this */
-
-	return retval;
+	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
+	v->rxsubchans = is_stereo ? V4L2_TUNER_SUB_STEREO : V4L2_TUNER_SUB_MONO;
+	v->audmode = radio->stereo ?
+		V4L2_TUNER_MODE_STEREO : V4L2_TUNER_MODE_MONO;
+	return 0;
 }
 
 /* vidioc_s_tuner - set tuner attributes */
 static int vidioc_s_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
-	struct amradio_device *radio = file->private_data;
-	int retval = -EINVAL;
+	struct amradio_device *radio = video_drvdata(file);
 
 	if (v->index > 0)
 		return -EINVAL;
@@ -353,34 +314,31 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 	/* mono/stereo selector */
 	switch (v->audmode) {
 	case V4L2_TUNER_MODE_MONO:
-		retval = amradio_set_stereo(radio, WANT_MONO);
-		break;
-	case V4L2_TUNER_MODE_STEREO:
-		retval = amradio_set_stereo(radio, WANT_STEREO);
-		break;
+		return amradio_set_stereo(radio, WANT_MONO);
+	default:
+		return amradio_set_stereo(radio, WANT_STEREO);
 	}
-
-	return retval;
 }
 
 /* vidioc_s_frequency - set tuner radio frequency */
 static int vidioc_s_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
 
-	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
+	if (f->tuner != 0)
 		return -EINVAL;
-	return amradio_setfreq(radio, f->frequency);
+	return amradio_set_freq(radio, clamp_t(unsigned, f->frequency,
+				FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL));
 }
 
 /* vidioc_g_frequency - get tuner radio frequency */
 static int vidioc_g_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
 
-	if (f->tuner != 0)
+	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
 	f->type = V4L2_TUNER_RADIO;
 	f->frequency = radio->curfreq;
@@ -388,148 +346,101 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 	return 0;
 }
 
-/* vidioc_queryctrl - enumerate control items */
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *qc)
+static int vidioc_s_hw_freq_seek(struct file *file, void *priv,
+		struct v4l2_hw_freq_seek *seek)
 {
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-	}
-
-	return -EINVAL;
-}
+	static u8 buf[8] = {
+		0x3d, 0x32, 0x0f, 0x08, 0x3d, 0x32, 0x0f, 0x08
+	};
+	struct amradio_device *radio = video_drvdata(file);
+	unsigned long timeout;
+	int retval;
 
-/* vidioc_g_ctrl - get the value of a control */
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct amradio_device *radio = file->private_data;
+	if (seek->tuner != 0 || !seek->wrap_around)
+		return -EINVAL;
 
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = radio->muted;
-		return 0;
+	retval = amradio_send_cmd(radio,
+			AMRADIO_SET_SEARCH_LVL, 0, buf, 8, false);
+	if (retval)
+		return retval;
+	amradio_set_freq(radio, radio->curfreq);
+	retval = amradio_send_cmd(radio,
+		seek->seek_upward ? AMRADIO_SET_SEARCH_UP : AMRADIO_SET_SEARCH_DOWN,
+		0, NULL, 0, false);
+	if (retval)
+		return retval;
+	timeout = jiffies + msecs_to_jiffies(30000);
+	for (;;) {
+		if (time_after(jiffies, timeout)) {
+			retval = -EAGAIN;
+			break;
+		}
+		if (schedule_timeout_interruptible(msecs_to_jiffies(10))) {
+			retval = -ERESTARTSYS;
+			break;
+		}
+		retval = amradio_send_cmd(radio, AMRADIO_GET_READY_FLAG,
+				0, NULL, 0, true);
+		if (retval)
+			continue;
+		amradio_send_cmd(radio, AMRADIO_GET_FREQ, 0, NULL, 0, true);
+		if (radio->buffer[1] || radio->buffer[2]) {
+			radio->curfreq = (radio->buffer[1] << 8) | radio->buffer[2];
+			radio->curfreq = (radio->curfreq - 0x10) * 200;
+			amradio_send_cmd(radio, AMRADIO_STOP_SEARCH,
+					0, NULL, 0, false);
+			amradio_set_freq(radio, radio->curfreq);
+			retval = 0;
+			break;
+		}
 	}
-
-	return -EINVAL;
+	amradio_send_cmd(radio, AMRADIO_STOP_SEARCH, 0, NULL, 0, false);
+	amradio_set_freq(radio, radio->curfreq);
+	return retval;
 }
 
-/* vidioc_s_ctrl - set the value of a control */
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
+static int usb_amradio_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct amradio_device *radio = file->private_data;
-	int retval = -EINVAL;
+	struct amradio_device *radio =
+		container_of(ctrl->handler, struct amradio_device, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value)
-			retval = amradio_set_mute(radio, AMRADIO_STOP);
-		else
-			retval = amradio_set_mute(radio, AMRADIO_START);
-
-		break;
+		return amradio_set_mute(radio, ctrl->val);
 	}
 
-	return retval;
-}
-
-/* vidioc_g_audio - get audio attributes */
-static int vidioc_g_audio(struct file *file, void *priv,
-				struct v4l2_audio *a)
-{
-	if (a->index > 1)
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-	a->capability = V4L2_AUDCAP_STEREO;
-	return 0;
-}
-
-/* vidioc_s_audio - set audio attributes  */
-static int vidioc_s_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	if (a->index != 0)
-		return -EINVAL;
-	return 0;
-}
-
-/* vidioc_g_input - get input */
-static int vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-/* vidioc_s_input - set input */
-static int vidioc_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	if (i != 0)
-		return -EINVAL;
-	return 0;
+	return -EINVAL;
 }
 
 static int usb_amradio_init(struct amradio_device *radio)
 {
 	int retval;
 
-	retval = amradio_set_mute(radio, AMRADIO_STOP);
+	retval = amradio_set_mute(radio, true);
 	if (retval)
 		goto out_err;
-
-	retval = amradio_set_stereo(radio, WANT_STEREO);
+	retval = amradio_set_stereo(radio, true);
 	if (retval)
 		goto out_err;
-
-	radio->initialized = 1;
-	goto out;
-
-out_err:
-	amradio_dev_err(&radio->videodev.dev, "initialization failed\n");
-out:
-	return retval;
-}
-
-/* open device - amradio_start() and amradio_setfreq() */
-static int usb_amradio_open(struct file *file)
-{
-	struct amradio_device *radio = video_drvdata(file);
-	int retval;
-
-	file->private_data = radio;
-	retval = usb_autopm_get_interface(radio->intf);
+	retval = amradio_set_freq(radio, radio->curfreq);
 	if (retval)
-		return retval;
+		goto out_err;
+	return 0;
 
-	if (unlikely(!radio->initialized)) {
-		retval = usb_amradio_init(radio);
-		if (retval)
-			usb_autopm_put_interface(radio->intf);
-	}
+out_err:
+	amradio_dev_err(&radio->vdev.dev, "initialization failed\n");
 	return retval;
 }
 
-/*close device */
-static int usb_amradio_close(struct file *file)
-{
-	struct amradio_device *radio = file->private_data;
-
-	if (video_is_registered(&radio->videodev))
-		usb_autopm_put_interface(radio->intf);
-	return 0;
-}
-
 /* Suspend device - stop device. Need to be checked and fixed */
 static int usb_amradio_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct amradio_device *radio = to_amradio_dev(usb_get_intfdata(intf));
 
 	mutex_lock(&radio->lock);
-	if (!radio->muted && radio->initialized) {
-		amradio_set_mute(radio, AMRADIO_STOP);
-		radio->muted = 0;
+	if (!radio->muted) {
+		amradio_set_mute(radio, true);
+		radio->muted = false;
 	}
 	mutex_unlock(&radio->lock);
 
@@ -543,31 +454,28 @@ static int usb_amradio_resume(struct usb_interface *intf)
 	struct amradio_device *radio = to_amradio_dev(usb_get_intfdata(intf));
 
 	mutex_lock(&radio->lock);
-	if (unlikely(!radio->initialized))
-		goto unlock;
-
-	if (radio->stereo)
-		amradio_set_stereo(radio, WANT_STEREO);
-	else
-		amradio_set_stereo(radio, WANT_MONO);
-
-	amradio_setfreq(radio, radio->curfreq);
+	amradio_set_stereo(radio, radio->stereo);
+	amradio_set_freq(radio, radio->curfreq);
 
 	if (!radio->muted)
-		amradio_set_mute(radio, AMRADIO_START);
+		amradio_set_mute(radio, false);
 
-unlock:
 	mutex_unlock(&radio->lock);
 
 	dev_info(&intf->dev, "coming out of suspend..\n");
 	return 0;
 }
 
+static const struct v4l2_ctrl_ops usb_amradio_ctrl_ops = {
+	.s_ctrl = usb_amradio_s_ctrl,
+};
+
 /* File system interface */
 static const struct v4l2_file_operations usb_amradio_fops = {
 	.owner		= THIS_MODULE,
-	.open		= usb_amradio_open,
-	.release	= usb_amradio_close,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl	= video_ioctl2,
 };
 
@@ -577,20 +485,19 @@ static const struct v4l2_ioctl_ops usb_amradio_ioctl_ops = {
 	.vidioc_s_tuner     = vidioc_s_tuner,
 	.vidioc_g_frequency = vidioc_g_frequency,
 	.vidioc_s_frequency = vidioc_s_frequency,
-	.vidioc_queryctrl   = vidioc_queryctrl,
-	.vidioc_g_ctrl      = vidioc_g_ctrl,
-	.vidioc_s_ctrl      = vidioc_s_ctrl,
-	.vidioc_g_audio     = vidioc_g_audio,
-	.vidioc_s_audio     = vidioc_s_audio,
-	.vidioc_g_input     = vidioc_g_input,
-	.vidioc_s_input     = vidioc_s_input,
+	.vidioc_s_hw_freq_seek = vidioc_s_hw_freq_seek,
+	.vidioc_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
-static void usb_amradio_video_device_release(struct video_device *videodev)
+static void usb_amradio_release(struct v4l2_device *v4l2_dev)
 {
-	struct amradio_device *radio = video_get_drvdata(videodev);
+	struct amradio_device *radio = to_amradio_dev(v4l2_dev);
 
 	/* free rest memory */
+	v4l2_ctrl_handler_free(&radio->hdl);
+	v4l2_device_unregister(&radio->v4l2_dev);
 	kfree(radio->buffer);
 	kfree(radio);
 }
@@ -624,23 +531,38 @@ static int usb_amradio_probe(struct usb_interface *intf,
 		goto err_v4l2;
 	}
 
+	v4l2_ctrl_handler_init(&radio->hdl, 1);
+	v4l2_ctrl_new_std(&radio->hdl, &usb_amradio_ctrl_ops,
+			  V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	if (radio->hdl.error) {
+		retval = radio->hdl.error;
+		dev_err(&intf->dev, "couldn't register control\n");
+		goto err_ctrl;
+	}
 	mutex_init(&radio->lock);
 
-	strlcpy(radio->videodev.name, radio->v4l2_dev.name,
-		sizeof(radio->videodev.name));
-	radio->videodev.v4l2_dev = &radio->v4l2_dev;
-	radio->videodev.fops = &usb_amradio_fops;
-	radio->videodev.ioctl_ops = &usb_amradio_ioctl_ops;
-	radio->videodev.release = usb_amradio_video_device_release;
-	radio->videodev.lock = &radio->lock;
+	radio->v4l2_dev.ctrl_handler = &radio->hdl;
+	radio->v4l2_dev.release = usb_amradio_release;
+	strlcpy(radio->vdev.name, radio->v4l2_dev.name,
+		sizeof(radio->vdev.name));
+	radio->vdev.v4l2_dev = &radio->v4l2_dev;
+	radio->vdev.fops = &usb_amradio_fops;
+	radio->vdev.ioctl_ops = &usb_amradio_ioctl_ops;
+	radio->vdev.release = video_device_release_empty;
+	radio->vdev.lock = &radio->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->vdev.flags);
 
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->intf = intf;
+	usb_set_intfdata(intf, &radio->v4l2_dev);
 	radio->curfreq = 95.16 * FREQ_MUL;
 
-	video_set_drvdata(&radio->videodev, radio);
+	video_set_drvdata(&radio->vdev, radio);
+	retval = usb_amradio_init(radio);
+	if (retval)
+		goto err_vdev;
 
-	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO,
+	retval = video_register_device(&radio->vdev, VFL_TYPE_RADIO,
 					radio_nr);
 	if (retval < 0) {
 		dev_err(&intf->dev, "could not register video device\n");
@@ -650,6 +572,8 @@ static int usb_amradio_probe(struct usb_interface *intf,
 	return 0;
 
 err_vdev:
+	v4l2_ctrl_handler_free(&radio->hdl);
+err_ctrl:
 	v4l2_device_unregister(&radio->v4l2_dev);
 err_v4l2:
 	kfree(radio->buffer);
@@ -659,4 +583,24 @@ err:
 	return retval;
 }
 
+/* USB Device ID List */
+static struct usb_device_id usb_amradio_device_table[] = {
+	{ USB_DEVICE_AND_INTERFACE_INFO(USB_AMRADIO_VENDOR, USB_AMRADIO_PRODUCT,
+							USB_CLASS_HID, 0, 0) },
+	{ }						/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_amradio_device_table);
+
+/* USB subsystem interface */
+static struct usb_driver usb_amradio_driver = {
+	.name			= MR800_DRIVER_NAME,
+	.probe			= usb_amradio_probe,
+	.disconnect		= usb_amradio_disconnect,
+	.suspend		= usb_amradio_suspend,
+	.resume			= usb_amradio_resume,
+	.reset_resume		= usb_amradio_resume,
+	.id_table		= usb_amradio_device_table,
+};
+
 module_usb_driver(usb_amradio_driver);
diff --git a/drivers/media/radio/radio-rtrack2.c b/drivers/media/radio/radio-rtrack2.c
index b275c5d0fe9a..b1f844c64fde 100644
--- a/drivers/media/radio/radio-rtrack2.c
+++ b/drivers/media/radio/radio-rtrack2.c
@@ -17,6 +17,7 @@
 #include <linux/videodev2.h>	/* kernel radio structs		*/
 #include <linux/mutex.h>
 #include <linux/io.h>		/* outb, outb_p			*/
+#include <linux/slab.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include "radio-isa.h"
diff --git a/drivers/media/radio/radio-sf16fmi.c b/drivers/media/radio/radio-sf16fmi.c
index 22c5743bf9db..a81d723b8c77 100644
--- a/drivers/media/radio/radio-sf16fmi.c
+++ b/drivers/media/radio/radio-sf16fmi.c
@@ -1,4 +1,4 @@
-/* SF16-FMI and SF16-FMP radio driver for Linux radio support
+/* SF16-FMI, SF16-FMP and SF16-FMD radio driver for Linux radio support
  * heavily based on rtrack driver...
  * (c) 1997 M. Kirkwood
  * (c) 1998 Petr Vandrovec, vandrove@vc.cvut.cz
@@ -11,7 +11,7 @@
  *
  *  Frequency control is done digitally -- ie out(port,encodefreq(95.8));
  *  No volume control - only mute/unmute - you have to use line volume
- *  control on SB-part of SF16-FMI/SF16-FMP
+ *  control on SB-part of SF16-FMI/SF16-FMP/SF16-FMD
  *
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@infradead.org>
  */
@@ -29,7 +29,7 @@
 #include <media/v4l2-ioctl.h>
 
 MODULE_AUTHOR("Petr Vandrovec, vandrove@vc.cvut.cz and M. Kirkwood");
-MODULE_DESCRIPTION("A driver for the SF16-FMI and SF16-FMP radio.");
+MODULE_DESCRIPTION("A driver for the SF16-FMI, SF16-FMP and SF16-FMD radio.");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.0.3");
 
@@ -37,7 +37,7 @@ static int io = -1;
 static int radio_nr = -1;
 
 module_param(io, int, 0);
-MODULE_PARM_DESC(io, "I/O address of the SF16-FMI or SF16-FMP card (0x284 or 0x384)");
+MODULE_PARM_DESC(io, "I/O address of the SF16-FMI/SF16-FMP/SF16-FMD card (0x284 or 0x384)");
 module_param(radio_nr, int, 0);
 
 struct fmi
@@ -130,7 +130,7 @@ static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *v)
 {
 	strlcpy(v->driver, "radio-sf16fmi", sizeof(v->driver));
-	strlcpy(v->card, "SF16-FMx radio", sizeof(v->card));
+	strlcpy(v->card, "SF16-FMI/FMP/FMD radio", sizeof(v->card));
 	strlcpy(v->bus_info, "ISA", sizeof(v->bus_info));
 	v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
 	return 0;
@@ -277,8 +277,12 @@ static const struct v4l2_ioctl_ops fmi_ioctl_ops = {
 
 /* ladis: this is my card. does any other types exist? */
 static struct isapnp_device_id id_table[] __devinitdata = {
+		/* SF16-FMI */
 	{	ISAPNP_ANY_ID, ISAPNP_ANY_ID,
 		ISAPNP_VENDOR('M','F','R'), ISAPNP_FUNCTION(0xad10), 0},
+		/* SF16-FMD */
+	{	ISAPNP_ANY_ID, ISAPNP_ANY_ID,
+		ISAPNP_VENDOR('M','F','R'), ISAPNP_FUNCTION(0xad12), 0},
 	{	ISAPNP_CARD_END, },
 };
 
diff --git a/drivers/media/radio/radio-sf16fmr2.c b/drivers/media/radio/radio-sf16fmr2.c
index 7c69214334bf..52b8011f1b23 100644
--- a/drivers/media/radio/radio-sf16fmr2.c
+++ b/drivers/media/radio/radio-sf16fmr2.c
@@ -1,4 +1,4 @@
-/* SF16-FMR2 radio driver for Linux
+/* SF16-FMR2 and SF16-FMD2 radio driver for Linux
  * Copyright (c) 2011 Ondrej Zary
  *
  * Original driver was (c) 2000-2002 Ziglio Frediano, freddy77@angelfire.com
@@ -13,15 +13,19 @@
 #include <linux/ioport.h>	/* request_region		*/
 #include <linux/io.h>		/* outb, outb_p			*/
 #include <linux/isa.h>
+#include <linux/pnp.h>
 #include <sound/tea575x-tuner.h>
 
 MODULE_AUTHOR("Ondrej Zary");
-MODULE_DESCRIPTION("MediaForte SF16-FMR2 FM radio card driver");
+MODULE_DESCRIPTION("MediaForte SF16-FMR2 and SF16-FMD2 FM radio card driver");
 MODULE_LICENSE("GPL");
 
-static int radio_nr = -1;
-module_param(radio_nr, int, 0444);
-MODULE_PARM_DESC(radio_nr, "Radio device number");
+/* these cards can only use two different ports (0x384 and 0x284) */
+#define FMR2_MAX 2
+
+static int radio_nr[FMR2_MAX] = { [0 ... (FMR2_MAX - 1)] = -1 };
+module_param_array(radio_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_nr, "Radio device numbers");
 
 struct fmr2 {
 	int io;
@@ -29,9 +33,15 @@ struct fmr2 {
 	struct snd_tea575x tea;
 	struct v4l2_ctrl *volume;
 	struct v4l2_ctrl *balance;
+	bool is_fmd2;
 };
 
-/* the port is hardwired so no need to support multiple cards */
+static int num_fmr2_cards;
+static struct fmr2 *fmr2_cards[FMR2_MAX];
+static bool isa_registered;
+static bool pnp_registered;
+
+/* the port is hardwired on SF16-FMR2 */
 #define FMR2_PORT	0x384
 
 /* TEA575x tuner pins */
@@ -174,7 +184,8 @@ static int fmr2_tea_ext_init(struct snd_tea575x *tea)
 {
 	struct fmr2 *fmr2 = tea->private_data;
 
-	if (inb(fmr2->io) & FMR2_HASVOL) {
+	/* FMR2 can have volume control, FMD2 can't (uses SB16 mixer) */
+	if (!fmr2->is_fmd2 && inb(fmr2->io) & FMR2_HASVOL) {
 		fmr2->volume = v4l2_ctrl_new_std(&tea->ctrl_handler, &fmr2_ctrl_ops, V4L2_CID_AUDIO_VOLUME, 0, 68, 2, 56);
 		fmr2->balance = v4l2_ctrl_new_std(&tea->ctrl_handler, &fmr2_ctrl_ops, V4L2_CID_AUDIO_BALANCE, -68, 68, 2, 0);
 		if (tea->ctrl_handler.error) {
@@ -186,22 +197,28 @@ static int fmr2_tea_ext_init(struct snd_tea575x *tea)
 	return 0;
 }
 
-static int __devinit fmr2_probe(struct device *pdev, unsigned int dev)
+static struct pnp_device_id fmr2_pnp_ids[] __devinitdata = {
+	{ .id = "MFRad13" }, /* tuner subdevice of SF16-FMD2 */
+	{ .id = "" }
+};
+MODULE_DEVICE_TABLE(pnp, fmr2_pnp_ids);
+
+static int __devinit fmr2_probe(struct fmr2 *fmr2, struct device *pdev, int io)
 {
-	struct fmr2 *fmr2;
-	int err;
+	int err, i;
+	char *card_name = fmr2->is_fmd2 ? "SF16-FMD2" : "SF16-FMR2";
 
-	fmr2 = kzalloc(sizeof(*fmr2), GFP_KERNEL);
-	if (fmr2 == NULL)
-		return -ENOMEM;
+	/* avoid errors if a card was already registered at given port */
+	for (i = 0; i < num_fmr2_cards; i++)
+		if (io == fmr2_cards[i]->io)
+			return -EBUSY;
 
-	strlcpy(fmr2->v4l2_dev.name, dev_name(pdev),
-			sizeof(fmr2->v4l2_dev.name));
-	fmr2->io = FMR2_PORT;
+	strlcpy(fmr2->v4l2_dev.name, "radio-sf16fmr2",
+			sizeof(fmr2->v4l2_dev.name)),
+	fmr2->io = io;
 
 	if (!request_region(fmr2->io, 2, fmr2->v4l2_dev.name)) {
 		printk(KERN_ERR "radio-sf16fmr2: I/O port 0x%x already in use\n", fmr2->io);
-		kfree(fmr2);
 		return -EBUSY;
 	}
 
@@ -210,56 +227,121 @@ static int __devinit fmr2_probe(struct device *pdev, unsigned int dev)
 	if (err < 0) {
 		v4l2_err(&fmr2->v4l2_dev, "Could not register v4l2_device\n");
 		release_region(fmr2->io, 2);
-		kfree(fmr2);
 		return err;
 	}
 	fmr2->tea.v4l2_dev = &fmr2->v4l2_dev;
 	fmr2->tea.private_data = fmr2;
-	fmr2->tea.radio_nr = radio_nr;
+	fmr2->tea.radio_nr = radio_nr[num_fmr2_cards];
 	fmr2->tea.ops = &fmr2_tea_ops;
 	fmr2->tea.ext_init = fmr2_tea_ext_init;
-	strlcpy(fmr2->tea.card, "SF16-FMR2", sizeof(fmr2->tea.card));
-	snprintf(fmr2->tea.bus_info, sizeof(fmr2->tea.bus_info), "ISA:%s",
-			fmr2->v4l2_dev.name);
+	strlcpy(fmr2->tea.card, card_name, sizeof(fmr2->tea.card));
+	snprintf(fmr2->tea.bus_info, sizeof(fmr2->tea.bus_info), "%s:%s",
+			fmr2->is_fmd2 ? "PnP" : "ISA", dev_name(pdev));
 
 	if (snd_tea575x_init(&fmr2->tea)) {
 		printk(KERN_ERR "radio-sf16fmr2: Unable to detect TEA575x tuner\n");
 		release_region(fmr2->io, 2);
-		kfree(fmr2);
 		return -ENODEV;
 	}
 
-	printk(KERN_INFO "radio-sf16fmr2: SF16-FMR2 radio card at 0x%x.\n", fmr2->io);
+	printk(KERN_INFO "radio-sf16fmr2: %s radio card at 0x%x.\n",
+			card_name, fmr2->io);
 	return 0;
 }
 
-static int __exit fmr2_remove(struct device *pdev, unsigned int dev)
+static int __devinit fmr2_isa_match(struct device *pdev, unsigned int ndev)
+{
+	struct fmr2 *fmr2 = kzalloc(sizeof(*fmr2), GFP_KERNEL);
+	if (!fmr2)
+		return 0;
+
+	if (fmr2_probe(fmr2, pdev, FMR2_PORT)) {
+		kfree(fmr2);
+		return 0;
+	}
+	dev_set_drvdata(pdev, fmr2);
+	fmr2_cards[num_fmr2_cards++] = fmr2;
+
+	return 1;
+}
+
+static int __devinit fmr2_pnp_probe(struct pnp_dev *pdev,
+				const struct pnp_device_id *id)
 {
-	struct fmr2 *fmr2 = dev_get_drvdata(pdev);
+	int ret;
+	struct fmr2 *fmr2 = kzalloc(sizeof(*fmr2), GFP_KERNEL);
+	if (!fmr2)
+		return -ENOMEM;
 
+	fmr2->is_fmd2 = true;
+	ret = fmr2_probe(fmr2, &pdev->dev, pnp_port_start(pdev, 0));
+	if (ret) {
+		kfree(fmr2);
+		return ret;
+	}
+	pnp_set_drvdata(pdev, fmr2);
+	fmr2_cards[num_fmr2_cards++] = fmr2;
+
+	return 0;
+}
+
+static void __devexit fmr2_remove(struct fmr2 *fmr2)
+{
 	snd_tea575x_exit(&fmr2->tea);
 	release_region(fmr2->io, 2);
 	v4l2_device_unregister(&fmr2->v4l2_dev);
 	kfree(fmr2);
+}
+
+static int __devexit fmr2_isa_remove(struct device *pdev, unsigned int ndev)
+{
+	fmr2_remove(dev_get_drvdata(pdev));
+	dev_set_drvdata(pdev, NULL);
+
 	return 0;
 }
 
-struct isa_driver fmr2_driver = {
-	.probe		= fmr2_probe,
-	.remove		= fmr2_remove,
+static void __devexit fmr2_pnp_remove(struct pnp_dev *pdev)
+{
+	fmr2_remove(pnp_get_drvdata(pdev));
+	pnp_set_drvdata(pdev, NULL);
+}
+
+struct isa_driver fmr2_isa_driver = {
+	.match		= fmr2_isa_match,
+	.remove		= __devexit_p(fmr2_isa_remove),
 	.driver		= {
 		.name	= "radio-sf16fmr2",
 	},
 };
 
+struct pnp_driver fmr2_pnp_driver = {
+	.name		= "radio-sf16fmr2",
+	.id_table	= fmr2_pnp_ids,
+	.probe		= fmr2_pnp_probe,
+	.remove		= __devexit_p(fmr2_pnp_remove),
+};
+
 static int __init fmr2_init(void)
 {
-	return isa_register_driver(&fmr2_driver, 1);
+	int ret;
+
+	ret = pnp_register_driver(&fmr2_pnp_driver);
+	if (!ret)
+		pnp_registered = true;
+	ret = isa_register_driver(&fmr2_isa_driver, 1);
+	if (!ret)
+		isa_registered = true;
+
+	return (pnp_registered || isa_registered) ? 0 : ret;
 }
 
 static void __exit fmr2_exit(void)
 {
-	isa_unregister_driver(&fmr2_driver);
+	if (pnp_registered)
+		pnp_unregister_driver(&fmr2_pnp_driver);
+	if (isa_registered)
+		isa_unregister_driver(&fmr2_isa_driver);
 }
 
 module_init(fmr2_init);
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index 5d9a90ac3a1c..7052adc0c0b0 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -223,7 +223,7 @@ static struct platform_driver timbradio_platform_driver = {
 		.owner	= THIS_MODULE,
 	},
 	.probe		= timbradio_probe,
-	.remove		= timbradio_remove,
+	.remove		= __devexit_p(timbradio_remove),
 };
 
 module_platform_driver(timbradio_platform_driver);
diff --git a/drivers/media/radio/saa7706h.c b/drivers/media/radio/saa7706h.c
index 9474706350f8..bb953ef75f61 100644
--- a/drivers/media/radio/saa7706h.c
+++ b/drivers/media/radio/saa7706h.c
@@ -430,7 +430,7 @@ static struct i2c_driver saa7706h_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= saa7706h_probe,
-	.remove		= saa7706h_remove,
+	.remove		= __devexit_p(saa7706h_remove),
 	.id_table	= saa7706h_id,
 };
 
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index 0e740c98786c..969cf494d85b 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -196,9 +196,9 @@ static int si470x_set_chan(struct si470x_device *radio, unsigned short chan)
 	}
 
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
-		dev_warn(&radio->videodev->dev, "tune does not complete\n");
+		dev_warn(&radio->videodev.dev, "tune does not complete\n");
 	if (timed_out)
-		dev_warn(&radio->videodev->dev,
+		dev_warn(&radio->videodev.dev,
 			"tune timed out after %u ms\n", tune_timeout);
 
 stop:
@@ -262,7 +262,7 @@ static int si470x_get_freq(struct si470x_device *radio, unsigned int *freq)
  */
 int si470x_set_freq(struct si470x_device *radio, unsigned int freq)
 {
-	unsigned int spacing, band_bottom;
+	unsigned int spacing, band_bottom, band_top;
 	unsigned short chan;
 
 	/* Spacing (kHz) */
@@ -278,19 +278,26 @@ int si470x_set_freq(struct si470x_device *radio, unsigned int freq)
 		spacing = 0.050 * FREQ_MUL; break;
 	};
 
-	/* Bottom of Band (MHz) */
+	/* Bottom/Top of Band (MHz) */
 	switch ((radio->registers[SYSCONFIG2] & SYSCONFIG2_BAND) >> 6) {
 	/* 0: 87.5 - 108 MHz (USA, Europe) */
 	case 0:
-		band_bottom = 87.5 * FREQ_MUL; break;
+		band_bottom = 87.5 * FREQ_MUL;
+		band_top = 108 * FREQ_MUL;
+		break;
 	/* 1: 76   - 108 MHz (Japan wide band) */
 	default:
-		band_bottom = 76   * FREQ_MUL; break;
+		band_bottom = 76 * FREQ_MUL;
+		band_top = 108 * FREQ_MUL;
+		break;
 	/* 2: 76   -  90 MHz (Japan) */
 	case 2:
-		band_bottom = 76   * FREQ_MUL; break;
+		band_bottom = 76 * FREQ_MUL;
+		band_top = 90 * FREQ_MUL;
+		break;
 	};
 
+	freq = clamp(freq, band_bottom, band_top);
 	/* Chan = [ Freq (Mhz) - Bottom of Band (MHz) ] / Spacing (kHz) */
 	chan = (freq - band_bottom) / spacing;
 
@@ -320,7 +327,7 @@ static int si470x_set_seek(struct si470x_device *radio,
 		radio->registers[POWERCFG] &= ~POWERCFG_SEEKUP;
 	retval = si470x_set_register(radio, POWERCFG);
 	if (retval < 0)
-		goto done;
+		return retval;
 
 	/* currently I2C driver only uses interrupt way to seek */
 	if (radio->stci_enabled) {
@@ -344,24 +351,19 @@ static int si470x_set_seek(struct si470x_device *radio,
 	}
 
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
-		dev_warn(&radio->videodev->dev, "seek does not complete\n");
+		dev_warn(&radio->videodev.dev, "seek does not complete\n");
 	if (radio->registers[STATUSRSSI] & STATUSRSSI_SF)
-		dev_warn(&radio->videodev->dev,
+		dev_warn(&radio->videodev.dev,
 			"seek failed / band limit reached\n");
-	if (timed_out)
-		dev_warn(&radio->videodev->dev,
-			"seek timed out after %u ms\n", seek_timeout);
 
 stop:
 	/* stop seeking */
 	radio->registers[POWERCFG] &= ~POWERCFG_SEEK;
 	retval = si470x_set_register(radio, POWERCFG);
 
-done:
 	/* try again, if timed out */
-	if ((retval == 0) && timed_out)
-		retval = -EAGAIN;
-
+	if (retval == 0 && timed_out)
+		return -EAGAIN;
 	return retval;
 }
 
@@ -463,7 +465,6 @@ static ssize_t si470x_fops_read(struct file *file, char __user *buf,
 	unsigned int block_count = 0;
 
 	/* switch on rds reception */
-	mutex_lock(&radio->lock);
 	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
 		si470x_rds_on(radio);
 
@@ -505,7 +506,6 @@ static ssize_t si470x_fops_read(struct file *file, char __user *buf,
 	}
 
 done:
-	mutex_unlock(&radio->lock);
 	return retval;
 }
 
@@ -517,19 +517,19 @@ static unsigned int si470x_fops_poll(struct file *file,
 		struct poll_table_struct *pts)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
+	unsigned long req_events = poll_requested_events(pts);
+	int retval = v4l2_ctrl_poll(file, pts);
 
-	/* switch on rds reception */
-
-	mutex_lock(&radio->lock);
-	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
-		si470x_rds_on(radio);
-	mutex_unlock(&radio->lock);
+	if (req_events & (POLLIN | POLLRDNORM)) {
+		/* switch on rds reception */
+		if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
+			si470x_rds_on(radio);
 
-	poll_wait(file, &radio->read_queue, pts);
+		poll_wait(file, &radio->read_queue, pts);
 
-	if (radio->rd_index != radio->wr_index)
-		retval = POLLIN | POLLRDNORM;
+		if (radio->rd_index != radio->wr_index)
+			retval |= POLLIN | POLLRDNORM;
+	}
 
 	return retval;
 }
@@ -553,134 +553,26 @@ static const struct v4l2_file_operations si470x_fops = {
  * Video4Linux Interface
  **************************************************************************/
 
-/*
- * si470x_vidioc_queryctrl - enumerate control items
- */
-static int si470x_vidioc_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *qc)
-{
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = -EINVAL;
-
-	/* abort if qc->id is below V4L2_CID_BASE */
-	if (qc->id < V4L2_CID_BASE)
-		goto done;
-
-	/* search video control */
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		return v4l2_ctrl_query_fill(qc, 0, 15, 1, 15);
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-	}
-
-	/* disable unsupported base controls */
-	/* to satisfy kradio and such apps */
-	if ((retval == -EINVAL) && (qc->id < V4L2_CID_LASTP1)) {
-		qc->flags = V4L2_CTRL_FLAG_DISABLED;
-		retval = 0;
-	}
 
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"query controls failed with %d\n", retval);
-	return retval;
-}
-
-
-/*
- * si470x_vidioc_g_ctrl - get the value of a control
- */
-static int si470x_vidioc_g_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
-{
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		ctrl->value = radio->registers[SYSCONFIG2] &
-				SYSCONFIG2_VOLUME;
-		break;
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = ((radio->registers[POWERCFG] &
-				POWERCFG_DMUTE) == 0) ? 1 : 0;
-		break;
-	default:
-		retval = -EINVAL;
-	}
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"get control failed with %d\n", retval);
-
-	mutex_unlock(&radio->lock);
-	return retval;
-}
-
-
-/*
- * si470x_vidioc_s_ctrl - set the value of a control
- */
-static int si470x_vidioc_s_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
+static int si470x_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
+	struct si470x_device *radio =
+		container_of(ctrl->handler, struct si470x_device, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_VOLUME:
 		radio->registers[SYSCONFIG2] &= ~SYSCONFIG2_VOLUME;
-		radio->registers[SYSCONFIG2] |= ctrl->value;
-		retval = si470x_set_register(radio, SYSCONFIG2);
-		break;
+		radio->registers[SYSCONFIG2] |= ctrl->val;
+		return si470x_set_register(radio, SYSCONFIG2);
 	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value == 1)
+		if (ctrl->val)
 			radio->registers[POWERCFG] &= ~POWERCFG_DMUTE;
 		else
 			radio->registers[POWERCFG] |= POWERCFG_DMUTE;
-		retval = si470x_set_register(radio, POWERCFG);
-		break;
+		return si470x_set_register(radio, POWERCFG);
 	default:
-		retval = -EINVAL;
+		return -EINVAL;
 	}
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set control failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
-}
-
-
-/*
- * si470x_vidioc_g_audio - get audio attributes
- */
-static int si470x_vidioc_g_audio(struct file *file, void *priv,
-		struct v4l2_audio *audio)
-{
-	/* driver constants */
-	audio->index = 0;
-	strcpy(audio->name, "Radio");
-	audio->capability = V4L2_AUDCAP_STEREO;
-	audio->mode = 0;
-
-	return 0;
 }
 
 
@@ -691,22 +583,14 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
 		struct v4l2_tuner *tuner)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
+	int retval;
 
-	if (tuner->index != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
+	if (tuner->index != 0)
+		return -EINVAL;
 
 	retval = si470x_get_register(radio, STATUSRSSI);
 	if (retval < 0)
-		goto done;
+		return retval;
 
 	/* driver constants */
 	strcpy(tuner->name, "FM");
@@ -737,7 +621,7 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_ST) == 0)
 		tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
 	else
-		tuner->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
+		tuner->rxsubchans = V4L2_TUNER_SUB_STEREO;
 	/* If there is a reliable method of detecting an RDS channel,
 	   then this code should check for that before setting this
 	   RDS subchannel. */
@@ -754,16 +638,13 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
 	tuner->signal = (radio->registers[STATUSRSSI] & STATUSRSSI_RSSI);
 	/* the ideal factor is 0xffff/75 = 873,8 */
 	tuner->signal = (tuner->signal * 873) + (8 * tuner->signal / 10);
+	if (tuner->signal > 0xffff)
+		tuner->signal = 0xffff;
 
 	/* automatic frequency control: -1: freq to low, 1 freq to high */
 	/* AFCRL does only indicate that freq. differs, not if too low/high */
 	tuner->afc = (radio->registers[STATUSRSSI] & STATUSRSSI_AFCRL) ? 1 : 0;
 
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"get tuner failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
 	return retval;
 }
 
@@ -775,16 +656,9 @@ static int si470x_vidioc_s_tuner(struct file *file, void *priv,
 		struct v4l2_tuner *tuner)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
 
 	if (tuner->index != 0)
-		goto done;
+		return -EINVAL;
 
 	/* mono/stereo selector */
 	switch (tuner->audmode) {
@@ -792,20 +666,12 @@ static int si470x_vidioc_s_tuner(struct file *file, void *priv,
 		radio->registers[POWERCFG] |= POWERCFG_MONO;  /* force mono */
 		break;
 	case V4L2_TUNER_MODE_STEREO:
+	default:
 		radio->registers[POWERCFG] &= ~POWERCFG_MONO; /* try stereo */
 		break;
-	default:
-		goto done;
 	}
 
-	retval = si470x_set_register(radio, POWERCFG);
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set tuner failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_set_register(radio, POWERCFG);
 }
 
 
@@ -816,28 +682,12 @@ static int si470x_vidioc_g_frequency(struct file *file, void *priv,
 		struct v4l2_frequency *freq)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	/* safety checks */
-	mutex_lock(&radio->lock);
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
 
-	if (freq->tuner != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
+	if (freq->tuner != 0)
+		return -EINVAL;
 
 	freq->type = V4L2_TUNER_RADIO;
-	retval = si470x_get_freq(radio, &freq->frequency);
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"get frequency failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_get_freq(radio, &freq->frequency);
 }
 
 
@@ -848,27 +698,11 @@ static int si470x_vidioc_s_frequency(struct file *file, void *priv,
 		struct v4l2_frequency *freq)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
 
-	if (freq->tuner != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
+	if (freq->tuner != 0)
+		return -EINVAL;
 
-	retval = si470x_set_freq(radio, freq->frequency);
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set frequency failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_set_freq(radio, freq->frequency);
 }
 
 
@@ -879,44 +713,29 @@ static int si470x_vidioc_s_hw_freq_seek(struct file *file, void *priv,
 		struct v4l2_hw_freq_seek *seek)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
-
-	if (seek->tuner != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
 
-	retval = si470x_set_seek(radio, seek->wrap_around, seek->seek_upward);
+	if (seek->tuner != 0)
+		return -EINVAL;
 
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set hardware frequency seek failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_set_seek(radio, seek->wrap_around, seek->seek_upward);
 }
 
+const struct v4l2_ctrl_ops si470x_ctrl_ops = {
+	.s_ctrl = si470x_s_ctrl,
+};
 
 /*
  * si470x_ioctl_ops - video device ioctl operations
  */
 static const struct v4l2_ioctl_ops si470x_ioctl_ops = {
 	.vidioc_querycap	= si470x_vidioc_querycap,
-	.vidioc_queryctrl	= si470x_vidioc_queryctrl,
-	.vidioc_g_ctrl		= si470x_vidioc_g_ctrl,
-	.vidioc_s_ctrl		= si470x_vidioc_s_ctrl,
-	.vidioc_g_audio		= si470x_vidioc_g_audio,
 	.vidioc_g_tuner		= si470x_vidioc_g_tuner,
 	.vidioc_s_tuner		= si470x_vidioc_s_tuner,
 	.vidioc_g_frequency	= si470x_vidioc_g_frequency,
 	.vidioc_s_frequency	= si470x_vidioc_s_frequency,
 	.vidioc_s_hw_freq_seek	= si470x_vidioc_s_hw_freq_seek,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 
@@ -926,6 +745,6 @@ static const struct v4l2_ioctl_ops si470x_ioctl_ops = {
 struct video_device si470x_viddev_template = {
 	.fops			= &si470x_fops,
 	.name			= DRIVER_NAME,
-	.release		= video_device_release,
+	.release		= video_device_release_empty,
 	.ioctl_ops		= &si470x_ioctl_ops,
 };
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index 9b546a5523f3..a80044c5874e 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -162,20 +162,6 @@ static int si470x_get_all_registers(struct si470x_device *radio)
 
 
 /**************************************************************************
- * General Driver Functions - DISCONNECT_CHECK
- **************************************************************************/
-
-/*
- * si470x_disconnect_check - check whether radio disconnects
- */
-int si470x_disconnect_check(struct si470x_device *radio)
-{
-	return 0;
-}
-
-
-
-/**************************************************************************
  * File Operations Interface
  **************************************************************************/
 
@@ -185,12 +171,12 @@ int si470x_disconnect_check(struct si470x_device *radio)
 int si470x_fops_open(struct file *file)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
+	int retval = v4l2_fh_open(file);
 
-	mutex_lock(&radio->lock);
-	radio->users++;
+	if (retval)
+		return retval;
 
-	if (radio->users == 1) {
+	if (v4l2_fh_is_singular_file(file)) {
 		/* start radio */
 		retval = si470x_start(radio);
 		if (retval < 0)
@@ -205,7 +191,8 @@ int si470x_fops_open(struct file *file)
 	}
 
 done:
-	mutex_unlock(&radio->lock);
+	if (retval)
+		v4l2_fh_release(file);
 	return retval;
 }
 
@@ -216,21 +203,12 @@ done:
 int si470x_fops_release(struct file *file)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	/* safety check */
-	if (!radio)
-		return -ENODEV;
 
-	mutex_lock(&radio->lock);
-	radio->users--;
-	if (radio->users == 0)
+	if (v4l2_fh_is_singular_file(file))
 		/* stop radio */
-		retval = si470x_stop(radio);
+		si470x_stop(radio);
 
-	mutex_unlock(&radio->lock);
-
-	return retval;
+	return v4l2_fh_release(file);
 }
 
 
@@ -371,32 +349,25 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 		goto err_initial;
 	}
 
-	radio->users = 0;
 	radio->client = client;
 	mutex_init(&radio->lock);
 
-	/* video device allocation and initialization */
-	radio->videodev = video_device_alloc();
-	if (!radio->videodev) {
-		retval = -ENOMEM;
-		goto err_radio;
-	}
-	memcpy(radio->videodev, &si470x_viddev_template,
-			sizeof(si470x_viddev_template));
-	video_set_drvdata(radio->videodev, radio);
+	/* video device initialization */
+	radio->videodev = si470x_viddev_template;
+	video_set_drvdata(&radio->videodev, radio);
 
 	/* power up : need 110ms */
 	radio->registers[POWERCFG] = POWERCFG_ENABLE;
 	if (si470x_set_register(radio, POWERCFG) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_radio;
 	}
 	msleep(110);
 
 	/* get device and chip versions */
 	if (si470x_get_all_registers(radio) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_radio;
 	}
 	dev_info(&client->dev, "DeviceID=0x%4.4hx ChipID=0x%4.4hx\n",
 			radio->registers[DEVICEID], radio->registers[CHIPID]);
@@ -427,7 +398,7 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 	radio->buffer = kmalloc(radio->buf_size, GFP_KERNEL);
 	if (!radio->buffer) {
 		retval = -EIO;
-		goto err_video;
+		goto err_radio;
 	}
 
 	/* rds buffer configuration */
@@ -447,7 +418,7 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 	}
 
 	/* register video device */
-	retval = video_register_device(radio->videodev, VFL_TYPE_RADIO,
+	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO,
 			radio_nr);
 	if (retval) {
 		dev_warn(&client->dev, "Could not register video device\n");
@@ -460,8 +431,6 @@ err_all:
 	free_irq(client->irq, radio);
 err_rds:
 	kfree(radio->buffer);
-err_video:
-	video_device_release(radio->videodev);
 err_radio:
 	kfree(radio);
 err_initial:
@@ -477,7 +446,7 @@ static __devexit int si470x_i2c_remove(struct i2c_client *client)
 	struct si470x_device *radio = i2c_get_clientdata(client);
 
 	free_irq(client->irq, radio);
-	video_unregister_device(radio->videodev);
+	video_unregister_device(&radio->videodev);
 	kfree(radio);
 
 	return 0;
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index b7debb67932a..e9f638761296 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -367,23 +367,6 @@ static int si470x_get_scratch_page_versions(struct si470x_device *radio)
 
 
 /**************************************************************************
- * General Driver Functions - DISCONNECT_CHECK
- **************************************************************************/
-
-/*
- * si470x_disconnect_check - check whether radio disconnects
- */
-int si470x_disconnect_check(struct si470x_device *radio)
-{
-	if (radio->disconnected)
-		return -EIO;
-	else
-		return 0;
-}
-
-
-
-/**************************************************************************
  * RDS Driver Functions
  **************************************************************************/
 
@@ -414,9 +397,6 @@ static void si470x_int_in_callback(struct urb *urb)
 		}
 	}
 
-	/* safety checks */
-	if (radio->disconnected)
-		return;
 	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
 		goto resubmit;
 
@@ -501,112 +481,30 @@ resubmit:
 }
 
 
-
-/**************************************************************************
- * File Operations Interface
- **************************************************************************/
-
-/*
- * si470x_fops_open - file open
- */
 int si470x_fops_open(struct file *file)
 {
-	struct si470x_device *radio = video_drvdata(file);
-	int retval;
-
-	mutex_lock(&radio->lock);
-	radio->users++;
-
-	retval = usb_autopm_get_interface(radio->intf);
-	if (retval < 0) {
-		radio->users--;
-		retval = -EIO;
-		goto done;
-	}
-
-	if (radio->users == 1) {
-		/* start radio */
-		retval = si470x_start(radio);
-		if (retval < 0) {
-			usb_autopm_put_interface(radio->intf);
-			goto done;
-		}
-
-		/* initialize interrupt urb */
-		usb_fill_int_urb(radio->int_in_urb, radio->usbdev,
-			usb_rcvintpipe(radio->usbdev,
-			radio->int_in_endpoint->bEndpointAddress),
-			radio->int_in_buffer,
-			le16_to_cpu(radio->int_in_endpoint->wMaxPacketSize),
-			si470x_int_in_callback,
-			radio,
-			radio->int_in_endpoint->bInterval);
-
-		radio->int_in_running = 1;
-		mb();
-
-		retval = usb_submit_urb(radio->int_in_urb, GFP_KERNEL);
-		if (retval) {
-			dev_info(&radio->intf->dev,
-				 "submitting int urb failed (%d)\n", retval);
-			radio->int_in_running = 0;
-			usb_autopm_put_interface(radio->intf);
-		}
-	}
-
-done:
-	mutex_unlock(&radio->lock);
-	return retval;
+	return v4l2_fh_open(file);
 }
 
-
-/*
- * si470x_fops_release - file release
- */
 int si470x_fops_release(struct file *file)
 {
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	/* safety check */
-	if (!radio) {
-		retval = -ENODEV;
-		goto done;
-	}
-
-	mutex_lock(&radio->lock);
-	radio->users--;
-	if (radio->users == 0) {
-		/* shutdown interrupt handler */
-		if (radio->int_in_running) {
-			radio->int_in_running = 0;
-		if (radio->int_in_urb)
-			usb_kill_urb(radio->int_in_urb);
-		}
-
-		if (radio->disconnected) {
-			video_unregister_device(radio->videodev);
-			kfree(radio->int_in_buffer);
-			kfree(radio->buffer);
-			mutex_unlock(&radio->lock);
-			kfree(radio);
-			goto done;
-		}
+	return v4l2_fh_release(file);
+}
 
-		/* cancel read processes */
-		wake_up_interruptible(&radio->read_queue);
+static void si470x_usb_release(struct v4l2_device *v4l2_dev)
+{
+	struct si470x_device *radio =
+		container_of(v4l2_dev, struct si470x_device, v4l2_dev);
 
-		/* stop radio */
-		retval = si470x_stop(radio);
-		usb_autopm_put_interface(radio->intf);
-	}
-	mutex_unlock(&radio->lock);
-done:
-	return retval;
+	usb_free_urb(radio->int_in_urb);
+	v4l2_ctrl_handler_free(&radio->hdl);
+	v4l2_device_unregister(&radio->v4l2_dev);
+	kfree(radio->int_in_buffer);
+	kfree(radio->buffer);
+	kfree(radio);
 }
 
 
-
 /**************************************************************************
  * Video4Linux Interface
  **************************************************************************/
@@ -623,13 +521,45 @@ int si470x_vidioc_querycap(struct file *file, void *priv,
 	strlcpy(capability->card, DRIVER_CARD, sizeof(capability->card));
 	usb_make_path(radio->usbdev, capability->bus_info,
 			sizeof(capability->bus_info));
-	capability->capabilities = V4L2_CAP_HW_FREQ_SEEK |
+	capability->device_caps = V4L2_CAP_HW_FREQ_SEEK |
 		V4L2_CAP_TUNER | V4L2_CAP_RADIO | V4L2_CAP_RDS_CAPTURE;
-
+	capability->capabilities = capability->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
 
+static int si470x_start_usb(struct si470x_device *radio)
+{
+	int retval;
+
+	/* start radio */
+	retval = si470x_start(radio);
+	if (retval < 0)
+		return retval;
+
+	v4l2_ctrl_handler_setup(&radio->hdl);
+
+	/* initialize interrupt urb */
+	usb_fill_int_urb(radio->int_in_urb, radio->usbdev,
+			usb_rcvintpipe(radio->usbdev,
+				radio->int_in_endpoint->bEndpointAddress),
+			radio->int_in_buffer,
+			le16_to_cpu(radio->int_in_endpoint->wMaxPacketSize),
+			si470x_int_in_callback,
+			radio,
+			radio->int_in_endpoint->bInterval);
+
+	radio->int_in_running = 1;
+	mb();
+
+	retval = usb_submit_urb(radio->int_in_urb, GFP_KERNEL);
+	if (retval) {
+		dev_info(&radio->intf->dev,
+				"submitting int urb failed (%d)\n", retval);
+		radio->int_in_running = 0;
+	}
+	return retval;
+}
 
 /**************************************************************************
  * USB Interface
@@ -653,8 +583,6 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 		retval = -ENOMEM;
 		goto err_initial;
 	}
-	radio->users = 0;
-	radio->disconnected = 0;
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->intf = intf;
 	mutex_init(&radio->lock);
@@ -691,20 +619,35 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 		goto err_intbuffer;
 	}
 
-	/* video device allocation and initialization */
-	radio->videodev = video_device_alloc();
-	if (!radio->videodev) {
-		retval = -ENOMEM;
+	radio->v4l2_dev.release = si470x_usb_release;
+	retval = v4l2_device_register(&intf->dev, &radio->v4l2_dev);
+	if (retval < 0) {
+		dev_err(&intf->dev, "couldn't register v4l2_device\n");
 		goto err_urb;
 	}
-	memcpy(radio->videodev, &si470x_viddev_template,
-			sizeof(si470x_viddev_template));
-	video_set_drvdata(radio->videodev, radio);
+
+	v4l2_ctrl_handler_init(&radio->hdl, 2);
+	v4l2_ctrl_new_std(&radio->hdl, &si470x_ctrl_ops,
+			  V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(&radio->hdl, &si470x_ctrl_ops,
+			  V4L2_CID_AUDIO_VOLUME, 0, 15, 1, 15);
+	if (radio->hdl.error) {
+		retval = radio->hdl.error;
+		dev_err(&intf->dev, "couldn't register control\n");
+		goto err_dev;
+	}
+	radio->videodev = si470x_viddev_template;
+	radio->videodev.ctrl_handler = &radio->hdl;
+	radio->videodev.lock = &radio->lock;
+	radio->videodev.v4l2_dev = &radio->v4l2_dev;
+	radio->videodev.release = video_device_release_empty;
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->videodev.flags);
+	video_set_drvdata(&radio->videodev, radio);
 
 	/* get device and chip versions */
 	if (si470x_get_all_registers(radio) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_ctrl;
 	}
 	dev_info(&intf->dev, "DeviceID=0x%4.4hx ChipID=0x%4.4hx\n",
 			radio->registers[DEVICEID], radio->registers[CHIPID]);
@@ -721,7 +664,7 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	/* get software and hardware versions */
 	if (si470x_get_scratch_page_versions(radio) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_ctrl;
 	}
 	dev_info(&intf->dev, "software version %d, hardware version %d\n",
 			radio->software_version, radio->hardware_version);
@@ -764,28 +707,35 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	radio->buffer = kmalloc(radio->buf_size, GFP_KERNEL);
 	if (!radio->buffer) {
 		retval = -EIO;
-		goto err_video;
+		goto err_ctrl;
 	}
 
 	/* rds buffer configuration */
 	radio->wr_index = 0;
 	radio->rd_index = 0;
 	init_waitqueue_head(&radio->read_queue);
+	usb_set_intfdata(intf, radio);
+
+	/* start radio */
+	retval = si470x_start_usb(radio);
+	if (retval < 0)
+		goto err_all;
 
 	/* register video device */
-	retval = video_register_device(radio->videodev, VFL_TYPE_RADIO,
+	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO,
 			radio_nr);
 	if (retval) {
-		dev_warn(&intf->dev, "Could not register video device\n");
+		dev_err(&intf->dev, "Could not register video device\n");
 		goto err_all;
 	}
-	usb_set_intfdata(intf, radio);
 
 	return 0;
 err_all:
 	kfree(radio->buffer);
-err_video:
-	video_device_release(radio->videodev);
+err_ctrl:
+	v4l2_ctrl_handler_free(&radio->hdl);
+err_dev:
+	v4l2_device_unregister(&radio->v4l2_dev);
 err_urb:
 	usb_free_urb(radio->int_in_urb);
 err_intbuffer:
@@ -803,8 +753,22 @@ err_initial:
 static int si470x_usb_driver_suspend(struct usb_interface *intf,
 		pm_message_t message)
 {
+	struct si470x_device *radio = usb_get_intfdata(intf);
+
 	dev_info(&intf->dev, "suspending now...\n");
 
+	/* shutdown interrupt handler */
+	if (radio->int_in_running) {
+		radio->int_in_running = 0;
+		if (radio->int_in_urb)
+			usb_kill_urb(radio->int_in_urb);
+	}
+
+	/* cancel read processes */
+	wake_up_interruptible(&radio->read_queue);
+
+	/* stop radio */
+	si470x_stop(radio);
 	return 0;
 }
 
@@ -814,9 +778,12 @@ static int si470x_usb_driver_suspend(struct usb_interface *intf,
  */
 static int si470x_usb_driver_resume(struct usb_interface *intf)
 {
+	struct si470x_device *radio = usb_get_intfdata(intf);
+
 	dev_info(&intf->dev, "resuming now...\n");
 
-	return 0;
+	/* start radio */
+	return si470x_start_usb(radio);
 }
 
 
@@ -828,28 +795,22 @@ static void si470x_usb_driver_disconnect(struct usb_interface *intf)
 	struct si470x_device *radio = usb_get_intfdata(intf);
 
 	mutex_lock(&radio->lock);
-	radio->disconnected = 1;
+	v4l2_device_disconnect(&radio->v4l2_dev);
+	video_unregister_device(&radio->videodev);
 	usb_set_intfdata(intf, NULL);
-	if (radio->users == 0) {
-		/* set led to disconnect state */
-		si470x_set_led_state(radio, BLINK_ORANGE_LED);
-
-		/* Free data structures. */
-		usb_free_urb(radio->int_in_urb);
-
-		kfree(radio->int_in_buffer);
-		video_unregister_device(radio->videodev);
-		kfree(radio->buffer);
-		mutex_unlock(&radio->lock);
-		kfree(radio);
-	} else {
-		mutex_unlock(&radio->lock);
-	}
+	mutex_unlock(&radio->lock);
+	v4l2_device_put(&radio->v4l2_dev);
 }
 
 
 /*
  * si470x_usb_driver - usb driver interface
+ *
+ * A note on suspend/resume: this driver had only empty suspend/resume
+ * functions, and when I tried to test suspend/resume it always disconnected
+ * instead of resuming (using my ADS InstantFM stick). So I've decided to
+ * remove these callbacks until someone else with better hardware can
+ * implement and test this.
  */
 static struct usb_driver si470x_usb_driver = {
 	.name			= DRIVER_NAME,
@@ -857,8 +818,8 @@ static struct usb_driver si470x_usb_driver = {
 	.disconnect		= si470x_usb_driver_disconnect,
 	.suspend		= si470x_usb_driver_suspend,
 	.resume			= si470x_usb_driver_resume,
+	.reset_resume		= si470x_usb_driver_resume,
 	.id_table		= si470x_usb_driver_id_table,
-	.supports_autosuspend	= 1,
 };
 
 module_usb_driver(si470x_usb_driver);
diff --git a/drivers/media/radio/si470x/radio-si470x.h b/drivers/media/radio/si470x/radio-si470x.h
index f300a55ed85c..4921cab8e0fa 100644
--- a/drivers/media/radio/si470x/radio-si470x.h
+++ b/drivers/media/radio/si470x/radio-si470x.h
@@ -36,6 +36,9 @@
 #include <linux/mutex.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-device.h>
 #include <asm/unaligned.h>
 
 
@@ -141,10 +144,9 @@
  * si470x_device - private data
  */
 struct si470x_device {
-	struct video_device *videodev;
-
-	/* driver management */
-	unsigned int users;
+	struct v4l2_device v4l2_dev;
+	struct video_device videodev;
+	struct v4l2_ctrl_handler hdl;
 
 	/* Silabs internal registers (0..15) */
 	unsigned short registers[RADIO_REGISTER_NUM];
@@ -174,9 +176,6 @@ struct si470x_device {
 	/* scratch page */
 	unsigned char software_version;
 	unsigned char hardware_version;
-
-	/* driver management */
-	unsigned char disconnected;
 #endif
 
 #if defined(CONFIG_I2C_SI470X) || defined(CONFIG_I2C_SI470X_MODULE)
@@ -213,6 +212,7 @@ struct si470x_device {
  * Common Functions
  **************************************************************************/
 extern struct video_device si470x_viddev_template;
+extern const struct v4l2_ctrl_ops si470x_ctrl_ops;
 int si470x_get_register(struct si470x_device *radio, int regnr);
 int si470x_set_register(struct si470x_device *radio, int regnr);
 int si470x_disconnect_check(struct si470x_device *radio);
diff --git a/drivers/media/radio/tef6862.c b/drivers/media/radio/tef6862.c
index 6418c4c9faf1..06d47e5cce9f 100644
--- a/drivers/media/radio/tef6862.c
+++ b/drivers/media/radio/tef6862.c
@@ -211,7 +211,7 @@ static struct i2c_driver tef6862_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= tef6862_probe,
-	.remove		= tef6862_remove,
+	.remove		= __devexit_p(tef6862_remove),
 	.id_table	= tef6862_id,
 };
 
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.c b/drivers/media/radio/wl128x/fmdrv_v4l2.c
index 077d369a0173..080b96a61f1a 100644
--- a/drivers/media/radio/wl128x/fmdrv_v4l2.c
+++ b/drivers/media/radio/wl128x/fmdrv_v4l2.c
@@ -518,6 +518,10 @@ int fm_v4l2_init_video_device(struct fmdev *fmdev, int radio_nr)
 	video_set_drvdata(gradio_dev, fmdev);
 
 	gradio_dev->lock = &fmdev->mutex;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &gradio_dev->flags);
 
 	/* Register with V4L2 subsystem as RADIO device */
 	if (video_register_device(gradio_dev, VFL_TYPE_RADIO, radio_nr)) {
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index a3fbb21350e9..f97eeb870455 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -69,6 +69,7 @@ config IR_JVC_DECODER
 config IR_SONY_DECODER
 	tristate "Enable IR raw decoder for the Sony protocol"
 	depends on RC_CORE
+	select BITREVERSE
 	default y
 
 	---help---
diff --git a/drivers/media/rc/ati_remote.c b/drivers/media/rc/ati_remote.c
index baf907b3ce76..7be377fc1be8 100644
--- a/drivers/media/rc/ati_remote.c
+++ b/drivers/media/rc/ati_remote.c
@@ -1,7 +1,7 @@
 /*
  *  USB ATI Remote support
  *
- *                Copyright (c) 2011 Anssi Hannula <anssi.hannula@iki.fi>
+ *                Copyright (c) 2011, 2012 Anssi Hannula <anssi.hannula@iki.fi>
  *  Version 2.2.0 Copyright (c) 2004 Torrey Hoffman <thoffman@arnor.net>
  *  Version 2.1.1 Copyright (c) 2002 Vladimir Dergachev
  *
@@ -151,13 +151,57 @@ MODULE_PARM_DESC(mouse, "Enable mouse device, default = yes");
 #undef err
 #define err(format, arg...) printk(KERN_ERR format , ## arg)
 
+struct ati_receiver_type {
+	/* either default_keymap or get_default_keymap should be set */
+	const char *default_keymap;
+	const char *(*get_default_keymap)(struct usb_interface *interface);
+};
+
+static const char *get_medion_keymap(struct usb_interface *interface)
+{
+	struct usb_device *udev = interface_to_usbdev(interface);
+
+	/*
+	 * There are many different Medion remotes shipped with a receiver
+	 * with the same usb id, but the receivers have subtle differences
+	 * in the USB descriptors allowing us to detect them.
+	 */
+
+	if (udev->manufacturer && udev->product) {
+		if (udev->actconfig->desc.bmAttributes & USB_CONFIG_ATT_WAKEUP) {
+
+			if (!strcmp(udev->manufacturer, "X10 Wireless Technology Inc")
+			    && !strcmp(udev->product, "USB Receiver"))
+				return RC_MAP_MEDION_X10_DIGITAINER;
+
+			if (!strcmp(udev->manufacturer, "X10 WTI")
+			    && !strcmp(udev->product, "RF receiver"))
+				return RC_MAP_MEDION_X10_OR2X;
+		} else {
+
+			 if (!strcmp(udev->manufacturer, "X10 Wireless Technology Inc")
+			    && !strcmp(udev->product, "USB Receiver"))
+				return RC_MAP_MEDION_X10;
+		}
+	}
+
+	dev_info(&interface->dev,
+		 "Unknown Medion X10 receiver, using default ati_remote Medion keymap\n");
+
+	return RC_MAP_MEDION_X10;
+}
+
+static const struct ati_receiver_type type_ati		= { .default_keymap = RC_MAP_ATI_X10 };
+static const struct ati_receiver_type type_medion	= { .get_default_keymap = get_medion_keymap };
+static const struct ati_receiver_type type_firefly	= { .default_keymap = RC_MAP_SNAPSTREAM_FIREFLY };
+
 static struct usb_device_id ati_remote_table[] = {
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA2_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, ATI_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, NVIDIA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, MEDION_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_MEDION_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, FIREFLY_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_SNAPSTREAM_FIREFLY },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA2_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, ATI_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, NVIDIA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, MEDION_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_medion },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, FIREFLY_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_firefly },
 	{}	/* Terminating entry */
 };
 
@@ -445,6 +489,7 @@ static void ati_remote_input_report(struct urb *urb)
 	int acc;
 	int remote_num;
 	unsigned char scancode;
+	u32 wheel_keycode = KEY_RESERVED;
 	int i;
 
 	/*
@@ -484,26 +529,33 @@ static void ati_remote_input_report(struct urb *urb)
 	 */
 	scancode = data[2] & 0x7f;
 
-	/* Look up event code index in the mouse translation table. */
-	for (i = 0; ati_remote_tbl[i].kind != KIND_END; i++) {
-		if (scancode == ati_remote_tbl[i].data) {
-			index = i;
-			break;
+	dbginfo(&ati_remote->interface->dev,
+		"channel 0x%02x; key data %02x, scancode %02x\n",
+		remote_num, data[2], scancode);
+
+	if (scancode >= 0x70) {
+		/*
+		 * This is either a mouse or scrollwheel event, depending on
+		 * the remote/keymap.
+		 * Get the keycode assigned to scancode 0x78/0x70. If it is
+		 * set, assume this is a scrollwheel up/down event.
+		 */
+		wheel_keycode = rc_g_keycode_from_table(ati_remote->rdev,
+							scancode & 0x78);
+
+		if (wheel_keycode == KEY_RESERVED) {
+			/* scrollwheel was not mapped, assume mouse */
+
+			/* Look up event code index in the mouse translation table. */
+			for (i = 0; ati_remote_tbl[i].kind != KIND_END; i++) {
+				if (scancode == ati_remote_tbl[i].data) {
+					index = i;
+					break;
+				}
+			}
 		}
 	}
 
-	if (index >= 0) {
-		dbginfo(&ati_remote->interface->dev,
-			"channel 0x%02x; mouse data %02x; index %d; keycode %d\n",
-			remote_num, data[2], index, ati_remote_tbl[index].code);
-		if (!dev)
-			return; /* no mouse device */
-	} else
-		dbginfo(&ati_remote->interface->dev,
-			"channel 0x%02x; key data %02x, scancode %02x\n",
-			remote_num, data[2], scancode);
-
-
 	if (index >= 0 && ati_remote_tbl[index].kind == KIND_LITERAL) {
 		input_event(dev, ati_remote_tbl[index].type,
 			ati_remote_tbl[index].code,
@@ -542,15 +594,29 @@ static void ati_remote_input_report(struct urb *urb)
 
 		if (index < 0) {
 			/* Not a mouse event, hand it to rc-core. */
-
-			/*
-			 * We don't use the rc-core repeat handling yet as
-			 * it would cause ghost repeats which would be a
-			 * regression for this driver.
-			 */
-			rc_keydown_notimeout(ati_remote->rdev, scancode,
-					     data[2]);
-			rc_keyup(ati_remote->rdev);
+			int count = 1;
+
+			if (wheel_keycode != KEY_RESERVED) {
+				/*
+				 * This is a scrollwheel event, send the
+				 * scroll up (0x78) / down (0x70) scancode
+				 * repeatedly as many times as indicated by
+				 * rest of the scancode.
+				 */
+				count = (scancode & 0x07) + 1;
+				scancode &= 0x78;
+			}
+
+			while (count--) {
+				/*
+				* We don't use the rc-core repeat handling yet as
+				* it would cause ghost repeats which would be a
+				* regression for this driver.
+				*/
+				rc_keydown_notimeout(ati_remote->rdev, scancode,
+						     data[2]);
+				rc_keyup(ati_remote->rdev);
+			}
 			return;
 		}
 
@@ -766,6 +832,7 @@ static int ati_remote_probe(struct usb_interface *interface, const struct usb_de
 	struct usb_device *udev = interface_to_usbdev(interface);
 	struct usb_host_interface *iface_host = interface->cur_altsetting;
 	struct usb_endpoint_descriptor *endpoint_in, *endpoint_out;
+	struct ati_receiver_type *type = (struct ati_receiver_type *)id->driver_info;
 	struct ati_remote *ati_remote;
 	struct input_dev *input_dev;
 	struct rc_dev *rc_dev;
@@ -827,10 +894,15 @@ static int ati_remote_probe(struct usb_interface *interface, const struct usb_de
 	snprintf(ati_remote->mouse_name, sizeof(ati_remote->mouse_name),
 		 "%s mouse", ati_remote->rc_name);
 
-	if (id->driver_info)
-		rc_dev->map_name = (const char *)id->driver_info;
-	else
-		rc_dev->map_name = RC_MAP_ATI_X10;
+	rc_dev->map_name = RC_MAP_ATI_X10; /* default map */
+
+	/* set default keymap according to receiver model */
+	if (type) {
+		if (type->default_keymap)
+			rc_dev->map_name = type->default_keymap;
+		else if (type->get_default_keymap)
+			rc_dev->map_name = type->get_default_keymap(interface);
+	}
 
 	ati_remote_rc_init(ati_remote);
 	mutex_init(&ati_remote->open_mutex);
diff --git a/drivers/media/rc/fintek-cir.c b/drivers/media/rc/fintek-cir.c
index 4a3a238bcfbc..6aabf7ae3a31 100644
--- a/drivers/media/rc/fintek-cir.c
+++ b/drivers/media/rc/fintek-cir.c
@@ -556,11 +556,11 @@ static int fintek_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id
 
 	if (request_irq(fintek->cir_irq, fintek_cir_isr, IRQF_SHARED,
 			FINTEK_DRIVER_NAME, (void *)fintek))
-		goto failure;
+		goto failure2;
 
 	ret = rc_register_device(rdev);
 	if (ret)
-		goto failure;
+		goto failure3;
 
 	device_init_wakeup(&pdev->dev, true);
 	fintek->rdev = rdev;
@@ -570,12 +570,11 @@ static int fintek_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id
 
 	return 0;
 
+failure3:
+	free_irq(fintek->cir_irq, fintek);
+failure2:
+	release_region(fintek->cir_addr, fintek->cir_port_len);
 failure:
-	if (fintek->cir_irq)
-		free_irq(fintek->cir_irq, fintek);
-	if (fintek->cir_addr)
-		release_region(fintek->cir_addr, fintek->cir_port_len);
-
 	rc_free_device(rdev);
 	kfree(fintek);
 
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index 7f26fdf2e54e..5dd0386604f0 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -255,7 +255,7 @@ static struct usb_device_id imon_usb_id_table[] = {
 static struct usb_driver imon_driver = {
 	.name		= MOD_NAME,
 	.probe		= imon_probe,
-	.disconnect	= imon_disconnect,
+	.disconnect	= __devexit_p(imon_disconnect),
 	.suspend	= imon_suspend,
 	.resume		= imon_resume,
 	.id_table	= imon_usb_id_table,
diff --git a/drivers/media/rc/ir-raw.c b/drivers/media/rc/ir-raw.c
index 95e630998aaf..a82025121345 100644
--- a/drivers/media/rc/ir-raw.c
+++ b/drivers/media/rc/ir-raw.c
@@ -46,9 +46,9 @@ static int ir_raw_event_thread(void *data)
 	while (!kthread_should_stop()) {
 
 		spin_lock_irq(&raw->lock);
-		retval = kfifo_out(&raw->kfifo, &ev, sizeof(ev));
+		retval = kfifo_len(&raw->kfifo);
 
-		if (!retval) {
+		if (retval < sizeof(ev)) {
 			set_current_state(TASK_INTERRUPTIBLE);
 
 			if (kthread_should_stop())
@@ -59,11 +59,9 @@ static int ir_raw_event_thread(void *data)
 			continue;
 		}
 
+		retval = kfifo_out(&raw->kfifo, &ev, sizeof(ev));
 		spin_unlock_irq(&raw->lock);
 
-
-		BUG_ON(retval != sizeof(ev));
-
 		mutex_lock(&ir_raw_handler_lock);
 		list_for_each_entry(handler, &ir_raw_handler_list, list)
 			handler->decode(raw->dev, ev);
diff --git a/drivers/media/rc/ir-sanyo-decoder.c b/drivers/media/rc/ir-sanyo-decoder.c
index d38fbdd0b25a..7e54ec57bcf9 100644
--- a/drivers/media/rc/ir-sanyo-decoder.c
+++ b/drivers/media/rc/ir-sanyo-decoder.c
@@ -56,7 +56,7 @@ static int ir_sanyo_decode(struct rc_dev *dev, struct ir_raw_event ev)
 {
 	struct sanyo_dec *data = &dev->raw->sanyo;
 	u32 scancode;
-	u8 address, not_address, command, not_command;
+	u8 address, command, not_command;
 
 	if (!(dev->raw->enabled_protocols & RC_TYPE_SANYO))
 		return 0;
@@ -154,7 +154,7 @@ static int ir_sanyo_decode(struct rc_dev *dev, struct ir_raw_event ev)
 			break;
 
 		address     = bitrev16((data->bits >> 29) & 0x1fff) >> 3;
-		not_address = bitrev16((data->bits >> 16) & 0x1fff) >> 3;
+		/* not_address = bitrev16((data->bits >> 16) & 0x1fff) >> 3; */
 		command	    = bitrev8((data->bits >>  8) & 0xff);
 		not_command = bitrev8((data->bits >>  0) & 0xff);
 
diff --git a/drivers/media/rc/ite-cir.c b/drivers/media/rc/ite-cir.c
index 0e49c99abf68..36fe5a349b95 100644
--- a/drivers/media/rc/ite-cir.c
+++ b/drivers/media/rc/ite-cir.c
@@ -1598,24 +1598,22 @@ static int ite_probe(struct pnp_dev *pdev, const struct pnp_device_id
 
 	if (request_irq(itdev->cir_irq, ite_cir_isr, IRQF_SHARED,
 			ITE_DRIVER_NAME, (void *)itdev))
-		goto failure;
+		goto failure2;
 
 	ret = rc_register_device(rdev);
 	if (ret)
-		goto failure;
+		goto failure3;
 
 	itdev->rdev = rdev;
 	ite_pr(KERN_NOTICE, "driver has been successfully loaded\n");
 
 	return 0;
 
+failure3:
+	free_irq(itdev->cir_irq, itdev);
+failure2:
+	release_region(itdev->cir_addr, itdev->params.io_region_size);
 failure:
-	if (itdev->cir_irq)
-		free_irq(itdev->cir_irq, itdev);
-
-	if (itdev->cir_addr)
-		release_region(itdev->cir_addr, itdev->params.io_region_size);
-
 	rc_free_device(rdev);
 	kfree(itdev);
 
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index 49ce2662f56b..ab84d66c67c1 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-anysee.o \
 			rc-apac-viewcomp.o \
 			rc-asus-pc39.o \
+			rc-asus-ps3-100.o \
 			rc-ati-tv-wonder-hd-600.o \
 			rc-ati-x10.o \
 			rc-avermedia-a16d.o \
@@ -52,6 +53,8 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-lme2510.o \
 			rc-manli.o \
 			rc-medion-x10.o \
+			rc-medion-x10-digitainer.o \
+			rc-medion-x10-or2x.o \
 			rc-msi-digivox-ii.o \
 			rc-msi-digivox-iii.o \
 			rc-msi-tvanywhere.o \
diff --git a/drivers/media/rc/keymaps/rc-asus-ps3-100.c b/drivers/media/rc/keymaps/rc-asus-ps3-100.c
new file mode 100644
index 000000000000..ba76609c5936
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-asus-ps3-100.c
@@ -0,0 +1,91 @@
+/* asus-ps3-100.h - Keytable for asus_ps3_100 Remote Controller
+ *
+ * Copyright (c) 2012 by Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * Based on a previous patch from Remi Schwartz <remi.schwartz@gmail.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 <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table asus_ps3_100[] = {
+	{ 0x081c, KEY_HOME },             /* home */
+	{ 0x081e, KEY_TV },               /* tv */
+	{ 0x0803, KEY_TEXT },             /* teletext */
+	{ 0x0829, KEY_POWER },            /* close */
+
+	{ 0x080b, KEY_RED },              /* red */
+	{ 0x080d, KEY_YELLOW },           /* yellow */
+	{ 0x0806, KEY_BLUE },             /* blue */
+	{ 0x0807, KEY_GREEN },            /* green */
+
+	/* Keys 0 to 9 */
+	{ 0x082a, KEY_0 },
+	{ 0x0816, KEY_1 },
+	{ 0x0812, KEY_2 },
+	{ 0x0814, KEY_3 },
+	{ 0x0836, KEY_4 },
+	{ 0x0832, KEY_5 },
+	{ 0x0834, KEY_6 },
+	{ 0x080e, KEY_7 },
+	{ 0x080a, KEY_8 },
+	{ 0x080c, KEY_9 },
+
+	{ 0x0815, KEY_VOLUMEUP },
+	{ 0x0826, KEY_VOLUMEDOWN },
+	{ 0x0835, KEY_CHANNELUP },        /* channel / program + */
+	{ 0x0824, KEY_CHANNELDOWN },      /* channel / program - */
+
+	{ 0x0808, KEY_UP },
+	{ 0x0804, KEY_DOWN },
+	{ 0x0818, KEY_LEFT },
+	{ 0x0810, KEY_RIGHT },
+	{ 0x0825, KEY_ENTER },            /* enter */
+
+	{ 0x0822, KEY_EXIT },             /* back */
+	{ 0x082c, KEY_AB },               /* recall */
+
+	{ 0x0820, KEY_AUDIO },            /* TV audio */
+	{ 0x0837, KEY_SCREEN },           /* snapshot */
+	{ 0x082e, KEY_ZOOM },             /* full screen */
+	{ 0x0802, KEY_MUTE },             /* mute */
+
+	{ 0x0831, KEY_REWIND },           /* backward << */
+	{ 0x0811, KEY_RECORD },           /* recording */
+	{ 0x0809, KEY_STOP },
+	{ 0x0805, KEY_FASTFORWARD },      /* forward >> */
+	{ 0x0821, KEY_PREVIOUS },         /* rew */
+	{ 0x081a, KEY_PAUSE },            /* pause */
+	{ 0x0839, KEY_PLAY },             /* play */
+	{ 0x0819, KEY_NEXT },             /* forward */
+};
+
+static struct rc_map_list asus_ps3_100_map = {
+.map = {
+	.scan    = asus_ps3_100,
+	.size    = ARRAY_SIZE(asus_ps3_100),
+	.rc_type = RC_TYPE_RC5,
+	.name    = RC_MAP_ASUS_PS3_100,
+}
+};
+
+static int __init init_rc_map_asus_ps3_100(void)
+{
+return rc_map_register(&asus_ps3_100_map);
+}
+
+static void __exit exit_rc_map_asus_ps3_100(void)
+{
+rc_map_unregister(&asus_ps3_100_map);
+}
+
+module_init(init_rc_map_asus_ps3_100)
+module_exit(exit_rc_map_asus_ps3_100)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
diff --git a/drivers/media/rc/keymaps/rc-it913x-v2.c b/drivers/media/rc/keymaps/rc-it913x-v2.c
index 28e376e18b99..bd42a30ec06f 100644
--- a/drivers/media/rc/keymaps/rc-it913x-v2.c
+++ b/drivers/media/rc/keymaps/rc-it913x-v2.c
@@ -40,7 +40,7 @@ static struct rc_map_table it913x_v2_rc[] = {
 	/* Type 2 */
 	/* keys stereo, snapshot unassigned */
 	{ 0x866b00, KEY_0 },
-	{ 0x866b1b, KEY_1 },
+	{ 0x866b01, KEY_1 },
 	{ 0x866b02, KEY_2 },
 	{ 0x866b03, KEY_3 },
 	{ 0x866b04, KEY_4 },
diff --git a/drivers/media/rc/keymaps/rc-medion-x10-digitainer.c b/drivers/media/rc/keymaps/rc-medion-x10-digitainer.c
new file mode 100644
index 000000000000..966f9b3c71da
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-medion-x10-digitainer.c
@@ -0,0 +1,123 @@
+/*
+ * Medion X10 RF remote keytable (Digitainer variant)
+ *
+ * Copyright (C) 2012 Anssi Hannula <anssi.hannula@iki.fi>
+ *
+ * This keymap is for a variant that has a distinctive scrollwheel instead of
+ * up/down buttons (tested with P/N 40009936 / 20018268), reportedly
+ * originally shipped with Medion Digitainer but now sold separately simply as
+ * an "X10" remote.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/module.h>
+#include <media/rc-map.h>
+
+static struct rc_map_table medion_x10_digitainer[] = {
+	{ 0x02, KEY_POWER },
+
+	{ 0x2c, KEY_TV },
+	{ 0x2d, KEY_VIDEO },
+	{ 0x04, KEY_DVD },    /* CD/DVD */
+	{ 0x16, KEY_TEXT },   /* "teletext" icon, i.e. a screen with lines */
+	{ 0x06, KEY_AUDIO },
+	{ 0x2e, KEY_RADIO },
+	{ 0x31, KEY_EPG },    /* a screen with an open book */
+	{ 0x05, KEY_IMAGES }, /* Photo */
+	{ 0x2f, KEY_INFO },
+
+	{ 0x78, KEY_UP },     /* scrollwheel up 1 notch */
+	/* 0x79..0x7f: 2-8 notches, driver repeats 0x78 entry */
+
+	{ 0x70, KEY_DOWN },   /* scrollwheel down 1 notch */
+	/* 0x71..0x77: 2-8 notches, driver repeats 0x70 entry */
+
+	{ 0x19, KEY_MENU },
+	{ 0x1d, KEY_LEFT },
+	{ 0x1e, KEY_OK },     /* scrollwheel press */
+	{ 0x1f, KEY_RIGHT },
+	{ 0x20, KEY_BACK },
+
+	{ 0x09, KEY_VOLUMEUP },
+	{ 0x08, KEY_VOLUMEDOWN },
+	{ 0x00, KEY_MUTE },
+
+	{ 0x1b, KEY_SELECT }, /* also has "U" rotated 90 degrees CCW */
+
+	{ 0x0b, KEY_CHANNELUP },
+	{ 0x0c, KEY_CHANNELDOWN },
+	{ 0x1c, KEY_LAST },
+
+	{ 0x32, KEY_RED },    /* also Audio */
+	{ 0x33, KEY_GREEN },  /* also Subtitle */
+	{ 0x34, KEY_YELLOW }, /* also Angle */
+	{ 0x35, KEY_BLUE },   /* also Title */
+
+	{ 0x28, KEY_STOP },
+	{ 0x29, KEY_PAUSE },
+	{ 0x25, KEY_PLAY },
+	{ 0x21, KEY_PREVIOUS },
+	{ 0x18, KEY_CAMERA },
+	{ 0x23, KEY_NEXT },
+	{ 0x24, KEY_REWIND },
+	{ 0x27, KEY_RECORD },
+	{ 0x26, KEY_FORWARD },
+
+	{ 0x0d, KEY_1 },
+	{ 0x0e, KEY_2 },
+	{ 0x0f, KEY_3 },
+	{ 0x10, KEY_4 },
+	{ 0x11, KEY_5 },
+	{ 0x12, KEY_6 },
+	{ 0x13, KEY_7 },
+	{ 0x14, KEY_8 },
+	{ 0x15, KEY_9 },
+	{ 0x17, KEY_0 },
+
+	/* these do not actually exist on this remote, but these scancodes
+	 * exist on all other Medion X10 remotes and adding them here allows
+	 * such remotes to be adequately usable with this keymap in case
+	 * this keymap is wrongly used with them (which is quite possible as
+	 * there are lots of different Medion X10 remotes): */
+	{ 0x1a, KEY_UP },
+	{ 0x22, KEY_DOWN },
+};
+
+static struct rc_map_list medion_x10_digitainer_map = {
+	.map = {
+		.scan    = medion_x10_digitainer,
+		.size    = ARRAY_SIZE(medion_x10_digitainer),
+		.rc_type = RC_TYPE_OTHER,
+		.name    = RC_MAP_MEDION_X10_DIGITAINER,
+	}
+};
+
+static int __init init_rc_map_medion_x10_digitainer(void)
+{
+	return rc_map_register(&medion_x10_digitainer_map);
+}
+
+static void __exit exit_rc_map_medion_x10_digitainer(void)
+{
+	rc_map_unregister(&medion_x10_digitainer_map);
+}
+
+module_init(init_rc_map_medion_x10_digitainer)
+module_exit(exit_rc_map_medion_x10_digitainer)
+
+MODULE_DESCRIPTION("Medion X10 RF remote keytable (Digitainer variant)");
+MODULE_AUTHOR("Anssi Hannula <anssi.hannula@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/keymaps/rc-medion-x10-or2x.c b/drivers/media/rc/keymaps/rc-medion-x10-or2x.c
new file mode 100644
index 000000000000..b077300ecb5c
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-medion-x10-or2x.c
@@ -0,0 +1,108 @@
+/*
+ * Medion X10 OR22/OR24 RF remote keytable
+ *
+ * Copyright (C) 2012 Anssi Hannula <anssi.hannula@iki.fi>
+ *
+ * This keymap is for several Medion X10 remotes that have the Windows MCE
+ * button. This has been tested with a "RF VISTA Remote Control", OR24V,
+ * P/N 20035335, but should work with other variants that have the same
+ * buttons, such as OR22V and OR24E.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/module.h>
+#include <media/rc-map.h>
+
+static struct rc_map_table medion_x10_or2x[] = {
+	{ 0x02, KEY_POWER },
+	{ 0x16, KEY_TEXT },   /* "T" in a box, for teletext */
+
+	{ 0x09, KEY_VOLUMEUP },
+	{ 0x08, KEY_VOLUMEDOWN },
+	{ 0x00, KEY_MUTE },
+	{ 0x0b, KEY_CHANNELUP },
+	{ 0x0c, KEY_CHANNELDOWN },
+
+	{ 0x32, KEY_RED },
+	{ 0x33, KEY_GREEN },
+	{ 0x34, KEY_YELLOW },
+	{ 0x35, KEY_BLUE },
+
+	{ 0x18, KEY_PVR },    /* record symbol inside a tv symbol */
+	{ 0x04, KEY_DVD },    /* disc symbol */
+	{ 0x31, KEY_EPG },    /* a tv schedule symbol */
+	{ 0x1c, KEY_TV },     /* play symbol inside a tv symbol */
+	{ 0x20, KEY_BACK },
+	{ 0x2f, KEY_INFO },
+
+	{ 0x1a, KEY_UP },
+	{ 0x22, KEY_DOWN },
+	{ 0x1d, KEY_LEFT },
+	{ 0x1f, KEY_RIGHT },
+	{ 0x1e, KEY_OK },
+
+	{ 0x1b, KEY_MEDIA },  /* Windows MCE button */
+
+	{ 0x21, KEY_PREVIOUS },
+	{ 0x23, KEY_NEXT },
+	{ 0x24, KEY_REWIND },
+	{ 0x26, KEY_FORWARD },
+	{ 0x25, KEY_PLAY },
+	{ 0x28, KEY_STOP },
+	{ 0x29, KEY_PAUSE },
+	{ 0x27, KEY_RECORD },
+
+	{ 0x0d, KEY_1 },
+	{ 0x0e, KEY_2 },
+	{ 0x0f, KEY_3 },
+	{ 0x10, KEY_4 },
+	{ 0x11, KEY_5 },
+	{ 0x12, KEY_6 },
+	{ 0x13, KEY_7 },
+	{ 0x14, KEY_8 },
+	{ 0x15, KEY_9 },
+	{ 0x17, KEY_0 },
+	{ 0x30, KEY_CLEAR },
+	{ 0x36, KEY_ENTER },
+	{ 0x37, KEY_NUMERIC_STAR },
+	{ 0x38, KEY_NUMERIC_POUND },
+};
+
+static struct rc_map_list medion_x10_or2x_map = {
+	.map = {
+		.scan    = medion_x10_or2x,
+		.size    = ARRAY_SIZE(medion_x10_or2x),
+		.rc_type = RC_TYPE_OTHER,
+		.name    = RC_MAP_MEDION_X10_OR2X,
+	}
+};
+
+static int __init init_rc_map_medion_x10_or2x(void)
+{
+	return rc_map_register(&medion_x10_or2x_map);
+}
+
+static void __exit exit_rc_map_medion_x10_or2x(void)
+{
+	rc_map_unregister(&medion_x10_or2x_map);
+}
+
+module_init(init_rc_map_medion_x10_or2x)
+module_exit(exit_rc_map_medion_x10_or2x)
+
+MODULE_DESCRIPTION("Medion X10 OR22/OR24 RF remote keytable");
+MODULE_AUTHOR("Anssi Hannula <anssi.hannula@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index e150a2e29a4b..84e06d3aa696 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -520,7 +520,7 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
 {
 	char codes[USB_BUFLEN * 3 + 1];
 	char inout[9];
-	u8 cmd, subcmd, data1, data2, data3, data4, data5;
+	u8 cmd, subcmd, data1, data2, data3, data4;
 	struct device *dev = ir->dev;
 	int i, start, skip = 0;
 	u32 carrier, period;
@@ -553,7 +553,6 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
 	data2  = buf[start + 3] & 0xff;
 	data3  = buf[start + 4] & 0xff;
 	data4  = buf[start + 5] & 0xff;
-	data5  = buf[start + 6] & 0xff;
 
 	switch (cmd) {
 	case MCE_CMD_NULL:
@@ -1443,7 +1442,7 @@ static int mceusb_dev_resume(struct usb_interface *intf)
 static struct usb_driver mceusb_dev_driver = {
 	.name =		DRIVER_NAME,
 	.probe =	mceusb_dev_probe,
-	.disconnect =	mceusb_dev_disconnect,
+	.disconnect =	__devexit_p(mceusb_dev_disconnect),
 	.suspend =	mceusb_dev_suspend,
 	.resume =	mceusb_dev_resume,
 	.reset_resume =	mceusb_dev_resume,
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 8b2c071ac0ab..dc8a7dddccd4 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -1075,19 +1075,19 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 
 	if (request_irq(nvt->cir_irq, nvt_cir_isr, IRQF_SHARED,
 			NVT_DRIVER_NAME, (void *)nvt))
-		goto failure;
+		goto failure2;
 
 	if (!request_region(nvt->cir_wake_addr,
 			    CIR_IOREG_LENGTH, NVT_DRIVER_NAME))
-		goto failure;
+		goto failure3;
 
 	if (request_irq(nvt->cir_wake_irq, nvt_cir_wake_isr, IRQF_SHARED,
 			NVT_DRIVER_NAME, (void *)nvt))
-		goto failure;
+		goto failure4;
 
 	ret = rc_register_device(rdev);
 	if (ret)
-		goto failure;
+		goto failure5;
 
 	device_init_wakeup(&pdev->dev, true);
 	nvt->rdev = rdev;
@@ -1099,17 +1099,15 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 
 	return 0;
 
+failure5:
+	free_irq(nvt->cir_wake_irq, nvt);
+failure4:
+	release_region(nvt->cir_wake_addr, CIR_IOREG_LENGTH);
+failure3:
+	free_irq(nvt->cir_irq, nvt);
+failure2:
+	release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
 failure:
-	if (nvt->cir_irq)
-		free_irq(nvt->cir_irq, nvt);
-	if (nvt->cir_addr)
-		release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
-
-	if (nvt->cir_wake_irq)
-		free_irq(nvt->cir_wake_irq, nvt);
-	if (nvt->cir_wake_addr)
-		release_region(nvt->cir_wake_addr, CIR_IOREG_LENGTH);
-
 	rc_free_device(rdev);
 	kfree(nvt);
 
diff --git a/drivers/media/rc/rc-loopback.c b/drivers/media/rc/rc-loopback.c
index efc6a514348a..fae1615e0ff2 100644
--- a/drivers/media/rc/rc-loopback.c
+++ b/drivers/media/rc/rc-loopback.c
@@ -221,7 +221,6 @@ static int __init loop_init(void)
 	rc->s_idle		= loop_set_idle;
 	rc->s_learning_mode	= loop_set_learning_mode;
 	rc->s_carrier_report	= loop_set_carrier_report;
-	rc->priv		= &loopdev;
 
 	loopdev.txmask		= RXMASK_REGULAR;
 	loopdev.txcarrier	= 36000;
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index ad95c67a4dba..2878b0ed9741 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -1277,7 +1277,7 @@ static int redrat3_dev_resume(struct usb_interface *intf)
 static struct usb_driver redrat3_dev_driver = {
 	.name		= DRIVER_NAME,
 	.probe		= redrat3_dev_probe,
-	.disconnect	= redrat3_dev_disconnect,
+	.disconnect	= __devexit_p(redrat3_dev_disconnect),
 	.suspend	= redrat3_dev_suspend,
 	.resume		= redrat3_dev_resume,
 	.reset_resume	= redrat3_dev_resume,
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index ce1e7ba940f6..99937c94d7df 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -472,6 +472,9 @@ comment "Camera sensor devices"
 config VIDEO_APTINA_PLL
 	tristate
 
+config VIDEO_SMIAPP_PLL
+	tristate
+
 config VIDEO_OV7670
 	tristate "OmniVision OV7670 sensor support"
 	depends on I2C && VIDEO_V4L2
@@ -556,6 +559,8 @@ config VIDEO_S5K6AA
 	  This is a V4L2 sensor-level driver for Samsung S5K6AA(FX) 1.3M
 	  camera sensor with an embedded SoC image signal processor.
 
+source "drivers/media/video/smiapp/Kconfig"
+
 comment "Flash devices"
 
 config VIDEO_ADP1653
@@ -644,6 +649,8 @@ menuconfig V4L_USB_DRIVERS
 
 if V4L_USB_DRIVERS
 
+source "drivers/media/video/au0828/Kconfig"
+
 source "drivers/media/video/uvc/Kconfig"
 
 source "drivers/media/video/gspca/Kconfig"
@@ -662,8 +669,6 @@ source "drivers/media/video/tm6000/Kconfig"
 
 source "drivers/media/video/usbvision/Kconfig"
 
-source "drivers/media/video/et61x251/Kconfig"
-
 source "drivers/media/video/sn9c102/Kconfig"
 
 source "drivers/media/video/pwc/Kconfig"
@@ -721,8 +726,6 @@ menuconfig V4L_PCI_DRIVERS
 
 if V4L_PCI_DRIVERS
 
-source "drivers/media/video/au0828/Kconfig"
-
 source "drivers/media/video/bt8xx/Kconfig"
 
 source "drivers/media/video/cx18/Kconfig"
@@ -794,6 +797,19 @@ source "drivers/media/video/saa7164/Kconfig"
 
 source "drivers/media/video/zoran/Kconfig"
 
+config STA2X11_VIP
+	tristate "STA2X11 VIP Video For Linux"
+	depends on STA2X11
+	select VIDEO_ADV7180 if VIDEO_HELPER_CHIPS_AUTO
+	select VIDEOBUF_DMA_CONTIG
+	depends on PCI && VIDEO_V4L2 && VIRT_TO_BUS
+	help
+	  Say Y for support for STA2X11 VIP (Video Input Port) capture
+	  device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sta2x11_vip.
+
 endif # V4L_PCI_DRIVERS
 
 #
@@ -1127,19 +1143,6 @@ config VIDEO_MX2
 	  This is a v4l2 driver for the i.MX27 and the i.MX25 Camera Sensor
 	  Interface
 
-config  VIDEO_SAMSUNG_S5P_FIMC
-	tristate "Samsung S5P and EXYNOS4 camera interface driver (EXPERIMENTAL)"
-	depends on VIDEO_V4L2 && I2C && PLAT_S5P && PM_RUNTIME && \
-		VIDEO_V4L2_SUBDEV_API && EXPERIMENTAL
-	select VIDEOBUF2_DMA_CONTIG
-	select V4L2_MEM2MEM_DEV
-	---help---
-	  This is a v4l2 driver for Samsung S5P and EXYNOS4 camera
-	  host interface and video postprocessor.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called s5p-fimc.
-
 config VIDEO_ATMEL_ISI
 	tristate "ATMEL Image Sensor Interface (ISI) support"
 	depends on VIDEO_DEV && SOC_CAMERA && ARCH_AT91
@@ -1148,16 +1151,7 @@ config VIDEO_ATMEL_ISI
 	  This module makes the ATMEL Image Sensor Interface available
 	  as a v4l2 device.
 
-config VIDEO_S5P_MIPI_CSIS
-	tristate "Samsung S5P and EXYNOS4 MIPI CSI receiver driver"
-	depends on VIDEO_V4L2 && PM_RUNTIME && PLAT_S5P
-	depends on VIDEO_V4L2_SUBDEV_API && REGULATOR
-	---help---
-	  This is a v4l2 driver for Samsung S5P/EXYNOS4 MIPI-CSI receiver.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called s5p-csis.
-
+source "drivers/media/video/s5p-fimc/Kconfig"
 source "drivers/media/video/s5p-tv/Kconfig"
 
 endif # V4L_PLATFORM_DRIVERS
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index a6282a3a6a82..d209de0e0ca8 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -79,9 +79,12 @@ obj-$(CONFIG_VIDEO_SR030PC30)	+= sr030pc30.o
 obj-$(CONFIG_VIDEO_NOON010PC30)	+= noon010pc30.o
 obj-$(CONFIG_VIDEO_M5MOLS)	+= m5mols/
 obj-$(CONFIG_VIDEO_S5K6AA)	+= s5k6aa.o
+obj-$(CONFIG_VIDEO_SMIAPP)	+= smiapp/
 obj-$(CONFIG_VIDEO_ADP1653)	+= adp1653.o
 obj-$(CONFIG_VIDEO_AS3645A)	+= as3645a.o
 
+obj-$(CONFIG_VIDEO_SMIAPP_PLL)	+= smiapp-pll.o
+
 obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
 obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
@@ -120,6 +123,7 @@ obj-$(CONFIG_VIDEO_TM6000) += tm6000/
 obj-$(CONFIG_VIDEO_MXB) += mxb.o
 obj-$(CONFIG_VIDEO_HEXIUM_ORION) += hexium_orion.o
 obj-$(CONFIG_VIDEO_HEXIUM_GEMINI) += hexium_gemini.o
+obj-$(CONFIG_STA2X11_VIP) += sta2x11_vip.o
 obj-$(CONFIG_VIDEO_TIMBERDALE)	+= timblogiw.o
 
 obj-$(CONFIG_VIDEOBUF_GEN) += videobuf-core.o
@@ -152,7 +156,6 @@ obj-$(CONFIG_USB_ZR364XX)       += zr364xx.o
 obj-$(CONFIG_USB_STKWEBCAM)     += stkwebcam.o
 
 obj-$(CONFIG_USB_SN9C102)       += sn9c102/
-obj-$(CONFIG_USB_ET61X251)      += et61x251/
 obj-$(CONFIG_USB_PWC)           += pwc/
 obj-$(CONFIG_USB_GSPCA)         += gspca/
 
diff --git a/drivers/media/video/adp1653.c b/drivers/media/video/adp1653.c
index 5b045b4a66fe..57e87090388d 100644
--- a/drivers/media/video/adp1653.c
+++ b/drivers/media/video/adp1653.c
@@ -34,7 +34,6 @@
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
-#include <linux/version.h>
 #include <media/adp1653.h>
 #include <media/v4l2-device.h>
 
@@ -282,19 +281,19 @@ adp1653_init_device(struct adp1653_flash *flash)
 		return -EIO;
 	}
 
-	mutex_lock(&flash->ctrls.lock);
+	mutex_lock(flash->ctrls.lock);
 	/* Reset faults before reading new ones. */
 	flash->fault = 0;
 	rval = adp1653_get_fault(flash);
-	mutex_unlock(&flash->ctrls.lock);
+	mutex_unlock(flash->ctrls.lock);
 	if (rval > 0) {
 		dev_err(&client->dev, "faults detected: 0x%1.1x\n", rval);
 		return -EIO;
 	}
 
-	mutex_lock(&flash->ctrls.lock);
+	mutex_lock(flash->ctrls.lock);
 	rval = adp1653_update_hw(flash);
-	mutex_unlock(&flash->ctrls.lock);
+	mutex_unlock(flash->ctrls.lock);
 	if (rval) {
 		dev_err(&client->dev,
 			"adp1653_update_hw failed at %s\n", __func__);
diff --git a/drivers/media/video/adv7180.c b/drivers/media/video/adv7180.c
index b8b6c4b0cad4..174bffacf117 100644
--- a/drivers/media/video/adv7180.c
+++ b/drivers/media/video/adv7180.c
@@ -48,6 +48,7 @@
 #define ADV7180_INPUT_CONTROL_PAL_COMB_N_PED		0xd0
 #define ADV7180_INPUT_CONTROL_PAL_SECAM			0xe0
 #define ADV7180_INPUT_CONTROL_PAL_SECAM_PED		0xf0
+#define ADV7180_INPUT_CONTROL_INSEL_MASK		0x0f
 
 #define ADV7180_EXTENDED_OUTPUT_CONTROL_REG		0x04
 #define ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS		0xC5
@@ -55,9 +56,29 @@
 #define ADV7180_AUTODETECT_ENABLE_REG			0x07
 #define ADV7180_AUTODETECT_DEFAULT			0x7f
 
+#define ADV7180_CON_REG		0x08	/*Unsigned */
+#define CON_REG_MIN		0
+#define CON_REG_DEF		128
+#define CON_REG_MAX		255
+
+#define ADV7180_BRI_REG		0x0a	/*Signed */
+#define BRI_REG_MIN		-128
+#define BRI_REG_DEF		0
+#define BRI_REG_MAX		127
+
+#define ADV7180_HUE_REG		0x0b	/*Signed, inverted */
+#define HUE_REG_MIN		-127
+#define HUE_REG_DEF		0
+#define HUE_REG_MAX		128
+
 #define ADV7180_ADI_CTRL_REG				0x0e
 #define ADV7180_ADI_CTRL_IRQ_SPACE			0x20
 
+#define ADV7180_PWR_MAN_REG		0x0f
+#define ADV7180_PWR_MAN_ON		0x04
+#define ADV7180_PWR_MAN_OFF		0x24
+#define ADV7180_PWR_MAN_RES		0x80
+
 #define ADV7180_STATUS1_REG				0x10
 #define ADV7180_STATUS1_IN_LOCK		0x01
 #define ADV7180_STATUS1_AUTOD_MASK	0x70
@@ -78,6 +99,12 @@
 #define ADV7180_ICONF1_PSYNC_ONLY	0x10
 #define ADV7180_ICONF1_ACTIVE_TO_CLR	0xC0
 
+#define ADV7180_SD_SAT_CB_REG	0xe3	/*Unsigned */
+#define ADV7180_SD_SAT_CR_REG	0xe4	/*Unsigned */
+#define SAT_REG_MIN		0
+#define SAT_REG_DEF		128
+#define SAT_REG_MAX		255
+
 #define ADV7180_IRQ1_LOCK	0x01
 #define ADV7180_IRQ1_UNLOCK	0x02
 #define ADV7180_ISR1_ADI	0x42
@@ -90,6 +117,9 @@
 #define ADV7180_IMR3_ADI	0x4C
 #define ADV7180_IMR4_ADI	0x50
 
+#define ADV7180_NTSC_V_BIT_END_REG	0xE6
+#define ADV7180_NTSC_V_BIT_END_MANUAL_NVEND	0x4F
+
 struct adv7180_state {
 	struct v4l2_subdev	sd;
 	struct work_struct	work;
@@ -97,6 +127,11 @@ struct adv7180_state {
 	int			irq;
 	v4l2_std_id		curr_norm;
 	bool			autodetect;
+	s8			brightness;
+	s16			hue;
+	u8			contrast;
+	u8			saturation;
+	u8			input;
 };
 
 static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
@@ -155,7 +190,7 @@ static u32 adv7180_status_to_v4l2(u8 status1)
 }
 
 static int __adv7180_status(struct i2c_client *client, u32 *status,
-	v4l2_std_id *std)
+			    v4l2_std_id *std)
 {
 	int status1 = i2c_smbus_read_byte_data(client, ADV7180_STATUS1_REG);
 
@@ -192,6 +227,36 @@ static int adv7180_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
 	return err;
 }
 
+static int adv7180_s_routing(struct v4l2_subdev *sd, u32 input,
+			     u32 output, u32 config)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (ret)
+		return ret;
+
+	/*We cannot discriminate between LQFP and 40-pin LFCSP, so accept
+	 * all inputs and let the card driver take care of validation
+	 */
+	if ((input & ADV7180_INPUT_CONTROL_INSEL_MASK) != input)
+		goto out;
+
+	ret = i2c_smbus_read_byte_data(client, ADV7180_INPUT_CONTROL_REG);
+
+	if (ret < 0)
+		goto out;
+
+	ret &= ~ADV7180_INPUT_CONTROL_INSEL_MASK;
+	ret = i2c_smbus_write_byte_data(client,
+					ADV7180_INPUT_CONTROL_REG, ret | input);
+	state->input = input;
+out:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
 static int adv7180_g_input_status(struct v4l2_subdev *sd, u32 *status)
 {
 	struct adv7180_state *state = to_state(sd);
@@ -205,7 +270,7 @@ static int adv7180_g_input_status(struct v4l2_subdev *sd, u32 *status)
 }
 
 static int adv7180_g_chip_ident(struct v4l2_subdev *sd,
-	struct v4l2_dbg_chip_ident *chip)
+				struct v4l2_dbg_chip_ident *chip)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -222,9 +287,10 @@ static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 
 	/* all standards -> autodetect */
 	if (std == V4L2_STD_ALL) {
-		ret = i2c_smbus_write_byte_data(client,
-			ADV7180_INPUT_CONTROL_REG,
-			ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM);
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+				ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM
+					      | state->input);
 		if (ret < 0)
 			goto out;
 
@@ -236,7 +302,8 @@ static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 			goto out;
 
 		ret = i2c_smbus_write_byte_data(client,
-			ADV7180_INPUT_CONTROL_REG, ret);
+						ADV7180_INPUT_CONTROL_REG,
+						ret | state->input);
 		if (ret < 0)
 			goto out;
 
@@ -249,14 +316,138 @@ out:
 	return ret;
 }
 
+static int adv7180_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
+{
+	switch (qc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return v4l2_ctrl_query_fill(qc, BRI_REG_MIN, BRI_REG_MAX,
+					    1, BRI_REG_DEF);
+	case V4L2_CID_HUE:
+		return v4l2_ctrl_query_fill(qc, HUE_REG_MIN, HUE_REG_MAX,
+					    1, HUE_REG_DEF);
+	case V4L2_CID_CONTRAST:
+		return v4l2_ctrl_query_fill(qc, CON_REG_MIN, CON_REG_MAX,
+					    1, CON_REG_DEF);
+	case V4L2_CID_SATURATION:
+		return v4l2_ctrl_query_fill(qc, SAT_REG_MIN, SAT_REG_MAX,
+					    1, SAT_REG_DEF);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int adv7180_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ctrl->value = state->brightness;
+		break;
+	case V4L2_CID_HUE:
+		ctrl->value = state->hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		ctrl->value = state->contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		ctrl->value = state->saturation;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7180_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct adv7180_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		if ((ctrl->value > BRI_REG_MAX)
+		    || (ctrl->value < BRI_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		state->brightness = ctrl->value;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_BRI_REG,
+						state->brightness);
+		break;
+	case V4L2_CID_HUE:
+		if ((ctrl->value > HUE_REG_MAX)
+		    || (ctrl->value < HUE_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		state->hue = ctrl->value;
+		/*Hue is inverted according to HSL chart */
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_HUE_REG, -state->hue);
+		break;
+	case V4L2_CID_CONTRAST:
+		if ((ctrl->value > CON_REG_MAX)
+		    || (ctrl->value < CON_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		state->contrast = ctrl->value;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_CON_REG,
+						state->contrast);
+		break;
+	case V4L2_CID_SATURATION:
+		if ((ctrl->value > SAT_REG_MAX)
+		    || (ctrl->value < SAT_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		/*
+		 *This could be V4L2_CID_BLUE_BALANCE/V4L2_CID_RED_BALANCE
+		 *Let's not confuse the user, everybody understands saturation
+		 */
+		state->saturation = ctrl->value;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_SD_SAT_CB_REG,
+						state->saturation);
+		if (ret < 0)
+			break;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_SD_SAT_CR_REG,
+						state->saturation);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
 static const struct v4l2_subdev_video_ops adv7180_video_ops = {
 	.querystd = adv7180_querystd,
 	.g_input_status = adv7180_g_input_status,
+	.s_routing = adv7180_s_routing,
 };
 
 static const struct v4l2_subdev_core_ops adv7180_core_ops = {
 	.g_chip_ident = adv7180_g_chip_ident,
 	.s_std = adv7180_s_std,
+	.queryctrl = adv7180_queryctrl,
+	.g_ctrl = adv7180_g_ctrl,
+	.s_ctrl = adv7180_s_ctrl,
 };
 
 static const struct v4l2_subdev_ops adv7180_ops = {
@@ -267,13 +458,13 @@ static const struct v4l2_subdev_ops adv7180_ops = {
 static void adv7180_work(struct work_struct *work)
 {
 	struct adv7180_state *state = container_of(work, struct adv7180_state,
-		work);
+						   work);
 	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
 	u8 isr3;
 
 	mutex_lock(&state->mutex);
 	i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
-		ADV7180_ADI_CTRL_IRQ_SPACE);
+				  ADV7180_ADI_CTRL_IRQ_SPACE);
 	isr3 = i2c_smbus_read_byte_data(client, ADV7180_ISR3_ADI);
 	/* clear */
 	i2c_smbus_write_byte_data(client, ADV7180_ICR3_ADI, isr3);
@@ -297,56 +488,51 @@ static irqreturn_t adv7180_irq(int irq, void *devid)
 	return IRQ_HANDLED;
 }
 
-/*
- * Generic i2c probe
- * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
- */
-
-static __devinit int adv7180_probe(struct i2c_client *client,
-			const struct i2c_device_id *id)
+static int init_device(struct i2c_client *client, struct adv7180_state *state)
 {
-	struct adv7180_state *state;
-	struct v4l2_subdev *sd;
 	int ret;
 
-	/* Check if the adapter supports the needed features */
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -EIO;
-
-	v4l_info(client, "chip found @ 0x%02x (%s)\n",
-			client->addr << 1, client->adapter->name);
-
-	state = kzalloc(sizeof(struct adv7180_state), GFP_KERNEL);
-	if (state == NULL) {
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	state->irq = client->irq;
-	INIT_WORK(&state->work, adv7180_work);
-	mutex_init(&state->mutex);
-	state->autodetect = true;
-	sd = &state->sd;
-	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
-
 	/* Initialize adv7180 */
 	/* Enable autodetection */
-	ret = i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
-		ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM);
-	if (ret < 0)
-		goto err_unreg_subdev;
+	if (state->autodetect) {
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+				ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM
+					      | state->input);
+		if (ret < 0)
+			return ret;
 
-	ret = i2c_smbus_write_byte_data(client, ADV7180_AUTODETECT_ENABLE_REG,
-		ADV7180_AUTODETECT_DEFAULT);
-	if (ret < 0)
-		goto err_unreg_subdev;
+		ret =
+		    i2c_smbus_write_byte_data(client,
+					      ADV7180_AUTODETECT_ENABLE_REG,
+					      ADV7180_AUTODETECT_DEFAULT);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = v4l2_std_to_adv7180(state->curr_norm);
+		if (ret < 0)
+			return ret;
 
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+					      ret | state->input);
+		if (ret < 0)
+			return ret;
+
+	}
 	/* ITU-R BT.656-4 compatible */
 	ret = i2c_smbus_write_byte_data(client,
-		ADV7180_EXTENDED_OUTPUT_CONTROL_REG,
-		ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
+			ADV7180_EXTENDED_OUTPUT_CONTROL_REG,
+			ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
 	if (ret < 0)
-		goto err_unreg_subdev;
+		return ret;
+
+	/* Manually set V bit end position in NTSC mode */
+	ret = i2c_smbus_write_byte_data(client,
+					ADV7180_NTSC_V_BIT_END_REG,
+					ADV7180_NTSC_V_BIT_END_MANUAL_NVEND);
+	if (ret < 0)
+		return ret;
 
 	/* read current norm */
 	__adv7180_status(client, NULL, &state->curr_norm);
@@ -354,45 +540,109 @@ static __devinit int adv7180_probe(struct i2c_client *client,
 	/* register for interrupts */
 	if (state->irq > 0) {
 		ret = request_irq(state->irq, adv7180_irq, 0, DRIVER_NAME,
-			state);
+				  state);
 		if (ret)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
-			ADV7180_ADI_CTRL_IRQ_SPACE);
+						ADV7180_ADI_CTRL_IRQ_SPACE);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		/* config the Interrupt pin to be active low */
 		ret = i2c_smbus_write_byte_data(client, ADV7180_ICONF1_ADI,
-			ADV7180_ICONF1_ACTIVE_LOW | ADV7180_ICONF1_PSYNC_ONLY);
+						ADV7180_ICONF1_ACTIVE_LOW |
+						ADV7180_ICONF1_PSYNC_ONLY);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR1_ADI, 0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR2_ADI, 0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		/* enable AD change interrupts interrupts */
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR3_ADI,
-			ADV7180_IRQ3_AD_CHANGE);
+						ADV7180_IRQ3_AD_CHANGE);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR4_ADI, 0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
-			0);
+						0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 	}
 
+	/*Set default value for controls */
+	ret = i2c_smbus_write_byte_data(client, ADV7180_BRI_REG,
+					state->brightness);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_HUE_REG, state->hue);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_CON_REG,
+					state->contrast);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CB_REG,
+					state->saturation);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CR_REG,
+					state->saturation);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static __devinit int adv7180_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	struct adv7180_state *state;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+		 client->addr, client->adapter->name);
+
+	state = kzalloc(sizeof(struct adv7180_state), GFP_KERNEL);
+	if (state == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	state->irq = client->irq;
+	INIT_WORK(&state->work, adv7180_work);
+	mutex_init(&state->mutex);
+	state->autodetect = true;
+	state->brightness = BRI_REG_DEF;
+	state->hue = HUE_REG_DEF;
+	state->contrast = CON_REG_DEF;
+	state->saturation = SAT_REG_DEF;
+	state->input = 0;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
+
+	ret = init_device(client, state);
+	if (0 != ret)
+		goto err_unreg_subdev;
 	return 0;
 
 err_unreg_subdev:
@@ -432,16 +682,49 @@ static const struct i2c_device_id adv7180_id[] = {
 	{},
 };
 
+#ifdef CONFIG_PM
+static int adv7180_suspend(struct i2c_client *client, pm_message_t state)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_PWR_MAN_REG,
+					ADV7180_PWR_MAN_OFF);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int adv7180_resume(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7180_state *state = to_state(sd);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_PWR_MAN_REG,
+					ADV7180_PWR_MAN_ON);
+	if (ret < 0)
+		return ret;
+	ret = init_device(client, state);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+#endif
+
 MODULE_DEVICE_TABLE(i2c, adv7180_id);
 
 static struct i2c_driver adv7180_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
-		.name	= DRIVER_NAME,
-	},
-	.probe		= adv7180_probe,
-	.remove		= __devexit_p(adv7180_remove),
-	.id_table	= adv7180_id,
+		   .owner = THIS_MODULE,
+		   .name = DRIVER_NAME,
+		   },
+	.probe = adv7180_probe,
+	.remove = __devexit_p(adv7180_remove),
+#ifdef CONFIG_PM
+	.suspend = adv7180_suspend,
+	.resume = adv7180_resume,
+#endif
+	.id_table = adv7180_id,
 };
 
 module_i2c_driver(adv7180_driver);
diff --git a/drivers/media/video/adv7343.c b/drivers/media/video/adv7343.c
index 119b60401bf3..2b5aa676a84e 100644
--- a/drivers/media/video/adv7343.c
+++ b/drivers/media/video/adv7343.c
@@ -130,14 +130,12 @@ static int adv7343_setstd(struct v4l2_subdev *sd, v4l2_std_id std)
 {
 	struct adv7343_state *state = to_state(sd);
 	struct adv7343_std_info *std_info;
-	int output_idx, num_std;
+	int num_std;
 	char *fsc_ptr;
 	u8 reg, val;
 	int err = 0;
 	int i = 0;
 
-	output_idx = state->output;
-
 	std_info = (struct adv7343_std_info *)stdinfo;
 	num_std = ARRAY_SIZE(stdinfo);
 
diff --git a/drivers/media/video/aptina-pll.c b/drivers/media/video/aptina-pll.c
index 0bd3813bb59d..8153a449846b 100644
--- a/drivers/media/video/aptina-pll.c
+++ b/drivers/media/video/aptina-pll.c
@@ -148,9 +148,8 @@ int aptina_pll_calculate(struct device *dev,
 		unsigned int mf_high;
 		unsigned int mf_low;
 
-		mf_low = max(roundup(mf_min, mf_inc),
-			     DIV_ROUND_UP(pll->ext_clock * p1,
-			       limits->int_clock_max * div));
+		mf_low = roundup(max(mf_min, DIV_ROUND_UP(pll->ext_clock * p1,
+					limits->int_clock_max * div)), mf_inc);
 		mf_high = min(mf_max, pll->ext_clock * p1 /
 			      (limits->int_clock_min * div));
 
diff --git a/drivers/media/video/arv.c b/drivers/media/video/arv.c
index b6ed44aebe30..e346d32d08ce 100644
--- a/drivers/media/video/arv.c
+++ b/drivers/media/video/arv.c
@@ -31,6 +31,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
 #include <linux/mutex.h>
 
 #include <asm/uaccess.h>
@@ -403,7 +404,8 @@ static int ar_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, ar->vdev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "Colour AR VGA", sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "Platform", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -709,6 +711,8 @@ static int ar_initialize(struct ar *ar)
 
 static const struct v4l2_file_operations ar_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
 	.read		= ar_read,
 	.unlocked_ioctl	= video_ioctl2,
 };
@@ -769,6 +773,7 @@ static int __init ar_init(void)
 	ar->vdev.fops = &ar_fops;
 	ar->vdev.ioctl_ops = &ar_ioctl_ops;
 	ar->vdev.release = video_device_release_empty;
+	set_bit(V4L2_FL_USE_FH_PRIO, &ar->vdev.flags);
 	video_set_drvdata(&ar->vdev, ar);
 
 	if (vga) {
diff --git a/drivers/media/video/as3645a.c b/drivers/media/video/as3645a.c
index 7a3371f044fc..c4b03572dce8 100644
--- a/drivers/media/video/as3645a.c
+++ b/drivers/media/video/as3645a.c
@@ -713,7 +713,7 @@ static int as3645a_resume(struct device *dev)
  * The number of LEDs reported in platform data is used to compute default
  * limits. Parameters passed through platform data can override those limits.
  */
-static int as3645a_init_controls(struct as3645a *flash)
+static int __devinit as3645a_init_controls(struct as3645a *flash)
 {
 	const struct as3645a_platform_data *pdata = flash->pdata;
 	struct v4l2_ctrl *ctrl;
@@ -804,8 +804,8 @@ static int as3645a_init_controls(struct as3645a *flash)
 	return flash->ctrls.error;
 }
 
-static int as3645a_probe(struct i2c_client *client,
-			 const struct i2c_device_id *devid)
+static int __devinit as3645a_probe(struct i2c_client *client,
+				   const struct i2c_device_id *devid)
 {
 	struct as3645a *flash;
 	int ret;
@@ -846,7 +846,7 @@ done:
 	return ret;
 }
 
-static int __exit as3645a_remove(struct i2c_client *client)
+static int __devexit as3645a_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct as3645a *flash = to_as3645a(subdev);
@@ -877,7 +877,7 @@ static struct i2c_driver as3645a_i2c_driver = {
 		.pm   = &as3645a_pm_ops,
 	},
 	.probe	= as3645a_probe,
-	.remove	= __exit_p(as3645a_remove),
+	.remove	= __devexit_p(as3645a_remove),
 	.id_table = as3645a_id_table,
 };
 
diff --git a/drivers/media/video/atmel-isi.c b/drivers/media/video/atmel-isi.c
index ec3f6a06f9c3..6274a91c25c7 100644
--- a/drivers/media/video/atmel-isi.c
+++ b/drivers/media/video/atmel-isi.c
@@ -260,7 +260,7 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct atmel_isi *isi = ici->priv;
 	unsigned long size;
-	int ret, bytes_per_line;
+	int ret;
 
 	/* Reset ISI */
 	ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
@@ -271,13 +271,7 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	/* Disable all interrupts */
 	isi_writel(isi, ISI_INTDIS, ~0UL);
 
-	bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	size = bytes_per_line * icd->user_height;
+	size = icd->sizeimage;
 
 	if (!*nbuffers || *nbuffers > MAX_BUFFER_NUM)
 		*nbuffers = MAX_BUFFER_NUM;
@@ -316,13 +310,8 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	struct atmel_isi *isi = ici->priv;
 	unsigned long size;
 	struct isi_dma_desc *desc;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
-	size = bytes_per_line * icd->user_height;
+	size = icd->sizeimage;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		dev_err(icd->parent, "%s data will not fit into plane (%lu < %lu)\n",
@@ -638,6 +627,7 @@ static const struct soc_mbus_pixelfmt isi_camera_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 };
 
diff --git a/drivers/media/video/au0828/Kconfig b/drivers/media/video/au0828/Kconfig
index 81ba9d9d1b52..23f7fd22f0eb 100644
--- a/drivers/media/video/au0828/Kconfig
+++ b/drivers/media/video/au0828/Kconfig
@@ -6,7 +6,8 @@ config VIDEO_AU0828
 	select I2C_ALGOBIT
 	select VIDEO_TVEEPROM
 	select VIDEOBUF_VMALLOC
-	select DVB_AU8522 if !DVB_FE_CUSTOMISE
+	select DVB_AU8522_DTV if !DVB_FE_CUSTOMISE
+	select DVB_AU8522_V4L if !DVB_FE_CUSTOMISE
 	select MEDIA_TUNER_XC5000 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_MXL5007T if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_TDA18271 if !MEDIA_TUNER_CUSTOMISE
diff --git a/drivers/media/video/au0828/au0828-cards.c b/drivers/media/video/au0828/au0828-cards.c
index 1c6015a04f96..e3fe9a6637f6 100644
--- a/drivers/media/video/au0828/au0828-cards.c
+++ b/drivers/media/video/au0828/au0828-cards.c
@@ -325,6 +325,8 @@ struct usb_device_id au0828_usb_id_table[] = {
 		.driver_info = AU0828_BOARD_HAUPPAUGE_HVR950Q_MXL },
 	{ USB_DEVICE(0x2040, 0x7281),
 		.driver_info = AU0828_BOARD_HAUPPAUGE_HVR950Q_MXL },
+	{ USB_DEVICE(0x05e1, 0x0480),
+		.driver_info = AU0828_BOARD_HAUPPAUGE_WOODBURY },
 	{ USB_DEVICE(0x2040, 0x8200),
 		.driver_info = AU0828_BOARD_HAUPPAUGE_WOODBURY },
 	{ USB_DEVICE(0x2040, 0x7260),
diff --git a/drivers/media/video/au0828/au0828-dvb.c b/drivers/media/video/au0828/au0828-dvb.c
index 518216743c9c..39ece8e24985 100644
--- a/drivers/media/video/au0828/au0828-dvb.c
+++ b/drivers/media/video/au0828/au0828-dvb.c
@@ -25,6 +25,7 @@
 #include <linux/device.h>
 #include <linux/suspend.h>
 #include <media/v4l2-common.h>
+#include <media/tuner.h>
 
 #include "au0828.h"
 #include "au8522.h"
@@ -79,9 +80,16 @@ static struct au8522_config hauppauge_woodbury_config = {
 	.vsb_if        = AU8522_IF_3_25MHZ,
 };
 
-static struct xc5000_config hauppauge_hvr950q_tunerconfig = {
+static struct xc5000_config hauppauge_xc5000a_config = {
 	.i2c_address      = 0x61,
 	.if_khz           = 6000,
+	.chip_id          = XC5000A,
+};
+
+static struct xc5000_config hauppauge_xc5000c_config = {
+	.i2c_address      = 0x61,
+	.if_khz           = 6000,
+	.chip_id          = XC5000C,
 };
 
 static struct mxl5007t_config mxl5007t_hvr950q_config = {
@@ -383,8 +391,19 @@ int au0828_dvb_register(struct au0828_dev *dev)
 				&hauppauge_hvr950q_config,
 				&dev->i2c_adap);
 		if (dvb->frontend != NULL)
-			dvb_attach(xc5000_attach, dvb->frontend, &dev->i2c_adap,
-				   &hauppauge_hvr950q_tunerconfig);
+			switch (dev->board.tuner_type) {
+			default:
+			case TUNER_XC5000:
+				dvb_attach(xc5000_attach, dvb->frontend,
+					   &dev->i2c_adap,
+					   &hauppauge_xc5000a_config);
+				break;
+			case TUNER_XC5000C:
+				dvb_attach(xc5000_attach, dvb->frontend,
+					   &dev->i2c_adap,
+					   &hauppauge_xc5000c_config);
+				break;
+			}
 		break;
 	case AU0828_BOARD_HAUPPAUGE_HVR950Q_MXL:
 		dvb->frontend = dvb_attach(au8522_attach,
@@ -411,7 +430,7 @@ int au0828_dvb_register(struct au0828_dev *dev)
 		if (dvb->frontend != NULL) {
 			dvb_attach(xc5000_attach, dvb->frontend,
 				&dev->i2c_adap,
-				&hauppauge_hvr950q_tunerconfig);
+				&hauppauge_xc5000a_config);
 		}
 		break;
 	default:
diff --git a/drivers/media/video/au0828/au0828-video.c b/drivers/media/video/au0828/au0828-video.c
index 0b3e481ffe8c..ac3dd733ab81 100644
--- a/drivers/media/video/au0828/au0828-video.c
+++ b/drivers/media/video/au0828/au0828-video.c
@@ -120,7 +120,7 @@ static void au0828_irq_callback(struct urb *urb)
 	struct au0828_dmaqueue  *dma_q = urb->context;
 	struct au0828_dev *dev = container_of(dma_q, struct au0828_dev, vidq);
 	unsigned long flags = 0;
-	int rc, i;
+	int i;
 
 	switch (urb->status) {
 	case 0:             /* success */
@@ -138,7 +138,7 @@ static void au0828_irq_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock_irqsave(&dev->slock, flags);
-	rc = dev->isoc_ctl.isoc_copy(dev, urb);
+	dev->isoc_ctl.isoc_copy(dev, urb);
 	spin_unlock_irqrestore(&dev->slock, flags);
 
 	/* Reset urb buffers */
@@ -1881,7 +1881,7 @@ int au0828_analog_register(struct au0828_dev *dev,
 	int retval = -ENOMEM;
 	struct usb_host_interface *iface_desc;
 	struct usb_endpoint_descriptor *endpoint;
-	int i;
+	int i, ret;
 
 	dprintk(1, "au0828_analog_register called!\n");
 
@@ -1951,8 +1951,8 @@ int au0828_analog_register(struct au0828_dev *dev,
 	dev->vbi_dev = video_device_alloc();
 	if (NULL == dev->vbi_dev) {
 		dprintk(1, "Can't allocate vbi_device.\n");
-		kfree(dev->vdev);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto err_vdev;
 	}
 
 	/* Fill the video capture device struct */
@@ -1971,8 +1971,8 @@ int au0828_analog_register(struct au0828_dev *dev,
 	if (retval != 0) {
 		dprintk(1, "unable to register video device (error = %d).\n",
 			retval);
-		video_device_release(dev->vdev);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err_vbi_dev;
 	}
 
 	/* Register the vbi device */
@@ -1981,13 +1981,18 @@ int au0828_analog_register(struct au0828_dev *dev,
 	if (retval != 0) {
 		dprintk(1, "unable to register vbi device (error = %d).\n",
 			retval);
-		video_device_release(dev->vbi_dev);
-		video_device_release(dev->vdev);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err_vbi_dev;
 	}
 
 	dprintk(1, "%s completed!\n", __func__);
 
 	return 0;
+
+err_vbi_dev:
+	video_device_release(dev->vbi_dev);
+err_vdev:
+	video_device_release(dev->vdev);
+	return ret;
 }
 
diff --git a/drivers/media/video/blackfin/bfin_capture.c b/drivers/media/video/blackfin/bfin_capture.c
index 514fcf742f5a..0aba45e34f70 100644
--- a/drivers/media/video/blackfin/bfin_capture.c
+++ b/drivers/media/video/blackfin/bfin_capture.c
@@ -942,6 +942,10 @@ static int __devinit bcap_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&bcap_dev->dma_queue);
 
 	vfd->lock = &bcap_dev->mutex;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	/* register video device */
 	ret = video_register_device(bcap_dev->video_dev, VFL_TYPE_GRABBER, -1);
diff --git a/drivers/media/video/bt8xx/bttv-driver.c b/drivers/media/video/bt8xx/bttv-driver.c
index e581b37be789..a9cfb0f4be48 100644
--- a/drivers/media/video/bt8xx/bttv-driver.c
+++ b/drivers/media/video/bt8xx/bttv-driver.c
@@ -663,7 +663,7 @@ static const struct v4l2_queryctrl bttv_ctls[] = {
 		.minimum       = 0,
 		.maximum       = 65535,
 		.step          = 128,
-		.default_value = 32768,
+		.default_value = 27648,
 		.type          = V4L2_CTRL_TYPE_INTEGER,
 	},{
 		.id            = V4L2_CID_SATURATION,
@@ -4394,7 +4394,7 @@ static int __devinit bttv_probe(struct pci_dev *dev,
 	if (!bttv_tvcards[btv->c.type].no_video) {
 		bttv_register_video(btv);
 		bt848_bright(btv,32768);
-		bt848_contrast(btv,32768);
+		bt848_contrast(btv, 27648);
 		bt848_hue(btv,32768);
 		bt848_sat(btv,32768);
 		audio_mute(btv, 1);
diff --git a/drivers/media/video/bw-qcam.c b/drivers/media/video/bw-qcam.c
index f09df9dffaae..2520219f01ba 100644
--- a/drivers/media/video/bw-qcam.c
+++ b/drivers/media/video/bw-qcam.c
@@ -77,6 +77,9 @@ OTHER DEALINGS IN THE SOFTWARE.
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 
 /* One from column A... */
 #define QC_NOTSET 0
@@ -103,6 +106,7 @@ OTHER DEALINGS IN THE SOFTWARE.
 struct qcam {
 	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
 	struct pardevice *pdev;
 	struct parport *pport;
 	struct mutex lock;
@@ -646,7 +650,8 @@ static int qcam_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "B&W Quickcam", sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -674,72 +679,6 @@ static int qcam_s_input(struct file *file, void *fh, unsigned int inp)
 	return (inp > 0) ? -EINVAL : 0;
 }
 
-static int qcam_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 180);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 192);
-	case V4L2_CID_GAMMA:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 105);
-	}
-	return -EINVAL;
-}
-
-static int qcam_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = qcam->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = qcam->contrast;
-		break;
-	case V4L2_CID_GAMMA:
-		ctrl->value = qcam->whitebal;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
-}
-
-static int qcam_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	mutex_lock(&qcam->lock);
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		qcam->brightness = ctrl->value;
-		break;
-	case V4L2_CID_CONTRAST:
-		qcam->contrast = ctrl->value;
-		break;
-	case V4L2_CID_GAMMA:
-		qcam->whitebal = ctrl->value;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	if (ret == 0) {
-		qc_setscanmode(qcam);
-		qcam->status |= QC_PARAM_CHANGE;
-	}
-	mutex_unlock(&qcam->lock);
-	return ret;
-}
-
 static int qcam_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
 {
 	struct qcam *qcam = video_drvdata(file);
@@ -856,8 +795,40 @@ static ssize_t qcam_read(struct file *file, char __user *buf,
 	return len;
 }
 
+static int qcam_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct qcam *qcam =
+		container_of(ctrl->handler, struct qcam, hdl);
+	int ret = 0;
+
+	mutex_lock(&qcam->lock);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		qcam->brightness = ctrl->val;
+		break;
+	case V4L2_CID_CONTRAST:
+		qcam->contrast = ctrl->val;
+		break;
+	case V4L2_CID_GAMMA:
+		qcam->whitebal = ctrl->val;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	if (ret == 0) {
+		qc_setscanmode(qcam);
+		qcam->status |= QC_PARAM_CHANGE;
+	}
+	mutex_unlock(&qcam->lock);
+	return ret;
+}
+
 static const struct v4l2_file_operations qcam_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl = video_ioctl2,
 	.read		= qcam_read,
 };
@@ -867,13 +838,17 @@ static const struct v4l2_ioctl_ops qcam_ioctl_ops = {
 	.vidioc_g_input      		    = qcam_g_input,
 	.vidioc_s_input      		    = qcam_s_input,
 	.vidioc_enum_input   		    = qcam_enum_input,
-	.vidioc_queryctrl 		    = qcam_queryctrl,
-	.vidioc_g_ctrl  		    = qcam_g_ctrl,
-	.vidioc_s_ctrl 			    = qcam_s_ctrl,
 	.vidioc_enum_fmt_vid_cap 	    = qcam_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap 		    = qcam_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap  		    = qcam_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap  	    = qcam_try_fmt_vid_cap,
+	.vidioc_log_status		    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ctrl_ops qcam_ctrl_ops = {
+	.s_ctrl = qcam_s_ctrl,
 };
 
 /* Initialize the QuickCam driver control structure.  This is where
@@ -897,19 +872,35 @@ static struct qcam *qcam_init(struct parport *port)
 		return NULL;
 	}
 
+	v4l2_ctrl_handler_init(&qcam->hdl, 3);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 180);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, 192);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_GAMMA, 0, 255, 1, 105);
+	if (qcam->hdl.error) {
+		v4l2_err(v4l2_dev, "couldn't register controls\n");
+		v4l2_ctrl_handler_free(&qcam->hdl);
+		kfree(qcam);
+		return NULL;
+	}
 	qcam->pport = port;
 	qcam->pdev = parport_register_device(port, "bw-qcam", NULL, NULL,
 			NULL, 0, NULL);
 	if (qcam->pdev == NULL) {
 		v4l2_err(v4l2_dev, "couldn't register for %s.\n", port->name);
+		v4l2_ctrl_handler_free(&qcam->hdl);
 		kfree(qcam);
 		return NULL;
 	}
 
 	strlcpy(qcam->vdev.name, "Connectix QuickCam", sizeof(qcam->vdev.name));
 	qcam->vdev.v4l2_dev = v4l2_dev;
+	qcam->vdev.ctrl_handler = &qcam->hdl;
 	qcam->vdev.fops = &qcam_fops;
 	qcam->vdev.ioctl_ops = &qcam_ioctl_ops;
+	set_bit(V4L2_FL_USE_FH_PRIO, &qcam->vdev.flags);
 	qcam->vdev.release = video_device_release_empty;
 	video_set_drvdata(&qcam->vdev, qcam);
 
@@ -1003,6 +994,7 @@ static int init_bwqcam(struct parport *port)
 static void close_bwqcam(struct qcam *qcam)
 {
 	video_unregister_device(&qcam->vdev);
+	v4l2_ctrl_handler_free(&qcam->hdl);
 	parport_unregister_device(qcam->pdev);
 	kfree(qcam);
 }
diff --git a/drivers/media/video/c-qcam.c b/drivers/media/video/c-qcam.c
index fda32f52554a..ec51e1f12e82 100644
--- a/drivers/media/video/c-qcam.c
+++ b/drivers/media/video/c-qcam.c
@@ -40,10 +40,14 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 
 struct qcam {
 	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
 	struct pardevice *pdev;
 	struct parport *pport;
 	int width, height;
@@ -378,7 +382,7 @@ get_fragment:
 static long qc_capture(struct qcam *qcam, char __user *buf, unsigned long len)
 {
 	struct v4l2_device *v4l2_dev = &qcam->v4l2_dev;
-	unsigned lines, pixelsperline, bitsperxfer;
+	unsigned lines, pixelsperline;
 	unsigned int is_bi_dir = qcam->bidirectional;
 	size_t wantlen, outptr = 0;
 	char tmpbuf[BUFSZ];
@@ -404,7 +408,6 @@ static long qc_capture(struct qcam *qcam, char __user *buf, unsigned long len)
 
 	lines = qcam->height;
 	pixelsperline = qcam->width;
-	bitsperxfer = (is_bi_dir) ? 24 : 8;
 
 	if (is_bi_dir) {
 		/* Turn the port around */
@@ -516,7 +519,8 @@ static int qcam_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "Color Quickcam", sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -544,73 +548,6 @@ static int qcam_s_input(struct file *file, void *fh, unsigned int inp)
 	return (inp > 0) ? -EINVAL : 0;
 }
 
-static int qcam_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 240);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 192);
-	case V4L2_CID_GAMMA:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	}
-	return -EINVAL;
-}
-
-static int qcam_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = qcam->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = qcam->contrast;
-		break;
-	case V4L2_CID_GAMMA:
-		ctrl->value = qcam->whitebal;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
-}
-
-static int qcam_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	mutex_lock(&qcam->lock);
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		qcam->brightness = ctrl->value;
-		break;
-	case V4L2_CID_CONTRAST:
-		qcam->contrast = ctrl->value;
-		break;
-	case V4L2_CID_GAMMA:
-		qcam->whitebal = ctrl->value;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	if (ret == 0) {
-		parport_claim_or_block(qcam->pdev);
-		qc_setup(qcam);
-		parport_release(qcam->pdev);
-	}
-	mutex_unlock(&qcam->lock);
-	return ret;
-}
-
 static int qcam_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
 {
 	struct qcam *qcam = video_drvdata(file);
@@ -714,8 +651,41 @@ static ssize_t qcam_read(struct file *file, char __user *buf,
 	return len;
 }
 
+static int qcam_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct qcam *qcam =
+		container_of(ctrl->handler, struct qcam, hdl);
+	int ret = 0;
+
+	mutex_lock(&qcam->lock);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		qcam->brightness = ctrl->val;
+		break;
+	case V4L2_CID_CONTRAST:
+		qcam->contrast = ctrl->val;
+		break;
+	case V4L2_CID_GAMMA:
+		qcam->whitebal = ctrl->val;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	if (ret == 0) {
+		parport_claim_or_block(qcam->pdev);
+		qc_setup(qcam);
+		parport_release(qcam->pdev);
+	}
+	mutex_unlock(&qcam->lock);
+	return ret;
+}
+
 static const struct v4l2_file_operations qcam_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl	= video_ioctl2,
 	.read		= qcam_read,
 };
@@ -725,13 +695,17 @@ static const struct v4l2_ioctl_ops qcam_ioctl_ops = {
 	.vidioc_g_input      		    = qcam_g_input,
 	.vidioc_s_input      		    = qcam_s_input,
 	.vidioc_enum_input   		    = qcam_enum_input,
-	.vidioc_queryctrl 		    = qcam_queryctrl,
-	.vidioc_g_ctrl  		    = qcam_g_ctrl,
-	.vidioc_s_ctrl 			    = qcam_s_ctrl,
-	.vidioc_enum_fmt_vid_cap 	    = qcam_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap	    = qcam_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap 		    = qcam_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap  		    = qcam_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap  	    = qcam_try_fmt_vid_cap,
+	.vidioc_log_status		    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ctrl_ops qcam_ctrl_ops = {
+	.s_ctrl = qcam_s_ctrl,
 };
 
 /* Initialize the QuickCam driver control structure. */
@@ -754,6 +728,20 @@ static struct qcam *qcam_init(struct parport *port)
 		return NULL;
 	}
 
+	v4l2_ctrl_handler_init(&qcam->hdl, 3);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 240);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, 192);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_GAMMA, 0, 255, 1, 128);
+	if (qcam->hdl.error) {
+		v4l2_err(v4l2_dev, "couldn't register controls\n");
+		v4l2_ctrl_handler_free(&qcam->hdl);
+		kfree(qcam);
+		return NULL;
+	}
+
 	qcam->pport = port;
 	qcam->pdev = parport_register_device(port, "c-qcam", NULL, NULL,
 					  NULL, 0, NULL);
@@ -762,6 +750,7 @@ static struct qcam *qcam_init(struct parport *port)
 
 	if (qcam->pdev == NULL) {
 		v4l2_err(v4l2_dev, "couldn't register for %s.\n", port->name);
+		v4l2_ctrl_handler_free(&qcam->hdl);
 		kfree(qcam);
 		return NULL;
 	}
@@ -771,6 +760,8 @@ static struct qcam *qcam_init(struct parport *port)
 	qcam->vdev.fops = &qcam_fops;
 	qcam->vdev.ioctl_ops = &qcam_ioctl_ops;
 	qcam->vdev.release = video_device_release_empty;
+	qcam->vdev.ctrl_handler = &qcam->hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &qcam->vdev.flags);
 	video_set_drvdata(&qcam->vdev, qcam);
 
 	mutex_init(&qcam->lock);
@@ -845,6 +836,7 @@ static int init_cqcam(struct parport *port)
 static void close_cqcam(struct qcam *qcam)
 {
 	video_unregister_device(&qcam->vdev);
+	v4l2_ctrl_handler_free(&qcam->hdl);
 	parport_unregister_device(qcam->pdev);
 	kfree(qcam);
 }
diff --git a/drivers/media/video/cpia2/cpia2.h b/drivers/media/video/cpia2/cpia2.h
index ab252188981b..cdef677d57ec 100644
--- a/drivers/media/video/cpia2/cpia2.h
+++ b/drivers/media/video/cpia2/cpia2.h
@@ -32,11 +32,12 @@
 #define __CPIA2_H__
 
 #include <linux/videodev2.h>
-#include <media/v4l2-common.h>
 #include <linux/usb.h>
 #include <linux/poll.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 
-#include "cpia2dev.h"
 #include "cpia2_registers.h"
 
 /* define for verbose debug output */
@@ -65,7 +66,6 @@
 
 /* Flicker Modes */
 #define NEVER_FLICKER   0
-#define ANTI_FLICKER_ON 1
 #define FLICKER_60      60
 #define FLICKER_50      50
 
@@ -148,7 +148,6 @@ enum {
 #define DEFAULT_BRIGHTNESS 0x46
 #define DEFAULT_CONTRAST 0x93
 #define DEFAULT_SATURATION 0x7f
-#define DEFAULT_TARGET_KB 0x30
 
 /* Power state */
 #define HI_POWER_MODE CPIA2_SYSTEM_CONTROL_HIGH_POWER
@@ -287,7 +286,6 @@ struct camera_params {
 	struct {
 		u8 cam_register;
 		u8 flicker_mode_req;	/* 1 if flicker on, else never flicker */
-		int mains_frequency;
 	} flicker_control;
 
 	struct {
@@ -337,7 +335,7 @@ struct camera_params {
 		u8 vc_control;
 		u8 vc_mp_direction;
 		u8 vc_mp_data;
-		u8 target_kb;
+		u8 quality;
 	} vc_params;
 
 	struct {
@@ -366,23 +364,23 @@ struct framebuf {
 	struct framebuf *next;
 };
 
-struct cpia2_fh {
-	enum v4l2_priority prio;
-	u8 mmapped;
-};
-
 struct camera_data {
 	/* locks */
+	struct v4l2_device v4l2_dev;
 	struct mutex v4l2_lock;	/* serialize file operations */
-	struct v4l2_prio_state prio;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* Lights control cluster */
+		struct v4l2_ctrl *top_light;
+		struct v4l2_ctrl *bottom_light;
+	};
+	struct v4l2_ctrl *usb_alt;
 
 	/* camera status */
-	volatile int present;	/* Is the camera still present? */
-	int open_count;		/* # of process that have camera open */
 	int first_image_seen;
-	u8 mains_freq;		/* for flicker control */
 	enum sensors sensor_type;
 	u8 flush;
+	struct v4l2_fh *stream_fh;
 	u8 mmapped;
 	int streaming;		/* 0 = no, 1 = yes */
 	int xfer_mode;		/* XFER_BULK or XFER_ISOC */
@@ -390,7 +388,7 @@ struct camera_data {
 
 	/* v4l */
 	int video_size;			/* VIDEO_SIZE_ */
-	struct video_device *vdev;	/* v4l videodev */
+	struct video_device vdev;	/* v4l videodev */
 	u32 width;
 	u32 height;			/* Its size */
 	__u32 pixelformat;       /* Format fourcc      */
@@ -425,6 +423,7 @@ struct camera_data {
 /* v4l */
 int cpia2_register_camera(struct camera_data *cam);
 void cpia2_unregister_camera(struct camera_data *cam);
+void cpia2_camera_release(struct v4l2_device *v4l2_dev);
 
 /* core */
 int cpia2_reset_camera(struct camera_data *cam);
@@ -443,7 +442,7 @@ int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd);
 int cpia2_do_command(struct camera_data *cam,
 		     unsigned int command,
 		     unsigned char direction, unsigned char param);
-struct camera_data *cpia2_init_camera_struct(void);
+struct camera_data *cpia2_init_camera_struct(struct usb_interface *intf);
 int cpia2_init_camera(struct camera_data *cam);
 int cpia2_allocate_buffers(struct camera_data *cam);
 void cpia2_free_buffers(struct camera_data *cam);
@@ -454,7 +453,6 @@ unsigned int cpia2_poll(struct camera_data *cam,
 int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma);
 void cpia2_set_property_flip(struct camera_data *cam, int prop_val);
 void cpia2_set_property_mirror(struct camera_data *cam, int prop_val);
-int cpia2_set_target_kb(struct camera_data *cam, unsigned char value);
 int cpia2_set_gpio(struct camera_data *cam, unsigned char setting);
 int cpia2_set_fps(struct camera_data *cam, int framerate);
 
diff --git a/drivers/media/video/cpia2/cpia2_core.c b/drivers/media/video/cpia2/cpia2_core.c
index ee91e295c90a..17188e234770 100644
--- a/drivers/media/video/cpia2/cpia2_core.c
+++ b/drivers/media/video/cpia2/cpia2_core.c
@@ -66,7 +66,6 @@ static int config_sensor_410(struct camera_data *cam,
 static int config_sensor_500(struct camera_data *cam,
 			    int reqwidth, int reqheight);
 static int set_all_properties(struct camera_data *cam);
-static void get_color_params(struct camera_data *cam);
 static void wake_system(struct camera_data *cam);
 static void set_lowlight_boost(struct camera_data *cam);
 static void reset_camera_struct(struct camera_data *cam);
@@ -453,15 +452,6 @@ int cpia2_do_command(struct camera_data *cam,
 		cam->params.version.vp_device_hi = cmd.buffer.block_data[0];
 		cam->params.version.vp_device_lo = cmd.buffer.block_data[1];
 		break;
-	case CPIA2_CMD_GET_VP_BRIGHTNESS:
-		cam->params.color_params.brightness = cmd.buffer.block_data[0];
-		break;
-	case CPIA2_CMD_GET_CONTRAST:
-		cam->params.color_params.contrast = cmd.buffer.block_data[0];
-		break;
-	case CPIA2_CMD_GET_VP_SATURATION:
-		cam->params.color_params.saturation = cmd.buffer.block_data[0];
-		break;
 	case CPIA2_CMD_GET_VP_GPIO_DATA:
 		cam->params.vp_params.gpio_data = cmd.buffer.block_data[0];
 		break;
@@ -617,6 +607,7 @@ int cpia2_reset_camera(struct camera_data *cam)
 {
 	u8 tmp_reg;
 	int retval = 0;
+	int target_kb;
 	int i;
 	struct cpia2_command cmd;
 
@@ -800,9 +791,16 @@ int cpia2_reset_camera(struct camera_data *cam)
 	}
 	cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);
 
-	/* Set target size (kb) on vc */
+	/* Set target size (kb) on vc
+	   This is a heuristic based on the quality parameter and the raw
+	   framesize in kB divided by 16 (the compression factor when the
+	   quality is 100%) */
+	target_kb = (cam->width * cam->height * 2 / 16384) *
+				cam->params.vc_params.quality / 100;
+	if (target_kb < 1)
+		target_kb = 1;
 	cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
-			 TRANSFER_WRITE, cam->params.vc_params.target_kb);
+			 TRANSFER_WRITE, target_kb);
 
 	/* Wiggle VC Reset */
 	/***
@@ -1538,23 +1536,17 @@ static int set_all_properties(struct camera_data *cam)
 	 * framerate and user_mode were already set (set_default_user_mode).
 	 **/
 
-	cpia2_set_color_params(cam);
-
 	cpia2_usb_change_streaming_alternate(cam,
 					  cam->params.camera_state.stream_mode);
 
-	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
-			 cam->params.vp_params.user_effects);
-
-	cpia2_set_flicker_mode(cam,
-			       cam->params.flicker_control.flicker_mode_req);
-
 	cpia2_do_command(cam,
 			 CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
 			 TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
 	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
 			 cam->params.vp_params.gpio_data);
 
+	v4l2_ctrl_handler_setup(&cam->hdl);
+
 	wake_system(cam);
 
 	set_lowlight_boost(cam);
@@ -1569,7 +1561,6 @@ static int set_all_properties(struct camera_data *cam)
  *****************************************************************************/
 void cpia2_save_camera_state(struct camera_data *cam)
 {
-	get_color_params(cam);
 	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
 	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
 			 0);
@@ -1577,30 +1568,6 @@ void cpia2_save_camera_state(struct camera_data *cam)
 	/* Don't get framerate or target_kb. Trust the values we already have */
 }
 
-/******************************************************************************
- *
- *  get_color_params
- *
- *****************************************************************************/
-static void get_color_params(struct camera_data *cam)
-{
-	cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS, TRANSFER_READ, 0);
-	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION, TRANSFER_READ, 0);
-	cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST, TRANSFER_READ, 0);
-}
-
-/******************************************************************************
- *
- *  cpia2_set_color_params
- *
- *****************************************************************************/
-void cpia2_set_color_params(struct camera_data *cam)
-{
-	DBG("Setting color params\n");
-	cpia2_set_brightness(cam, cam->params.color_params.brightness);
-	cpia2_set_contrast(cam, cam->params.color_params.contrast);
-	cpia2_set_saturation(cam, cam->params.color_params.saturation);
-}
 
 /******************************************************************************
  *
@@ -1664,15 +1631,9 @@ int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
 
 	switch(mode) {
 	case NEVER_FLICKER:
-		cam->params.flicker_control.flicker_mode_req = mode;
-		break;
 	case FLICKER_60:
-		cam->params.flicker_control.flicker_mode_req = mode;
-		cam->params.flicker_control.mains_frequency = 60;
-		break;
 	case FLICKER_50:
 		cam->params.flicker_control.flicker_mode_req = mode;
-		cam->params.flicker_control.mains_frequency = 50;
 		break;
 	default:
 		err = -EINVAL;
@@ -1701,6 +1662,7 @@ void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
 	{
 		cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
 	}
+	cam->params.vp_params.user_effects = cam_reg;
 	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
 			 cam_reg);
 }
@@ -1725,37 +1687,13 @@ void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
 	{
 		cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
 	}
+	cam->params.vp_params.user_effects = cam_reg;
 	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
 			 cam_reg);
 }
 
 /******************************************************************************
  *
- *  set_target_kb
- *
- *  The new Target KB is set in cam->params.vc_params.target_kb and
- *  activates on reset.
- *****************************************************************************/
-
-int cpia2_set_target_kb(struct camera_data *cam, unsigned char value)
-{
-	DBG("Requested target_kb = %d\n", value);
-	if (value != cam->params.vc_params.target_kb) {
-
-		cpia2_usb_stream_pause(cam);
-
-		/* reset camera for new target_kb */
-		cam->params.vc_params.target_kb = value;
-		cpia2_reset_camera(cam);
-
-		cpia2_usb_stream_resume(cam);
-	}
-
-	return 0;
-}
-
-/******************************************************************************
- *
  *  cpia2_set_gpio
  *
  *****************************************************************************/
@@ -1843,7 +1781,7 @@ void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
 	if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
 		value++;
 	DBG("Setting brightness to %d (0x%0x)\n", value, value);
-	cpia2_do_command(cam,CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE,value);
+	cpia2_do_command(cam, CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE, value);
 }
 
 /******************************************************************************
@@ -1854,7 +1792,6 @@ void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
 void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
 {
 	DBG("Setting contrast to %d (0x%0x)\n", value, value);
-	cam->params.color_params.contrast = value;
 	cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
 }
 
@@ -1866,7 +1803,6 @@ void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
 void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
 {
 	DBG("Setting saturation to %d (0x%0x)\n", value, value);
-	cam->params.color_params.saturation = value;
 	cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
 }
 
@@ -2168,14 +2104,10 @@ static void reset_camera_struct(struct camera_data *cam)
 	/***
 	 * The following parameter values are the defaults from the register map.
 	 ***/
-	cam->params.color_params.brightness = DEFAULT_BRIGHTNESS;
-	cam->params.color_params.contrast = DEFAULT_CONTRAST;
-	cam->params.color_params.saturation = DEFAULT_SATURATION;
 	cam->params.vp_params.lowlight_boost = 0;
 
 	/* FlickerModes */
 	cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
-	cam->params.flicker_control.mains_frequency = 60;
 
 	/* jpeg params */
 	cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
@@ -2188,7 +2120,7 @@ static void reset_camera_struct(struct camera_data *cam)
 	cam->params.vp_params.gpio_data = 0;
 
 	/* Target kb params */
-	cam->params.vc_params.target_kb = DEFAULT_TARGET_KB;
+	cam->params.vc_params.quality = 100;
 
 	/***
 	 * Set Sensor FPS as fast as possible.
@@ -2228,7 +2160,7 @@ static void reset_camera_struct(struct camera_data *cam)
  *
  *  Initializes camera struct, does not call reset to fill in defaults.
  *****************************************************************************/
-struct camera_data *cpia2_init_camera_struct(void)
+struct camera_data *cpia2_init_camera_struct(struct usb_interface *intf)
 {
 	struct camera_data *cam;
 
@@ -2239,8 +2171,13 @@ struct camera_data *cpia2_init_camera_struct(void)
 		return NULL;
 	}
 
+	cam->v4l2_dev.release = cpia2_camera_release;
+	if (v4l2_device_register(&intf->dev, &cam->v4l2_dev) < 0) {
+		v4l2_err(&cam->v4l2_dev, "couldn't register v4l2_device\n");
+		kfree(cam);
+		return NULL;
+	}
 
-	cam->present = 1;
 	mutex_init(&cam->v4l2_lock);
 	init_waitqueue_head(&cam->wq_stream);
 
@@ -2373,11 +2310,6 @@ long cpia2_read(struct camera_data *cam,
 		return -EINVAL;
 	}
 
-	if (!cam->present) {
-		LOG("%s: camera removed\n",__func__);
-		return 0;	/* EOF */
-	}
-
 	if (!cam->streaming) {
 		/* Start streaming */
 		cpia2_usb_stream_start(cam,
@@ -2393,12 +2325,12 @@ long cpia2_read(struct camera_data *cam,
 	if (frame->status != FRAME_READY) {
 		mutex_unlock(&cam->v4l2_lock);
 		wait_event_interruptible(cam->wq_stream,
-			       !cam->present ||
+			       !video_is_registered(&cam->vdev) ||
 			       (frame = cam->curbuff)->status == FRAME_READY);
 		mutex_lock(&cam->v4l2_lock);
 		if (signal_pending(current))
 			return -ERESTARTSYS;
-		if (!cam->present)
+		if (!video_is_registered(&cam->vdev))
 			return 0;
 	}
 
@@ -2423,17 +2355,10 @@ long cpia2_read(struct camera_data *cam,
 unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
 			poll_table *wait)
 {
-	unsigned int status=0;
+	unsigned int status = v4l2_ctrl_poll(filp, wait);
 
-	if (!cam) {
-		ERR("%s: Internal error, camera_data not found!\n",__func__);
-		return POLLERR;
-	}
-
-	if (!cam->present)
-		return POLLHUP;
-
-	if(!cam->streaming) {
+	if ((poll_requested_events(wait) & (POLLIN | POLLRDNORM)) &&
+			!cam->streaming) {
 		/* Start streaming */
 		cpia2_usb_stream_start(cam,
 				       cam->params.camera_state.stream_mode);
@@ -2441,10 +2366,8 @@ unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
 
 	poll_wait(filp, &cam->wq_stream, wait);
 
-	if(!cam->present)
-		status = POLLHUP;
-	else if(cam->curbuff->status == FRAME_READY)
-		status = POLLIN | POLLRDNORM;
+	if (cam->curbuff->status == FRAME_READY)
+		status |= POLLIN | POLLRDNORM;
 
 	return status;
 }
@@ -2462,12 +2385,9 @@ int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
 	unsigned long start = (unsigned long) adr;
 	unsigned long page, pos;
 
-	if (!cam)
-		return -ENODEV;
-
 	DBG("mmap offset:%ld size:%ld\n", start_offset, size);
 
-	if (!cam->present)
+	if (!video_is_registered(&cam->vdev))
 		return -ENODEV;
 
 	if (size > cam->frame_size*cam->num_frames  ||
diff --git a/drivers/media/video/cpia2/cpia2_usb.c b/drivers/media/video/cpia2/cpia2_usb.c
index 59c797c15277..95b5d6e7cdc4 100644
--- a/drivers/media/video/cpia2/cpia2_usb.c
+++ b/drivers/media/video/cpia2/cpia2_usb.c
@@ -54,6 +54,8 @@ static void cpia2_usb_complete(struct urb *urb);
 static int cpia2_usb_probe(struct usb_interface *intf,
 			   const struct usb_device_id *id);
 static void cpia2_usb_disconnect(struct usb_interface *intf);
+static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message);
+static int cpia2_usb_resume(struct usb_interface *intf);
 
 static void free_sbufs(struct camera_data *cam);
 static void add_APPn(struct camera_data *cam);
@@ -74,6 +76,9 @@ static struct usb_driver cpia2_driver = {
 	.name		= "cpia2",
 	.probe		= cpia2_usb_probe,
 	.disconnect	= cpia2_usb_disconnect,
+	.suspend	= cpia2_usb_suspend,
+	.resume		= cpia2_usb_resume,
+	.reset_resume	= cpia2_usb_resume,
 	.id_table	= cpia2_id_table
 };
 
@@ -218,10 +223,9 @@ static void cpia2_usb_complete(struct urb *urb)
 		return;
 	}
 
-	if (!cam->streaming || !cam->present || cam->open_count == 0) {
-		LOG("Will now stop the streaming: streaming = %d, "
-		    "present=%d, open_count=%d\n",
-		    cam->streaming, cam->present, cam->open_count);
+	if (!cam->streaming || !video_is_registered(&cam->vdev)) {
+		LOG("Will now stop the streaming: streaming = %d, present=%d\n",
+		    cam->streaming, video_is_registered(&cam->vdev));
 		return;
 	}
 
@@ -392,7 +396,7 @@ static int configure_transfer_mode(struct camera_data *cam, unsigned int alt)
 	struct cpia2_command cmd;
 	unsigned char reg;
 
-	if(!cam->present)
+	if (!video_is_registered(&cam->vdev))
 		return -ENODEV;
 
 	/***
@@ -752,8 +756,8 @@ int cpia2_usb_stream_pause(struct camera_data *cam)
 {
 	int ret = 0;
 	if(cam->streaming) {
-		ret = set_alternate(cam, USBIF_CMDONLY);
 		free_sbufs(cam);
+		ret = set_alternate(cam, USBIF_CMDONLY);
 	}
 	return ret;
 }
@@ -770,6 +774,10 @@ int cpia2_usb_stream_resume(struct camera_data *cam)
 		cam->first_image_seen = 0;
 		ret = set_alternate(cam, cam->params.camera_state.stream_mode);
 		if(ret == 0) {
+			/* for some reason the user effects need to be set
+			   again when starting streaming. */
+			cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+					cam->params.vp_params.user_effects);
 			ret = submit_urbs(cam);
 		}
 	}
@@ -784,6 +792,7 @@ int cpia2_usb_stream_resume(struct camera_data *cam)
 int cpia2_usb_stream_stop(struct camera_data *cam)
 {
 	int ret;
+
 	ret = cpia2_usb_stream_pause(cam);
 	cam->streaming = 0;
 	configure_transfer_mode(cam, 0);
@@ -812,7 +821,8 @@ static int cpia2_usb_probe(struct usb_interface *intf,
 	/* If we get to this point, we found a CPiA2 camera */
 	LOG("CPiA2 USB camera found\n");
 
-	if((cam = cpia2_init_camera_struct()) == NULL)
+	cam = cpia2_init_camera_struct(intf);
+	if (cam == NULL)
 		return -ENOMEM;
 
 	cam->dev = udev;
@@ -825,16 +835,9 @@ static int cpia2_usb_probe(struct usb_interface *intf,
 		return ret;
 	}
 
-	if ((ret = cpia2_register_camera(cam)) < 0) {
-		ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
-		kfree(cam);
-		return ret;
-	}
-
 
 	if((ret = cpia2_init_camera(cam)) < 0) {
 		ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
-		cpia2_unregister_camera(cam);
 		kfree(cam);
 		return ret;
 	}
@@ -853,6 +856,13 @@ static int cpia2_usb_probe(struct usb_interface *intf,
 
 	usb_set_intfdata(intf, cam);
 
+	ret = cpia2_register_camera(cam);
+	if (ret < 0) {
+		ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
+		kfree(cam);
+		return ret;
+	}
+
 	return 0;
 }
 
@@ -865,13 +875,16 @@ static void cpia2_usb_disconnect(struct usb_interface *intf)
 {
 	struct camera_data *cam = usb_get_intfdata(intf);
 	usb_set_intfdata(intf, NULL);
-	cam->present = 0;
 
 	DBG("Stopping stream\n");
 	cpia2_usb_stream_stop(cam);
 
+	mutex_lock(&cam->v4l2_lock);
 	DBG("Unregistering camera\n");
 	cpia2_unregister_camera(cam);
+	v4l2_device_disconnect(&cam->v4l2_dev);
+	mutex_unlock(&cam->v4l2_lock);
+	v4l2_device_put(&cam->v4l2_dev);
 
 	if(cam->buffers) {
 		DBG("Wakeup waiting processes\n");
@@ -884,14 +897,41 @@ static void cpia2_usb_disconnect(struct usb_interface *intf)
 	DBG("Releasing interface\n");
 	usb_driver_release_interface(&cpia2_driver, intf);
 
-	if (cam->open_count == 0) {
-		DBG("Freeing camera structure\n");
-		kfree(cam);
+	LOG("CPiA2 camera disconnected.\n");
+}
+
+static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+
+	mutex_lock(&cam->v4l2_lock);
+	if (cam->streaming) {
+		cpia2_usb_stream_stop(cam);
+		cam->streaming = 1;
 	}
+	mutex_unlock(&cam->v4l2_lock);
 
-	LOG("CPiA2 camera disconnected.\n");
+	dev_info(&intf->dev, "going into suspend..\n");
+	return 0;
 }
 
+/* Resume device - start device. */
+static int cpia2_usb_resume(struct usb_interface *intf)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+
+	mutex_lock(&cam->v4l2_lock);
+	v4l2_ctrl_handler_setup(&cam->hdl);
+	if (cam->streaming) {
+		cam->streaming = 0;
+		cpia2_usb_stream_start(cam,
+				cam->params.camera_state.stream_mode);
+	}
+	mutex_unlock(&cam->v4l2_lock);
+
+	dev_info(&intf->dev, "coming out of suspend..\n");
+	return 0;
+}
 
 /******************************************************************************
  *
diff --git a/drivers/media/video/cpia2/cpia2_v4l.c b/drivers/media/video/cpia2/cpia2_v4l.c
index 077eb1db80a1..55e92902a76c 100644
--- a/drivers/media/video/cpia2/cpia2_v4l.c
+++ b/drivers/media/video/cpia2/cpia2_v4l.c
@@ -39,15 +39,15 @@
 #include <linux/videodev2.h>
 #include <linux/stringify.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 
 #include "cpia2.h"
-#include "cpia2dev.h"
 
 static int video_nr = -1;
 module_param(video_nr, int, 0);
-MODULE_PARM_DESC(video_nr,"video device to register (0=/dev/video0, etc)");
+MODULE_PARM_DESC(video_nr, "video device to register (0=/dev/video0, etc)");
 
-static int buffer_size = 68*1024;
+static int buffer_size = 68 * 1024;
 module_param(buffer_size, int, 0);
 MODULE_PARM_DESC(buffer_size, "Size for each frame buffer in bytes (default 68k)");
 
@@ -62,18 +62,10 @@ MODULE_PARM_DESC(alternate, "USB Alternate (" __stringify(USBIF_ISO_1) "-"
 		 __stringify(USBIF_ISO_6) ", default "
 		 __stringify(DEFAULT_ALT) ")");
 
-static int flicker_freq = 60;
-module_param(flicker_freq, int, 0);
-MODULE_PARM_DESC(flicker_freq, "Flicker frequency (" __stringify(50) "or"
-		 __stringify(60) ", default "
-		 __stringify(60) ")");
-
-static int flicker_mode = NEVER_FLICKER;
+static int flicker_mode;
 module_param(flicker_mode, int, 0);
-MODULE_PARM_DESC(flicker_mode,
-		 "Flicker supression (" __stringify(NEVER_FLICKER) "or"
-		 __stringify(ANTI_FLICKER_ON) ", default "
-		 __stringify(NEVER_FLICKER) ")");
+MODULE_PARM_DESC(flicker_mode, "Flicker frequency (0 (disabled), " __stringify(50) " or "
+		 __stringify(60) ", default 0)");
 
 MODULE_AUTHOR("Steve Miller (STMicroelectronics) <steve.miller@st.com>");
 MODULE_DESCRIPTION("V4L-driver for STMicroelectronics CPiA2 based cameras");
@@ -82,153 +74,7 @@ MODULE_LICENSE("GPL");
 MODULE_VERSION(CPIA_VERSION);
 
 #define ABOUT "V4L-Driver for Vision CPiA2 based cameras"
-
-struct control_menu_info {
-	int value;
-	char name[32];
-};
-
-static struct control_menu_info framerate_controls[] =
-{
-	{ CPIA2_VP_FRAMERATE_6_25, "6.25 fps" },
-	{ CPIA2_VP_FRAMERATE_7_5,  "7.5 fps"  },
-	{ CPIA2_VP_FRAMERATE_12_5, "12.5 fps" },
-	{ CPIA2_VP_FRAMERATE_15,   "15 fps"   },
-	{ CPIA2_VP_FRAMERATE_25,   "25 fps"   },
-	{ CPIA2_VP_FRAMERATE_30,   "30 fps"   },
-};
-#define NUM_FRAMERATE_CONTROLS (ARRAY_SIZE(framerate_controls))
-
-static struct control_menu_info flicker_controls[] =
-{
-	{ NEVER_FLICKER, "Off" },
-	{ FLICKER_50,    "50 Hz" },
-	{ FLICKER_60,    "60 Hz"  },
-};
-#define NUM_FLICKER_CONTROLS (ARRAY_SIZE(flicker_controls))
-
-static struct control_menu_info lights_controls[] =
-{
-	{ 0,   "Off" },
-	{ 64,  "Top" },
-	{ 128, "Bottom"  },
-	{ 192, "Both"  },
-};
-#define NUM_LIGHTS_CONTROLS (ARRAY_SIZE(lights_controls))
-#define GPIO_LIGHTS_MASK 192
-
-static struct v4l2_queryctrl controls[] = {
-	{
-		.id            = V4L2_CID_BRIGHTNESS,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Brightness",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_BRIGHTNESS,
-	},
-	{
-		.id            = V4L2_CID_CONTRAST,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Contrast",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_CONTRAST,
-	},
-	{
-		.id            = V4L2_CID_SATURATION,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Saturation",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_SATURATION,
-	},
-	{
-		.id            = V4L2_CID_HFLIP,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		.name          = "Mirror Horizontally",
-		.minimum       = 0,
-		.maximum       = 1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = V4L2_CID_VFLIP,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		.name          = "Flip Vertically",
-		.minimum       = 0,
-		.maximum       = 1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_TARGET_KB,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Target KB",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_TARGET_KB,
-	},
-	{
-		.id            = CPIA2_CID_GPIO,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "GPIO",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_FLICKER_MODE,
-		.type          = V4L2_CTRL_TYPE_MENU,
-		.name          = "Flicker Reduction",
-		.minimum       = 0,
-		.maximum       = NUM_FLICKER_CONTROLS-1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_FRAMERATE,
-		.type          = V4L2_CTRL_TYPE_MENU,
-		.name          = "Framerate",
-		.minimum       = 0,
-		.maximum       = NUM_FRAMERATE_CONTROLS-1,
-		.step          = 1,
-		.default_value = NUM_FRAMERATE_CONTROLS-1,
-	},
-	{
-		.id            = CPIA2_CID_USB_ALT,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "USB Alternate",
-		.minimum       = USBIF_ISO_1,
-		.maximum       = USBIF_ISO_6,
-		.step          = 1,
-		.default_value = DEFAULT_ALT,
-	},
-	{
-		.id            = CPIA2_CID_LIGHTS,
-		.type          = V4L2_CTRL_TYPE_MENU,
-		.name          = "Lights",
-		.minimum       = 0,
-		.maximum       = NUM_LIGHTS_CONTROLS-1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_RESET_CAMERA,
-		.type          = V4L2_CTRL_TYPE_BUTTON,
-		.name          = "Reset Camera",
-		.minimum       = 0,
-		.maximum       = 0,
-		.step          = 0,
-		.default_value = 0,
-	},
-};
-#define NUM_CONTROLS (ARRAY_SIZE(controls))
-
+#define CPIA2_CID_USB_ALT (V4L2_CID_USER_BASE | 0xf000)
 
 /******************************************************************************
  *
@@ -238,38 +84,27 @@ static struct v4l2_queryctrl controls[] = {
 static int cpia2_open(struct file *file)
 {
 	struct camera_data *cam = video_drvdata(file);
-	struct cpia2_fh *fh;
-
-	if (!cam) {
-		ERR("Internal error, camera_data not found!\n");
-		return -ENODEV;
-	}
+	int retval = v4l2_fh_open(file);
 
-	if (!cam->present)
-		return -ENODEV;
+	if (retval)
+		return retval;
 
-	if (cam->open_count == 0) {
-		if (cpia2_allocate_buffers(cam))
+	if (v4l2_fh_is_singular_file(file)) {
+		if (cpia2_allocate_buffers(cam)) {
+			v4l2_fh_release(file);
 			return -ENOMEM;
+		}
 
 		/* reset the camera */
-		if (cpia2_reset_camera(cam) < 0)
+		if (cpia2_reset_camera(cam) < 0) {
+			v4l2_fh_release(file);
 			return -EIO;
+		}
 
 		cam->APP_len = 0;
 		cam->COM_len = 0;
 	}
 
-	fh = kmalloc(sizeof(*fh), GFP_KERNEL);
-	if (!fh)
-		return -ENOMEM;
-	file->private_data = fh;
-	fh->prio = V4L2_PRIORITY_UNSET;
-	v4l2_prio_open(&cam->prio, &fh->prio);
-	fh->mmapped = 0;
-
-	++cam->open_count;
-
 	cpia2_dbg_dump_registers(cam);
 	return 0;
 }
@@ -283,37 +118,22 @@ static int cpia2_close(struct file *file)
 {
 	struct video_device *dev = video_devdata(file);
 	struct camera_data *cam = video_get_drvdata(dev);
-	struct cpia2_fh *fh = file->private_data;
 
-	if (cam->present &&
-	    (cam->open_count == 1 || fh->prio == V4L2_PRIORITY_RECORD)) {
+	if (video_is_registered(&cam->vdev) && v4l2_fh_is_singular_file(file)) {
 		cpia2_usb_stream_stop(cam);
 
-		if (cam->open_count == 1) {
-			/* save camera state for later open */
-			cpia2_save_camera_state(cam);
+		/* save camera state for later open */
+		cpia2_save_camera_state(cam);
 
-			cpia2_set_low_power(cam);
-			cpia2_free_buffers(cam);
-		}
+		cpia2_set_low_power(cam);
+		cpia2_free_buffers(cam);
 	}
 
-	if (fh->mmapped)
+	if (cam->stream_fh == file->private_data) {
+		cam->stream_fh = NULL;
 		cam->mmapped = 0;
-	v4l2_prio_close(&cam->prio, fh->prio);
-	file->private_data = NULL;
-	kfree(fh);
-
-	if (--cam->open_count == 0) {
-		cpia2_free_buffers(cam);
-		if (!cam->present) {
-			video_unregister_device(dev);
-			kfree(cam);
-			return 0;
-		}
 	}
-
-	return 0;
+	return v4l2_fh_release(file);
 }
 
 /******************************************************************************
@@ -327,16 +147,9 @@ static ssize_t cpia2_v4l_read(struct file *file, char __user *buf, size_t count,
 	struct camera_data *cam = video_drvdata(file);
 	int noblock = file->f_flags&O_NONBLOCK;
 
-	struct cpia2_fh *fh = file->private_data;
-
 	if(!cam)
 		return -EINVAL;
 
-	/* Priority check */
-	if(fh->prio != V4L2_PRIORITY_RECORD) {
-		return -EBUSY;
-	}
-
 	return cpia2_read(cam, buf, count, noblock);
 }
 
@@ -349,15 +162,6 @@ static ssize_t cpia2_v4l_read(struct file *file, char __user *buf, size_t count,
 static unsigned int cpia2_v4l_poll(struct file *filp, struct poll_table_struct *wait)
 {
 	struct camera_data *cam = video_drvdata(filp);
-	struct cpia2_fh *fh = filp->private_data;
-
-	if(!cam)
-		return POLLERR;
-
-	/* Priority check */
-	if(fh->prio != V4L2_PRIORITY_RECORD) {
-		return POLLERR;
-	}
 
 	return cpia2_poll(cam, filp, wait);
 }
@@ -384,36 +188,13 @@ static int sync(struct camera_data *cam, int frame_nr)
 		mutex_lock(&cam->v4l2_lock);
 		if (signal_pending(current))
 			return -ERESTARTSYS;
-		if(!cam->present)
+		if (!video_is_registered(&cam->vdev))
 			return -ENOTTY;
 	}
 }
 
 /******************************************************************************
  *
- *  ioctl_set_gpio
- *
- *****************************************************************************/
-
-static long cpia2_default(struct file *file, void *fh, bool valid_prio,
-			  int cmd, void *arg)
-{
-	struct camera_data *cam = video_drvdata(file);
-	__u32 gpio_val;
-
-	if (cmd != CPIA2_CID_GPIO)
-		return -EINVAL;
-
-	gpio_val = *(__u32*) arg;
-
-	if (gpio_val &~ 0xFFU)
-		return -EINVAL;
-
-	return cpia2_set_gpio(cam, (unsigned char)gpio_val);
-}
-
-/******************************************************************************
- *
  *  ioctl_querycap
  *
  *  V4L2 device capabilities
@@ -465,9 +246,11 @@ static int cpia2_querycap(struct file *file, void *fh, struct v4l2_capability *v
 	if (usb_make_path(cam->dev, vc->bus_info, sizeof(vc->bus_info)) <0)
 		memset(vc->bus_info,0, sizeof(vc->bus_info));
 
-	vc->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+	vc->device_caps = V4L2_CAP_VIDEO_CAPTURE |
 			   V4L2_CAP_READWRITE |
 			   V4L2_CAP_STREAMING;
+	vc->capabilities = vc->device_caps |
+			   V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
@@ -610,22 +393,12 @@ static int cpia2_s_fmt_vid_cap(struct file *file, void *_fh,
 					struct v4l2_format *f)
 {
 	struct camera_data *cam = video_drvdata(file);
-	struct cpia2_fh *fh = _fh;
 	int err, frame;
 
-	err = v4l2_prio_check(&cam->prio, fh->prio);
-	if (err)
-		return err;
 	err = cpia2_try_fmt_vid_cap(file, _fh, f);
 	if(err != 0)
 		return err;
 
-	/* Ensure that only this process can change the format. */
-	err = v4l2_prio_change(&cam->prio, &fh->prio, V4L2_PRIORITY_RECORD);
-	if(err != 0) {
-		return err;
-	}
-
 	cam->pixelformat = f->fmt.pix.pixelformat;
 
 	/* NOTE: This should be set to 1 for MJPEG, but some apps don't handle
@@ -713,240 +486,126 @@ static int cpia2_cropcap(struct file *file, void *fh, struct v4l2_cropcap *c)
 	return 0;
 }
 
-/******************************************************************************
- *
- *  ioctl_queryctrl
- *
- *  V4L2 query possible control variables
- *
- *****************************************************************************/
+struct framerate_info {
+	int value;
+	struct v4l2_fract period;
+};
 
-static int cpia2_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *c)
+static const struct framerate_info framerate_controls[] = {
+	{ CPIA2_VP_FRAMERATE_6_25, { 4, 25 } },
+	{ CPIA2_VP_FRAMERATE_7_5,  { 2, 15 } },
+	{ CPIA2_VP_FRAMERATE_12_5, { 2, 25 } },
+	{ CPIA2_VP_FRAMERATE_15,   { 1, 15 } },
+	{ CPIA2_VP_FRAMERATE_25,   { 1, 25 } },
+	{ CPIA2_VP_FRAMERATE_30,   { 1, 30 } },
+};
+
+static int cpia2_g_parm(struct file *file, void *fh, struct v4l2_streamparm *p)
 {
 	struct camera_data *cam = video_drvdata(file);
+	struct v4l2_captureparm *cap = &p->parm.capture;
 	int i;
 
-	for(i=0; i<NUM_CONTROLS; ++i) {
-		if(c->id == controls[i].id) {
-			memcpy(c, controls+i, sizeof(*c));
-			break;
-		}
-	}
-
-	if(i == NUM_CONTROLS)
+	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	/* Some devices have additional limitations */
-	switch(c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		/***
-		 * Don't let the register be set to zero - bug in VP4
-		 * flash of full brightness
-		 ***/
-		if (cam->params.pnp_id.device_type == DEVICE_STV_672)
-			c->minimum = 1;
-		break;
-	case V4L2_CID_VFLIP:
-		// VP5 Only
-		if(cam->params.pnp_id.device_type == DEVICE_STV_672)
-			c->flags |= V4L2_CTRL_FLAG_DISABLED;
-		break;
-	case CPIA2_CID_FRAMERATE:
-		if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
-		   cam->params.version.sensor_flags==CPIA2_VP_SENSOR_FLAGS_500){
-			// Maximum 15fps
-			for(i=0; i<c->maximum; ++i) {
-				if(framerate_controls[i].value ==
-				   CPIA2_VP_FRAMERATE_15) {
-					c->maximum = i;
-					c->default_value = i;
-				}
-			}
+	cap->capability = V4L2_CAP_TIMEPERFRAME;
+	cap->readbuffers = cam->num_frames;
+	for (i = 0; i < ARRAY_SIZE(framerate_controls); i++)
+		if (cam->params.vp_params.frame_rate == framerate_controls[i].value) {
+			cap->timeperframe = framerate_controls[i].period;
+			break;
 		}
-		break;
-	case CPIA2_CID_FLICKER_MODE:
-		// Flicker control only valid for 672.
-		if(cam->params.pnp_id.device_type != DEVICE_STV_672)
-			c->flags |= V4L2_CTRL_FLAG_DISABLED;
-		break;
-	case CPIA2_CID_LIGHTS:
-		// Light control only valid for the QX5 Microscope.
-		if(cam->params.pnp_id.product != 0x151)
-			c->flags |= V4L2_CTRL_FLAG_DISABLED;
-		break;
-	default:
-		break;
-	}
-
 	return 0;
 }
 
-/******************************************************************************
- *
- *  ioctl_querymenu
- *
- *  V4L2 query possible control variables
- *
- *****************************************************************************/
-
-static int cpia2_querymenu(struct file *file, void *fh, struct v4l2_querymenu *m)
+static int cpia2_s_parm(struct file *file, void *fh, struct v4l2_streamparm *p)
 {
 	struct camera_data *cam = video_drvdata(file);
+	struct v4l2_captureparm *cap = &p->parm.capture;
+	struct v4l2_fract tpf = cap->timeperframe;
+	int max = ARRAY_SIZE(framerate_controls) - 1;
+	int ret;
+	int i;
 
-	switch(m->id) {
-	case CPIA2_CID_FLICKER_MODE:
-		if (m->index >= NUM_FLICKER_CONTROLS)
-			return -EINVAL;
+	ret = cpia2_g_parm(file, fh, p);
+	if (ret || !tpf.denominator || !tpf.numerator)
+		return ret;
+
+	/* Maximum 15 fps for this model */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+		max -= 2;
+	for (i = 0; i <= max; i++) {
+		struct v4l2_fract f1 = tpf;
+		struct v4l2_fract f2 = framerate_controls[i].period;
+
+		f1.numerator *= f2.denominator;
+		f2.numerator *= f1.denominator;
+		if (f1.numerator >= f2.numerator)
+			break;
+	}
+	if (i > max)
+		i = max;
+	cap->timeperframe = framerate_controls[i].period;
+	return cpia2_set_fps(cam, framerate_controls[i].value);
+}
 
-		strcpy(m->name, flicker_controls[m->index].name);
-		break;
-	case CPIA2_CID_FRAMERATE:
-	    {
-		int maximum = NUM_FRAMERATE_CONTROLS - 1;
-		if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
-		   cam->params.version.sensor_flags==CPIA2_VP_SENSOR_FLAGS_500){
-			// Maximum 15fps
-			int i;
-			for(i=0; i<maximum; ++i) {
-				if(framerate_controls[i].value ==
-				   CPIA2_VP_FRAMERATE_15)
-					maximum = i;
-			}
-		}
-		if (m->index > maximum)
-			return -EINVAL;
+static const struct {
+	u32 width;
+	u32 height;
+} cpia2_framesizes[] = {
+	{ 640, 480 },
+	{ 352, 288 },
+	{ 320, 240 },
+	{ 288, 216 },
+	{ 256, 192 },
+	{ 224, 168 },
+	{ 192, 144 },
+	{ 176, 144 },
+};
 
-		strcpy(m->name, framerate_controls[m->index].name);
-		break;
-	    }
-	case CPIA2_CID_LIGHTS:
-		if (m->index >= NUM_LIGHTS_CONTROLS)
-			return -EINVAL;
+static int cpia2_enum_framesizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *fsize)
+{
 
-		strcpy(m->name, lights_controls[m->index].name);
-		break;
-	default:
+	if (fsize->pixel_format != V4L2_PIX_FMT_MJPEG &&
+	    fsize->pixel_format != V4L2_PIX_FMT_JPEG)
 		return -EINVAL;
-	}
+	if (fsize->index >= ARRAY_SIZE(cpia2_framesizes))
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = cpia2_framesizes[fsize->index].width;
+	fsize->discrete.height = cpia2_framesizes[fsize->index].height;
 
 	return 0;
 }
 
-/******************************************************************************
- *
- *  ioctl_g_ctrl
- *
- *  V4L2 get the value of a control variable
- *
- *****************************************************************************/
-
-static int cpia2_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+static int cpia2_enum_frameintervals(struct file *file, void *fh,
+					   struct v4l2_frmivalenum *fival)
 {
 	struct camera_data *cam = video_drvdata(file);
+	int max = ARRAY_SIZE(framerate_controls) - 1;
+	int i;
 
-	switch(c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.color_params.brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.color_params.contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.color_params.saturation;
-		break;
-	case V4L2_CID_HFLIP:
-		cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS,
-				 TRANSFER_READ, 0);
-		c->value = (cam->params.vp_params.user_effects &
-			    CPIA2_VP_USER_EFFECTS_MIRROR) != 0;
-		break;
-	case V4L2_CID_VFLIP:
-		cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS,
-				 TRANSFER_READ, 0);
-		c->value = (cam->params.vp_params.user_effects &
-			    CPIA2_VP_USER_EFFECTS_FLIP) != 0;
-		break;
-	case CPIA2_CID_TARGET_KB:
-		c->value = cam->params.vc_params.target_kb;
-		break;
-	case CPIA2_CID_GPIO:
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_GPIO_DATA,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.vp_params.gpio_data;
-		break;
-	case CPIA2_CID_FLICKER_MODE:
-	{
-		int i, mode;
-		cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
-				 TRANSFER_READ, 0);
-		if(cam->params.flicker_control.cam_register &
-		   CPIA2_VP_FLICKER_MODES_NEVER_FLICKER) {
-			mode = NEVER_FLICKER;
-		} else {
-		    if(cam->params.flicker_control.cam_register &
-		       CPIA2_VP_FLICKER_MODES_50HZ) {
-			mode = FLICKER_50;
-		    } else {
-			mode = FLICKER_60;
-		    }
-		}
-		for(i=0; i<NUM_FLICKER_CONTROLS; i++) {
-			if(flicker_controls[i].value == mode) {
-				c->value = i;
-				break;
-			}
-		}
-		if(i == NUM_FLICKER_CONTROLS)
-			return -EINVAL;
-		break;
-	}
-	case CPIA2_CID_FRAMERATE:
-	{
-		int maximum = NUM_FRAMERATE_CONTROLS - 1;
-		int i;
-		for(i=0; i<= maximum; i++) {
-			if(cam->params.vp_params.frame_rate ==
-			   framerate_controls[i].value)
-				break;
-		}
-		if(i > maximum)
-			return -EINVAL;
-		c->value = i;
-		break;
-	}
-	case CPIA2_CID_USB_ALT:
-		c->value = cam->params.camera_state.stream_mode;
-		break;
-	case CPIA2_CID_LIGHTS:
-	{
-		int i;
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_GPIO_DATA,
-				 TRANSFER_READ, 0);
-		for(i=0; i<NUM_LIGHTS_CONTROLS; i++) {
-			if((cam->params.vp_params.gpio_data&GPIO_LIGHTS_MASK) ==
-			   lights_controls[i].value) {
-				break;
-			}
-		}
-		if(i == NUM_LIGHTS_CONTROLS)
-			return -EINVAL;
-		c->value = i;
-		break;
-	}
-	case CPIA2_CID_RESET_CAMERA:
+	if (fival->pixel_format != V4L2_PIX_FMT_MJPEG &&
+	    fival->pixel_format != V4L2_PIX_FMT_JPEG)
 		return -EINVAL;
-	default:
-		return -EINVAL;
-	}
-
-	DBG("Get control id:%d, value:%d\n", c->id, c->value);
 
+	/* Maximum 15 fps for this model */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+		max -= 2;
+	if (fival->index > max)
+		return -EINVAL;
+	for (i = 0; i < ARRAY_SIZE(cpia2_framesizes); i++)
+		if (fival->width == cpia2_framesizes[i].width &&
+		    fival->height == cpia2_framesizes[i].height)
+			break;
+	if (i == ARRAY_SIZE(cpia2_framesizes))
+		return -EINVAL;
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = framerate_controls[fival->index].period;
 	return 0;
 }
 
@@ -958,72 +617,54 @@ static int cpia2_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
  *
  *****************************************************************************/
 
-static int cpia2_s_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+static int cpia2_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct camera_data *cam = video_drvdata(file);
-	int i;
-	int retval = 0;
+	struct camera_data *cam =
+		container_of(ctrl->handler, struct camera_data, hdl);
+	static const int flicker_table[] = {
+		NEVER_FLICKER,
+		FLICKER_50,
+		FLICKER_60,
+	};
 
-	DBG("Set control id:%d, value:%d\n", c->id, c->value);
-
-	/* Check that the value is in range */
-	for(i=0; i<NUM_CONTROLS; i++) {
-		if(c->id == controls[i].id) {
-			if(c->value < controls[i].minimum ||
-			   c->value > controls[i].maximum) {
-				return -EINVAL;
-			}
-			break;
-		}
-	}
-	if(i == NUM_CONTROLS)
-		return -EINVAL;
+	DBG("Set control id:%d, value:%d\n", ctrl->id, ctrl->val);
 
-	switch(c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		cpia2_set_brightness(cam, c->value);
+		cpia2_set_brightness(cam, ctrl->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		cpia2_set_contrast(cam, c->value);
+		cpia2_set_contrast(cam, ctrl->val);
 		break;
 	case V4L2_CID_SATURATION:
-		cpia2_set_saturation(cam, c->value);
+		cpia2_set_saturation(cam, ctrl->val);
 		break;
 	case V4L2_CID_HFLIP:
-		cpia2_set_property_mirror(cam, c->value);
+		cpia2_set_property_mirror(cam, ctrl->val);
 		break;
 	case V4L2_CID_VFLIP:
-		cpia2_set_property_flip(cam, c->value);
+		cpia2_set_property_flip(cam, ctrl->val);
 		break;
-	case CPIA2_CID_TARGET_KB:
-		retval = cpia2_set_target_kb(cam, c->value);
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		return cpia2_set_flicker_mode(cam, flicker_table[ctrl->val]);
+	case V4L2_CID_ILLUMINATORS_1:
+		return cpia2_set_gpio(cam, (cam->top_light->val << 6) |
+					   (cam->bottom_light->val << 7));
+	case V4L2_CID_JPEG_ACTIVE_MARKER:
+		cam->params.compression.inhibit_htables =
+			!(ctrl->val & V4L2_JPEG_ACTIVE_MARKER_DHT);
 		break;
-	case CPIA2_CID_GPIO:
-		retval = cpia2_set_gpio(cam, c->value);
-		break;
-	case CPIA2_CID_FLICKER_MODE:
-		retval = cpia2_set_flicker_mode(cam,
-					      flicker_controls[c->value].value);
-		break;
-	case CPIA2_CID_FRAMERATE:
-		retval = cpia2_set_fps(cam, framerate_controls[c->value].value);
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		cam->params.vc_params.quality = ctrl->val;
 		break;
 	case CPIA2_CID_USB_ALT:
-		retval = cpia2_usb_change_streaming_alternate(cam, c->value);
-		break;
-	case CPIA2_CID_LIGHTS:
-		retval = cpia2_set_gpio(cam, lights_controls[c->value].value);
-		break;
-	case CPIA2_CID_RESET_CAMERA:
-		cpia2_usb_stream_pause(cam);
-		cpia2_reset_camera(cam);
-		cpia2_usb_stream_resume(cam);
+		cam->params.camera_state.stream_mode = ctrl->val;
 		break;
 	default:
-		retval = -EINVAL;
+		return -EINVAL;
 	}
 
-	return retval;
+	return 0;
 }
 
 /******************************************************************************
@@ -1084,6 +725,8 @@ static int cpia2_s_jpegcomp(struct file *file, void *fh, struct v4l2_jpegcompres
 
 	cam->params.compression.inhibit_htables =
 		!(parms->jpeg_markers & V4L2_JPEG_MARKER_DHT);
+	parms->jpeg_markers &= V4L2_JPEG_MARKER_DQT | V4L2_JPEG_MARKER_DRI |
+			       V4L2_JPEG_MARKER_DHT;
 
 	if(parms->APP_len != 0) {
 		if(parms->APP_len > 0 &&
@@ -1270,12 +913,12 @@ static int cpia2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 		struct framebuf *cb=cam->curbuff;
 		mutex_unlock(&cam->v4l2_lock);
 		wait_event_interruptible(cam->wq_stream,
-					 !cam->present ||
+					 !video_is_registered(&cam->vdev) ||
 					 (cb=cam->curbuff)->status == FRAME_READY);
 		mutex_lock(&cam->v4l2_lock);
 		if (signal_pending(current))
 			return -ERESTARTSYS;
-		if(!cam->present)
+		if (!video_is_registered(&cam->vdev))
 			return -ENOTTY;
 		frame = cb->num;
 	}
@@ -1299,56 +942,39 @@ static int cpia2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 	return 0;
 }
 
-static int cpia2_g_priority(struct file *file, void *_fh, enum v4l2_priority *p)
-{
-	struct cpia2_fh *fh = _fh;
-
-	*p = fh->prio;
-	return 0;
-}
-
-static int cpia2_s_priority(struct file *file, void *_fh, enum v4l2_priority prio)
-{
-	struct camera_data *cam = video_drvdata(file);
-	struct cpia2_fh *fh = _fh;
-
-	if (cam->streaming && prio != fh->prio &&
-			fh->prio == V4L2_PRIORITY_RECORD)
-		/* Can't drop record priority while streaming */
-		return -EBUSY;
-
-	if (prio == V4L2_PRIORITY_RECORD && prio != fh->prio &&
-			v4l2_prio_max(&cam->prio) == V4L2_PRIORITY_RECORD)
-		/* Only one program can record at a time */
-		return -EBUSY;
-	return v4l2_prio_change(&cam->prio, &fh->prio, prio);
-}
-
 static int cpia2_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 {
 	struct camera_data *cam = video_drvdata(file);
+	int ret = -EINVAL;
 
 	DBG("VIDIOC_STREAMON, streaming=%d\n", cam->streaming);
 	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	if (!cam->streaming)
-		return cpia2_usb_stream_start(cam,
+	if (!cam->streaming) {
+		ret = cpia2_usb_stream_start(cam,
 				cam->params.camera_state.stream_mode);
-	return -EINVAL;
+		if (!ret)
+			v4l2_ctrl_grab(cam->usb_alt, true);
+	}
+	return ret;
 }
 
 static int cpia2_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 {
 	struct camera_data *cam = video_drvdata(file);
+	int ret = -EINVAL;
 
 	DBG("VIDIOC_STREAMOFF, streaming=%d\n", cam->streaming);
 	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	if (cam->streaming)
-		return cpia2_usb_stream_stop(cam);
-	return -EINVAL;
+	if (cam->streaming) {
+		ret = cpia2_usb_stream_stop(cam);
+		if (!ret)
+			v4l2_ctrl_grab(cam->usb_alt, false);
+	}
+	return ret;
 }
 
 /******************************************************************************
@@ -1361,16 +987,10 @@ static int cpia2_mmap(struct file *file, struct vm_area_struct *area)
 	struct camera_data *cam = video_drvdata(file);
 	int retval;
 
-	/* Priority check */
-	struct cpia2_fh *fh = file->private_data;
-	if(fh->prio != V4L2_PRIORITY_RECORD) {
-		return -EBUSY;
-	}
-
 	retval = cpia2_remap_buffer(cam, area);
 
 	if(!retval)
-		fh->mmapped = 1;
+		cam->stream_fh = file->private_data;
 	return retval;
 }
 
@@ -1388,15 +1008,13 @@ static void reset_camera_struct_v4l(struct camera_data *cam)
 	cam->frame_size = buffer_size;
 	cam->num_frames = num_buffers;
 
-	/* FlickerModes */
+	/* Flicker modes */
 	cam->params.flicker_control.flicker_mode_req = flicker_mode;
-	cam->params.flicker_control.mains_frequency = flicker_freq;
 
-	/* streamMode */
+	/* stream modes */
 	cam->params.camera_state.stream_mode = alternate;
 
 	cam->pixelformat = V4L2_PIX_FMT_JPEG;
-	v4l2_prio_init(&cam->prio);
 }
 
 static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
@@ -1408,10 +1026,6 @@ static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap		    = cpia2_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap		    = cpia2_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap		    = cpia2_try_fmt_vid_cap,
-	.vidioc_queryctrl		    = cpia2_queryctrl,
-	.vidioc_querymenu		    = cpia2_querymenu,
-	.vidioc_g_ctrl			    = cpia2_g_ctrl,
-	.vidioc_s_ctrl			    = cpia2_s_ctrl,
 	.vidioc_g_jpegcomp		    = cpia2_g_jpegcomp,
 	.vidioc_s_jpegcomp		    = cpia2_s_jpegcomp,
 	.vidioc_cropcap			    = cpia2_cropcap,
@@ -1421,9 +1035,12 @@ static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
 	.vidioc_dqbuf			    = cpia2_dqbuf,
 	.vidioc_streamon		    = cpia2_streamon,
 	.vidioc_streamoff		    = cpia2_streamoff,
-	.vidioc_g_priority		    = cpia2_g_priority,
-	.vidioc_s_priority		    = cpia2_s_priority,
-	.vidioc_default			    = cpia2_default,
+	.vidioc_s_parm			    = cpia2_s_parm,
+	.vidioc_g_parm			    = cpia2_g_parm,
+	.vidioc_enum_framesizes		    = cpia2_enum_framesizes,
+	.vidioc_enum_frameintervals	    = cpia2_enum_frameintervals,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
 };
 
 /***
@@ -1444,7 +1061,21 @@ static struct video_device cpia2_template = {
 	.name =		"CPiA2 Camera",
 	.fops =		&cpia2_fops,
 	.ioctl_ops =	&cpia2_ioctl_ops,
-	.release =	video_device_release,
+	.release =	video_device_release_empty,
+};
+
+void cpia2_camera_release(struct v4l2_device *v4l2_dev)
+{
+	struct camera_data *cam =
+		container_of(v4l2_dev, struct camera_data, v4l2_dev);
+
+	v4l2_ctrl_handler_free(&cam->hdl);
+	v4l2_device_unregister(&cam->v4l2_dev);
+	kfree(cam);
+}
+
+static const struct v4l2_ctrl_ops cpia2_ctrl_ops = {
+	.s_ctrl = cpia2_s_ctrl,
 };
 
 /******************************************************************************
@@ -1454,20 +1085,78 @@ static struct video_device cpia2_template = {
  *****************************************************************************/
 int cpia2_register_camera(struct camera_data *cam)
 {
-	cam->vdev = video_device_alloc();
-	if(!cam->vdev)
-		return -ENOMEM;
+	struct v4l2_ctrl_handler *hdl = &cam->hdl;
+	struct v4l2_ctrl_config cpia2_usb_alt = {
+		.ops = &cpia2_ctrl_ops,
+		.id = CPIA2_CID_USB_ALT,
+		.name = "USB Alternate",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = USBIF_ISO_1,
+		.max = USBIF_ISO_6,
+		.step = 1,
+	};
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 12);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_BRIGHTNESS,
+			cam->params.pnp_id.device_type == DEVICE_STV_672 ? 1 : 0,
+			255, 1, DEFAULT_BRIGHTNESS);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, DEFAULT_CONTRAST);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, DEFAULT_SATURATION);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_JPEG_ACTIVE_MARKER, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DHT, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DHT);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 1,
+			100, 1, 100);
+	cpia2_usb_alt.def = alternate;
+	cam->usb_alt = v4l2_ctrl_new_custom(hdl, &cpia2_usb_alt, NULL);
+	/* VP5 Only */
+	if (cam->params.pnp_id.device_type != DEVICE_STV_672)
+		v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	/* Flicker control only valid for 672 */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+		v4l2_ctrl_new_std_menu(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0, 0);
+	/* Light control only valid for the QX5 Microscope */
+	if (cam->params.pnp_id.product == 0x151) {
+		cam->top_light = v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+				V4L2_CID_ILLUMINATORS_1, 0, 1, 1, 0);
+		cam->bottom_light = v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+				V4L2_CID_ILLUMINATORS_2, 0, 1, 1, 0);
+		v4l2_ctrl_cluster(2, &cam->top_light);
+	}
 
-	memcpy(cam->vdev, &cpia2_template, sizeof(cpia2_template));
-	video_set_drvdata(cam->vdev, cam);
-	cam->vdev->lock = &cam->v4l2_lock;
+	if (hdl->error) {
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	cam->vdev = cpia2_template;
+	video_set_drvdata(&cam->vdev, cam);
+	cam->vdev.lock = &cam->v4l2_lock;
+	cam->vdev.ctrl_handler = hdl;
+	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	set_bit(V4L2_FL_USE_FH_PRIO, &cam->vdev.flags);
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &cam->vdev.flags);
 
 	reset_camera_struct_v4l(cam);
 
 	/* register v4l device */
-	if (video_register_device(cam->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
+	if (video_register_device(&cam->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
 		ERR("video_register_device failed\n");
-		video_device_release(cam->vdev);
 		return -ENODEV;
 	}
 
@@ -1481,13 +1170,7 @@ int cpia2_register_camera(struct camera_data *cam)
  *****************************************************************************/
 void cpia2_unregister_camera(struct camera_data *cam)
 {
-	if (!cam->open_count) {
-		video_unregister_device(cam->vdev);
-	} else {
-		LOG("%s removed while open, deferring "
-		    "video_unregister_device\n",
-		    video_device_node_name(cam->vdev));
-	}
+	video_unregister_device(&cam->vdev);
 }
 
 /******************************************************************************
@@ -1524,23 +1207,12 @@ static void __init check_parameters(void)
 		LOG("alternate specified is invalid, using %d\n", alternate);
 	}
 
-	if (flicker_mode != NEVER_FLICKER && flicker_mode != ANTI_FLICKER_ON) {
-		flicker_mode = NEVER_FLICKER;
+	if (flicker_mode != 0 && flicker_mode != FLICKER_50 && flicker_mode != FLICKER_60) {
+		flicker_mode = 0;
 		LOG("Flicker mode specified is invalid, using %d\n",
 		    flicker_mode);
 	}
 
-	if (flicker_freq != FLICKER_50 && flicker_freq != FLICKER_60) {
-		flicker_freq = FLICKER_60;
-		LOG("Flicker mode specified is invalid, using %d\n",
-		    flicker_freq);
-	}
-
-	if(video_nr < -1 || video_nr > 64) {
-		video_nr = -1;
-		LOG("invalid video_nr specified, must be -1 to 64\n");
-	}
-
 	DBG("Using %d buffers, each %d bytes, alternate=%d\n",
 	    num_buffers, buffer_size, alternate);
 }
diff --git a/drivers/media/video/cpia2/cpia2dev.h b/drivers/media/video/cpia2/cpia2dev.h
deleted file mode 100644
index f66691fe5a35..000000000000
--- a/drivers/media/video/cpia2/cpia2dev.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- *  Filename: cpia2dev.h
- *
- *  Copyright 2001, STMicrolectronics, Inc.
- *
- *  Contact:  steve.miller@st.com
- *
- *  Description:
- *     This file provides definitions for applications wanting to use the
- *     cpia2 driver beyond the generic v4l capabilities.
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- ****************************************************************************/
-
-#ifndef CPIA2_DEV_HEADER
-#define CPIA2_DEV_HEADER
-
-#include <linux/videodev2.h>
-
-/***
- * The following defines are ioctl numbers based on video4linux private ioctls,
- * which can range from 192 (BASE_VIDIOCPRIVATE) to 255. All of these take int
- * args
- */
-#define CPIA2_IOC_SET_GPIO         _IOW('v', BASE_VIDIOC_PRIVATE + 17, __u32)
-
-/* V4L2 driver specific controls */
-#define CPIA2_CID_TARGET_KB     (V4L2_CID_PRIVATE_BASE+0)
-#define CPIA2_CID_GPIO          (V4L2_CID_PRIVATE_BASE+1)
-#define CPIA2_CID_FLICKER_MODE  (V4L2_CID_PRIVATE_BASE+2)
-#define CPIA2_CID_FRAMERATE     (V4L2_CID_PRIVATE_BASE+3)
-#define CPIA2_CID_USB_ALT       (V4L2_CID_PRIVATE_BASE+4)
-#define CPIA2_CID_LIGHTS        (V4L2_CID_PRIVATE_BASE+5)
-#define CPIA2_CID_RESET_CAMERA  (V4L2_CID_PRIVATE_BASE+6)
-
-#endif
diff --git a/drivers/media/video/cx18/cx18-alsa-main.c b/drivers/media/video/cx18/cx18-alsa-main.c
index e118361c2e7b..6d2a98246b6d 100644
--- a/drivers/media/video/cx18/cx18-alsa-main.c
+++ b/drivers/media/video/cx18/cx18-alsa-main.c
@@ -285,6 +285,7 @@ static void __exit cx18_alsa_exit(void)
 
 	drv = driver_find("cx18", &pci_bus_type);
 	ret = driver_for_each_device(drv, NULL, NULL, cx18_alsa_exit_callback);
+	(void)ret;	/* suppress compiler warning */
 
 	cx18_ext_init = NULL;
 	printk(KERN_INFO "cx18-alsa: module unload complete\n");
diff --git a/drivers/media/video/cx18/cx18-alsa-pcm.c b/drivers/media/video/cx18/cx18-alsa-pcm.c
index 82d195be9197..7a5b84a86bb3 100644
--- a/drivers/media/video/cx18/cx18-alsa-pcm.c
+++ b/drivers/media/video/cx18/cx18-alsa-pcm.c
@@ -190,7 +190,7 @@ static int snd_cx18_pcm_capture_open(struct snd_pcm_substream *substream)
 	ret = cx18_start_v4l2_encode_stream(s);
 	snd_cx18_unlock(cxsc);
 
-	return 0;
+	return ret;
 }
 
 static int snd_cx18_pcm_capture_close(struct snd_pcm_substream *substream)
@@ -199,12 +199,11 @@ static int snd_cx18_pcm_capture_close(struct snd_pcm_substream *substream)
 	struct v4l2_device *v4l2_dev = cxsc->v4l2_dev;
 	struct cx18 *cx = to_cx18(v4l2_dev);
 	struct cx18_stream *s;
-	int ret;
 
 	/* Instruct the cx18 to stop sending packets */
 	snd_cx18_lock(cxsc);
 	s = &cx->streams[CX18_ENC_STREAM_TYPE_PCM];
-	ret = cx18_stop_v4l2_encode_stream(s, 0);
+	cx18_stop_v4l2_encode_stream(s, 0);
 	clear_bit(CX18_F_S_STREAMING, &s->s_flags);
 
 	cx18_release_stream(s);
@@ -252,13 +251,10 @@ static int snd_pcm_alloc_vmalloc_buffer(struct snd_pcm_substream *subs,
 static int snd_cx18_pcm_hw_params(struct snd_pcm_substream *substream,
 			 struct snd_pcm_hw_params *params)
 {
-	int ret;
-
 	dprintk("%s called\n", __func__);
 
-	ret = snd_pcm_alloc_vmalloc_buffer(substream,
+	return snd_pcm_alloc_vmalloc_buffer(substream,
 					   params_buffer_bytes(params));
-	return 0;
 }
 
 static int snd_cx18_pcm_hw_free(struct snd_pcm_substream *substream)
diff --git a/drivers/media/video/cx18/cx18-ioctl.c b/drivers/media/video/cx18/cx18-ioctl.c
index be49f68ddf37..35fde4e931f5 100644
--- a/drivers/media/video/cx18/cx18-ioctl.c
+++ b/drivers/media/video/cx18/cx18-ioctl.c
@@ -1137,7 +1137,7 @@ static long cx18_default(struct file *file, void *fh, bool valid_prio,
 	}
 
 	default:
-		return -EINVAL;
+		return -ENOTTY;
 	}
 	return 0;
 }
diff --git a/drivers/media/video/cx18/cx18-mailbox.c b/drivers/media/video/cx18/cx18-mailbox.c
index 0c7796e76ac0..ed8118390b02 100644
--- a/drivers/media/video/cx18/cx18-mailbox.c
+++ b/drivers/media/video/cx18/cx18-mailbox.c
@@ -595,9 +595,8 @@ void cx18_api_epu_cmd_irq(struct cx18 *cx, int rpu)
 static int cx18_api_call(struct cx18 *cx, u32 cmd, int args, u32 data[])
 {
 	const struct cx18_api_info *info = find_api_info(cmd);
-	u32 state, irq, req, ack, err;
+	u32 irq, req, ack, err;
 	struct cx18_mailbox __iomem *mb;
-	u32 __iomem *xpu_state;
 	wait_queue_head_t *waitq;
 	struct mutex *mb_lock;
 	unsigned long int t0, timeout, ret;
@@ -628,14 +627,12 @@ static int cx18_api_call(struct cx18 *cx, u32 cmd, int args, u32 data[])
 		mb_lock = &cx->epu2apu_mb_lock;
 		irq = IRQ_EPU_TO_APU;
 		mb = &cx->scb->epu2apu_mb;
-		xpu_state = &cx->scb->apu_state;
 		break;
 	case CPU:
 		waitq = &cx->mb_cpu_waitq;
 		mb_lock = &cx->epu2cpu_mb_lock;
 		irq = IRQ_EPU_TO_CPU;
 		mb = &cx->scb->epu2cpu_mb;
-		xpu_state = &cx->scb->cpu_state;
 		break;
 	default:
 		CX18_WARN("Unknown RPU (%d) for API call\n", info->rpu);
@@ -653,7 +650,6 @@ static int cx18_api_call(struct cx18 *cx, u32 cmd, int args, u32 data[])
 	 * by a signal, we may get here and find a busy mailbox.  After waiting,
 	 * mark it "not busy" from our end, if the XPU hasn't ack'ed it still.
 	 */
-	state = cx18_readl(cx, xpu_state);
 	req = cx18_readl(cx, &mb->request);
 	timeout = msecs_to_jiffies(10);
 	ret = wait_event_timeout(*waitq,
diff --git a/drivers/media/video/cx18/cx18-streams.c b/drivers/media/video/cx18/cx18-streams.c
index 638cca156b58..4185bcb80ca3 100644
--- a/drivers/media/video/cx18/cx18-streams.c
+++ b/drivers/media/video/cx18/cx18-streams.c
@@ -980,7 +980,6 @@ void cx18_stop_all_captures(struct cx18 *cx)
 int cx18_stop_v4l2_encode_stream(struct cx18_stream *s, int gop_end)
 {
 	struct cx18 *cx = s->cx;
-	unsigned long then;
 
 	if (!cx18_stream_enabled(s))
 		return -EINVAL;
@@ -999,8 +998,6 @@ int cx18_stop_v4l2_encode_stream(struct cx18_stream *s, int gop_end)
 	else
 		cx18_vapi(cx, CX18_CPU_CAPTURE_STOP, 1, s->handle);
 
-	then = jiffies;
-
 	if (s->type == CX18_ENC_STREAM_TYPE_MPG && gop_end) {
 		CX18_INFO("ignoring gop_end: not (yet?) supported by the firmware\n");
 	}
diff --git a/drivers/media/video/cx231xx/cx231xx-417.c b/drivers/media/video/cx231xx/cx231xx-417.c
index d4327dab5a36..ce2f62238a19 100644
--- a/drivers/media/video/cx231xx/cx231xx-417.c
+++ b/drivers/media/video/cx231xx/cx231xx-417.c
@@ -1095,7 +1095,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 {
 	int version;
 	int retval;
-	u32 i, data[7];
+	u32 i;
 	u32 val = 0;
 
 	dprintk(1, "%s()\n", __func__);
@@ -1154,6 +1154,11 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 		CX231xx_CUSTOM_EXTENSION_USR_DATA, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 		0, 0);
 */
+
+#if 0
+	/* TODO */
+	u32 data[7];
+
 	/* Setup to capture VBI */
 	data[0] = 0x0001BD00;
 	data[1] = 1;          /* frames per interrupt */
@@ -1162,7 +1167,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 	data[4] = 0x206080C0; /* stop codes */
 	data[5] = 6;          /* lines */
 	data[6] = 64;         /* BPL */
-/*
+
 	cx231xx_api_cmd(dev, CX2341X_ENC_SET_VBI_CONFIG, 7, 0, data[0], data[1],
 		data[2], data[3], data[4], data[5], data[6]);
 
@@ -1175,7 +1180,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 		cx231xx_api_cmd(dev, CX2341X_ENC_SET_VBI_LINE, 5, 0,
 				i | 0x80000000, valid, 0, 0, 0);
 	}
-*/
+#endif
 /*	cx231xx_api_cmd(dev, CX2341X_ENC_MUTE_AUDIO, 1, 0, CX231xx_UNMUTE);
 	msleep(60);
 */
@@ -1792,17 +1797,16 @@ static int vidioc_streamon(struct file *file, void *priv,
 	struct cx231xx_fh  *fh  = file->private_data;
 
 	struct cx231xx *dev = fh->dev;
-	int rc = 0;
 	dprintk(3, "enter vidioc_streamon()\n");
 		cx231xx_set_alt_setting(dev, INDEX_TS1, 0);
-		rc = cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
+		cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
 		if (dev->USE_ISO)
-			rc = cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
+			cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
 				       CX231XX_NUM_BUFS,
 				       dev->video_mode.max_pkt_size,
 				       cx231xx_isoc_copy);
 		else {
-			rc = cx231xx_init_bulk(dev, 320,
+			cx231xx_init_bulk(dev, 320,
 				       5,
 				       dev->ts1_mode.max_pkt_size,
 				       cx231xx_bulk_copy);
diff --git a/drivers/media/video/cx231xx/cx231xx-audio.c b/drivers/media/video/cx231xx/cx231xx-audio.c
index a2c2b7d343ec..068f78dc5d13 100644
--- a/drivers/media/video/cx231xx/cx231xx-audio.c
+++ b/drivers/media/video/cx231xx/cx231xx-audio.c
@@ -523,21 +523,24 @@ static int snd_cx231xx_pcm_close(struct snd_pcm_substream *substream)
 static int snd_cx231xx_hw_capture_params(struct snd_pcm_substream *substream,
 					 struct snd_pcm_hw_params *hw_params)
 {
-	unsigned int channels, rate, format;
 	int ret;
 
 	dprintk("Setting capture parameters\n");
 
 	ret = snd_pcm_alloc_vmalloc_buffer(substream,
 					   params_buffer_bytes(hw_params));
+#if 0
+	/* TODO: set up cx231xx audio chip to deliver the correct audio format,
+	   current default is 48000hz multiplexed => 96000hz mono
+	   which shouldn't matter since analogue TV only supports mono */
+	unsigned int channels, rate, format;
+
 	format = params_format(hw_params);
 	rate = params_rate(hw_params);
 	channels = params_channels(hw_params);
+#endif
 
-	/* TODO: set up cx231xx audio chip to deliver the correct audio format,
-	   current default is 48000hz multiplexed => 96000hz mono
-	   which shouldn't matter since analogue TV only supports mono */
-	return 0;
+	return ret;
 }
 
 static int snd_cx231xx_hw_capture_free(struct snd_pcm_substream *substream)
@@ -586,7 +589,7 @@ static int snd_cx231xx_capture_trigger(struct snd_pcm_substream *substream,
 				       int cmd)
 {
 	struct cx231xx *dev = snd_pcm_substream_chip(substream);
-	int retval;
+	int retval = 0;
 
 	if (dev->state & DEV_DISCONNECTED)
 		return -ENODEV;
@@ -601,12 +604,13 @@ static int snd_cx231xx_capture_trigger(struct snd_pcm_substream *substream,
 		break;
 	default:
 		retval = -EINVAL;
+		break;
 	}
 	spin_unlock(&dev->adev.slock);
 
 	schedule_work(&dev->wq_trigger);
 
-	return 0;
+	return retval;
 }
 
 static snd_pcm_uframes_t snd_cx231xx_capture_pointer(struct snd_pcm_substream
diff --git a/drivers/media/video/cx231xx/cx231xx-avcore.c b/drivers/media/video/cx231xx/cx231xx-avcore.c
index 53ff26e7abf7..b085a3c6dc04 100644
--- a/drivers/media/video/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/video/cx231xx/cx231xx-avcore.c
@@ -934,33 +934,29 @@ int cx231xx_set_decoder_video_input(struct cx231xx *dev,
 void cx231xx_enable656(struct cx231xx *dev)
 {
 	u8 temp = 0;
-	int status;
 	/*enable TS1 data[0:7] as output to export 656*/
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL0, 0xFF);
+	vid_blk_write_byte(dev, TS1_PIN_CTL0, 0xFF);
 
 	/*enable TS1 clock as output to export 656*/
 
-	status = vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
+	vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
 	temp = temp|0x04;
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
-
+	vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
 }
 EXPORT_SYMBOL_GPL(cx231xx_enable656);
 
 void cx231xx_disable656(struct cx231xx *dev)
 {
 	u8 temp = 0;
-	int status;
-
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL0, 0x00);
+	vid_blk_write_byte(dev, TS1_PIN_CTL0, 0x00);
 
-	status = vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
+	vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
 	temp = temp&0xFB;
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
+	vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
 }
 EXPORT_SYMBOL_GPL(cx231xx_disable656);
 
@@ -1320,117 +1316,115 @@ void update_HH_register_after_set_DIF(struct cx231xx *dev)
 
 void cx231xx_dump_HH_reg(struct cx231xx *dev)
 {
-	u8 status = 0;
 	u32 value = 0;
 	u16  i = 0;
 
 	value = 0x45005390;
-	status = vid_blk_write_word(dev, 0x104, value);
+	vid_blk_write_word(dev, 0x104, value);
 
 	for (i = 0x100; i < 0x140; i++) {
-		status = vid_blk_read_word(dev, i, &value);
+		vid_blk_read_word(dev, i, &value);
 		cx231xx_info("reg0x%x=0x%x\n", i, value);
 		i = i+3;
 	}
 
 	for (i = 0x300; i < 0x400; i++) {
-		status = vid_blk_read_word(dev, i, &value);
+		vid_blk_read_word(dev, i, &value);
 		cx231xx_info("reg0x%x=0x%x\n", i, value);
 		i = i+3;
 	}
 
 	for (i = 0x400; i < 0x440; i++) {
-		status = vid_blk_read_word(dev, i,  &value);
+		vid_blk_read_word(dev, i,  &value);
 		cx231xx_info("reg0x%x=0x%x\n", i, value);
 		i = i+3;
 	}
 
-	status = vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
+	vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
 	cx231xx_info("AFE_CTRL_C2HH_SRC_CTRL=0x%x\n", value);
 	vid_blk_write_word(dev, AFE_CTRL_C2HH_SRC_CTRL, 0x4485D390);
-	status = vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
+	vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
 	cx231xx_info("AFE_CTRL_C2HH_SRC_CTRL=0x%x\n", value);
 }
 
 void cx231xx_dump_SC_reg(struct cx231xx *dev)
 {
 	u8 value[4] = { 0, 0, 0, 0 };
-	int status = 0;
 	cx231xx_info("cx231xx_dump_SC_reg!\n");
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, BOARD_CFG_STAT,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, BOARD_CFG_STAT,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", BOARD_CFG_STAT, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS_MODE_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS_MODE_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS_MODE_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_CFG_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_CFG_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS1_CFG_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_LENGTH_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_LENGTH_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS1_LENGTH_REG, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_CFG_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_CFG_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS2_CFG_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_LENGTH_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_LENGTH_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS2_LENGTH_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, EP_MODE_SET,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, EP_MODE_SET,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", EP_MODE_SET, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN1,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN1,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_PTN1, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN2,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN2,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_PTN2, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN3,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN3,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_PTN3, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK0,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK0,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_MASK0, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK1,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK1,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_MASK1, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK2,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK2,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_MASK2, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_GAIN,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_GAIN,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_GAIN, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_CAR_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_CAR_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_CAR_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG1,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG1,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_OT_CFG1, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG2,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG2,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_OT_CFG2, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, PWR_CTL_EN,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, PWR_CTL_EN,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", PWR_CTL_EN, value[0],
 				 value[1], value[2], value[3]);
@@ -1441,18 +1435,15 @@ void cx231xx_dump_SC_reg(struct cx231xx *dev)
 void cx231xx_Setup_AFE_for_LowIF(struct cx231xx *dev)
 
 {
-	u8 status = 0;
 	u8 value = 0;
 
-
-
-	status = afe_read_byte(dev, ADC_STATUS2_CH3, &value);
+	afe_read_byte(dev, ADC_STATUS2_CH3, &value);
 	value = (value & 0xFE)|0x01;
-	status = afe_write_byte(dev, ADC_STATUS2_CH3, value);
+	afe_write_byte(dev, ADC_STATUS2_CH3, value);
 
-	status = afe_read_byte(dev, ADC_STATUS2_CH3, &value);
+	afe_read_byte(dev, ADC_STATUS2_CH3, &value);
 	value = (value & 0xFE)|0x00;
-	status = afe_write_byte(dev, ADC_STATUS2_CH3, value);
+	afe_write_byte(dev, ADC_STATUS2_CH3, value);
 
 
 /*
@@ -1464,44 +1455,43 @@ void cx231xx_Setup_AFE_for_LowIF(struct cx231xx *dev)
 		for low-if agc defect
 */
 
-	status = afe_read_byte(dev, ADC_NTF_PRECLMP_EN_CH3, &value);
+	afe_read_byte(dev, ADC_NTF_PRECLMP_EN_CH3, &value);
 	value = (value & 0xFC)|0x00;
-	status = afe_write_byte(dev, ADC_NTF_PRECLMP_EN_CH3, value);
+	afe_write_byte(dev, ADC_NTF_PRECLMP_EN_CH3, value);
 
-	status = afe_read_byte(dev, ADC_INPUT_CH3, &value);
+	afe_read_byte(dev, ADC_INPUT_CH3, &value);
 	value = (value & 0xF9)|0x02;
-	status = afe_write_byte(dev, ADC_INPUT_CH3, value);
+	afe_write_byte(dev, ADC_INPUT_CH3, value);
 
-	status = afe_read_byte(dev, ADC_FB_FRCRST_CH3, &value);
+	afe_read_byte(dev, ADC_FB_FRCRST_CH3, &value);
 	value = (value & 0xFB)|0x04;
-	status = afe_write_byte(dev, ADC_FB_FRCRST_CH3, value);
+	afe_write_byte(dev, ADC_FB_FRCRST_CH3, value);
 
-	status = afe_read_byte(dev, ADC_DCSERVO_DEM_CH3, &value);
+	afe_read_byte(dev, ADC_DCSERVO_DEM_CH3, &value);
 	value = (value & 0xFC)|0x03;
-	status = afe_write_byte(dev, ADC_DCSERVO_DEM_CH3, value);
+	afe_write_byte(dev, ADC_DCSERVO_DEM_CH3, value);
 
-	status = afe_read_byte(dev, ADC_CTRL_DAC1_CH3, &value);
+	afe_read_byte(dev, ADC_CTRL_DAC1_CH3, &value);
 	value = (value & 0xFB)|0x04;
-	status = afe_write_byte(dev, ADC_CTRL_DAC1_CH3, value);
+	afe_write_byte(dev, ADC_CTRL_DAC1_CH3, value);
 
-	status = afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
+	afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
 	value = (value & 0xF8)|0x06;
-	status = afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
+	afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
 
-	status = afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
+	afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
 	value = (value & 0x8F)|0x40;
-	status = afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
+	afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
 
-	status = afe_read_byte(dev, ADC_PWRDN_CLAMP_CH3, &value);
+	afe_read_byte(dev, ADC_PWRDN_CLAMP_CH3, &value);
 	value = (value & 0xDF)|0x20;
-	status = afe_write_byte(dev, ADC_PWRDN_CLAMP_CH3, value);
+	afe_write_byte(dev, ADC_PWRDN_CLAMP_CH3, value);
 }
 
 void cx231xx_set_Colibri_For_LowIF(struct cx231xx *dev, u32 if_freq,
 		 u8 spectral_invert, u32 mode)
 {
 	u32 colibri_carrier_offset = 0;
-	u8 status = 0;
 	u32 func_mode = 0x01; /* Device has a DIF if this function is called */
 	u32 standard = 0;
 	u8 value[4] = { 0, 0, 0, 0 };
@@ -1511,15 +1501,15 @@ void cx231xx_set_Colibri_For_LowIF(struct cx231xx *dev, u32 if_freq,
 	value[1] = (u8) 0x6F;
 	value[2] = (u8) 0x6F;
 	value[3] = (u8) 0x6F;
-	status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
 					PWR_CTL_EN, value, 4);
 
 	/*Set colibri for low IF*/
-	status = cx231xx_afe_set_mode(dev, AFE_MODE_LOW_IF);
+	cx231xx_afe_set_mode(dev, AFE_MODE_LOW_IF);
 
 	/* Set C2HH for low IF operation.*/
 	standard = dev->norm;
-	status = cx231xx_dif_configure_C2HH_for_low_IF(dev, dev->active_mode,
+	cx231xx_dif_configure_C2HH_for_low_IF(dev, dev->active_mode,
 						       func_mode, standard);
 
 	/* Get colibri offsets.*/
@@ -1556,7 +1546,6 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 		 u8 spectral_invert, u32 mode)
 {
 	unsigned long pll_freq_word;
-	int status = 0;
 	u32 dif_misc_ctrl_value = 0;
 	u64 pll_freq_u64 = 0;
 	u32 i = 0;
@@ -1567,7 +1556,7 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 
 	if (mode == TUNER_MODE_FM_RADIO) {
 		pll_freq_word = 0x905A1CAC;
-		status = vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
+		vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
 
 	} else /*KSPROPERTY_TUNER_MODE_TV*/{
 		/* Calculate the PLL frequency word based on the adjusted if_freq*/
@@ -1576,23 +1565,23 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 		do_div(pll_freq_u64, 50000000);
 		pll_freq_word = (u32)pll_freq_u64;
 		/*pll_freq_word = 0x3463497;*/
-		status = vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
+		vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
 
 	if (spectral_invert) {
 		if_freq -= 400000;
 		/* Enable Spectral Invert*/
-		status = vid_blk_read_word(dev, DIF_MISC_CTRL,
+		vid_blk_read_word(dev, DIF_MISC_CTRL,
 					&dif_misc_ctrl_value);
 		dif_misc_ctrl_value = dif_misc_ctrl_value | 0x00200000;
-		status = vid_blk_write_word(dev, DIF_MISC_CTRL,
+		vid_blk_write_word(dev, DIF_MISC_CTRL,
 					dif_misc_ctrl_value);
 	} else {
 		if_freq += 400000;
 		/* Disable Spectral Invert*/
-		status = vid_blk_read_word(dev, DIF_MISC_CTRL,
+		vid_blk_read_word(dev, DIF_MISC_CTRL,
 					&dif_misc_ctrl_value);
 		dif_misc_ctrl_value = dif_misc_ctrl_value & 0xFFDFFFFF;
-		status = vid_blk_write_word(dev, DIF_MISC_CTRL,
+		vid_blk_write_word(dev, DIF_MISC_CTRL,
 					dif_misc_ctrl_value);
 	}
 
@@ -1606,10 +1595,10 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 	}
 
 	cx231xx_info("Enter IF=%zd\n",
-			sizeof(Dif_set_array)/sizeof(struct dif_settings));
-	for (i = 0; i < sizeof(Dif_set_array)/sizeof(struct dif_settings); i++) {
+			ARRAY_SIZE(Dif_set_array));
+	for (i = 0; i < ARRAY_SIZE(Dif_set_array); i++) {
 		if (Dif_set_array[i].if_freq == if_freq) {
-			status = vid_blk_write_word(dev,
+			vid_blk_write_word(dev,
 			Dif_set_array[i].register_address, Dif_set_array[i].value);
 		}
 	}
@@ -3090,31 +3079,30 @@ int cx231xx_gpio_i2c_read(struct cx231xx *dev, u8 dev_addr, u8 *buf, u8 len)
  */
 int cx231xx_gpio_i2c_write(struct cx231xx *dev, u8 dev_addr, u8 *buf, u8 len)
 {
-	int status = 0;
 	int i = 0;
 
 	/* get the lock */
 	mutex_lock(&dev->gpio_i2c_lock);
 
 	/* start */
-	status = cx231xx_gpio_i2c_start(dev);
+	cx231xx_gpio_i2c_start(dev);
 
 	/* write dev_addr */
-	status = cx231xx_gpio_i2c_write_byte(dev, dev_addr << 1);
+	cx231xx_gpio_i2c_write_byte(dev, dev_addr << 1);
 
 	/* read Ack */
-	status = cx231xx_gpio_i2c_read_ack(dev);
+	cx231xx_gpio_i2c_read_ack(dev);
 
 	for (i = 0; i < len; i++) {
 		/* Write data */
-		status = cx231xx_gpio_i2c_write_byte(dev, buf[i]);
+		cx231xx_gpio_i2c_write_byte(dev, buf[i]);
 
 		/* read Ack */
-		status = cx231xx_gpio_i2c_read_ack(dev);
+		cx231xx_gpio_i2c_read_ack(dev);
 	}
 
 	/* write End */
-	status = cx231xx_gpio_i2c_end(dev);
+	cx231xx_gpio_i2c_end(dev);
 
 	/* release the lock */
 	mutex_unlock(&dev->gpio_i2c_lock);
diff --git a/drivers/media/video/cx231xx/cx231xx-core.c b/drivers/media/video/cx231xx/cx231xx-core.c
index 08dd930f882a..05358d486135 100644
--- a/drivers/media/video/cx231xx/cx231xx-core.c
+++ b/drivers/media/video/cx231xx/cx231xx-core.c
@@ -754,7 +754,7 @@ int cx231xx_set_mode(struct cx231xx *dev, enum cx231xx_mode set_mode)
 		}
 	}
 
-	return 0;
+	return errCode ? -EINVAL : 0;
 }
 EXPORT_SYMBOL_GPL(cx231xx_set_mode);
 
@@ -764,7 +764,7 @@ int cx231xx_ep5_bulkout(struct cx231xx *dev, u8 *firmware, u16 size)
 	int actlen, ret = -ENOMEM;
 	u32 *buffer;
 
-buffer = kzalloc(4096, GFP_KERNEL);
+	buffer = kzalloc(4096, GFP_KERNEL);
 	if (buffer == NULL) {
 		cx231xx_info("out of mem\n");
 		return -ENOMEM;
@@ -772,16 +772,16 @@ buffer = kzalloc(4096, GFP_KERNEL);
 	memcpy(&buffer[0], firmware, 4096);
 
 	ret = usb_bulk_msg(dev->udev, usb_sndbulkpipe(dev->udev, 5),
-				 buffer, 4096, &actlen, 2000);
+			buffer, 4096, &actlen, 2000);
 
 	if (ret)
 		cx231xx_info("bulk message failed: %d (%d/%d)", ret,
-				 size, actlen);
+				size, actlen);
 	else {
 		errCode = actlen != size ? -1 : 0;
 	}
-kfree(buffer);
-	return 0;
+	kfree(buffer);
+	return errCode;
 }
 
 /*****************************************************************
@@ -797,7 +797,7 @@ static void cx231xx_isoc_irq_callback(struct urb *urb)
 	struct cx231xx_video_mode *vmode =
 	    container_of(dma_q, struct cx231xx_video_mode, vidq);
 	struct cx231xx *dev = container_of(vmode, struct cx231xx, video_mode);
-	int rc, i;
+	int i;
 
 	switch (urb->status) {
 	case 0:		/* success */
@@ -814,7 +814,7 @@ static void cx231xx_isoc_irq_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock(&dev->video_mode.slock);
-	rc = dev->video_mode.isoc_ctl.isoc_copy(dev, urb);
+	dev->video_mode.isoc_ctl.isoc_copy(dev, urb);
 	spin_unlock(&dev->video_mode.slock);
 
 	/* Reset urb buffers */
@@ -822,7 +822,6 @@ static void cx231xx_isoc_irq_callback(struct urb *urb)
 		urb->iso_frame_desc[i].status = 0;
 		urb->iso_frame_desc[i].actual_length = 0;
 	}
-	urb->status = 0;
 
 	urb->status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (urb->status) {
@@ -843,7 +842,6 @@ static void cx231xx_bulk_irq_callback(struct urb *urb)
 	struct cx231xx_video_mode *vmode =
 	    container_of(dma_q, struct cx231xx_video_mode, vidq);
 	struct cx231xx *dev = container_of(vmode, struct cx231xx, video_mode);
-	int rc;
 
 	switch (urb->status) {
 	case 0:		/* success */
@@ -860,12 +858,10 @@ static void cx231xx_bulk_irq_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock(&dev->video_mode.slock);
-	rc = dev->video_mode.bulk_ctl.bulk_copy(dev, urb);
+	dev->video_mode.bulk_ctl.bulk_copy(dev, urb);
 	spin_unlock(&dev->video_mode.slock);
 
 	/* Reset urb buffers */
-	urb->status = 0;
-
 	urb->status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (urb->status) {
 		cx231xx_isocdbg("urb resubmit failed (error=%i)\n",
@@ -1231,42 +1227,40 @@ int cx231xx_init_bulk(struct cx231xx *dev, int max_packets,
 EXPORT_SYMBOL_GPL(cx231xx_init_bulk);
 void cx231xx_stop_TS1(struct cx231xx *dev)
 {
-	int status = 0;
 	u8 val[4] = { 0, 0, 0, 0 };
 
-			val[0] = 0x00;
-			val[1] = 0x03;
-			val[2] = 0x00;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS_MODE_REG, val, 4);
-
-			val[0] = 0x00;
-			val[1] = 0x70;
-			val[2] = 0x04;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS1_CFG_REG, val, 4);
+	val[0] = 0x00;
+	val[1] = 0x03;
+	val[2] = 0x00;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS_MODE_REG, val, 4);
+
+	val[0] = 0x00;
+	val[1] = 0x70;
+	val[2] = 0x04;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS1_CFG_REG, val, 4);
 }
 /* EXPORT_SYMBOL_GPL(cx231xx_stop_TS1); */
 void cx231xx_start_TS1(struct cx231xx *dev)
 {
-	int status = 0;
 	u8 val[4] = { 0, 0, 0, 0 };
 
-			val[0] = 0x03;
-			val[1] = 0x03;
-			val[2] = 0x00;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS_MODE_REG, val, 4);
-
-			val[0] = 0x04;
-			val[1] = 0xA3;
-			val[2] = 0x3B;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS1_CFG_REG, val, 4);
+	val[0] = 0x03;
+	val[1] = 0x03;
+	val[2] = 0x00;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS_MODE_REG, val, 4);
+
+	val[0] = 0x04;
+	val[1] = 0xA3;
+	val[2] = 0x3B;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS1_CFG_REG, val, 4);
 }
 /* EXPORT_SYMBOL_GPL(cx231xx_start_TS1); */
 /*****************************************************************
diff --git a/drivers/media/video/cx231xx/cx231xx-vbi.c b/drivers/media/video/cx231xx/cx231xx-vbi.c
index 8cdee5f78f13..3d15314e1f88 100644
--- a/drivers/media/video/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/video/cx231xx/cx231xx-vbi.c
@@ -83,7 +83,6 @@ static inline void print_err_status(struct cx231xx *dev, int packet, int status)
  */
 static inline int cx231xx_isoc_vbi_copy(struct cx231xx *dev, struct urb *urb)
 {
-	struct cx231xx_buffer *buf;
 	struct cx231xx_dmaqueue *dma_q = urb->context;
 	int rc = 1;
 	unsigned char *p_buffer;
@@ -102,8 +101,6 @@ static inline int cx231xx_isoc_vbi_copy(struct cx231xx *dev, struct urb *urb)
 			return 0;
 	}
 
-	buf = dev->vbi_mode.bulk_ctl.buf;
-
 	/* get buffer pointer and length */
 	p_buffer = urb->transfer_buffer;
 	buffer_size = urb->actual_length;
@@ -310,7 +307,6 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 	struct cx231xx_video_mode *vmode =
 	    container_of(dma_q, struct cx231xx_video_mode, vidq);
 	struct cx231xx *dev = container_of(vmode, struct cx231xx, vbi_mode);
-	int rc;
 
 	switch (urb->status) {
 	case 0:		/* success */
@@ -328,7 +324,7 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock(&dev->vbi_mode.slock);
-	rc = dev->vbi_mode.bulk_ctl.bulk_copy(dev, urb);
+	dev->vbi_mode.bulk_ctl.bulk_copy(dev, urb);
 	spin_unlock(&dev->vbi_mode.slock);
 
 	/* Reset status */
diff --git a/drivers/media/video/cx231xx/cx231xx-video.c b/drivers/media/video/cx231xx/cx231xx-video.c
index 7f916f0685e9..523aa49d6b86 100644
--- a/drivers/media/video/cx231xx/cx231xx-video.c
+++ b/drivers/media/video/cx231xx/cx231xx-video.c
@@ -326,9 +326,7 @@ static inline void get_next_buf(struct cx231xx_dmaqueue *dma_q,
  */
 static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 {
-	struct cx231xx_buffer *buf;
 	struct cx231xx_dmaqueue *dma_q = urb->context;
-	unsigned char *outp = NULL;
 	int i, rc = 1;
 	unsigned char *p_buffer;
 	u32 bytes_parsed = 0, buffer_size = 0;
@@ -346,10 +344,6 @@ static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 			return 0;
 	}
 
-	buf = dev->video_mode.isoc_ctl.buf;
-	if (buf != NULL)
-		outp = videobuf_to_vmalloc(&buf->vb);
-
 	for (i = 0; i < urb->number_of_packets; i++) {
 		int status = urb->iso_frame_desc[i].status;
 
@@ -429,9 +423,7 @@ static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 
 static inline int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
 {
-	struct cx231xx_buffer *buf;
 	struct cx231xx_dmaqueue *dma_q = urb->context;
-	unsigned char *outp = NULL;
 	int rc = 1;
 	unsigned char *p_buffer;
 	u32 bytes_parsed = 0, buffer_size = 0;
@@ -449,10 +441,6 @@ static inline int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
 			return 0;
 	}
 
-	buf = dev->video_mode.bulk_ctl.buf;
-	if (buf != NULL)
-		outp = videobuf_to_vmalloc(&buf->vb);
-
 	if (1) {
 
 		/*  get buffer pointer and length */
@@ -701,13 +689,9 @@ void cx231xx_reset_video_buffer(struct cx231xx *dev,
 		buf = dev->video_mode.bulk_ctl.buf;
 
 	if (buf == NULL) {
-		u8 *outp = NULL;
 		/* first try to get the buffer */
 		get_next_buf(dma_q, &buf);
 
-		if (buf)
-			outp = videobuf_to_vmalloc(&buf->vb);
-
 		dma_q->pos = 0;
 		dma_q->field1_done = 0;
 		dma_q->current_field = -1;
@@ -2561,6 +2545,10 @@ static struct video_device *cx231xx_vdev_init(struct cx231xx *dev,
 	vfd->release = video_device_release;
 	vfd->debug = video_debug;
 	vfd->lock = &dev->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s", dev->name, type_name);
 
diff --git a/drivers/media/video/cx23885/cx23885-cards.c b/drivers/media/video/cx23885/cx23885-cards.c
index 19b5499d2624..13739e002a63 100644
--- a/drivers/media/video/cx23885/cx23885-cards.c
+++ b/drivers/media/video/cx23885/cx23885-cards.c
@@ -497,6 +497,10 @@ struct cx23885_board cx23885_boards[] = {
 		.name		= "TerraTec Cinergy T PCIe Dual",
 		.portb		= CX23885_MPEG_DVB,
 		.portc		= CX23885_MPEG_DVB,
+	},
+	[CX23885_BOARD_TEVII_S471] = {
+		.name		= "TeVii S471",
+		.portb		= CX23885_MPEG_DVB,
 	}
 };
 const unsigned int cx23885_bcount = ARRAY_SIZE(cx23885_boards);
@@ -705,6 +709,10 @@ struct cx23885_subid cx23885_subids[] = {
 		.subvendor = 0x153b,
 		.subdevice = 0x117e,
 		.card      = CX23885_BOARD_TERRATEC_CINERGY_T_PCIE_DUAL,
+	}, {
+		.subvendor = 0xd471,
+		.subdevice = 0x9022,
+		.card      = CX23885_BOARD_TEVII_S471,
 	},
 };
 const unsigned int cx23885_idcount = ARRAY_SIZE(cx23885_subids);
@@ -1460,6 +1468,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		ts1->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
 		break;
 	case CX23885_BOARD_TEVII_S470:
+	case CX23885_BOARD_TEVII_S471:
 	case CX23885_BOARD_DVBWORLD_2005:
 		ts1->gen_ctrl_val  = 0x5; /* Parallel */
 		ts1->ts_clk_en_val = 0x1; /* Enable TS_CLK */
diff --git a/drivers/media/video/cx23885/cx23885-core.c b/drivers/media/video/cx23885/cx23885-core.c
index 6ad227029a0f..697728f09430 100644
--- a/drivers/media/video/cx23885/cx23885-core.c
+++ b/drivers/media/video/cx23885/cx23885-core.c
@@ -1046,6 +1046,13 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 	if (cx23885_boards[dev->board].ci_type > 0)
 		cx_clear(RDR_RDRCTL1, 1 << 8);
 
+	switch (dev->board) {
+	case CX23885_BOARD_TEVII_S470:
+	case CX23885_BOARD_TEVII_S471:
+		cx_clear(RDR_RDRCTL1, 1 << 8);
+		break;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/media/video/cx23885/cx23885-dvb.c b/drivers/media/video/cx23885/cx23885-dvb.c
index 6835eb1fc093..a80a92c47455 100644
--- a/drivers/media/video/cx23885/cx23885-dvb.c
+++ b/drivers/media/video/cx23885/cx23885-dvb.c
@@ -1173,6 +1173,13 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		}
 		break;
+	case CX23885_BOARD_TEVII_S471:
+		i2c_bus = &dev->i2c_bus[1];
+
+		fe0->dvb.frontend = dvb_attach(ds3000_attach,
+					&tevii_ds3000_config,
+					&i2c_bus->i2c_adap);
+		break;
 	default:
 		printk(KERN_INFO "%s: The frontend of your DVB/ATSC card "
 			" isn't supported yet\n",
diff --git a/drivers/media/video/cx23885/cx23885.h b/drivers/media/video/cx23885/cx23885.h
index f020f0568df4..d884784a1c85 100644
--- a/drivers/media/video/cx23885/cx23885.h
+++ b/drivers/media/video/cx23885/cx23885.h
@@ -89,6 +89,7 @@
 #define CX23885_BOARD_MPX885                   32
 #define CX23885_BOARD_MYGICA_X8507             33
 #define CX23885_BOARD_TERRATEC_CINERGY_T_PCIE_DUAL 34
+#define CX23885_BOARD_TEVII_S471               35
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
diff --git a/drivers/media/video/cx23885/cx23888-ir.c b/drivers/media/video/cx23885/cx23888-ir.c
index bb1ce346425d..c2bc39c58f82 100644
--- a/drivers/media/video/cx23885/cx23888-ir.c
+++ b/drivers/media/video/cx23885/cx23888-ir.c
@@ -331,9 +331,7 @@ static u64 ns_to_pulse_clocks(u32 ns)
 
 static u16 pulse_clocks_to_clock_divider(u64 count)
 {
-	u32 rem;
-
-	rem = do_div(count, (FIFO_RXTX << 2) | 0x3);
+	do_div(count, (FIFO_RXTX << 2) | 0x3);
 
 	/* net result needs to be rounded down and decremented by 1 */
 	if (count > RXCLK_RCD + 1)
diff --git a/drivers/media/video/cx25821/cx25821-alsa.c b/drivers/media/video/cx25821/cx25821-alsa.c
index 03cfac476b03..1858a45dd081 100644
--- a/drivers/media/video/cx25821/cx25821-alsa.c
+++ b/drivers/media/video/cx25821/cx25821-alsa.c
@@ -290,11 +290,9 @@ static irqreturn_t cx25821_irq(int irq, void *dev_id)
 	u32 status, pci_status;
 	u32 audint_status, audint_mask;
 	int loop, handled = 0;
-	int audint_count = 0;
 
 	audint_status = cx_read(AUD_A_INT_STAT);
 	audint_mask = cx_read(AUD_A_INT_MSK);
-	audint_count = cx_read(AUD_A_GPCNT);
 	status = cx_read(PCI_INT_STAT);
 
 	for (loop = 0; loop < 1; loop++) {
diff --git a/drivers/media/video/cx25821/cx25821-audio-upstream.c b/drivers/media/video/cx25821/cx25821-audio-upstream.c
index 20c7ca3351a8..8b2a99975c23 100644
--- a/drivers/media/video/cx25821/cx25821-audio-upstream.c
+++ b/drivers/media/video/cx25821/cx25821-audio-upstream.c
@@ -585,7 +585,7 @@ int cx25821_audio_upstream_irq(struct cx25821_dev *dev, int chan_num,
 static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 {
 	struct cx25821_dev *dev = dev_id;
-	u32 msk_stat, audio_status;
+	u32 audio_status;
 	int handled = 0;
 	struct sram_channel *sram_ch;
 
@@ -594,7 +594,6 @@ static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 
 	sram_ch = dev->channels[dev->_audio_upstream_channel].sram_channels;
 
-	msk_stat = cx_read(sram_ch->int_mstat);
 	audio_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
diff --git a/drivers/media/video/cx25821/cx25821-core.c b/drivers/media/video/cx25821/cx25821-core.c
index 7930ca58349f..83c1aa6b2e6c 100644
--- a/drivers/media/video/cx25821/cx25821-core.c
+++ b/drivers/media/video/cx25821/cx25821-core.c
@@ -379,14 +379,6 @@ static inline int i2c_slave_did_ack(struct i2c_adapter *i2c_adap)
 	return cx_read(bus->reg_stat) & 0x01;
 }
 
-void cx_i2c_read_print(struct cx25821_dev *dev, u32 reg, const char *reg_string)
-{
-	int tmp = 0;
-	u32 value = 0;
-
-	value = cx25821_i2c_read(&dev->i2c_bus[0], reg, &tmp);
-}
-
 static void cx25821_registers_init(struct cx25821_dev *dev)
 {
 	u32 tmp;
@@ -895,7 +887,7 @@ static void cx25821_iounmap(struct cx25821_dev *dev)
 
 static int cx25821_dev_setup(struct cx25821_dev *dev)
 {
-	int io_size = 0, i;
+	int i;
 
 	pr_info("\n***********************************\n");
 	pr_info("cx25821 set up\n");
@@ -960,7 +952,6 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 
 	/* PCIe stuff */
 	dev->base_io_addr = pci_resource_start(dev->pci, 0);
-	io_size = pci_resource_len(dev->pci, 0);
 
 	if (!dev->base_io_addr) {
 		CX25821_ERR("No PCI Memory resources, exiting!\n");
@@ -1317,13 +1308,12 @@ void cx25821_free_buffer(struct videobuf_queue *q, struct cx25821_buffer *buf)
 static irqreturn_t cx25821_irq(int irq, void *dev_id)
 {
 	struct cx25821_dev *dev = dev_id;
-	u32 pci_status, pci_mask;
+	u32 pci_status;
 	u32 vid_status;
 	int i, handled = 0;
 	u32 mask[8] = { 1, 2, 4, 8, 16, 32, 64, 128 };
 
 	pci_status = cx_read(PCI_INT_STAT);
-	pci_mask = cx_read(PCI_INT_MSK);
 
 	if (pci_status == 0)
 		goto out;
diff --git a/drivers/media/video/cx25821/cx25821-i2c.c b/drivers/media/video/cx25821/cx25821-i2c.c
index 12d7300fa1e9..6311180f430c 100644
--- a/drivers/media/video/cx25821/cx25821-i2c.c
+++ b/drivers/media/video/cx25821/cx25821-i2c.c
@@ -361,7 +361,6 @@ void cx25821_av_clk(struct cx25821_dev *dev, int enable)
 int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value)
 {
 	struct i2c_client *client = &bus->i2c_client;
-	int retval = 0;
 	int v = 0;
 	u8 addr[2] = { 0, 0 };
 	u8 buf[4] = { 0, 0, 0, 0 };
@@ -385,7 +384,7 @@ int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value)
 	msgs[0].addr = 0x44;
 	msgs[1].addr = 0x44;
 
-	retval = i2c_xfer(client->adapter, msgs, 2);
+	i2c_xfer(client->adapter, msgs, 2);
 
 	v = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
 	*value = v;
diff --git a/drivers/media/video/cx25821/cx25821-medusa-video.c b/drivers/media/video/cx25821/cx25821-medusa-video.c
index 298a68d98c2f..313fb20a0b47 100644
--- a/drivers/media/video/cx25821/cx25821-medusa-video.c
+++ b/drivers/media/video/cx25821/cx25821-medusa-video.c
@@ -35,7 +35,6 @@
 static void medusa_enable_bluefield_output(struct cx25821_dev *dev, int channel,
 					   int enable)
 {
-	int ret_val = 1;
 	u32 value = 0;
 	u32 tmp = 0;
 	int out_ctrl = OUT_CTRL1;
@@ -79,13 +78,13 @@ static void medusa_enable_bluefield_output(struct cx25821_dev *dev, int channel,
 	value &= 0xFFFFFF7F;	/* clear BLUE_FIELD_EN */
 	if (enable)
 		value |= 0x00000080;	/* set BLUE_FIELD_EN */
-	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl, value);
+	cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl, value);
 
 	value = cx25821_i2c_read(&dev->i2c_bus[0], out_ctrl_ns, &tmp);
 	value &= 0xFFFFFF7F;
 	if (enable)
 		value |= 0x00000080;	/* set BLUE_FIELD_EN */
-	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl_ns, value);
+	cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl_ns, value);
 }
 
 static int medusa_initialize_ntsc(struct cx25821_dev *dev)
@@ -431,7 +430,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 {
 	int decoder = 0;
 	int decoder_count = 0;
-	int ret_val = 0;
 	u32 hscale = 0x0;
 	u32 vscale = 0x0;
 	const int MAX_WIDTH = 720;
@@ -482,9 +480,9 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 
 	for (; decoder < decoder_count; decoder++) {
 		/* write scaling values for each decoder */
-		ret_val = cx25821_i2c_write(&dev->i2c_bus[0],
+		cx25821_i2c_write(&dev->i2c_bus[0],
 				HSCALE_CTRL + (0x200 * decoder), hscale);
-		ret_val = cx25821_i2c_write(&dev->i2c_bus[0],
+		cx25821_i2c_write(&dev->i2c_bus[0],
 				VSCALE_CTRL + (0x200 * decoder), vscale);
 	}
 
@@ -494,7 +492,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 				       int duration)
 {
-	int ret_val = 0;
 	u32 fld_cnt = 0;
 	u32 tmp = 0;
 	u32 disp_cnt_reg = DISP_AB_CNT;
@@ -537,7 +534,7 @@ static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 		fld_cnt |= ((u32) duration) << 16;
 	}
 
-	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], disp_cnt_reg, fld_cnt);
+	cx25821_i2c_write(&dev->i2c_bus[0], disp_cnt_reg, fld_cnt);
 
 	mutex_unlock(&dev->lock);
 }
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
index 5a157cf4a95e..c8c94fbf5d8d 100644
--- a/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
+++ b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
@@ -587,7 +587,7 @@ int cx25821_video_upstream_irq_ch2(struct cx25821_dev *dev, int chan_num,
 static irqreturn_t cx25821_upstream_irq_ch2(int irq, void *dev_id)
 {
 	struct cx25821_dev *dev = dev_id;
-	u32 msk_stat, vid_status;
+	u32 vid_status;
 	int handled = 0;
 	int channel_num = 0;
 	struct sram_channel *sram_ch;
@@ -598,7 +598,6 @@ static irqreturn_t cx25821_upstream_irq_ch2(int irq, void *dev_id)
 	channel_num = VID_UPSTREAM_SRAM_CHANNEL_J;
 	sram_ch = dev->channels[channel_num].sram_channels;
 
-	msk_stat = cx_read(sram_ch->int_mstat);
 	vid_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream.c b/drivers/media/video/cx25821/cx25821-video-upstream.c
index 21e7d657f049..52c13e0b6492 100644
--- a/drivers/media/video/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/video/cx25821/cx25821-video-upstream.c
@@ -637,7 +637,7 @@ int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 static irqreturn_t cx25821_upstream_irq(int irq, void *dev_id)
 {
 	struct cx25821_dev *dev = dev_id;
-	u32 msk_stat, vid_status;
+	u32 vid_status;
 	int handled = 0;
 	int channel_num = 0;
 	struct sram_channel *sram_ch;
@@ -649,7 +649,6 @@ static irqreturn_t cx25821_upstream_irq(int irq, void *dev_id)
 
 	sram_ch = dev->channels[channel_num].sram_channels;
 
-	msk_stat = cx_read(sram_ch->int_mstat);
 	vid_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
diff --git a/drivers/media/video/cx25821/cx25821-video.c b/drivers/media/video/cx25821/cx25821-video.c
index ffd8bc79c02e..b38d4379cc36 100644
--- a/drivers/media/video/cx25821/cx25821-video.c
+++ b/drivers/media/video/cx25821/cx25821-video.c
@@ -109,25 +109,6 @@ struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc)
 	return NULL;
 }
 
-void cx25821_dump_video_queue(struct cx25821_dev *dev,
-			      struct cx25821_dmaqueue *q)
-{
-	struct cx25821_buffer *buf;
-	struct list_head *item;
-	dprintk(1, "%s()\n", __func__);
-
-	if (!list_empty(&q->active)) {
-		list_for_each(item, &q->active)
-			buf = list_entry(item, struct cx25821_buffer, vb.queue);
-	}
-
-	if (!list_empty(&q->queued)) {
-		list_for_each(item, &q->queued)
-			buf = list_entry(item, struct cx25821_buffer, vb.queue);
-	}
-
-}
-
 void cx25821_video_wakeup(struct cx25821_dev *dev, struct cx25821_dmaqueue *q,
 			  u32 count)
 {
@@ -557,7 +538,7 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	struct cx25821_buffer *buf =
 		container_of(vb, struct cx25821_buffer, vb);
 	int rc, init_buffer = 0;
-	u32 line0_offset, line1_offset;
+	u32 line0_offset;
 	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
 	int bpl_local = LINE_SIZE_D1;
 	int channel_opened = fh->channel_id;
@@ -639,7 +620,6 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 		case V4L2_FIELD_INTERLACED:
 			/* All other formats are top field first */
 			line0_offset = 0;
-			line1_offset = buf->bpl;
 			dprintk(1, "top field first\n");
 
 			cx25821_risc_buffer(dev->pci, &buf->risc,
@@ -1830,7 +1810,6 @@ static long video_ioctl_set(struct file *file, unsigned int cmd,
 	int i = 0;
 	int cif_enable = 0;
 	int cif_width = 0;
-	u32 value = 0;
 
 	data_from_user = (struct downstream_user_struct *)arg;
 
@@ -1914,7 +1893,7 @@ static long video_ioctl_set(struct file *file, unsigned int cmd,
 		cx_write(data_from_user->reg_address, data_from_user->reg_data);
 		break;
 	case MEDUSA_READ:
-		value = cx25821_i2c_read(&dev->i2c_bus[0],
+		cx25821_i2c_read(&dev->i2c_bus[0],
 					 (u16) data_from_user->reg_address,
 					 &data_from_user->reg_data);
 		break;
diff --git a/drivers/media/video/cx25821/cx25821-video.h b/drivers/media/video/cx25821/cx25821-video.h
index d0d9538ca5b3..9652a5e35ba2 100644
--- a/drivers/media/video/cx25821/cx25821-video.h
+++ b/drivers/media/video/cx25821/cx25821-video.h
@@ -86,8 +86,6 @@ extern struct cx25821_fmt formats[];
 extern struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc);
 extern struct cx25821_data timeout_data[MAX_VID_CHANNEL_NUM];
 
-extern void cx25821_dump_video_queue(struct cx25821_dev *dev,
-				     struct cx25821_dmaqueue *q);
 extern void cx25821_video_wakeup(struct cx25821_dev *dev,
 				 struct cx25821_dmaqueue *q, u32 count);
 
diff --git a/drivers/media/video/cx25840/cx25840-ir.c b/drivers/media/video/cx25840/cx25840-ir.c
index 13c380ebb562..38ce76ed1924 100644
--- a/drivers/media/video/cx25840/cx25840-ir.c
+++ b/drivers/media/video/cx25840/cx25840-ir.c
@@ -316,9 +316,7 @@ static u64 ns_to_pulse_clocks(u32 ns)
 
 static u16 pulse_clocks_to_clock_divider(u64 count)
 {
-	u32 rem;
-
-	rem = do_div(count, (FIFO_RXTX << 2) | 0x3);
+	do_div(count, (FIFO_RXTX << 2) | 0x3);
 
 	/* net result needs to be rounded down and decremented by 1 */
 	if (count > RXCLK_RCD + 1)
@@ -860,12 +858,10 @@ static int cx25840_ir_tx_write(struct v4l2_subdev *sd, u8 *buf, size_t count,
 			       ssize_t *num)
 {
 	struct cx25840_ir_state *ir_state = to_ir_state(sd);
-	struct i2c_client *c;
 
 	if (ir_state == NULL)
 		return -ENODEV;
 
-	c = ir_state->c;
 #if 0
 	/*
 	 * FIXME - the code below is an incomplete and untested sketch of what
diff --git a/drivers/media/video/davinci/Kconfig b/drivers/media/video/davinci/Kconfig
index 60a456ebdc7c..9337b5605c90 100644
--- a/drivers/media/video/davinci/Kconfig
+++ b/drivers/media/video/davinci/Kconfig
@@ -40,6 +40,7 @@ config VIDEO_VPSS_SYSTEM
 config VIDEO_VPFE_CAPTURE
 	tristate "VPFE Video Capture Driver"
 	depends on VIDEO_V4L2 && (ARCH_DAVINCI || ARCH_OMAP3)
+	depends on I2C
 	select VIDEOBUF_DMA_CONTIG
 	help
 	  Support for DMx/AMx VPFE based frame grabber. This is the
diff --git a/drivers/media/video/davinci/vpbe_display.c b/drivers/media/video/davinci/vpbe_display.c
index 1f3b1c729252..e106b72810a9 100644
--- a/drivers/media/video/davinci/vpbe_display.c
+++ b/drivers/media/video/davinci/vpbe_display.c
@@ -1618,6 +1618,10 @@ static __devinit int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
 	vbd->ioctl_ops	= &vpbe_ioctl_ops;
 	vbd->minor	= -1;
 	vbd->v4l2_dev   = &disp_dev->vpbe_dev->v4l2_dev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vbd->flags);
 	vbd->lock	= &vpbe_display_layer->opslock;
 
 	if (disp_dev->vpbe_dev->current_timings.timings_type &
diff --git a/drivers/media/video/davinci/vpfe_capture.c b/drivers/media/video/davinci/vpfe_capture.c
index 20cf271a774b..49a845fb804a 100644
--- a/drivers/media/video/davinci/vpfe_capture.c
+++ b/drivers/media/video/davinci/vpfe_capture.c
@@ -1761,7 +1761,7 @@ static long vpfe_param_handler(struct file *file, void *priv,
 		}
 		break;
 	default:
-		ret = -EINVAL;
+		ret = -ENOTTY;
 	}
 unlock_out:
 	mutex_unlock(&vpfe_dev->lock);
diff --git a/drivers/media/video/davinci/vpif_capture.c b/drivers/media/video/davinci/vpif_capture.c
index 6504e40a31dd..96046957bf21 100644
--- a/drivers/media/video/davinci/vpif_capture.c
+++ b/drivers/media/video/davinci/vpif_capture.c
@@ -2228,6 +2228,10 @@ static __init int vpif_probe(struct platform_device *pdev)
 		common = &(ch->common[VPIF_VIDEO_INDEX]);
 		spin_lock_init(&common->irqlock);
 		mutex_init(&common->lock);
+		/* Locking in file operations other than ioctl should be done
+		   by the driver, not the V4L2 core.
+		   This driver needs auditing so that this flag can be removed. */
+		set_bit(V4L2_FL_LOCK_ALL_FOPS, &ch->video_dev->flags);
 		ch->video_dev->lock = &common->lock;
 		/* Initialize prio member of channel object */
 		v4l2_prio_init(&ch->prio);
diff --git a/drivers/media/video/davinci/vpif_display.c b/drivers/media/video/davinci/vpif_display.c
index 7fa34b4fae26..e6488ee7db18 100644
--- a/drivers/media/video/davinci/vpif_display.c
+++ b/drivers/media/video/davinci/vpif_display.c
@@ -1778,6 +1778,10 @@ static __init int vpif_probe(struct platform_device *pdev)
 		v4l2_prio_init(&ch->prio);
 		ch->common[VPIF_VIDEO_INDEX].fmt.type =
 						V4L2_BUF_TYPE_VIDEO_OUTPUT;
+		/* Locking in file operations other than ioctl should be done
+		   by the driver, not the V4L2 core.
+		   This driver needs auditing so that this flag can be removed. */
+		set_bit(V4L2_FL_LOCK_ALL_FOPS, &ch->video_dev->flags);
 		ch->video_dev->lock = &common->lock;
 
 		/* register video device */
diff --git a/drivers/media/video/em28xx/Kconfig b/drivers/media/video/em28xx/Kconfig
index f6f622e123bd..928ef0d0429f 100644
--- a/drivers/media/video/em28xx/Kconfig
+++ b/drivers/media/video/em28xx/Kconfig
@@ -49,10 +49,10 @@ config VIDEO_EM28XX_DVB
 	  Empiatech em28xx chips.
 
 config VIDEO_EM28XX_RC
-        bool "EM28XX Remote Controller support"
+        tristate "EM28XX Remote Controller support"
         depends on RC_CORE
         depends on VIDEO_EM28XX
         depends on !(RC_CORE=m && VIDEO_EM28XX=y)
-        default y
+        default VIDEO_EM28XX
         ---help---
           Enables Remote Controller support on em28xx driver.
diff --git a/drivers/media/video/em28xx/Makefile b/drivers/media/video/em28xx/Makefile
index 2abdf76c5203..c8b338d4be05 100644
--- a/drivers/media/video/em28xx/Makefile
+++ b/drivers/media/video/em28xx/Makefile
@@ -1,16 +1,15 @@
 em28xx-y :=	em28xx-video.o em28xx-i2c.o em28xx-cards.o
 em28xx-y +=	em28xx-core.o  em28xx-vbi.o
 
-em28xx-$(CONFIG_VIDEO_EM28XX_RC) += em28xx-input.o
-
 em28xx-alsa-objs := em28xx-audio.o
+em28xx-rc-objs := em28xx-input.o
 
 obj-$(CONFIG_VIDEO_EM28XX) += em28xx.o
 obj-$(CONFIG_VIDEO_EM28XX_ALSA) += em28xx-alsa.o
 obj-$(CONFIG_VIDEO_EM28XX_DVB) += em28xx-dvb.o
+obj-$(CONFIG_VIDEO_EM28XX_RC) += em28xx-rc.o
 
 ccflags-y += -Idrivers/media/video
 ccflags-y += -Idrivers/media/common/tuners
 ccflags-y += -Idrivers/media/dvb/dvb-core
 ccflags-y += -Idrivers/media/dvb/frontends
-
diff --git a/drivers/media/video/em28xx/em28xx-audio.c b/drivers/media/video/em28xx/em28xx-audio.c
index e2a7b77c39c7..d7e2a3dc5525 100644
--- a/drivers/media/video/em28xx/em28xx-audio.c
+++ b/drivers/media/video/em28xx/em28xx-audio.c
@@ -343,7 +343,6 @@ static int snd_em28xx_pcm_close(struct snd_pcm_substream *substream)
 static int snd_em28xx_hw_capture_params(struct snd_pcm_substream *substream,
 					struct snd_pcm_hw_params *hw_params)
 {
-	unsigned int channels, rate, format;
 	int ret;
 
 	dprintk("Setting capture parameters\n");
@@ -352,13 +351,17 @@ static int snd_em28xx_hw_capture_params(struct snd_pcm_substream *substream,
 				params_buffer_bytes(hw_params));
 	if (ret < 0)
 		return ret;
+#if 0
+	/* TODO: set up em28xx audio chip to deliver the correct audio format,
+	   current default is 48000hz multiplexed => 96000hz mono
+	   which shouldn't matter since analogue TV only supports mono */
+	unsigned int channels, rate, format;
+
 	format = params_format(hw_params);
 	rate = params_rate(hw_params);
 	channels = params_channels(hw_params);
+#endif
 
-	/* TODO: set up em28xx audio chip to deliver the correct audio format,
-	   current default is 48000hz multiplexed => 96000hz mono
-	   which shouldn't matter since analogue TV only supports mono */
 	return 0;
 }
 
diff --git a/drivers/media/video/em28xx/em28xx-cards.c b/drivers/media/video/em28xx/em28xx-cards.c
index 9fd8cc7dbb23..20a7e24de6fb 100644
--- a/drivers/media/video/em28xx/em28xx-cards.c
+++ b/drivers/media/video/em28xx/em28xx-cards.c
@@ -69,6 +69,8 @@ struct em28xx_hash_table {
 	unsigned int  tuner;
 };
 
+static void em28xx_pre_card_setup(struct em28xx *dev);
+
 /*
  *  Reset sequences for analog/digital modes
  */
@@ -2361,7 +2363,7 @@ static int em28xx_hint_sensor(struct em28xx *dev)
 /* Since em28xx_pre_card_setup() requires a proper dev->model,
  * this won't work for boards with generic PCI IDs
  */
-void em28xx_pre_card_setup(struct em28xx *dev)
+static void em28xx_pre_card_setup(struct em28xx *dev)
 {
 	/* Set the initial XCLK and I2C clock values based on the board
 	   definition */
@@ -2661,55 +2663,7 @@ static int em28xx_hint_board(struct em28xx *dev)
 	return -1;
 }
 
-/* ----------------------------------------------------------------------- */
-void em28xx_register_i2c_ir(struct em28xx *dev)
-{
-	/* Leadtek winfast tv USBII deluxe can find a non working IR-device */
-	/* at address 0x18, so if that address is needed for another board in */
-	/* the future, please put it after 0x1f. */
-	struct i2c_board_info info;
-	const unsigned short addr_list[] = {
-		 0x1f, 0x30, 0x47, I2C_CLIENT_END
-	};
-
-	if (disable_ir)
-		return;
-
-	memset(&info, 0, sizeof(struct i2c_board_info));
-	memset(&dev->init_data, 0, sizeof(dev->init_data));
-	strlcpy(info.type, "ir_video", I2C_NAME_SIZE);
-
-	/* detect & configure */
-	switch (dev->model) {
-	case EM2800_BOARD_TERRATEC_CINERGY_200:
-	case EM2820_BOARD_TERRATEC_CINERGY_250:
-		dev->init_data.ir_codes = RC_MAP_EM_TERRATEC;
-		dev->init_data.get_key = em28xx_get_key_terratec;
-		dev->init_data.name = "i2c IR (EM28XX Terratec)";
-		break;
-	case EM2820_BOARD_PINNACLE_USB_2:
-		dev->init_data.ir_codes = RC_MAP_PINNACLE_GREY;
-		dev->init_data.get_key = em28xx_get_key_pinnacle_usb_grey;
-		dev->init_data.name = "i2c IR (EM28XX Pinnacle PCTV)";
-		break;
-	case EM2820_BOARD_HAUPPAUGE_WINTV_USB_2:
-		dev->init_data.ir_codes = RC_MAP_HAUPPAUGE;
-		dev->init_data.get_key = em28xx_get_key_em_haup;
-		dev->init_data.name = "i2c IR (EM2840 Hauppauge)";
-		break;
-	case EM2820_BOARD_LEADTEK_WINFAST_USBII_DELUXE:
-		dev->init_data.ir_codes = RC_MAP_WINFAST_USBII_DELUXE;
-		dev->init_data.get_key = em28xx_get_key_winfast_usbii_deluxe;
-		dev->init_data.name = "i2c IR (EM2820 Winfast TV USBII Deluxe)";
-		break;
-	}
-
-	if (dev->init_data.name)
-		info.platform_data = &dev->init_data;
-	i2c_new_probed_device(&dev->i2c_adap, &info, addr_list, NULL);
-}
-
-void em28xx_card_setup(struct em28xx *dev)
+static void em28xx_card_setup(struct em28xx *dev)
 {
 	/*
 	 * If the device can be a webcam, seek for a sensor.
@@ -2849,13 +2803,6 @@ void em28xx_card_setup(struct em28xx *dev)
 		break;
 	}
 
-#if defined(CONFIG_MODULES) && defined(MODULE)
-	if (dev->board.has_ir_i2c && !disable_ir)
-		request_module("ir-kbd-i2c");
-#endif
-	if (dev->board.has_snapshot_button)
-		em28xx_register_snapshot_button(dev);
-
 	if (dev->board.valid == EM28XX_BOARD_NOT_VALIDATED) {
 		em28xx_errdev("\n\n");
 		em28xx_errdev("The support for this board weren't "
@@ -2929,9 +2876,6 @@ void em28xx_card_setup(struct em28xx *dev)
 	}
 
 	em28xx_tuner_setup(dev);
-
-	if(!disable_ir)
-		em28xx_ir_init(dev);
 }
 
 
@@ -2948,6 +2892,8 @@ static void request_module_async(struct work_struct *work)
 
 	if (dev->board.has_dvb)
 		request_module("em28xx-dvb");
+	if (dev->board.has_ir_i2c && !disable_ir)
+		request_module("em28xx-rc");
 }
 
 static void request_modules(struct em28xx *dev)
@@ -2972,12 +2918,6 @@ static void flush_request_modules(struct em28xx *dev)
 */
 void em28xx_release_resources(struct em28xx *dev)
 {
-	if (dev->sbutton_input_dev)
-		em28xx_deregister_snapshot_button(dev);
-
-	if (dev->ir)
-		em28xx_ir_fini(dev);
-
 	/*FIXME: I2C IR should be disconnected */
 
 	em28xx_release_analog_resources(dev);
@@ -3005,9 +2945,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 	dev->udev = udev;
 	mutex_init(&dev->ctrl_urb_lock);
 	spin_lock_init(&dev->slock);
-	init_waitqueue_head(&dev->open);
-	init_waitqueue_head(&dev->wait_frame);
-	init_waitqueue_head(&dev->wait_stream);
 
 	dev->em28xx_write_regs = em28xx_write_regs;
 	dev->em28xx_read_reg = em28xx_read_reg;
@@ -3140,9 +3077,7 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 
 	/* init video dma queues */
 	INIT_LIST_HEAD(&dev->vidq.active);
-	INIT_LIST_HEAD(&dev->vidq.queued);
 	INIT_LIST_HEAD(&dev->vbiq.active);
-	INIT_LIST_HEAD(&dev->vbiq.queued);
 
 	if (dev->board.has_msp34xx) {
 		/* Send a reset to other chips via gpio */
@@ -3447,8 +3382,6 @@ static void em28xx_usb_disconnect(struct usb_interface *interface)
 	   resources */
 	mutex_lock(&dev->lock);
 
-	wake_up_interruptible_all(&dev->open);
-
 	v4l2_device_disconnect(&dev->v4l2_dev);
 
 	if (dev->users) {
@@ -3460,8 +3393,6 @@ static void em28xx_usb_disconnect(struct usb_interface *interface)
 		dev->state |= DEV_MISCONFIGURED;
 		em28xx_uninit_isoc(dev, dev->mode);
 		dev->state |= DEV_DISCONNECTED;
-		wake_up_interruptible(&dev->wait_frame);
-		wake_up_interruptible(&dev->wait_stream);
 	} else {
 		dev->state |= DEV_DISCONNECTED;
 		em28xx_release_resources(dev);
diff --git a/drivers/media/video/em28xx/em28xx-core.c b/drivers/media/video/em28xx/em28xx-core.c
index 53a9fb91e97e..5717bdee8f1b 100644
--- a/drivers/media/video/em28xx/em28xx-core.c
+++ b/drivers/media/video/em28xx/em28xx-core.c
@@ -139,6 +139,7 @@ int em28xx_read_reg(struct em28xx *dev, u16 reg)
 {
 	return em28xx_read_reg_req(dev, USB_REQ_GET_STATUS, reg);
 }
+EXPORT_SYMBOL_GPL(em28xx_read_reg);
 
 /*
  * em28xx_write_regs_req()
@@ -205,6 +206,7 @@ int em28xx_write_regs(struct em28xx *dev, u16 reg, char *buf, int len)
 
 	return rc;
 }
+EXPORT_SYMBOL_GPL(em28xx_write_regs);
 
 /* Write a single register */
 int em28xx_write_reg(struct em28xx *dev, u16 reg, u8 val)
@@ -239,6 +241,7 @@ int em28xx_write_reg_bits(struct em28xx *dev, u16 reg, u8 val,
 
 	return em28xx_write_regs(dev, reg, &newval, 1);
 }
+EXPORT_SYMBOL_GPL(em28xx_write_reg_bits);
 
 /*
  * em28xx_is_ac97_ready()
@@ -666,7 +669,6 @@ int em28xx_capture_start(struct em28xx *dev, int start)
 
 	return rc;
 }
-EXPORT_SYMBOL_GPL(em28xx_capture_start);
 
 int em28xx_vbi_supported(struct em28xx *dev)
 {
@@ -975,7 +977,6 @@ void em28xx_uninit_isoc(struct em28xx *dev, enum em28xx_mode mode)
 	else
 		isoc_bufs = &dev->isoc_ctl.analog_bufs;
 
-	dev->isoc_ctl.nfields = -1;
 	for (i = 0; i < isoc_bufs->num_bufs; i++) {
 		urb = isoc_bufs->urb[i];
 		if (urb) {
@@ -1008,6 +1009,31 @@ void em28xx_uninit_isoc(struct em28xx *dev, enum em28xx_mode mode)
 EXPORT_SYMBOL_GPL(em28xx_uninit_isoc);
 
 /*
+ * Stop URBs
+ */
+void em28xx_stop_urbs(struct em28xx *dev)
+{
+	int i;
+	struct urb *urb;
+	struct em28xx_usb_isoc_bufs *isoc_bufs = &dev->isoc_ctl.digital_bufs;
+
+	em28xx_isocdbg("em28xx: called em28xx_stop_urbs\n");
+
+	for (i = 0; i < isoc_bufs->num_bufs; i++) {
+		urb = isoc_bufs->urb[i];
+		if (urb) {
+			if (!irqs_disabled())
+				usb_kill_urb(urb);
+			else
+				usb_unlink_urb(urb);
+		}
+	}
+
+	em28xx_capture_start(dev, 0);
+}
+EXPORT_SYMBOL_GPL(em28xx_stop_urbs);
+
+/*
  * Allocate URBs
  */
 int em28xx_alloc_isoc(struct em28xx *dev, enum em28xx_mode mode,
diff --git a/drivers/media/video/em28xx/em28xx-dvb.c b/drivers/media/video/em28xx/em28xx-dvb.c
index 503a8d5b5382..16410ac20092 100644
--- a/drivers/media/video/em28xx/em28xx-dvb.c
+++ b/drivers/media/video/em28xx/em28xx-dvb.c
@@ -183,7 +183,7 @@ static int em28xx_stop_streaming(struct em28xx_dvb *dvb)
 {
 	struct em28xx *dev = dvb->adapter.priv;
 
-	em28xx_capture_start(dev, 0);
+	em28xx_stop_urbs(dev);
 
 	em28xx_set_mode(dev, EM28XX_SUSPEND);
 
@@ -336,6 +336,8 @@ struct drxk_config pctv_520e_drxk = {
 	.single_master = 1,
 	.microcode_name = "dvb-demod-drxk-pctv.fw",
 	.chunk_size = 58,
+	.antenna_dvbt = true, /* disable LNA */
+	.antenna_gpio = (1 << 2), /* disable LNA */
 };
 
 static int drxk_gate_ctrl(struct dvb_frontend *fe, int enable)
@@ -474,8 +476,8 @@ static void terratec_h5_init(struct em28xx *dev)
 static void pctv_520e_init(struct em28xx *dev)
 {
 	/*
-	 * Init TDA8295(?) analog demodulator. Looks like I2C traffic to
-	 * digital demodulator and tuner are routed via TDA8295.
+	 * Init AVF4910B analog decoder. Looks like I2C traffic to
+	 * digital demodulator and tuner are routed via AVF4910B.
 	 */
 	int i;
 	struct {
@@ -542,7 +544,8 @@ static struct cxd2820r_config em28xx_cxd2820r_config = {
 	.i2c_address = (0xd8 >> 1),
 	.ts_mode = CXD2820R_TS_SERIAL,
 
-	/* enable LNA for DVB-T2 and DVB-C */
+	/* enable LNA for DVB-T, DVB-T2 and DVB-C */
+	.gpio_dvbt[0] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | CXD2820R_GPIO_L,
 	.gpio_dvbt2[0] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | CXD2820R_GPIO_L,
 	.gpio_dvbc[0] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | CXD2820R_GPIO_L,
 };
diff --git a/drivers/media/video/em28xx/em28xx-i2c.c b/drivers/media/video/em28xx/em28xx-i2c.c
index a88e169dba23..185db65b766e 100644
--- a/drivers/media/video/em28xx/em28xx-i2c.c
+++ b/drivers/media/video/em28xx/em28xx-i2c.c
@@ -553,9 +553,6 @@ int em28xx_i2c_register(struct em28xx *dev)
 	if (i2c_scan)
 		em28xx_do_i2c_scan(dev);
 
-	/* Instantiate the IR receiver device, if present */
-	em28xx_register_i2c_ir(dev);
-
 	return 0;
 }
 
diff --git a/drivers/media/video/em28xx/em28xx-input.c b/drivers/media/video/em28xx/em28xx-input.c
index 2630b265b0e8..fce5f7680c99 100644
--- a/drivers/media/video/em28xx/em28xx-input.c
+++ b/drivers/media/video/em28xx/em28xx-input.c
@@ -80,7 +80,7 @@ struct em28xx_IR {
  I2C IR based get keycodes - should be used with ir-kbd-i2c
  **********************************************************/
 
-int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+static int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 {
 	unsigned char b;
 
@@ -108,7 +108,7 @@ int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 	return 1;
 }
 
-int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+static int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 {
 	unsigned char buf[2];
 	u16 code;
@@ -157,7 +157,7 @@ int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 	return 1;
 }
 
-int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
+static int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
 				     u32 *ir_raw)
 {
 	unsigned char buf[3];
@@ -179,7 +179,8 @@ int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
 	return 1;
 }
 
-int em28xx_get_key_winfast_usbii_deluxe(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+static int em28xx_get_key_winfast_usbii_deluxe(struct IR_i2c *ir, u32 *ir_key,
+					u32 *ir_raw)
 {
 	unsigned char subaddr, keydetect, key;
 
@@ -387,7 +388,138 @@ int em28xx_ir_change_protocol(struct rc_dev *rc_dev, u64 rc_type)
 	return rc;
 }
 
-int em28xx_ir_init(struct em28xx *dev)
+static void em28xx_register_i2c_ir(struct em28xx *dev)
+{
+	/* Leadtek winfast tv USBII deluxe can find a non working IR-device */
+	/* at address 0x18, so if that address is needed for another board in */
+	/* the future, please put it after 0x1f. */
+	struct i2c_board_info info;
+	const unsigned short addr_list[] = {
+		 0x1f, 0x30, 0x47, I2C_CLIENT_END
+	};
+
+	memset(&info, 0, sizeof(struct i2c_board_info));
+	memset(&dev->init_data, 0, sizeof(dev->init_data));
+	strlcpy(info.type, "ir_video", I2C_NAME_SIZE);
+
+	/* detect & configure */
+	switch (dev->model) {
+	case EM2800_BOARD_TERRATEC_CINERGY_200:
+	case EM2820_BOARD_TERRATEC_CINERGY_250:
+		dev->init_data.ir_codes = RC_MAP_EM_TERRATEC;
+		dev->init_data.get_key = em28xx_get_key_terratec;
+		dev->init_data.name = "i2c IR (EM28XX Terratec)";
+		break;
+	case EM2820_BOARD_PINNACLE_USB_2:
+		dev->init_data.ir_codes = RC_MAP_PINNACLE_GREY;
+		dev->init_data.get_key = em28xx_get_key_pinnacle_usb_grey;
+		dev->init_data.name = "i2c IR (EM28XX Pinnacle PCTV)";
+		break;
+	case EM2820_BOARD_HAUPPAUGE_WINTV_USB_2:
+		dev->init_data.ir_codes = RC_MAP_HAUPPAUGE;
+		dev->init_data.get_key = em28xx_get_key_em_haup;
+		dev->init_data.name = "i2c IR (EM2840 Hauppauge)";
+		break;
+	case EM2820_BOARD_LEADTEK_WINFAST_USBII_DELUXE:
+		dev->init_data.ir_codes = RC_MAP_WINFAST_USBII_DELUXE;
+		dev->init_data.get_key = em28xx_get_key_winfast_usbii_deluxe;
+		dev->init_data.name = "i2c IR (EM2820 Winfast TV USBII Deluxe)";
+		break;
+	}
+
+	if (dev->init_data.name)
+		info.platform_data = &dev->init_data;
+	i2c_new_probed_device(&dev->i2c_adap, &info, addr_list, NULL);
+}
+
+/**********************************************************
+ Handle Webcam snapshot button
+ **********************************************************/
+
+static void em28xx_query_sbutton(struct work_struct *work)
+{
+	/* Poll the register and see if the button is depressed */
+	struct em28xx *dev =
+		container_of(work, struct em28xx, sbutton_query_work.work);
+	int ret;
+
+	ret = em28xx_read_reg(dev, EM28XX_R0C_USBSUSP);
+
+	if (ret & EM28XX_R0C_USBSUSP_SNAPSHOT) {
+		u8 cleared;
+		/* Button is depressed, clear the register */
+		cleared = ((u8) ret) & ~EM28XX_R0C_USBSUSP_SNAPSHOT;
+		em28xx_write_regs(dev, EM28XX_R0C_USBSUSP, &cleared, 1);
+
+		/* Not emulate the keypress */
+		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
+				 1);
+		/* Now unpress the key */
+		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
+				 0);
+	}
+
+	/* Schedule next poll */
+	schedule_delayed_work(&dev->sbutton_query_work,
+			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
+}
+
+static void em28xx_register_snapshot_button(struct em28xx *dev)
+{
+	struct input_dev *input_dev;
+	int err;
+
+	em28xx_info("Registering snapshot button...\n");
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		em28xx_errdev("input_allocate_device failed\n");
+		return;
+	}
+
+	usb_make_path(dev->udev, dev->snapshot_button_path,
+		      sizeof(dev->snapshot_button_path));
+	strlcat(dev->snapshot_button_path, "/sbutton",
+		sizeof(dev->snapshot_button_path));
+	INIT_DELAYED_WORK(&dev->sbutton_query_work, em28xx_query_sbutton);
+
+	input_dev->name = "em28xx snapshot button";
+	input_dev->phys = dev->snapshot_button_path;
+	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
+	set_bit(EM28XX_SNAPSHOT_KEY, input_dev->keybit);
+	input_dev->keycodesize = 0;
+	input_dev->keycodemax = 0;
+	input_dev->id.bustype = BUS_USB;
+	input_dev->id.vendor = le16_to_cpu(dev->udev->descriptor.idVendor);
+	input_dev->id.product = le16_to_cpu(dev->udev->descriptor.idProduct);
+	input_dev->id.version = 1;
+	input_dev->dev.parent = &dev->udev->dev;
+
+	err = input_register_device(input_dev);
+	if (err) {
+		em28xx_errdev("input_register_device failed\n");
+		input_free_device(input_dev);
+		return;
+	}
+
+	dev->sbutton_input_dev = input_dev;
+	schedule_delayed_work(&dev->sbutton_query_work,
+			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
+	return;
+
+}
+
+static void em28xx_deregister_snapshot_button(struct em28xx *dev)
+{
+	if (dev->sbutton_input_dev != NULL) {
+		em28xx_info("Deregistering snapshot button\n");
+		cancel_delayed_work_sync(&dev->sbutton_query_work);
+		input_unregister_device(dev->sbutton_input_dev);
+		dev->sbutton_input_dev = NULL;
+	}
+	return;
+}
+
+static int em28xx_ir_init(struct em28xx *dev)
 {
 	struct em28xx_IR *ir;
 	struct rc_dev *rc;
@@ -448,6 +580,15 @@ int em28xx_ir_init(struct em28xx *dev)
 	if (err)
 		goto err_out_stop;
 
+	em28xx_register_i2c_ir(dev);
+
+#if defined(CONFIG_MODULES) && defined(MODULE)
+	if (dev->board.has_ir_i2c)
+		request_module("ir-kbd-i2c");
+#endif
+	if (dev->board.has_snapshot_button)
+		em28xx_register_snapshot_button(dev);
+
 	return 0;
 
  err_out_stop:
@@ -458,10 +599,12 @@ int em28xx_ir_init(struct em28xx *dev)
 	return err;
 }
 
-int em28xx_ir_fini(struct em28xx *dev)
+static int em28xx_ir_fini(struct em28xx *dev)
 {
 	struct em28xx_IR *ir = dev->ir;
 
+	em28xx_deregister_snapshot_button(dev);
+
 	/* skip detach on non attached boards */
 	if (!ir)
 		return 0;
@@ -475,89 +618,26 @@ int em28xx_ir_fini(struct em28xx *dev)
 	return 0;
 }
 
-/**********************************************************
- Handle Webcam snapshot button
- **********************************************************/
+static struct em28xx_ops rc_ops = {
+	.id   = EM28XX_RC,
+	.name = "Em28xx Input Extension",
+	.init = em28xx_ir_init,
+	.fini = em28xx_ir_fini,
+};
 
-static void em28xx_query_sbutton(struct work_struct *work)
+static int __init em28xx_rc_register(void)
 {
-	/* Poll the register and see if the button is depressed */
-	struct em28xx *dev =
-		container_of(work, struct em28xx, sbutton_query_work.work);
-	int ret;
-
-	ret = em28xx_read_reg(dev, EM28XX_R0C_USBSUSP);
-
-	if (ret & EM28XX_R0C_USBSUSP_SNAPSHOT) {
-		u8 cleared;
-		/* Button is depressed, clear the register */
-		cleared = ((u8) ret) & ~EM28XX_R0C_USBSUSP_SNAPSHOT;
-		em28xx_write_regs(dev, EM28XX_R0C_USBSUSP, &cleared, 1);
-
-		/* Not emulate the keypress */
-		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
-				 1);
-		/* Now unpress the key */
-		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
-				 0);
-	}
-
-	/* Schedule next poll */
-	schedule_delayed_work(&dev->sbutton_query_work,
-			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
+	return em28xx_register_extension(&rc_ops);
 }
 
-void em28xx_register_snapshot_button(struct em28xx *dev)
+static void __exit em28xx_rc_unregister(void)
 {
-	struct input_dev *input_dev;
-	int err;
-
-	em28xx_info("Registering snapshot button...\n");
-	input_dev = input_allocate_device();
-	if (!input_dev) {
-		em28xx_errdev("input_allocate_device failed\n");
-		return;
-	}
-
-	usb_make_path(dev->udev, dev->snapshot_button_path,
-		      sizeof(dev->snapshot_button_path));
-	strlcat(dev->snapshot_button_path, "/sbutton",
-		sizeof(dev->snapshot_button_path));
-	INIT_DELAYED_WORK(&dev->sbutton_query_work, em28xx_query_sbutton);
-
-	input_dev->name = "em28xx snapshot button";
-	input_dev->phys = dev->snapshot_button_path;
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
-	set_bit(EM28XX_SNAPSHOT_KEY, input_dev->keybit);
-	input_dev->keycodesize = 0;
-	input_dev->keycodemax = 0;
-	input_dev->id.bustype = BUS_USB;
-	input_dev->id.vendor = le16_to_cpu(dev->udev->descriptor.idVendor);
-	input_dev->id.product = le16_to_cpu(dev->udev->descriptor.idProduct);
-	input_dev->id.version = 1;
-	input_dev->dev.parent = &dev->udev->dev;
-
-	err = input_register_device(input_dev);
-	if (err) {
-		em28xx_errdev("input_register_device failed\n");
-		input_free_device(input_dev);
-		return;
-	}
-
-	dev->sbutton_input_dev = input_dev;
-	schedule_delayed_work(&dev->sbutton_query_work,
-			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
-	return;
-
+	em28xx_unregister_extension(&rc_ops);
 }
 
-void em28xx_deregister_snapshot_button(struct em28xx *dev)
-{
-	if (dev->sbutton_input_dev != NULL) {
-		em28xx_info("Deregistering snapshot button\n");
-		cancel_delayed_work_sync(&dev->sbutton_query_work);
-		input_unregister_device(dev->sbutton_input_dev);
-		dev->sbutton_input_dev = NULL;
-	}
-	return;
-}
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_DESCRIPTION("Em28xx Input driver");
+
+module_init(em28xx_rc_register);
+module_exit(em28xx_rc_unregister);
diff --git a/drivers/media/video/em28xx/em28xx-video.c b/drivers/media/video/em28xx/em28xx-video.c
index 324b695c0724..50f5f4fc2148 100644
--- a/drivers/media/video/em28xx/em28xx-video.c
+++ b/drivers/media/video/em28xx/em28xx-video.c
@@ -1305,9 +1305,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 	if (0 == INPUT(i)->type)
 		return -EINVAL;
 
-	dev->ctl_input = i;
-
-	video_mux(dev, dev->ctl_input);
+	video_mux(dev, i);
 	return 0;
 }
 
@@ -2262,6 +2260,7 @@ static int em28xx_v4l2_close(struct file *filp)
 			em28xx_release_resources(dev);
 			kfree(dev->alt_max_pkt_size);
 			kfree(dev);
+			kfree(fh);
 			return 0;
 		}
 
@@ -2286,7 +2285,6 @@ static int em28xx_v4l2_close(struct file *filp)
 	videobuf_mmap_free(&fh->vb_vbiq);
 	kfree(fh);
 	dev->users--;
-	wake_up_interruptible_nr(&dev->open, 1);
 	return 0;
 }
 
@@ -2497,6 +2495,10 @@ static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 	vfd->release	= video_device_release;
 	vfd->debug	= video_debug;
 	vfd->lock	= &dev->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s",
 		 dev->name, type_name);
@@ -2518,7 +2520,6 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	dev->norm = em28xx_video_template.current_norm;
 	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
 	dev->interlaced = EM28XX_INTERLACED_DEFAULT;
-	dev->ctl_input = 0;
 
 	/* Analog specific initialization */
 	dev->format = &format[0];
@@ -2532,7 +2533,7 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	em28xx_set_video_format(dev, format[0].fourcc,
 				maxw, norm_maxh(dev));
 
-	video_mux(dev, dev->ctl_input);
+	video_mux(dev, 0);
 
 	/* Audio defaults */
 	dev->mute = 1;
diff --git a/drivers/media/video/em28xx/em28xx.h b/drivers/media/video/em28xx/em28xx.h
index 2868b19f8b54..8757523e6863 100644
--- a/drivers/media/video/em28xx/em28xx.h
+++ b/drivers/media/video/em28xx/em28xx.h
@@ -226,24 +226,10 @@ struct em28xx_usb_isoc_ctl {
 		/* isoc transfer buffers for digital mode */
 	struct em28xx_usb_isoc_bufs	digital_bufs;
 
-		/* Last buffer command and region */
-	u8				cmd;
-	int				pos, size, pktsize;
-
-		/* Last field: ODD or EVEN? */
-	int				field;
-
-		/* Stores incomplete commands */
-	u32				tmp_buf;
-	int				tmp_buf_len;
-
 		/* Stores already requested buffers */
 	struct em28xx_buffer    	*vid_buf;
 	struct em28xx_buffer    	*vbi_buf;
 
-		/* Stores the number of received fields */
-	int				nfields;
-
 		/* isoc urb callback */
 	int (*isoc_copy) (struct em28xx *dev, struct urb *urb);
 
@@ -264,12 +250,10 @@ struct em28xx_buffer {
 
 	struct list_head frame;
 	int top_field;
-	int receiving;
 };
 
 struct em28xx_dmaqueue {
 	struct list_head       active;
-	struct list_head       queued;
 
 	wait_queue_head_t          wq;
 
@@ -277,13 +261,6 @@ struct em28xx_dmaqueue {
 	int                        pos;
 };
 
-/* io methods */
-enum em28xx_io_method {
-	IO_NONE,
-	IO_READ,
-	IO_MMAP,
-};
-
 /* inputs */
 
 #define MAX_EM28XX_INPUT 4
@@ -467,6 +444,7 @@ enum em28xx_dev_state {
 /* em28xx extensions */
 #define EM28XX_AUDIO   0x10
 #define EM28XX_DVB     0x20
+#define EM28XX_RC      0x30
 
 /* em28xx resource types (used for res_get/res_lock etc */
 #define EM28XX_RESOURCE_VIDEO 0x01
@@ -577,7 +555,6 @@ struct em28xx {
 
 	/* states */
 	enum em28xx_dev_state state;
-	enum em28xx_io_method io;
 
 	/* vbi related state tracking */
 	int capture_type;
@@ -593,7 +570,6 @@ struct em28xx {
 	struct mutex ctrl_urb_lock;	/* protects urb_buf */
 	/* spinlock_t queue_lock; */
 	struct list_head inqueue, outqueue;
-	wait_queue_head_t open, wait_frame, wait_stream;
 	struct video_device *vbi_dev;
 	struct video_device *radio_dev;
 
@@ -695,6 +671,7 @@ int em28xx_init_isoc(struct em28xx *dev, enum em28xx_mode mode,
 		     int max_packets, int num_bufs, int max_pkt_size,
 		     int (*isoc_copy) (struct em28xx *dev, struct urb *urb));
 void em28xx_uninit_isoc(struct em28xx *dev, enum em28xx_mode mode);
+void em28xx_stop_urbs(struct em28xx *dev);
 int em28xx_isoc_dvb_max_packetsize(struct em28xx *dev);
 int em28xx_set_mode(struct em28xx *dev, enum em28xx_mode set_mode);
 int em28xx_gpio_set(struct em28xx *dev, struct em28xx_reg_seq *gpio);
@@ -710,45 +687,12 @@ void em28xx_release_analog_resources(struct em28xx *dev);
 
 /* Provided by em28xx-cards.c */
 extern int em2800_variant_detect(struct usb_device *udev, int model);
-extern void em28xx_pre_card_setup(struct em28xx *dev);
-extern void em28xx_card_setup(struct em28xx *dev);
 extern struct em28xx_board em28xx_boards[];
 extern struct usb_device_id em28xx_id_table[];
 extern const unsigned int em28xx_bcount;
-void em28xx_register_i2c_ir(struct em28xx *dev);
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg);
 void em28xx_release_resources(struct em28xx *dev);
 
-/* Provided by em28xx-input.c */
-
-#ifdef CONFIG_VIDEO_EM28XX_RC
-
-int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw);
-int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw);
-int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
-				     u32 *ir_raw);
-int em28xx_get_key_winfast_usbii_deluxe(struct IR_i2c *ir, u32 *ir_key,
-				     u32 *ir_raw);
-void em28xx_register_snapshot_button(struct em28xx *dev);
-void em28xx_deregister_snapshot_button(struct em28xx *dev);
-
-int em28xx_ir_init(struct em28xx *dev);
-int em28xx_ir_fini(struct em28xx *dev);
-
-#else
-
-#define em28xx_get_key_terratec			NULL
-#define em28xx_get_key_em_haup			NULL
-#define em28xx_get_key_pinnacle_usb_grey	NULL
-#define em28xx_get_key_winfast_usbii_deluxe	NULL
-
-static inline void em28xx_register_snapshot_button(struct em28xx *dev) {}
-static inline void em28xx_deregister_snapshot_button(struct em28xx *dev) {}
-static inline int em28xx_ir_init(struct em28xx *dev) { return 0; }
-static inline int em28xx_ir_fini(struct em28xx *dev) { return 0; }
-
-#endif
-
 /* Provided by em28xx-vbi.c */
 extern struct videobuf_queue_ops em28xx_vbi_qops;
 
diff --git a/drivers/media/video/et61x251/Kconfig b/drivers/media/video/et61x251/Kconfig
deleted file mode 100644
index 87981b078fe6..000000000000
--- a/drivers/media/video/et61x251/Kconfig
+++ /dev/null
@@ -1,18 +0,0 @@
-config USB_ET61X251
-	tristate "USB ET61X[12]51 PC Camera Controller support (DEPRECATED)"
-	depends on VIDEO_V4L2
-	default n
-	---help---
-	  This driver is DEPRECATED please use the gspca zc3xx module
-	  instead.
-
-	  Say Y here if you want support for cameras based on Etoms ET61X151
-	  or ET61X251 PC Camera Controllers.
-
-	  See <file:Documentation/video4linux/et61x251.txt> for more info.
-
-	  This driver uses the Video For Linux API. You must say Y or M to
-	  "Video For Linux" to use this driver.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called et61x251.
diff --git a/drivers/media/video/et61x251/Makefile b/drivers/media/video/et61x251/Makefile
deleted file mode 100644
index 2ff4db9ec882..000000000000
--- a/drivers/media/video/et61x251/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-et61x251-objs   := et61x251_core.o et61x251_tas5130d1b.o
-
-obj-$(CONFIG_USB_ET61X251)      += et61x251.o
-
diff --git a/drivers/media/video/et61x251/et61x251.h b/drivers/media/video/et61x251/et61x251.h
deleted file mode 100644
index 337ded4a6388..000000000000
--- a/drivers/media/video/et61x251/et61x251.h
+++ /dev/null
@@ -1,213 +0,0 @@
-/***************************************************************************
- * V4L2 driver for ET61X[12]51 PC Camera Controllers                       *
- *                                                                         *
- * Copyright (C) 2006 by Luca Risolia <luca.risolia@studio.unibo.it>       *
- *                                                                         *
- * This program is free software; you can redistribute it and/or modify    *
- * it under the terms of the GNU General Public License as published by    *
- * the Free Software Foundation; either version 2 of the License, or       *
- * (at your option) any later version.                                     *
- *                                                                         *
- * This program is distributed in the hope that it will be useful,         *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
- * GNU General Public License for more details.                            *
- *                                                                         *
- * You should have received a copy of the GNU General Public License       *
- * along with this program; if not, write to the Free Software             *
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.               *
- ***************************************************************************/
-
-#ifndef _ET61X251_H_
-#define _ET61X251_H_
-
-#include <linux/usb.h>
-#include <linux/videodev2.h>
-#include <media/v4l2-common.h>
-#include <linux/device.h>
-#include <linux/list.h>
-#include <linux/spinlock.h>
-#include <linux/time.h>
-#include <linux/wait.h>
-#include <linux/types.h>
-#include <linux/param.h>
-#include <linux/rwsem.h>
-#include <linux/mutex.h>
-#include <linux/stddef.h>
-#include <linux/string.h>
-#include <linux/kref.h>
-
-#include "et61x251_sensor.h"
-
-/*****************************************************************************/
-
-#define ET61X251_DEBUG
-#define ET61X251_DEBUG_LEVEL         2
-#define ET61X251_MAX_DEVICES         64
-#define ET61X251_PRESERVE_IMGSCALE   0
-#define ET61X251_FORCE_MUNMAP        0
-#define ET61X251_MAX_FRAMES          32
-#define ET61X251_COMPRESSION_QUALITY 0
-#define ET61X251_URBS                2
-#define ET61X251_ISO_PACKETS         7
-#define ET61X251_ALTERNATE_SETTING   13
-#define ET61X251_URB_TIMEOUT         msecs_to_jiffies(2 * ET61X251_ISO_PACKETS)
-#define ET61X251_CTRL_TIMEOUT        100
-#define ET61X251_FRAME_TIMEOUT       2
-
-/*****************************************************************************/
-
-static const struct usb_device_id et61x251_id_table[] = {
-	{ USB_DEVICE(0x102c, 0x6251), },
-	{ }
-};
-
-ET61X251_SENSOR_TABLE
-
-/*****************************************************************************/
-
-enum et61x251_frame_state {
-	F_UNUSED,
-	F_QUEUED,
-	F_GRABBING,
-	F_DONE,
-	F_ERROR,
-};
-
-struct et61x251_frame_t {
-	void* bufmem;
-	struct v4l2_buffer buf;
-	enum et61x251_frame_state state;
-	struct list_head frame;
-	unsigned long vma_use_count;
-};
-
-enum et61x251_dev_state {
-	DEV_INITIALIZED = 0x01,
-	DEV_DISCONNECTED = 0x02,
-	DEV_MISCONFIGURED = 0x04,
-};
-
-enum et61x251_io_method {
-	IO_NONE,
-	IO_READ,
-	IO_MMAP,
-};
-
-enum et61x251_stream_state {
-	STREAM_OFF,
-	STREAM_INTERRUPT,
-	STREAM_ON,
-};
-
-struct et61x251_sysfs_attr {
-	u8 reg, i2c_reg;
-};
-
-struct et61x251_module_param {
-	u8 force_munmap;
-	u16 frame_timeout;
-};
-
-static DEFINE_MUTEX(et61x251_sysfs_lock);
-static DECLARE_RWSEM(et61x251_dev_lock);
-
-struct et61x251_device {
-	struct video_device* v4ldev;
-
-	struct et61x251_sensor sensor;
-
-	struct usb_device* usbdev;
-	struct urb* urb[ET61X251_URBS];
-	void* transfer_buffer[ET61X251_URBS];
-	u8* control_buffer;
-
-	struct et61x251_frame_t *frame_current, frame[ET61X251_MAX_FRAMES];
-	struct list_head inqueue, outqueue;
-	u32 frame_count, nbuffers, nreadbuffers;
-
-	enum et61x251_io_method io;
-	enum et61x251_stream_state stream;
-
-	struct v4l2_jpegcompression compression;
-
-	struct et61x251_sysfs_attr sysfs;
-	struct et61x251_module_param module_param;
-
-	struct kref kref;
-	enum et61x251_dev_state state;
-	u8 users;
-
-	struct completion probe;
-	struct mutex open_mutex, fileop_mutex;
-	spinlock_t queue_lock;
-	wait_queue_head_t wait_open, wait_frame, wait_stream;
-};
-
-/*****************************************************************************/
-
-struct et61x251_device*
-et61x251_match_id(struct et61x251_device* cam, const struct usb_device_id *id)
-{
-	return usb_match_id(usb_ifnum_to_if(cam->usbdev, 0), id) ? cam : NULL;
-}
-
-
-void
-et61x251_attach_sensor(struct et61x251_device* cam,
-		       const struct et61x251_sensor* sensor)
-{
-	memcpy(&cam->sensor, sensor, sizeof(struct et61x251_sensor));
-}
-
-/*****************************************************************************/
-
-#undef DBG
-#undef KDBG
-#ifdef ET61X251_DEBUG
-#define DBG(level, fmt, ...)						\
-do {									\
-	if (debug >= (level)) {						\
-		if ((level) == 1)					\
-			dev_err(&cam->usbdev->dev, fmt "\n",		\
-				##__VA_ARGS__);				\
-		else if ((level) == 2)					\
-			dev_info(&cam->usbdev->dev, fmt "\n",		\
-				 ##__VA_ARGS__);			\
-		else if ((level) >= 3)					\
-			dev_info(&cam->usbdev->dev, "[%s:%s:%d] " fmt "\n", \
-				 __FILE__, __func__, __LINE__,		\
-				 ##__VA_ARGS__);			\
-	}								\
-} while (0)
-#define KDBG(level, fmt, ...)						\
-do {									\
-	if (debug >= (level)) {						\
-		if ((level) == 1 || (level) == 2)			\
-			pr_info(fmt "\n", ##__VA_ARGS__);		\
-		else if ((level) == 3)					\
-			pr_debug("[%s:%s:%d] " fmt "\n",		\
-				 __FILE__,  __func__, __LINE__,		\
-				 ##__VA_ARGS__);			\
-	}								\
-} while (0)
-#define V4LDBG(level, name, cmd)					\
-do {									\
-	if (debug >= (level))						\
-		v4l_print_ioctl(name, cmd);				\
-} while (0)
-#else
-#define DBG(level, fmt, ...) do {;} while(0)
-#define KDBG(level, fmt, ...) do {;} while(0)
-#define V4LDBG(level, name, cmd) do {;} while(0)
-#endif
-
-#undef PDBG
-#define PDBG(fmt, ...)							\
-	dev_info(&cam->usbdev->dev, "[%s:%s:%d] " fmt "\n",		\
-		 __FILE__, __func__, __LINE__, ##__VA_ARGS__)
-
-#undef PDBGG
-#define PDBGG(fmt, args...) do {;} while (0) /* placeholder */
-
-#endif /* _ET61X251_H_ */
diff --git a/drivers/media/video/et61x251/et61x251_core.c b/drivers/media/video/et61x251/et61x251_core.c
deleted file mode 100644
index 5539f09440ac..000000000000
--- a/drivers/media/video/et61x251/et61x251_core.c
+++ /dev/null
@@ -1,2683 +0,0 @@
-/***************************************************************************
- * V4L2 driver for ET61X[12]51 PC Camera Controllers                       *
- *                                                                         *
- * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it>  *
- *                                                                         *
- * This program is free software; you can redistribute it and/or modify    *
- * it under the terms of the GNU General Public License as published by    *
- * the Free Software Foundation; either version 2 of the License, or       *
- * (at your option) any later version.                                     *
- *                                                                         *
- * This program is distributed in the hope that it will be useful,         *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
- * GNU General Public License for more details.                            *
- *                                                                         *
- * You should have received a copy of the GNU General Public License       *
- * along with this program; if not, write to the Free Software             *
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.               *
- ***************************************************************************/
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/version.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/param.h>
-#include <linux/errno.h>
-#include <linux/slab.h>
-#include <linux/device.h>
-#include <linux/fs.h>
-#include <linux/delay.h>
-#include <linux/compiler.h>
-#include <linux/ioctl.h>
-#include <linux/poll.h>
-#include <linux/stat.h>
-#include <linux/mm.h>
-#include <linux/vmalloc.h>
-#include <linux/page-flags.h>
-#include <media/v4l2-ioctl.h>
-#include <asm/byteorder.h>
-#include <asm/page.h>
-#include <asm/uaccess.h>
-
-#include "et61x251.h"
-
-/*****************************************************************************/
-
-#define ET61X251_MODULE_NAME    "V4L2 driver for ET61X[12]51 "                \
-				"PC Camera Controllers"
-#define ET61X251_MODULE_AUTHOR  "(C) 2006-2007 Luca Risolia"
-#define ET61X251_AUTHOR_EMAIL   "<luca.risolia@studio.unibo.it>"
-#define ET61X251_MODULE_LICENSE "GPL"
-#define ET61X251_MODULE_VERSION "1.1.10"
-
-/*****************************************************************************/
-
-MODULE_DEVICE_TABLE(usb, et61x251_id_table);
-
-MODULE_AUTHOR(ET61X251_MODULE_AUTHOR " " ET61X251_AUTHOR_EMAIL);
-MODULE_DESCRIPTION(ET61X251_MODULE_NAME);
-MODULE_VERSION(ET61X251_MODULE_VERSION);
-MODULE_LICENSE(ET61X251_MODULE_LICENSE);
-
-static short video_nr[] = {[0 ... ET61X251_MAX_DEVICES-1] = -1};
-module_param_array(video_nr, short, NULL, 0444);
-MODULE_PARM_DESC(video_nr,
-		 "\n<-1|n[,...]> Specify V4L2 minor mode number."
-		 "\n -1 = use next available (default)"
-		 "\n  n = use minor number n (integer >= 0)"
-		 "\nYou can specify up to "
-		 __MODULE_STRING(ET61X251_MAX_DEVICES) " cameras this way."
-		 "\nFor example:"
-		 "\nvideo_nr=-1,2,-1 would assign minor number 2 to"
-		 "\nthe second registered camera and use auto for the first"
-		 "\none and for every other camera."
-		 "\n");
-
-static bool force_munmap[] = {[0 ... ET61X251_MAX_DEVICES-1] =
-			      ET61X251_FORCE_MUNMAP};
-module_param_array(force_munmap, bool, NULL, 0444);
-MODULE_PARM_DESC(force_munmap,
-		 "\n<0|1[,...]> Force the application to unmap previously"
-		 "\nmapped buffer memory before calling any VIDIOC_S_CROP or"
-		 "\nVIDIOC_S_FMT ioctl's. Not all the applications support"
-		 "\nthis feature. This parameter is specific for each"
-		 "\ndetected camera."
-		 "\n 0 = do not force memory unmapping"
-		 "\n 1 = force memory unmapping (save memory)"
-		 "\nDefault value is "__MODULE_STRING(ET61X251_FORCE_MUNMAP)"."
-		 "\n");
-
-static unsigned int frame_timeout[] = {[0 ... ET61X251_MAX_DEVICES-1] =
-				       ET61X251_FRAME_TIMEOUT};
-module_param_array(frame_timeout, uint, NULL, 0644);
-MODULE_PARM_DESC(frame_timeout,
-		 "\n<n[,...]> Timeout for a video frame in seconds."
-		 "\nThis parameter is specific for each detected camera."
-		 "\nDefault value is "
-		 __MODULE_STRING(ET61X251_FRAME_TIMEOUT)"."
-		 "\n");
-
-#ifdef ET61X251_DEBUG
-static unsigned short debug = ET61X251_DEBUG_LEVEL;
-module_param(debug, ushort, 0644);
-MODULE_PARM_DESC(debug,
-		 "\n<n> Debugging information level, from 0 to 3:"
-		 "\n0 = none (use carefully)"
-		 "\n1 = critical errors"
-		 "\n2 = significant informations"
-		 "\n3 = more verbose messages"
-		 "\nLevel 3 is useful for testing only, when only "
-		 "one device is used."
-		 "\nDefault value is "__MODULE_STRING(ET61X251_DEBUG_LEVEL)"."
-		 "\n");
-#endif
-
-/*****************************************************************************/
-
-static u32
-et61x251_request_buffers(struct et61x251_device* cam, u32 count,
-			 enum et61x251_io_method io)
-{
-	struct v4l2_pix_format* p = &(cam->sensor.pix_format);
-	struct v4l2_rect* r = &(cam->sensor.cropcap.bounds);
-	const size_t imagesize = cam->module_param.force_munmap ||
-				 io == IO_READ ?
-				 (p->width * p->height * p->priv) / 8 :
-				 (r->width * r->height * p->priv) / 8;
-	void* buff = NULL;
-	u32 i;
-
-	if (count > ET61X251_MAX_FRAMES)
-		count = ET61X251_MAX_FRAMES;
-
-	cam->nbuffers = count;
-	while (cam->nbuffers > 0) {
-		if ((buff = vmalloc_32_user(cam->nbuffers *
-					    PAGE_ALIGN(imagesize))))
-			break;
-		cam->nbuffers--;
-	}
-
-	for (i = 0; i < cam->nbuffers; i++) {
-		cam->frame[i].bufmem = buff + i*PAGE_ALIGN(imagesize);
-		cam->frame[i].buf.index = i;
-		cam->frame[i].buf.m.offset = i*PAGE_ALIGN(imagesize);
-		cam->frame[i].buf.length = imagesize;
-		cam->frame[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		cam->frame[i].buf.sequence = 0;
-		cam->frame[i].buf.field = V4L2_FIELD_NONE;
-		cam->frame[i].buf.memory = V4L2_MEMORY_MMAP;
-		cam->frame[i].buf.flags = 0;
-	}
-
-	return cam->nbuffers;
-}
-
-
-static void et61x251_release_buffers(struct et61x251_device* cam)
-{
-	if (cam->nbuffers) {
-		vfree(cam->frame[0].bufmem);
-		cam->nbuffers = 0;
-	}
-	cam->frame_current = NULL;
-}
-
-
-static void et61x251_empty_framequeues(struct et61x251_device* cam)
-{
-	u32 i;
-
-	INIT_LIST_HEAD(&cam->inqueue);
-	INIT_LIST_HEAD(&cam->outqueue);
-
-	for (i = 0; i < ET61X251_MAX_FRAMES; i++) {
-		cam->frame[i].state = F_UNUSED;
-		cam->frame[i].buf.bytesused = 0;
-	}
-}
-
-
-static void et61x251_requeue_outqueue(struct et61x251_device* cam)
-{
-	struct et61x251_frame_t *i;
-
-	list_for_each_entry(i, &cam->outqueue, frame) {
-		i->state = F_QUEUED;
-		list_add(&i->frame, &cam->inqueue);
-	}
-
-	INIT_LIST_HEAD(&cam->outqueue);
-}
-
-
-static void et61x251_queue_unusedframes(struct et61x251_device* cam)
-{
-	unsigned long lock_flags;
-	u32 i;
-
-	for (i = 0; i < cam->nbuffers; i++)
-		if (cam->frame[i].state == F_UNUSED) {
-			cam->frame[i].state = F_QUEUED;
-			spin_lock_irqsave(&cam->queue_lock, lock_flags);
-			list_add_tail(&cam->frame[i].frame, &cam->inqueue);
-			spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-		}
-}
-
-/*****************************************************************************/
-
-int et61x251_write_reg(struct et61x251_device* cam, u8 value, u16 index)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* buff = cam->control_buffer;
-	int res;
-
-	*buff = value;
-
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0) {
-		DBG(3, "Failed to write a register (value 0x%02X, index "
-		       "0x%02X, error %d)", value, index, res);
-		return -1;
-	}
-
-	return 0;
-}
-
-
-static int et61x251_read_reg(struct et61x251_device* cam, u16 index)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* buff = cam->control_buffer;
-	int res;
-
-	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
-			      0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		DBG(3, "Failed to read a register (index 0x%02X, error %d)",
-		    index, res);
-
-	return (res >= 0) ? (int)(*buff) : -1;
-}
-
-
-static int
-et61x251_i2c_wait(struct et61x251_device* cam,
-		  const struct et61x251_sensor* sensor)
-{
-	int i, r;
-
-	for (i = 1; i <= 8; i++) {
-		if (sensor->interface == ET61X251_I2C_3WIRES) {
-			r = et61x251_read_reg(cam, 0x8e);
-			if (!(r & 0x02) && (r >= 0))
-				return 0;
-		} else {
-			r = et61x251_read_reg(cam, 0x8b);
-			if (!(r & 0x01) && (r >= 0))
-				return 0;
-		}
-		if (r < 0)
-			return -EIO;
-		udelay(8*8); /* minimum for sensors at 400kHz */
-	}
-
-	return -EBUSY;
-}
-
-
-int
-et61x251_i2c_raw_write(struct et61x251_device* cam, u8 n, u8 data1, u8 data2,
-		       u8 data3, u8 data4, u8 data5, u8 data6, u8 data7,
-		       u8 data8, u8 address)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
-	int err = 0, res;
-
-	data[0] = data2;
-	data[1] = data3;
-	data[2] = data4;
-	data[3] = data5;
-	data[4] = data6;
-	data[5] = data7;
-	data[6] = data8;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x81, data, n-1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	data[0] = address;
-	data[1] = cam->sensor.i2c_slave_id;
-	data[2] = cam->sensor.rsta | 0x02 | (n << 4);
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	/* Start writing through the serial interface */
-	data[0] = data1;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	err += et61x251_i2c_wait(cam, &cam->sensor);
-
-	if (err)
-		DBG(3, "I2C raw write failed for %s image sensor",
-		    cam->sensor.name);
-
-	PDBGG("I2C raw write: %u bytes, address = 0x%02X, data1 = 0x%02X, "
-	      "data2 = 0x%02X, data3 = 0x%02X, data4 = 0x%02X, data5 = 0x%02X,"
-	      " data6 = 0x%02X, data7 = 0x%02X, data8 = 0x%02X", n, address,
-	      data1, data2, data3, data4, data5, data6, data7, data8);
-
-	return err ? -1 : 0;
-
-}
-
-
-/*****************************************************************************/
-
-static void et61x251_urb_complete(struct urb *urb)
-{
-	struct et61x251_device* cam = urb->context;
-	struct et61x251_frame_t** f;
-	size_t imagesize;
-	u8 i;
-	int err = 0;
-
-	if (urb->status == -ENOENT)
-		return;
-
-	f = &cam->frame_current;
-
-	if (cam->stream == STREAM_INTERRUPT) {
-		cam->stream = STREAM_OFF;
-		if ((*f))
-			(*f)->state = F_QUEUED;
-		DBG(3, "Stream interrupted");
-		wake_up(&cam->wait_stream);
-	}
-
-	if (cam->state & DEV_DISCONNECTED)
-		return;
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		wake_up_interruptible(&cam->wait_frame);
-		return;
-	}
-
-	if (cam->stream == STREAM_OFF || list_empty(&cam->inqueue))
-		goto resubmit_urb;
-
-	if (!(*f))
-		(*f) = list_entry(cam->inqueue.next, struct et61x251_frame_t,
-				  frame);
-
-	imagesize = (cam->sensor.pix_format.width *
-		     cam->sensor.pix_format.height *
-		     cam->sensor.pix_format.priv) / 8;
-
-	for (i = 0; i < urb->number_of_packets; i++) {
-		unsigned int len, status;
-		void *pos;
-		u8* b1, * b2, sof;
-		const u8 VOID_BYTES = 6;
-		size_t imglen;
-
-		len = urb->iso_frame_desc[i].actual_length;
-		status = urb->iso_frame_desc[i].status;
-		pos = urb->iso_frame_desc[i].offset + urb->transfer_buffer;
-
-		if (status) {
-			DBG(3, "Error in isochronous frame");
-			(*f)->state = F_ERROR;
-			continue;
-		}
-
-		b1 = pos++;
-		b2 = pos++;
-		sof = ((*b1 & 0x3f) == 63);
-		imglen = ((*b1 & 0xc0) << 2) | *b2;
-
-		PDBGG("Isochrnous frame: length %u, #%u i, image length %zu",
-		      len, i, imglen);
-
-		if ((*f)->state == F_QUEUED || (*f)->state == F_ERROR)
-start_of_frame:
-			if (sof) {
-				(*f)->state = F_GRABBING;
-				(*f)->buf.bytesused = 0;
-				do_gettimeofday(&(*f)->buf.timestamp);
-				pos += 22;
-				DBG(3, "SOF detected: new video frame");
-			}
-
-		if ((*f)->state == F_GRABBING) {
-			if (sof && (*f)->buf.bytesused) {
-				if (cam->sensor.pix_format.pixelformat ==
-							 V4L2_PIX_FMT_ET61X251)
-					goto end_of_frame;
-				else {
-					DBG(3, "Not expected SOF detected "
-					       "after %lu bytes",
-					   (unsigned long)(*f)->buf.bytesused);
-					(*f)->state = F_ERROR;
-					continue;
-				}
-			}
-
-			if ((*f)->buf.bytesused + imglen > imagesize) {
-				DBG(3, "Video frame size exceeded");
-				(*f)->state = F_ERROR;
-				continue;
-			}
-
-			pos += VOID_BYTES;
-
-			memcpy((*f)->bufmem+(*f)->buf.bytesused, pos, imglen);
-			(*f)->buf.bytesused += imglen;
-
-			if ((*f)->buf.bytesused == imagesize) {
-				u32 b;
-end_of_frame:
-				b = (*f)->buf.bytesused;
-				(*f)->state = F_DONE;
-				(*f)->buf.sequence= ++cam->frame_count;
-				spin_lock(&cam->queue_lock);
-				list_move_tail(&(*f)->frame, &cam->outqueue);
-				if (!list_empty(&cam->inqueue))
-					(*f) = list_entry(cam->inqueue.next,
-						       struct et61x251_frame_t,
-							  frame);
-				else
-					(*f) = NULL;
-				spin_unlock(&cam->queue_lock);
-				DBG(3, "Video frame captured: : %lu bytes",
-				       (unsigned long)(b));
-
-				if (!(*f))
-					goto resubmit_urb;
-
-				if (sof &&
-				    cam->sensor.pix_format.pixelformat ==
-							 V4L2_PIX_FMT_ET61X251)
-					goto start_of_frame;
-			}
-		}
-	}
-
-resubmit_urb:
-	urb->dev = cam->usbdev;
-	err = usb_submit_urb(urb, GFP_ATOMIC);
-	if (err < 0 && err != -EPERM) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "usb_submit_urb() failed");
-	}
-
-	wake_up_interruptible(&cam->wait_frame);
-}
-
-
-static int et61x251_start_transfer(struct et61x251_device* cam)
-{
-	struct usb_device *udev = cam->usbdev;
-	struct urb* urb;
-	struct usb_host_interface* altsetting = usb_altnum_to_altsetting(
-						   usb_ifnum_to_if(udev, 0),
-						   ET61X251_ALTERNATE_SETTING);
-	const unsigned int psz = le16_to_cpu(altsetting->
-					     endpoint[0].desc.wMaxPacketSize);
-	s8 i, j;
-	int err = 0;
-
-	for (i = 0; i < ET61X251_URBS; i++) {
-		cam->transfer_buffer[i] = kzalloc(ET61X251_ISO_PACKETS * psz,
-						  GFP_KERNEL);
-		if (!cam->transfer_buffer[i]) {
-			err = -ENOMEM;
-			DBG(1, "Not enough memory");
-			goto free_buffers;
-		}
-	}
-
-	for (i = 0; i < ET61X251_URBS; i++) {
-		urb = usb_alloc_urb(ET61X251_ISO_PACKETS, GFP_KERNEL);
-		cam->urb[i] = urb;
-		if (!urb) {
-			err = -ENOMEM;
-			DBG(1, "usb_alloc_urb() failed");
-			goto free_urbs;
-		}
-		urb->dev = udev;
-		urb->context = cam;
-		urb->pipe = usb_rcvisocpipe(udev, 1);
-		urb->transfer_flags = URB_ISO_ASAP;
-		urb->number_of_packets = ET61X251_ISO_PACKETS;
-		urb->complete = et61x251_urb_complete;
-		urb->transfer_buffer = cam->transfer_buffer[i];
-		urb->transfer_buffer_length = psz * ET61X251_ISO_PACKETS;
-		urb->interval = 1;
-		for (j = 0; j < ET61X251_ISO_PACKETS; j++) {
-			urb->iso_frame_desc[j].offset = psz * j;
-			urb->iso_frame_desc[j].length = psz;
-		}
-	}
-
-	err = et61x251_write_reg(cam, 0x01, 0x03);
-	err = et61x251_write_reg(cam, 0x00, 0x03);
-	err = et61x251_write_reg(cam, 0x08, 0x03);
-	if (err) {
-		err = -EIO;
-		DBG(1, "I/O hardware error");
-		goto free_urbs;
-	}
-
-	err = usb_set_interface(udev, 0, ET61X251_ALTERNATE_SETTING);
-	if (err) {
-		DBG(1, "usb_set_interface() failed");
-		goto free_urbs;
-	}
-
-	cam->frame_current = NULL;
-
-	for (i = 0; i < ET61X251_URBS; i++) {
-		err = usb_submit_urb(cam->urb[i], GFP_KERNEL);
-		if (err) {
-			for (j = i-1; j >= 0; j--)
-				usb_kill_urb(cam->urb[j]);
-			DBG(1, "usb_submit_urb() failed, error %d", err);
-			goto free_urbs;
-		}
-	}
-
-	return 0;
-
-free_urbs:
-	for (i = 0; (i < ET61X251_URBS) && cam->urb[i]; i++)
-		usb_free_urb(cam->urb[i]);
-
-free_buffers:
-	for (i = 0; (i < ET61X251_URBS) && cam->transfer_buffer[i]; i++)
-		kfree(cam->transfer_buffer[i]);
-
-	return err;
-}
-
-
-static int et61x251_stop_transfer(struct et61x251_device* cam)
-{
-	struct usb_device *udev = cam->usbdev;
-	s8 i;
-	int err = 0;
-
-	if (cam->state & DEV_DISCONNECTED)
-		return 0;
-
-	for (i = ET61X251_URBS-1; i >= 0; i--) {
-		usb_kill_urb(cam->urb[i]);
-		usb_free_urb(cam->urb[i]);
-		kfree(cam->transfer_buffer[i]);
-	}
-
-	err = usb_set_interface(udev, 0, 0); /* 0 Mb/s */
-	if (err)
-		DBG(3, "usb_set_interface() failed");
-
-	return err;
-}
-
-
-static int et61x251_stream_interrupt(struct et61x251_device* cam)
-{
-	long timeout;
-
-	cam->stream = STREAM_INTERRUPT;
-	timeout = wait_event_timeout(cam->wait_stream,
-				     (cam->stream == STREAM_OFF) ||
-				     (cam->state & DEV_DISCONNECTED),
-				     ET61X251_URB_TIMEOUT);
-	if (cam->state & DEV_DISCONNECTED)
-		return -ENODEV;
-	else if (cam->stream != STREAM_OFF) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "URB timeout reached. The camera is misconfigured. To "
-		       "use it, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	return 0;
-}
-
-/*****************************************************************************/
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-
-static int et61x251_i2c_try_read(struct et61x251_device* cam,
-				 const struct et61x251_sensor* sensor,
-				 u8 address)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
-	int err = 0, res;
-
-	data[0] = address;
-	data[1] = cam->sensor.i2c_slave_id;
-	data[2] = cam->sensor.rsta | 0x10;
-	data[3] = !(et61x251_read_reg(cam, 0x8b) & 0x02);
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x88, data, 4, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	err += et61x251_i2c_wait(cam, sensor);
-
-	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
-			      0, 0x80, data, 8, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	if (err)
-		DBG(3, "I2C read failed for %s image sensor", sensor->name);
-
-	PDBGG("I2C read: address 0x%02X, value: 0x%02X", address, data[0]);
-
-	return err ? -1 : (int)data[0];
-}
-
-
-static int et61x251_i2c_try_write(struct et61x251_device* cam,
-				  const struct et61x251_sensor* sensor,
-				  u8 address, u8 value)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
-	int err = 0, res;
-
-	data[0] = address;
-	data[1] = cam->sensor.i2c_slave_id;
-	data[2] = cam->sensor.rsta | 0x12;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	data[0] = value;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	err += et61x251_i2c_wait(cam, sensor);
-
-	if (err)
-		DBG(3, "I2C write failed for %s image sensor", sensor->name);
-
-	PDBGG("I2C write: address 0x%02X, value: 0x%02X", address, value);
-
-	return err ? -1 : 0;
-}
-
-static int et61x251_i2c_read(struct et61x251_device* cam, u8 address)
-{
-	return et61x251_i2c_try_read(cam, &cam->sensor, address);
-}
-
-static int et61x251_i2c_write(struct et61x251_device* cam,
-			      u8 address, u8 value)
-{
-	return et61x251_i2c_try_write(cam, &cam->sensor, address, value);
-}
-
-static u8 et61x251_strtou8(const char* buff, size_t len, ssize_t* count)
-{
-	char str[5];
-	char* endp;
-	unsigned long val;
-
-	if (len < 4) {
-		strncpy(str, buff, len);
-		str[len] = '\0';
-	} else {
-		strncpy(str, buff, 4);
-		str[4] = '\0';
-	}
-
-	val = simple_strtoul(str, &endp, 0);
-
-	*count = 0;
-	if (val <= 0xff)
-		*count = (ssize_t)(endp - str);
-	if ((*count) && (len == *count+1) && (buff[*count] == '\n'))
-		*count += 1;
-
-	return (u8)val;
-}
-
-/*
-   NOTE 1: being inside one of the following methods implies that the v4l
-	   device exists for sure (see kobjects and reference counters)
-   NOTE 2: buffers are PAGE_SIZE long
-*/
-
-static ssize_t et61x251_show_reg(struct device* cd,
-				 struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	count = sprintf(buf, "%u\n", cam->sysfs.reg);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_reg(struct device* cd,
-		   struct device_attribute *attr, const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 index;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	index = et61x251_strtou8(buf, len, &count);
-	if (index > 0x8e || !count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	cam->sysfs.reg = index;
-
-	DBG(2, "Moved ET61X[12]51 register index to 0x%02X", cam->sysfs.reg);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t et61x251_show_val(struct device* cd,
-				 struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-	int val;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	if ((val = et61x251_read_reg(cam, cam->sysfs.reg)) < 0) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	count = sprintf(buf, "%d\n", val);
-
-	DBG(3, "Read bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_val(struct device* cd, struct device_attribute *attr,
-		   const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 value;
-	ssize_t count;
-	int err;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	value = et61x251_strtou8(buf, len, &count);
-	if (!count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	err = et61x251_write_reg(cam, value, cam->sysfs.reg);
-	if (err) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	DBG(2, "Written ET61X[12]51 reg. 0x%02X, val. 0x%02X",
-	    cam->sysfs.reg, value);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t et61x251_show_i2c_reg(struct device* cd,
-				     struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	count = sprintf(buf, "%u\n", cam->sysfs.i2c_reg);
-
-	DBG(3, "Read bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_i2c_reg(struct device* cd, struct device_attribute *attr,
-		       const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 index;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	index = et61x251_strtou8(buf, len, &count);
-	if (!count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	cam->sysfs.i2c_reg = index;
-
-	DBG(2, "Moved sensor register index to 0x%02X", cam->sysfs.i2c_reg);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t et61x251_show_i2c_val(struct device* cd,
-				     struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-	int val;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENOSYS;
-	}
-
-	if ((val = et61x251_i2c_read(cam, cam->sysfs.i2c_reg)) < 0) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	count = sprintf(buf, "%d\n", val);
-
-	DBG(3, "Read bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_i2c_val(struct device* cd, struct device_attribute *attr,
-		       const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 value;
-	ssize_t count;
-	int err;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENOSYS;
-	}
-
-	value = et61x251_strtou8(buf, len, &count);
-	if (!count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	err = et61x251_i2c_write(cam, cam->sysfs.i2c_reg, value);
-	if (err) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	DBG(2, "Written sensor reg. 0x%02X, val. 0x%02X",
-	    cam->sysfs.i2c_reg, value);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static DEVICE_ATTR(reg, S_IRUGO | S_IWUSR,
-		   et61x251_show_reg, et61x251_store_reg);
-static DEVICE_ATTR(val, S_IRUGO | S_IWUSR,
-		   et61x251_show_val, et61x251_store_val);
-static DEVICE_ATTR(i2c_reg, S_IRUGO | S_IWUSR,
-		   et61x251_show_i2c_reg, et61x251_store_i2c_reg);
-static DEVICE_ATTR(i2c_val, S_IRUGO | S_IWUSR,
-		   et61x251_show_i2c_val, et61x251_store_i2c_val);
-
-
-static int et61x251_create_sysfs(struct et61x251_device* cam)
-{
-	struct device *classdev = &(cam->v4ldev->dev);
-	int err = 0;
-
-	if ((err = device_create_file(classdev, &dev_attr_reg)))
-		goto err_out;
-	if ((err = device_create_file(classdev, &dev_attr_val)))
-		goto err_reg;
-
-	if (cam->sensor.sysfs_ops) {
-		if ((err = device_create_file(classdev, &dev_attr_i2c_reg)))
-			goto err_val;
-		if ((err = device_create_file(classdev, &dev_attr_i2c_val)))
-			goto err_i2c_reg;
-	}
-
-err_i2c_reg:
-	if (cam->sensor.sysfs_ops)
-		device_remove_file(classdev, &dev_attr_i2c_reg);
-err_val:
-	device_remove_file(classdev, &dev_attr_val);
-err_reg:
-	device_remove_file(classdev, &dev_attr_reg);
-err_out:
-	return err;
-}
-#endif /* CONFIG_VIDEO_ADV_DEBUG */
-
-/*****************************************************************************/
-
-static int
-et61x251_set_pix_format(struct et61x251_device* cam,
-			struct v4l2_pix_format* pix)
-{
-	int r, err = 0;
-
-	if ((r = et61x251_read_reg(cam, 0x12)) < 0)
-		err += r;
-	if (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
-		err += et61x251_write_reg(cam, r & 0xfd, 0x12);
-	else
-		err += et61x251_write_reg(cam, r | 0x02, 0x12);
-
-	return err ? -EIO : 0;
-}
-
-
-static int
-et61x251_set_compression(struct et61x251_device* cam,
-			 struct v4l2_jpegcompression* compression)
-{
-	int r, err = 0;
-
-	if ((r = et61x251_read_reg(cam, 0x12)) < 0)
-		err += r;
-	if (compression->quality == 0)
-		err += et61x251_write_reg(cam, r & 0xfb, 0x12);
-	else
-		err += et61x251_write_reg(cam, r | 0x04, 0x12);
-
-	return err ? -EIO : 0;
-}
-
-
-static int et61x251_set_scale(struct et61x251_device* cam, u8 scale)
-{
-	int r = 0, err = 0;
-
-	r = et61x251_read_reg(cam, 0x12);
-	if (r < 0)
-		err += r;
-
-	if (scale == 1)
-		err += et61x251_write_reg(cam, r & ~0x01, 0x12);
-	else if (scale == 2)
-		err += et61x251_write_reg(cam, r | 0x01, 0x12);
-
-	if (err)
-		return -EIO;
-
-	PDBGG("Scaling factor: %u", scale);
-
-	return 0;
-}
-
-
-static int
-et61x251_set_crop(struct et61x251_device* cam, struct v4l2_rect* rect)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	u16 fmw_sx = (u16)(rect->left - s->cropcap.bounds.left +
-			   s->active_pixel.left),
-	    fmw_sy = (u16)(rect->top - s->cropcap.bounds.top +
-			   s->active_pixel.top),
-	    fmw_length = (u16)(rect->width),
-	    fmw_height = (u16)(rect->height);
-	int err = 0;
-
-	err += et61x251_write_reg(cam, fmw_sx & 0xff, 0x69);
-	err += et61x251_write_reg(cam, fmw_sy & 0xff, 0x6a);
-	err += et61x251_write_reg(cam, fmw_length & 0xff, 0x6b);
-	err += et61x251_write_reg(cam, fmw_height & 0xff, 0x6c);
-	err += et61x251_write_reg(cam, (fmw_sx >> 8) | ((fmw_sy & 0x300) >> 6)
-				       | ((fmw_length & 0x300) >> 4)
-				       | ((fmw_height & 0x300) >> 2), 0x6d);
-	if (err)
-		return -EIO;
-
-	PDBGG("fmw_sx, fmw_sy, fmw_length, fmw_height: %u %u %u %u",
-	      fmw_sx, fmw_sy, fmw_length, fmw_height);
-
-	return 0;
-}
-
-
-static int et61x251_init(struct et61x251_device* cam)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_control ctrl;
-	struct v4l2_queryctrl *qctrl;
-	struct v4l2_rect* rect;
-	u8 i = 0;
-	int err = 0;
-
-	if (!(cam->state & DEV_INITIALIZED)) {
-		mutex_init(&cam->open_mutex);
-		init_waitqueue_head(&cam->wait_open);
-		qctrl = s->qctrl;
-		rect = &(s->cropcap.defrect);
-		cam->compression.quality = ET61X251_COMPRESSION_QUALITY;
-	} else { /* use current values */
-		qctrl = s->_qctrl;
-		rect = &(s->_rect);
-	}
-
-	err += et61x251_set_scale(cam, rect->width / s->pix_format.width);
-	err += et61x251_set_crop(cam, rect);
-	if (err)
-		return err;
-
-	if (s->init) {
-		err = s->init(cam);
-		if (err) {
-			DBG(3, "Sensor initialization failed");
-			return err;
-		}
-	}
-
-	err += et61x251_set_compression(cam, &cam->compression);
-	err += et61x251_set_pix_format(cam, &s->pix_format);
-	if (s->set_pix_format)
-		err += s->set_pix_format(cam, &s->pix_format);
-	if (err)
-		return err;
-
-	if (s->pix_format.pixelformat == V4L2_PIX_FMT_ET61X251)
-		DBG(3, "Compressed video format is active, quality %d",
-		    cam->compression.quality);
-	else
-		DBG(3, "Uncompressed video format is active");
-
-	if (s->set_crop)
-		if ((err = s->set_crop(cam, rect))) {
-			DBG(3, "set_crop() failed");
-			return err;
-		}
-
-	if (s->set_ctrl) {
-		for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
-			if (s->qctrl[i].id != 0 &&
-			    !(s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)) {
-				ctrl.id = s->qctrl[i].id;
-				ctrl.value = qctrl[i].default_value;
-				err = s->set_ctrl(cam, &ctrl);
-				if (err) {
-					DBG(3, "Set %s control failed",
-					    s->qctrl[i].name);
-					return err;
-				}
-				DBG(3, "Image sensor supports '%s' control",
-				    s->qctrl[i].name);
-			}
-	}
-
-	if (!(cam->state & DEV_INITIALIZED)) {
-		mutex_init(&cam->fileop_mutex);
-		spin_lock_init(&cam->queue_lock);
-		init_waitqueue_head(&cam->wait_frame);
-		init_waitqueue_head(&cam->wait_stream);
-		cam->nreadbuffers = 2;
-		memcpy(s->_qctrl, s->qctrl, sizeof(s->qctrl));
-		memcpy(&(s->_rect), &(s->cropcap.defrect),
-		       sizeof(struct v4l2_rect));
-		cam->state |= DEV_INITIALIZED;
-	}
-
-	DBG(2, "Initialization succeeded");
-	return 0;
-}
-
-/*****************************************************************************/
-
-static void et61x251_release_resources(struct kref *kref)
-{
-	struct et61x251_device *cam;
-
-	mutex_lock(&et61x251_sysfs_lock);
-
-	cam = container_of(kref, struct et61x251_device, kref);
-
-	DBG(2, "V4L2 device %s deregistered",
-	    video_device_node_name(cam->v4ldev));
-	video_set_drvdata(cam->v4ldev, NULL);
-	video_unregister_device(cam->v4ldev);
-	usb_put_dev(cam->usbdev);
-	kfree(cam->control_buffer);
-	kfree(cam);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-}
-
-
-static int et61x251_open(struct file *filp)
-{
-	struct et61x251_device* cam;
-	int err = 0;
-
-	if (!down_read_trylock(&et61x251_dev_lock))
-		return -ERESTARTSYS;
-
-	cam = video_drvdata(filp);
-
-	if (wait_for_completion_interruptible(&cam->probe)) {
-		up_read(&et61x251_dev_lock);
-		return -ERESTARTSYS;
-	}
-
-	kref_get(&cam->kref);
-
-	if (mutex_lock_interruptible(&cam->open_mutex)) {
-		kref_put(&cam->kref, et61x251_release_resources);
-		up_read(&et61x251_dev_lock);
-		return -ERESTARTSYS;
-	}
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		err = -ENODEV;
-		goto out;
-	}
-
-	if (cam->users) {
-		DBG(2, "Device %s is already in use",
-		       video_device_node_name(cam->v4ldev));
-		DBG(3, "Simultaneous opens are not supported");
-		if ((filp->f_flags & O_NONBLOCK) ||
-		    (filp->f_flags & O_NDELAY)) {
-			err = -EWOULDBLOCK;
-			goto out;
-		}
-		DBG(2, "A blocking open() has been requested. Wait for the "
-		       "device to be released...");
-		up_read(&et61x251_dev_lock);
-		err = wait_event_interruptible_exclusive(cam->wait_open,
-						(cam->state & DEV_DISCONNECTED)
-							 || !cam->users);
-		down_read(&et61x251_dev_lock);
-		if (err)
-			goto out;
-		if (cam->state & DEV_DISCONNECTED) {
-			err = -ENODEV;
-			goto out;
-		}
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		err = et61x251_init(cam);
-		if (err) {
-			DBG(1, "Initialization failed again. "
-			       "I will retry on next open().");
-			goto out;
-		}
-		cam->state &= ~DEV_MISCONFIGURED;
-	}
-
-	if ((err = et61x251_start_transfer(cam)))
-		goto out;
-
-	filp->private_data = cam;
-	cam->users++;
-	cam->io = IO_NONE;
-	cam->stream = STREAM_OFF;
-	cam->nbuffers = 0;
-	cam->frame_count = 0;
-	et61x251_empty_framequeues(cam);
-
-	DBG(3, "Video device %s is open",
-	    video_device_node_name(cam->v4ldev));
-
-out:
-	mutex_unlock(&cam->open_mutex);
-	if (err)
-		kref_put(&cam->kref, et61x251_release_resources);
-	up_read(&et61x251_dev_lock);
-	return err;
-}
-
-
-static int et61x251_release(struct file *filp)
-{
-	struct et61x251_device* cam;
-
-	down_write(&et61x251_dev_lock);
-
-	cam = video_drvdata(filp);
-
-	et61x251_stop_transfer(cam);
-	et61x251_release_buffers(cam);
-	cam->users--;
-	wake_up_interruptible_nr(&cam->wait_open, 1);
-
-	DBG(3, "Video device %s closed",
-	    video_device_node_name(cam->v4ldev));
-
-	kref_put(&cam->kref, et61x251_release_resources);
-
-	up_write(&et61x251_dev_lock);
-
-	return 0;
-}
-
-
-static ssize_t
-et61x251_read(struct file* filp, char __user * buf,
-	      size_t count, loff_t* f_pos)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	struct et61x251_frame_t* f, * i;
-	unsigned long lock_flags;
-	long timeout;
-	int err = 0;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return -ERESTARTSYS;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		mutex_unlock(&cam->fileop_mutex);
-		return -ENODEV;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EIO;
-	}
-
-	if (cam->io == IO_MMAP) {
-		DBG(3, "Close and open the device again to choose the read "
-		       "method");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EBUSY;
-	}
-
-	if (cam->io == IO_NONE) {
-		if (!et61x251_request_buffers(cam, cam->nreadbuffers,
-					      IO_READ)) {
-			DBG(1, "read() failed, not enough memory");
-			mutex_unlock(&cam->fileop_mutex);
-			return -ENOMEM;
-		}
-		cam->io = IO_READ;
-		cam->stream = STREAM_ON;
-	}
-
-	if (list_empty(&cam->inqueue)) {
-		if (!list_empty(&cam->outqueue))
-			et61x251_empty_framequeues(cam);
-		et61x251_queue_unusedframes(cam);
-	}
-
-	if (!count) {
-		mutex_unlock(&cam->fileop_mutex);
-		return 0;
-	}
-
-	if (list_empty(&cam->outqueue)) {
-		if (filp->f_flags & O_NONBLOCK) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -EAGAIN;
-		}
-		timeout = wait_event_interruptible_timeout
-			  ( cam->wait_frame,
-			    (!list_empty(&cam->outqueue)) ||
-			    (cam->state & DEV_DISCONNECTED) ||
-			    (cam->state & DEV_MISCONFIGURED),
-			    msecs_to_jiffies(
-				cam->module_param.frame_timeout * 1000
-			    )
-			  );
-		if (timeout < 0) {
-			mutex_unlock(&cam->fileop_mutex);
-			return timeout;
-		}
-		if (cam->state & DEV_DISCONNECTED) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -ENODEV;
-		}
-		if (!timeout || (cam->state & DEV_MISCONFIGURED)) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -EIO;
-		}
-	}
-
-	f = list_entry(cam->outqueue.prev, struct et61x251_frame_t, frame);
-
-	if (count > f->buf.bytesused)
-		count = f->buf.bytesused;
-
-	if (copy_to_user(buf, f->bufmem, count)) {
-		err = -EFAULT;
-		goto exit;
-	}
-	*f_pos += count;
-
-exit:
-	spin_lock_irqsave(&cam->queue_lock, lock_flags);
-	list_for_each_entry(i, &cam->outqueue, frame)
-		i->state = F_UNUSED;
-	INIT_LIST_HEAD(&cam->outqueue);
-	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-
-	et61x251_queue_unusedframes(cam);
-
-	PDBGG("Frame #%lu, bytes read: %zu",
-	      (unsigned long)f->buf.index, count);
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return err ? err : count;
-}
-
-
-static unsigned int et61x251_poll(struct file *filp, poll_table *wait)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	struct et61x251_frame_t* f;
-	unsigned long lock_flags;
-	unsigned int mask = 0;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return POLLERR;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		goto error;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		goto error;
-	}
-
-	if (cam->io == IO_NONE) {
-		if (!et61x251_request_buffers(cam, cam->nreadbuffers,
-					      IO_READ)) {
-			DBG(1, "poll() failed, not enough memory");
-			goto error;
-		}
-		cam->io = IO_READ;
-		cam->stream = STREAM_ON;
-	}
-
-	if (cam->io == IO_READ) {
-		spin_lock_irqsave(&cam->queue_lock, lock_flags);
-		list_for_each_entry(f, &cam->outqueue, frame)
-			f->state = F_UNUSED;
-		INIT_LIST_HEAD(&cam->outqueue);
-		spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-		et61x251_queue_unusedframes(cam);
-	}
-
-	poll_wait(filp, &cam->wait_frame, wait);
-
-	if (!list_empty(&cam->outqueue))
-		mask |= POLLIN | POLLRDNORM;
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return mask;
-
-error:
-	mutex_unlock(&cam->fileop_mutex);
-	return POLLERR;
-}
-
-
-static void et61x251_vm_open(struct vm_area_struct* vma)
-{
-	struct et61x251_frame_t* f = vma->vm_private_data;
-	f->vma_use_count++;
-}
-
-
-static void et61x251_vm_close(struct vm_area_struct* vma)
-{
-	/* NOTE: buffers are not freed here */
-	struct et61x251_frame_t* f = vma->vm_private_data;
-	f->vma_use_count--;
-}
-
-
-static const struct vm_operations_struct et61x251_vm_ops = {
-	.open = et61x251_vm_open,
-	.close = et61x251_vm_close,
-};
-
-
-static int et61x251_mmap(struct file* filp, struct vm_area_struct *vma)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	unsigned long size = vma->vm_end - vma->vm_start,
-		      start = vma->vm_start;
-	void *pos;
-	u32 i;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return -ERESTARTSYS;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		mutex_unlock(&cam->fileop_mutex);
-		return -ENODEV;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EIO;
-	}
-
-	if (!(vma->vm_flags & (VM_WRITE | VM_READ))) {
-		mutex_unlock(&cam->fileop_mutex);
-		return -EACCES;
-	}
-
-	if (cam->io != IO_MMAP ||
-	    size != PAGE_ALIGN(cam->frame[0].buf.length)) {
-		mutex_unlock(&cam->fileop_mutex);
-		return -EINVAL;
-	}
-
-	for (i = 0; i < cam->nbuffers; i++) {
-		if ((cam->frame[i].buf.m.offset>>PAGE_SHIFT) == vma->vm_pgoff)
-			break;
-	}
-	if (i == cam->nbuffers) {
-		mutex_unlock(&cam->fileop_mutex);
-		return -EINVAL;
-	}
-
-	vma->vm_flags |= VM_IO;
-	vma->vm_flags |= VM_RESERVED;
-
-	pos = cam->frame[i].bufmem;
-	while (size > 0) { /* size is page-aligned */
-		if (vm_insert_page(vma, start, vmalloc_to_page(pos))) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -EAGAIN;
-		}
-		start += PAGE_SIZE;
-		pos += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-
-	vma->vm_ops = &et61x251_vm_ops;
-	vma->vm_private_data = &cam->frame[i];
-	et61x251_vm_open(vma);
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return 0;
-}
-
-/*****************************************************************************/
-
-static int
-et61x251_vidioc_querycap(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_capability cap = {
-		.driver = "et61x251",
-		.version = LINUX_VERSION_CODE,
-		.capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
-				V4L2_CAP_STREAMING,
-	};
-
-	strlcpy(cap.card, cam->v4ldev->name, sizeof(cap.card));
-	if (usb_make_path(cam->usbdev, cap.bus_info, sizeof(cap.bus_info)) < 0)
-		strlcpy(cap.bus_info, dev_name(&cam->usbdev->dev),
-			sizeof(cap.bus_info));
-
-	if (copy_to_user(arg, &cap, sizeof(cap)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_enuminput(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_input i;
-
-	if (copy_from_user(&i, arg, sizeof(i)))
-		return -EFAULT;
-
-	if (i.index)
-		return -EINVAL;
-
-	memset(&i, 0, sizeof(i));
-	strcpy(i.name, "Camera");
-	i.type = V4L2_INPUT_TYPE_CAMERA;
-	i.capabilities = V4L2_IN_CAP_STD;
-
-	if (copy_to_user(arg, &i, sizeof(i)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_input(struct et61x251_device* cam, void __user * arg)
-{
-	int index = 0;
-
-	if (copy_to_user(arg, &index, sizeof(index)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_input(struct et61x251_device* cam, void __user * arg)
-{
-	int index;
-
-	if (copy_from_user(&index, arg, sizeof(index)))
-		return -EFAULT;
-
-	if (index != 0)
-		return -EINVAL;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_query_ctrl(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_queryctrl qc;
-	u8 i;
-
-	if (copy_from_user(&qc, arg, sizeof(qc)))
-		return -EFAULT;
-
-	for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
-		if (qc.id && qc.id == s->qctrl[i].id) {
-			memcpy(&qc, &(s->qctrl[i]), sizeof(qc));
-			if (copy_to_user(arg, &qc, sizeof(qc)))
-				return -EFAULT;
-			return 0;
-		}
-
-	return -EINVAL;
-}
-
-
-static int
-et61x251_vidioc_g_ctrl(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_control ctrl;
-	int err = 0;
-	u8 i;
-
-	if (!s->get_ctrl && !s->set_ctrl)
-		return -EINVAL;
-
-	if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
-		return -EFAULT;
-
-	if (!s->get_ctrl) {
-		for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
-			if (ctrl.id == s->qctrl[i].id) {
-				ctrl.value = s->_qctrl[i].default_value;
-				goto exit;
-			}
-		return -EINVAL;
-	} else
-		err = s->get_ctrl(cam, &ctrl);
-
-exit:
-	if (copy_to_user(arg, &ctrl, sizeof(ctrl)))
-		return -EFAULT;
-
-	return err;
-}
-
-
-static int
-et61x251_vidioc_s_ctrl(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_control ctrl;
-	u8 i;
-	int err = 0;
-
-	if (!s->set_ctrl)
-		return -EINVAL;
-
-	if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
-		return -EFAULT;
-
-	for (i = 0; i < ARRAY_SIZE(s->qctrl); i++) {
-		if (ctrl.id == s->qctrl[i].id) {
-			if (s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)
-				return -EINVAL;
-			if (ctrl.value < s->qctrl[i].minimum ||
-			    ctrl.value > s->qctrl[i].maximum)
-				return -ERANGE;
-			ctrl.value -= ctrl.value % s->qctrl[i].step;
-			break;
-		}
-	}
-	if (i == ARRAY_SIZE(s->qctrl))
-		return -EINVAL;
-	if ((err = s->set_ctrl(cam, &ctrl)))
-		return err;
-
-	s->_qctrl[i].default_value = ctrl.value;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_cropcap(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_cropcap* cc = &(cam->sensor.cropcap);
-
-	cc->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	cc->pixelaspect.numerator = 1;
-	cc->pixelaspect.denominator = 1;
-
-	if (copy_to_user(arg, cc, sizeof(*cc)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_crop(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_crop crop = {
-		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
-	};
-
-	memcpy(&(crop.c), &(s->_rect), sizeof(struct v4l2_rect));
-
-	if (copy_to_user(arg, &crop, sizeof(crop)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_crop(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_crop crop;
-	struct v4l2_rect* rect;
-	struct v4l2_rect* bounds = &(s->cropcap.bounds);
-	struct v4l2_pix_format* pix_format = &(s->pix_format);
-	u8 scale;
-	const enum et61x251_stream_state stream = cam->stream;
-	const u32 nbuffers = cam->nbuffers;
-	u32 i;
-	int err = 0;
-
-	if (copy_from_user(&crop, arg, sizeof(crop)))
-		return -EFAULT;
-
-	rect = &(crop.c);
-
-	if (crop.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	if (cam->module_param.force_munmap)
-		for (i = 0; i < cam->nbuffers; i++)
-			if (cam->frame[i].vma_use_count) {
-				DBG(3, "VIDIOC_S_CROP failed. "
-				       "Unmap the buffers first.");
-				return -EBUSY;
-			}
-
-	/* Preserve R,G or B origin */
-	rect->left = (s->_rect.left & 1L) ? rect->left | 1L : rect->left & ~1L;
-	rect->top = (s->_rect.top & 1L) ? rect->top | 1L : rect->top & ~1L;
-
-	if (rect->width < 16)
-		rect->width = 16;
-	if (rect->height < 16)
-		rect->height = 16;
-	if (rect->width > bounds->width)
-		rect->width = bounds->width;
-	if (rect->height > bounds->height)
-		rect->height = bounds->height;
-	if (rect->left < bounds->left)
-		rect->left = bounds->left;
-	if (rect->top < bounds->top)
-		rect->top = bounds->top;
-	if (rect->left + rect->width > bounds->left + bounds->width)
-		rect->left = bounds->left+bounds->width - rect->width;
-	if (rect->top + rect->height > bounds->top + bounds->height)
-		rect->top = bounds->top+bounds->height - rect->height;
-
-	rect->width &= ~15L;
-	rect->height &= ~15L;
-
-	if (ET61X251_PRESERVE_IMGSCALE) {
-		/* Calculate the actual scaling factor */
-		u32 a, b;
-		a = rect->width * rect->height;
-		b = pix_format->width * pix_format->height;
-		scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
-	} else
-		scale = 1;
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	if (copy_to_user(arg, &crop, sizeof(crop))) {
-		cam->stream = stream;
-		return -EFAULT;
-	}
-
-	if (cam->module_param.force_munmap || cam->io == IO_READ)
-		et61x251_release_buffers(cam);
-
-	err = et61x251_set_crop(cam, rect);
-	if (s->set_crop)
-		err += s->set_crop(cam, rect);
-	err += et61x251_set_scale(cam, scale);
-
-	if (err) { /* atomic, no rollback in ioctl() */
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_CROP failed because of hardware problems. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	s->pix_format.width = rect->width/scale;
-	s->pix_format.height = rect->height/scale;
-	memcpy(&(s->_rect), rect, sizeof(*rect));
-
-	if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
-	    nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_CROP failed because of not enough memory. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -ENOMEM;
-	}
-
-	if (cam->io == IO_READ)
-		et61x251_empty_framequeues(cam);
-	else if (cam->module_param.force_munmap)
-		et61x251_requeue_outqueue(cam);
-
-	cam->stream = stream;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_enum_framesizes(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_frmsizeenum frmsize;
-
-	if (copy_from_user(&frmsize, arg, sizeof(frmsize)))
-		return -EFAULT;
-
-	if (frmsize.index != 0)
-		return -EINVAL;
-
-	if (frmsize.pixel_format != V4L2_PIX_FMT_ET61X251 &&
-	    frmsize.pixel_format != V4L2_PIX_FMT_SBGGR8)
-		return -EINVAL;
-
-	frmsize.type = V4L2_FRMSIZE_TYPE_STEPWISE;
-	frmsize.stepwise.min_width = frmsize.stepwise.step_width = 16;
-	frmsize.stepwise.min_height = frmsize.stepwise.step_height = 16;
-	frmsize.stepwise.max_width = cam->sensor.cropcap.bounds.width;
-	frmsize.stepwise.max_height = cam->sensor.cropcap.bounds.height;
-	memset(&frmsize.reserved, 0, sizeof(frmsize.reserved));
-
-	if (copy_to_user(arg, &frmsize, sizeof(frmsize)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_enum_fmt(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_fmtdesc fmtd;
-
-	if (copy_from_user(&fmtd, arg, sizeof(fmtd)))
-		return -EFAULT;
-
-	if (fmtd.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	if (fmtd.index == 0) {
-		strcpy(fmtd.description, "bayer rgb");
-		fmtd.pixelformat = V4L2_PIX_FMT_SBGGR8;
-	} else if (fmtd.index == 1) {
-		strcpy(fmtd.description, "compressed");
-		fmtd.pixelformat = V4L2_PIX_FMT_ET61X251;
-		fmtd.flags = V4L2_FMT_FLAG_COMPRESSED;
-	} else
-		return -EINVAL;
-
-	fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	memset(&fmtd.reserved, 0, sizeof(fmtd.reserved));
-
-	if (copy_to_user(arg, &fmtd, sizeof(fmtd)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_fmt(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_format format;
-	struct v4l2_pix_format* pfmt = &(cam->sensor.pix_format);
-
-	if (copy_from_user(&format, arg, sizeof(format)))
-		return -EFAULT;
-
-	if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	pfmt->colorspace = (pfmt->pixelformat == V4L2_PIX_FMT_ET61X251) ?
-			   0 : V4L2_COLORSPACE_SRGB;
-	pfmt->bytesperline = (pfmt->pixelformat==V4L2_PIX_FMT_ET61X251)
-			     ? 0 : (pfmt->width * pfmt->priv) / 8;
-	pfmt->sizeimage = pfmt->height * ((pfmt->width*pfmt->priv)/8);
-	pfmt->field = V4L2_FIELD_NONE;
-	memcpy(&(format.fmt.pix), pfmt, sizeof(*pfmt));
-
-	if (copy_to_user(arg, &format, sizeof(format)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_try_s_fmt(struct et61x251_device* cam, unsigned int cmd,
-			  void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_format format;
-	struct v4l2_pix_format* pix;
-	struct v4l2_pix_format* pfmt = &(s->pix_format);
-	struct v4l2_rect* bounds = &(s->cropcap.bounds);
-	struct v4l2_rect rect;
-	u8 scale;
-	const enum et61x251_stream_state stream = cam->stream;
-	const u32 nbuffers = cam->nbuffers;
-	u32 i;
-	int err = 0;
-
-	if (copy_from_user(&format, arg, sizeof(format)))
-		return -EFAULT;
-
-	pix = &(format.fmt.pix);
-
-	if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	memcpy(&rect, &(s->_rect), sizeof(rect));
-
-	{ /* calculate the actual scaling factor */
-		u32 a, b;
-		a = rect.width * rect.height;
-		b = pix->width * pix->height;
-		scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
-	}
-
-	rect.width = scale * pix->width;
-	rect.height = scale * pix->height;
-
-	if (rect.width < 16)
-		rect.width = 16;
-	if (rect.height < 16)
-		rect.height = 16;
-	if (rect.width > bounds->left + bounds->width - rect.left)
-		rect.width = bounds->left + bounds->width - rect.left;
-	if (rect.height > bounds->top + bounds->height - rect.top)
-		rect.height = bounds->top + bounds->height - rect.top;
-
-	rect.width &= ~15L;
-	rect.height &= ~15L;
-
-	{ /* adjust the scaling factor */
-		u32 a, b;
-		a = rect.width * rect.height;
-		b = pix->width * pix->height;
-		scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
-	}
-
-	pix->width = rect.width / scale;
-	pix->height = rect.height / scale;
-
-	if (pix->pixelformat != V4L2_PIX_FMT_ET61X251 &&
-	    pix->pixelformat != V4L2_PIX_FMT_SBGGR8)
-		pix->pixelformat = pfmt->pixelformat;
-	pix->priv = pfmt->priv; /* bpp */
-	pix->colorspace = (pix->pixelformat == V4L2_PIX_FMT_ET61X251) ?
-			  0 : V4L2_COLORSPACE_SRGB;
-	pix->colorspace = pfmt->colorspace;
-	pix->bytesperline = (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
-			    ? 0 : (pix->width * pix->priv) / 8;
-	pix->sizeimage = pix->height * ((pix->width * pix->priv) / 8);
-	pix->field = V4L2_FIELD_NONE;
-
-	if (cmd == VIDIOC_TRY_FMT) {
-		if (copy_to_user(arg, &format, sizeof(format)))
-			return -EFAULT;
-		return 0;
-	}
-
-	if (cam->module_param.force_munmap)
-		for (i = 0; i < cam->nbuffers; i++)
-			if (cam->frame[i].vma_use_count) {
-				DBG(3, "VIDIOC_S_FMT failed. "
-				       "Unmap the buffers first.");
-				return -EBUSY;
-			}
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	if (copy_to_user(arg, &format, sizeof(format))) {
-		cam->stream = stream;
-		return -EFAULT;
-	}
-
-	if (cam->module_param.force_munmap || cam->io == IO_READ)
-		et61x251_release_buffers(cam);
-
-	err += et61x251_set_pix_format(cam, pix);
-	err += et61x251_set_crop(cam, &rect);
-	if (s->set_pix_format)
-		err += s->set_pix_format(cam, pix);
-	if (s->set_crop)
-		err += s->set_crop(cam, &rect);
-	err += et61x251_set_scale(cam, scale);
-
-	if (err) { /* atomic, no rollback in ioctl() */
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_FMT failed because of hardware problems. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	memcpy(pfmt, pix, sizeof(*pix));
-	memcpy(&(s->_rect), &rect, sizeof(rect));
-
-	if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
-	    nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_FMT failed because of not enough memory. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -ENOMEM;
-	}
-
-	if (cam->io == IO_READ)
-		et61x251_empty_framequeues(cam);
-	else if (cam->module_param.force_munmap)
-		et61x251_requeue_outqueue(cam);
-
-	cam->stream = stream;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_jpegcomp(struct et61x251_device* cam, void __user * arg)
-{
-	if (copy_to_user(arg, &cam->compression,
-			 sizeof(cam->compression)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_jpegcomp(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_jpegcompression jc;
-	const enum et61x251_stream_state stream = cam->stream;
-	int err = 0;
-
-	if (copy_from_user(&jc, arg, sizeof(jc)))
-		return -EFAULT;
-
-	if (jc.quality != 0 && jc.quality != 1)
-		return -EINVAL;
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	err += et61x251_set_compression(cam, &jc);
-	if (err) { /* atomic, no rollback in ioctl() */
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_JPEGCOMP failed because of hardware "
-		       "problems. To use the camera, close and open "
-		       "%s again.", video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	cam->compression.quality = jc.quality;
-
-	cam->stream = stream;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_reqbufs(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_requestbuffers rb;
-	u32 i;
-	int err;
-
-	if (copy_from_user(&rb, arg, sizeof(rb)))
-		return -EFAULT;
-
-	if (rb.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    rb.memory != V4L2_MEMORY_MMAP)
-		return -EINVAL;
-
-	if (cam->io == IO_READ) {
-		DBG(3, "Close and open the device again to choose the mmap "
-		       "I/O method");
-		return -EBUSY;
-	}
-
-	for (i = 0; i < cam->nbuffers; i++)
-		if (cam->frame[i].vma_use_count) {
-			DBG(3, "VIDIOC_REQBUFS failed. "
-			       "Previous buffers are still mapped.");
-			return -EBUSY;
-		}
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	et61x251_empty_framequeues(cam);
-
-	et61x251_release_buffers(cam);
-	if (rb.count)
-		rb.count = et61x251_request_buffers(cam, rb.count, IO_MMAP);
-
-	if (copy_to_user(arg, &rb, sizeof(rb))) {
-		et61x251_release_buffers(cam);
-		cam->io = IO_NONE;
-		return -EFAULT;
-	}
-
-	cam->io = rb.count ? IO_MMAP : IO_NONE;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_querybuf(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_buffer b;
-
-	if (copy_from_user(&b, arg, sizeof(b)))
-		return -EFAULT;
-
-	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    b.index >= cam->nbuffers || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	memcpy(&b, &cam->frame[b.index].buf, sizeof(b));
-
-	if (cam->frame[b.index].vma_use_count)
-		b.flags |= V4L2_BUF_FLAG_MAPPED;
-
-	if (cam->frame[b.index].state == F_DONE)
-		b.flags |= V4L2_BUF_FLAG_DONE;
-	else if (cam->frame[b.index].state != F_UNUSED)
-		b.flags |= V4L2_BUF_FLAG_QUEUED;
-
-	if (copy_to_user(arg, &b, sizeof(b)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_qbuf(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_buffer b;
-	unsigned long lock_flags;
-
-	if (copy_from_user(&b, arg, sizeof(b)))
-		return -EFAULT;
-
-	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    b.index >= cam->nbuffers || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	if (cam->frame[b.index].state != F_UNUSED)
-		return -EINVAL;
-
-	cam->frame[b.index].state = F_QUEUED;
-
-	spin_lock_irqsave(&cam->queue_lock, lock_flags);
-	list_add_tail(&cam->frame[b.index].frame, &cam->inqueue);
-	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-
-	PDBGG("Frame #%lu queued", (unsigned long)b.index);
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_dqbuf(struct et61x251_device* cam, struct file* filp,
-		      void __user * arg)
-{
-	struct v4l2_buffer b;
-	struct et61x251_frame_t *f;
-	unsigned long lock_flags;
-	long timeout;
-
-	if (copy_from_user(&b, arg, sizeof(b)))
-		return -EFAULT;
-
-	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io!= IO_MMAP)
-		return -EINVAL;
-
-	if (list_empty(&cam->outqueue)) {
-		if (cam->stream == STREAM_OFF)
-			return -EINVAL;
-		if (filp->f_flags & O_NONBLOCK)
-			return -EAGAIN;
-		timeout = wait_event_interruptible_timeout
-			  ( cam->wait_frame,
-			    (!list_empty(&cam->outqueue)) ||
-			    (cam->state & DEV_DISCONNECTED) ||
-			    (cam->state & DEV_MISCONFIGURED),
-			    cam->module_param.frame_timeout *
-			    1000 * msecs_to_jiffies(1) );
-		if (timeout < 0)
-			return timeout;
-		if (cam->state & DEV_DISCONNECTED)
-			return -ENODEV;
-		if (!timeout || (cam->state & DEV_MISCONFIGURED))
-			return -EIO;
-	}
-
-	spin_lock_irqsave(&cam->queue_lock, lock_flags);
-	f = list_entry(cam->outqueue.next, struct et61x251_frame_t, frame);
-	list_del(cam->outqueue.next);
-	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-
-	f->state = F_UNUSED;
-
-	memcpy(&b, &f->buf, sizeof(b));
-	if (f->vma_use_count)
-		b.flags |= V4L2_BUF_FLAG_MAPPED;
-
-	if (copy_to_user(arg, &b, sizeof(b)))
-		return -EFAULT;
-
-	PDBGG("Frame #%lu dequeued", (unsigned long)f->buf.index);
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_streamon(struct et61x251_device* cam, void __user * arg)
-{
-	int type;
-
-	if (copy_from_user(&type, arg, sizeof(type)))
-		return -EFAULT;
-
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	cam->stream = STREAM_ON;
-
-	DBG(3, "Stream on");
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_streamoff(struct et61x251_device* cam, void __user * arg)
-{
-	int type, err;
-
-	if (copy_from_user(&type, arg, sizeof(type)))
-		return -EFAULT;
-
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	et61x251_empty_framequeues(cam);
-
-	DBG(3, "Stream off");
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_parm(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_streamparm sp;
-
-	if (copy_from_user(&sp, arg, sizeof(sp)))
-		return -EFAULT;
-
-	if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	sp.parm.capture.extendedmode = 0;
-	sp.parm.capture.readbuffers = cam->nreadbuffers;
-
-	if (copy_to_user(arg, &sp, sizeof(sp)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_parm(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_streamparm sp;
-
-	if (copy_from_user(&sp, arg, sizeof(sp)))
-		return -EFAULT;
-
-	if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	sp.parm.capture.extendedmode = 0;
-
-	if (sp.parm.capture.readbuffers == 0)
-		sp.parm.capture.readbuffers = cam->nreadbuffers;
-
-	if (sp.parm.capture.readbuffers > ET61X251_MAX_FRAMES)
-		sp.parm.capture.readbuffers = ET61X251_MAX_FRAMES;
-
-	if (copy_to_user(arg, &sp, sizeof(sp)))
-		return -EFAULT;
-
-	cam->nreadbuffers = sp.parm.capture.readbuffers;
-
-	return 0;
-}
-
-
-static long et61x251_ioctl_v4l2(struct file *filp,
-			       unsigned int cmd, void __user *arg)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-
-	switch (cmd) {
-
-	case VIDIOC_QUERYCAP:
-		return et61x251_vidioc_querycap(cam, arg);
-
-	case VIDIOC_ENUMINPUT:
-		return et61x251_vidioc_enuminput(cam, arg);
-
-	case VIDIOC_G_INPUT:
-		return et61x251_vidioc_g_input(cam, arg);
-
-	case VIDIOC_S_INPUT:
-		return et61x251_vidioc_s_input(cam, arg);
-
-	case VIDIOC_QUERYCTRL:
-		return et61x251_vidioc_query_ctrl(cam, arg);
-
-	case VIDIOC_G_CTRL:
-		return et61x251_vidioc_g_ctrl(cam, arg);
-
-	case VIDIOC_S_CTRL:
-		return et61x251_vidioc_s_ctrl(cam, arg);
-
-	case VIDIOC_CROPCAP:
-		return et61x251_vidioc_cropcap(cam, arg);
-
-	case VIDIOC_G_CROP:
-		return et61x251_vidioc_g_crop(cam, arg);
-
-	case VIDIOC_S_CROP:
-		return et61x251_vidioc_s_crop(cam, arg);
-
-	case VIDIOC_ENUM_FMT:
-		return et61x251_vidioc_enum_fmt(cam, arg);
-
-	case VIDIOC_G_FMT:
-		return et61x251_vidioc_g_fmt(cam, arg);
-
-	case VIDIOC_TRY_FMT:
-	case VIDIOC_S_FMT:
-		return et61x251_vidioc_try_s_fmt(cam, cmd, arg);
-
-	case VIDIOC_ENUM_FRAMESIZES:
-		return et61x251_vidioc_enum_framesizes(cam, arg);
-
-	case VIDIOC_G_JPEGCOMP:
-		return et61x251_vidioc_g_jpegcomp(cam, arg);
-
-	case VIDIOC_S_JPEGCOMP:
-		return et61x251_vidioc_s_jpegcomp(cam, arg);
-
-	case VIDIOC_REQBUFS:
-		return et61x251_vidioc_reqbufs(cam, arg);
-
-	case VIDIOC_QUERYBUF:
-		return et61x251_vidioc_querybuf(cam, arg);
-
-	case VIDIOC_QBUF:
-		return et61x251_vidioc_qbuf(cam, arg);
-
-	case VIDIOC_DQBUF:
-		return et61x251_vidioc_dqbuf(cam, filp, arg);
-
-	case VIDIOC_STREAMON:
-		return et61x251_vidioc_streamon(cam, arg);
-
-	case VIDIOC_STREAMOFF:
-		return et61x251_vidioc_streamoff(cam, arg);
-
-	case VIDIOC_G_PARM:
-		return et61x251_vidioc_g_parm(cam, arg);
-
-	case VIDIOC_S_PARM:
-		return et61x251_vidioc_s_parm(cam, arg);
-
-	default:
-		return -ENOTTY;
-
-	}
-}
-
-
-static long et61x251_ioctl(struct file *filp,
-			 unsigned int cmd, unsigned long arg)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	long err = 0;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return -ERESTARTSYS;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		mutex_unlock(&cam->fileop_mutex);
-		return -ENODEV;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EIO;
-	}
-
-	V4LDBG(3, "et61x251", cmd);
-
-	err = et61x251_ioctl_v4l2(filp, cmd, (void __user *)arg);
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return err;
-}
-
-
-static const struct v4l2_file_operations et61x251_fops = {
-	.owner = THIS_MODULE,
-	.open =    et61x251_open,
-	.release = et61x251_release,
-	.unlocked_ioctl =   et61x251_ioctl,
-	.read =    et61x251_read,
-	.poll =    et61x251_poll,
-	.mmap =    et61x251_mmap,
-};
-
-/*****************************************************************************/
-
-/* It exists a single interface only. We do not need to validate anything. */
-static int
-et61x251_usb_probe(struct usb_interface* intf, const struct usb_device_id* id)
-{
-	struct usb_device *udev = interface_to_usbdev(intf);
-	struct et61x251_device* cam;
-	static unsigned int dev_nr;
-	unsigned int i;
-	int err = 0;
-
-	if (!(cam = kzalloc(sizeof(struct et61x251_device), GFP_KERNEL)))
-		return -ENOMEM;
-
-	cam->usbdev = udev;
-
-	if (!(cam->control_buffer = kzalloc(8, GFP_KERNEL))) {
-		DBG(1, "kmalloc() failed");
-		err = -ENOMEM;
-		goto fail;
-	}
-
-	if (!(cam->v4ldev = video_device_alloc())) {
-		DBG(1, "video_device_alloc() failed");
-		err = -ENOMEM;
-		goto fail;
-	}
-
-	DBG(2, "ET61X[12]51 PC Camera Controller detected "
-	       "(vid/pid 0x%04X:0x%04X)",id->idVendor, id->idProduct);
-
-	for  (i = 0; et61x251_sensor_table[i]; i++) {
-		err = et61x251_sensor_table[i](cam);
-		if (!err)
-			break;
-	}
-
-	if (!err)
-		DBG(2, "%s image sensor detected", cam->sensor.name);
-	else {
-		DBG(1, "No supported image sensor detected");
-		err = -ENODEV;
-		goto fail;
-	}
-
-	if (et61x251_init(cam)) {
-		DBG(1, "Initialization failed. I will retry on open().");
-		cam->state |= DEV_MISCONFIGURED;
-	}
-
-	strcpy(cam->v4ldev->name, "ET61X[12]51 PC Camera");
-	cam->v4ldev->fops = &et61x251_fops;
-	cam->v4ldev->release = video_device_release;
-	cam->v4ldev->parent = &udev->dev;
-	video_set_drvdata(cam->v4ldev, cam);
-
-	init_completion(&cam->probe);
-
-	err = video_register_device(cam->v4ldev, VFL_TYPE_GRABBER,
-				    video_nr[dev_nr]);
-	if (err) {
-		DBG(1, "V4L2 device registration failed");
-		if (err == -ENFILE && video_nr[dev_nr] == -1)
-			DBG(1, "Free /dev/videoX node not found");
-		video_nr[dev_nr] = -1;
-		dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
-		complete_all(&cam->probe);
-		goto fail;
-	}
-
-	DBG(2, "V4L2 device registered as %s",
-	    video_device_node_name(cam->v4ldev));
-
-	cam->module_param.force_munmap = force_munmap[dev_nr];
-	cam->module_param.frame_timeout = frame_timeout[dev_nr];
-
-	dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-	err = et61x251_create_sysfs(cam);
-	if (!err)
-		DBG(2, "Optional device control through 'sysfs' "
-		       "interface ready");
-	else
-		DBG(2, "Failed to create 'sysfs' interface for optional "
-		       "device controlling. Error #%d", err);
-#else
-	DBG(2, "Optional device control through 'sysfs' interface disabled");
-	DBG(3, "Compile the kernel with the 'CONFIG_VIDEO_ADV_DEBUG' "
-	       "configuration option to enable it.");
-#endif
-
-	usb_set_intfdata(intf, cam);
-	kref_init(&cam->kref);
-	usb_get_dev(cam->usbdev);
-
-	complete_all(&cam->probe);
-
-	return 0;
-
-fail:
-	if (cam) {
-		kfree(cam->control_buffer);
-		if (cam->v4ldev)
-			video_device_release(cam->v4ldev);
-		kfree(cam);
-	}
-	return err;
-}
-
-
-static void et61x251_usb_disconnect(struct usb_interface* intf)
-{
-	struct et61x251_device* cam;
-
-	down_write(&et61x251_dev_lock);
-
-	cam = usb_get_intfdata(intf);
-
-	DBG(2, "Disconnecting %s...", cam->v4ldev->name);
-
-	if (cam->users) {
-		DBG(2, "Device %s is open! Deregistration and memory "
-		       "deallocation are deferred.",
-		    video_device_node_name(cam->v4ldev));
-		cam->state |= DEV_MISCONFIGURED;
-		et61x251_stop_transfer(cam);
-		cam->state |= DEV_DISCONNECTED;
-		wake_up_interruptible(&cam->wait_frame);
-		wake_up(&cam->wait_stream);
-	} else
-		cam->state |= DEV_DISCONNECTED;
-
-	wake_up_interruptible_all(&cam->wait_open);
-
-	kref_put(&cam->kref, et61x251_release_resources);
-
-	up_write(&et61x251_dev_lock);
-}
-
-
-static struct usb_driver et61x251_usb_driver = {
-	.name =       "et61x251",
-	.id_table =   et61x251_id_table,
-	.probe =      et61x251_usb_probe,
-	.disconnect = et61x251_usb_disconnect,
-};
-
-module_usb_driver(et61x251_usb_driver);
diff --git a/drivers/media/video/et61x251/et61x251_sensor.h b/drivers/media/video/et61x251/et61x251_sensor.h
deleted file mode 100644
index 71a03148cb09..000000000000
--- a/drivers/media/video/et61x251/et61x251_sensor.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/***************************************************************************
- * API for image sensors connected to ET61X[12]51 PC Camera Controllers    *
- *                                                                         *
- * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it>  *
- *                                                                         *
- * This program is free software; you can redistribute it and/or modify    *
- * it under the terms of the GNU General Public License as published by    *
- * the Free Software Foundation; either version 2 of the License, or       *
- * (at your option) any later version.                                     *
- *                                                                         *
- * This program is distributed in the hope that it will be useful,         *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
- * GNU General Public License for more details.                            *
- *                                                                         *
- * You should have received a copy of the GNU General Public License       *
- * along with this program; if not, write to the Free Software             *
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.               *
- ***************************************************************************/
-
-#ifndef _ET61X251_SENSOR_H_
-#define _ET61X251_SENSOR_H_
-
-#include <linux/usb.h>
-#include <linux/videodev2.h>
-#include <linux/device.h>
-#include <linux/stddef.h>
-#include <linux/errno.h>
-#include <asm/types.h>
-
-struct et61x251_device;
-struct et61x251_sensor;
-
-/*****************************************************************************/
-
-extern int et61x251_probe_tas5130d1b(struct et61x251_device* cam);
-
-#define ET61X251_SENSOR_TABLE                                                 \
-/* Weak detections must go at the end of the list */                          \
-static int (*et61x251_sensor_table[])(struct et61x251_device*) = {            \
-	&et61x251_probe_tas5130d1b,                                           \
-	NULL,                                                                 \
-};
-
-extern struct et61x251_device*
-et61x251_match_id(struct et61x251_device* cam, const struct usb_device_id *id);
-
-extern void
-et61x251_attach_sensor(struct et61x251_device* cam,
-		       const struct et61x251_sensor* sensor);
-
-/*****************************************************************************/
-
-extern int et61x251_write_reg(struct et61x251_device*, u8 value, u16 index);
-extern int et61x251_i2c_raw_write(struct et61x251_device*, u8 n, u8 data1,
-				  u8 data2, u8 data3, u8 data4, u8 data5,
-				  u8 data6, u8 data7, u8 data8, u8 address);
-
-/*****************************************************************************/
-
-enum et61x251_i2c_sysfs_ops {
-	ET61X251_I2C_READ = 0x01,
-	ET61X251_I2C_WRITE = 0x02,
-};
-
-enum et61x251_i2c_interface {
-	ET61X251_I2C_2WIRES,
-	ET61X251_I2C_3WIRES,
-};
-
-/* Repeat start condition when RSTA is high */
-enum et61x251_i2c_rsta {
-	ET61X251_I2C_RSTA_STOP = 0x00, /* stop then start */
-	ET61X251_I2C_RSTA_REPEAT = 0x01, /* repeat start */
-};
-
-#define ET61X251_MAX_CTRLS (V4L2_CID_LASTP1-V4L2_CID_BASE+10)
-
-struct et61x251_sensor {
-	char name[32];
-
-	enum et61x251_i2c_sysfs_ops sysfs_ops;
-
-	enum et61x251_i2c_interface interface;
-	u8 i2c_slave_id;
-	enum et61x251_i2c_rsta rsta;
-	struct v4l2_rect active_pixel; /* left and top define FVSX and FVSY */
-
-	struct v4l2_queryctrl qctrl[ET61X251_MAX_CTRLS];
-	struct v4l2_cropcap cropcap;
-	struct v4l2_pix_format pix_format;
-
-	int (*init)(struct et61x251_device* cam);
-	int (*get_ctrl)(struct et61x251_device* cam,
-			struct v4l2_control* ctrl);
-	int (*set_ctrl)(struct et61x251_device* cam,
-			const struct v4l2_control* ctrl);
-	int (*set_crop)(struct et61x251_device* cam,
-			const struct v4l2_rect* rect);
-	int (*set_pix_format)(struct et61x251_device* cam,
-			      const struct v4l2_pix_format* pix);
-
-	/* Private */
-	struct v4l2_queryctrl _qctrl[ET61X251_MAX_CTRLS];
-	struct v4l2_rect _rect;
-};
-
-#endif /* _ET61X251_SENSOR_H_ */
diff --git a/drivers/media/video/et61x251/et61x251_tas5130d1b.c b/drivers/media/video/et61x251/et61x251_tas5130d1b.c
deleted file mode 100644
index ced2e167935d..000000000000
--- a/drivers/media/video/et61x251/et61x251_tas5130d1b.c
+++ /dev/null
@@ -1,143 +0,0 @@
-/***************************************************************************
- * Plug-in for TAS5130D1B image sensor connected to the ET61X[12]51        *
- * PC Camera Controllers                                                   *
- *                                                                         *
- * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it>  *
- *                                                                         *
- * This program is free software; you can redistribute it and/or modify    *
- * it under the terms of the GNU General Public License as published by    *
- * the Free Software Foundation; either version 2 of the License, or       *
- * (at your option) any later version.                                     *
- *                                                                         *
- * This program is distributed in the hope that it will be useful,         *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
- * GNU General Public License for more details.                            *
- *                                                                         *
- * You should have received a copy of the GNU General Public License       *
- * along with this program; if not, write to the Free Software             *
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.               *
- ***************************************************************************/
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include "et61x251_sensor.h"
-
-
-static int tas5130d1b_init(struct et61x251_device* cam)
-{
-	int err = 0;
-
-	err += et61x251_write_reg(cam, 0x14, 0x01);
-	err += et61x251_write_reg(cam, 0x1b, 0x02);
-	err += et61x251_write_reg(cam, 0x02, 0x12);
-	err += et61x251_write_reg(cam, 0x0e, 0x60);
-	err += et61x251_write_reg(cam, 0x80, 0x61);
-	err += et61x251_write_reg(cam, 0xf0, 0x62);
-	err += et61x251_write_reg(cam, 0x03, 0x63);
-	err += et61x251_write_reg(cam, 0x14, 0x64);
-	err += et61x251_write_reg(cam, 0xf4, 0x65);
-	err += et61x251_write_reg(cam, 0x01, 0x66);
-	err += et61x251_write_reg(cam, 0x05, 0x67);
-	err += et61x251_write_reg(cam, 0x8f, 0x68);
-	err += et61x251_write_reg(cam, 0x0f, 0x8d);
-	err += et61x251_write_reg(cam, 0x08, 0x8e);
-
-	return err;
-}
-
-
-static int tas5130d1b_set_ctrl(struct et61x251_device* cam,
-			       const struct v4l2_control* ctrl)
-{
-	int err = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_GAIN:
-		err += et61x251_i2c_raw_write(cam, 2, 0x20,
-					      0xf6-ctrl->value, 0, 0, 0,
-					      0, 0, 0, 0);
-		break;
-	case V4L2_CID_EXPOSURE:
-		err += et61x251_i2c_raw_write(cam, 2, 0x40,
-					      0x47-ctrl->value, 0, 0, 0,
-					      0, 0, 0, 0);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return err ? -EIO : 0;
-}
-
-
-static const struct et61x251_sensor tas5130d1b = {
-	.name = "TAS5130D1B",
-	.interface = ET61X251_I2C_3WIRES,
-	.rsta = ET61X251_I2C_RSTA_STOP,
-	.active_pixel = {
-		.left = 106,
-		.top = 13,
-	},
-	.init = &tas5130d1b_init,
-	.qctrl = {
-		{
-			.id = V4L2_CID_GAIN,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "global gain",
-			.minimum = 0x00,
-			.maximum = 0xf6,
-			.step = 0x02,
-			.default_value = 0x0d,
-			.flags = 0,
-		},
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "exposure",
-			.minimum = 0x00,
-			.maximum = 0x47,
-			.step = 0x01,
-			.default_value = 0x23,
-			.flags = 0,
-		},
-	},
-	.set_ctrl = &tas5130d1b_set_ctrl,
-	.cropcap = {
-		.bounds = {
-			.left = 0,
-			.top = 0,
-			.width = 640,
-			.height = 480,
-		},
-		.defrect = {
-			.left = 0,
-			.top = 0,
-			.width = 640,
-			.height = 480,
-		},
-	},
-	.pix_format = {
-		.width = 640,
-		.height = 480,
-		.pixelformat = V4L2_PIX_FMT_SBGGR8,
-		.priv = 8,
-	},
-};
-
-
-int et61x251_probe_tas5130d1b(struct et61x251_device* cam)
-{
-	const struct usb_device_id tas5130d1b_id_table[] = {
-		{ USB_DEVICE(0x102c, 0x6251), },
-		{ }
-	};
-
-	/* Sensor detection is based on USB pid/vid */
-	if (!et61x251_match_id(cam, tas5130d1b_id_table))
-		return -ENODEV;
-
-	et61x251_attach_sensor(cam, &tas5130d1b);
-
-	return 0;
-}
diff --git a/drivers/media/video/fsl-viu.c b/drivers/media/video/fsl-viu.c
index 27e3e0c0b219..777486f7cadb 100644
--- a/drivers/media/video/fsl-viu.c
+++ b/drivers/media/video/fsl-viu.c
@@ -1544,6 +1544,10 @@ static int __devinit viu_of_probe(struct platform_device *op)
 
 	/* initialize locks */
 	mutex_init(&viu_dev->lock);
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &viu_dev->vdev->flags);
 	viu_dev->vdev->lock = &viu_dev->lock;
 	spin_lock_init(&viu_dev->slock);
 
diff --git a/drivers/media/video/gspca/Makefile b/drivers/media/video/gspca/Makefile
index 79ebe46e1ad7..c901da0bd657 100644
--- a/drivers/media/video/gspca/Makefile
+++ b/drivers/media/video/gspca/Makefile
@@ -43,7 +43,7 @@ obj-$(CONFIG_USB_GSPCA_VICAM)    += gspca_vicam.o
 obj-$(CONFIG_USB_GSPCA_XIRLINK_CIT) += gspca_xirlink_cit.o
 obj-$(CONFIG_USB_GSPCA_ZC3XX)    += gspca_zc3xx.o
 
-gspca_main-objs     := gspca.o
+gspca_main-objs     := gspca.o autogain_functions.o
 gspca_benq-objs     := benq.o
 gspca_conex-objs    := conex.o
 gspca_cpia1-objs    := cpia1.o
diff --git a/drivers/media/video/gspca/autogain_functions.c b/drivers/media/video/gspca/autogain_functions.c
new file mode 100644
index 000000000000..67db674bb044
--- /dev/null
+++ b/drivers/media/video/gspca/autogain_functions.c
@@ -0,0 +1,178 @@
+/*
+ * Functions for auto gain.
+ *
+ * Copyright (C) 2010-2012 Hans de Goede <hdegoede@redhat.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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#include "gspca.h"
+
+/* auto gain and exposure algorithm based on the knee algorithm described here:
+   http://ytse.tricolour.net/docs/LowLightOptimization.html
+
+   Returns 0 if no changes were made, 1 if the gain and or exposure settings
+   where changed. */
+int gspca_expo_autogain(
+			struct gspca_dev *gspca_dev,
+			int avg_lum,
+			int desired_avg_lum,
+			int deadzone,
+			int gain_knee,
+			int exposure_knee)
+{
+	s32 gain, orig_gain, exposure, orig_exposure;
+	int i, steps, retval = 0;
+
+	if (v4l2_ctrl_g_ctrl(gspca_dev->autogain) == 0)
+	        return 0;
+
+	orig_gain = gain = v4l2_ctrl_g_ctrl(gspca_dev->gain);
+	orig_exposure = exposure = v4l2_ctrl_g_ctrl(gspca_dev->exposure);
+
+	/* If we are of a multiple of deadzone, do multiple steps to reach the
+	   desired lumination fast (with the risc of a slight overshoot) */
+	steps = abs(desired_avg_lum - avg_lum) / deadzone;
+
+	PDEBUG(D_FRAM, "autogain: lum: %d, desired: %d, steps: %d",
+		avg_lum, desired_avg_lum, steps);
+
+	for (i = 0; i < steps; i++) {
+		if (avg_lum > desired_avg_lum) {
+			if (gain > gain_knee)
+				gain--;
+			else if (exposure > exposure_knee)
+				exposure--;
+			else if (gain > gspca_dev->gain->default_value)
+				gain--;
+			else if (exposure > gspca_dev->exposure->minimum)
+				exposure--;
+			else if (gain > gspca_dev->gain->minimum)
+				gain--;
+			else
+				break;
+		} else {
+			if (gain < gspca_dev->gain->default_value)
+				gain++;
+			else if (exposure < exposure_knee)
+				exposure++;
+			else if (gain < gain_knee)
+				gain++;
+			else if (exposure < gspca_dev->exposure->maximum)
+				exposure++;
+			else if (gain < gspca_dev->gain->maximum)
+				gain++;
+			else
+				break;
+		}
+	}
+
+	if (gain != orig_gain) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->gain, gain);
+		retval = 1;
+	}
+	if (exposure != orig_exposure) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->exposure, exposure);
+		retval = 1;
+	}
+
+	if (retval)
+		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
+			gain, exposure);
+	return retval;
+}
+EXPORT_SYMBOL(gspca_expo_autogain);
+
+/* Autogain + exposure algorithm for cameras with a coarse exposure control
+   (usually this means we can only control the clockdiv to change exposure)
+   As changing the clockdiv so that the fps drops from 30 to 15 fps for
+   example, will lead to a huge exposure change (it effectively doubles),
+   this algorithm normally tries to only adjust the gain (between 40 and
+   80 %) and if that does not help, only then changes exposure. This leads
+   to a much more stable image then using the knee algorithm which at
+   certain points of the knee graph will only try to adjust exposure,
+   which leads to oscilating as one exposure step is huge.
+
+   Returns 0 if no changes were made, 1 if the gain and or exposure settings
+   where changed. */
+int gspca_coarse_grained_expo_autogain(
+			struct gspca_dev *gspca_dev,
+			int avg_lum,
+			int desired_avg_lum,
+			int deadzone)
+{
+	s32 gain_low, gain_high, gain, orig_gain, exposure, orig_exposure;
+	int steps, retval = 0;
+
+	if (v4l2_ctrl_g_ctrl(gspca_dev->autogain) == 0)
+	        return 0;
+
+	orig_gain = gain = v4l2_ctrl_g_ctrl(gspca_dev->gain);
+	orig_exposure = exposure = v4l2_ctrl_g_ctrl(gspca_dev->exposure);
+
+	gain_low  = (gspca_dev->gain->maximum - gspca_dev->gain->minimum) /
+		    5 * 2 + gspca_dev->gain->minimum;
+	gain_high = (gspca_dev->gain->maximum - gspca_dev->gain->minimum) /
+		    5 * 4 + gspca_dev->gain->minimum;
+
+	/* If we are of a multiple of deadzone, do multiple steps to reach the
+	   desired lumination fast (with the risc of a slight overshoot) */
+	steps = (desired_avg_lum - avg_lum) / deadzone;
+
+	PDEBUG(D_FRAM, "autogain: lum: %d, desired: %d, steps: %d",
+		avg_lum, desired_avg_lum, steps);
+
+	if ((gain + steps) > gain_high &&
+	    exposure < gspca_dev->exposure->maximum) {
+		gain = gain_high;
+		gspca_dev->exp_too_low_cnt++;
+		gspca_dev->exp_too_high_cnt = 0;
+	} else if ((gain + steps) < gain_low &&
+		   exposure > gspca_dev->exposure->minimum) {
+		gain = gain_low;
+		gspca_dev->exp_too_high_cnt++;
+		gspca_dev->exp_too_low_cnt = 0;
+	} else {
+		gain += steps;
+		if (gain > gspca_dev->gain->maximum)
+			gain = gspca_dev->gain->maximum;
+		else if (gain < gspca_dev->gain->minimum)
+			gain = gspca_dev->gain->minimum;
+		gspca_dev->exp_too_high_cnt = 0;
+		gspca_dev->exp_too_low_cnt = 0;
+	}
+
+	if (gspca_dev->exp_too_high_cnt > 3) {
+		exposure--;
+		gspca_dev->exp_too_high_cnt = 0;
+	} else if (gspca_dev->exp_too_low_cnt > 3) {
+		exposure++;
+		gspca_dev->exp_too_low_cnt = 0;
+	}
+
+	if (gain != orig_gain) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->gain, gain);
+		retval = 1;
+	}
+	if (exposure != orig_exposure) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->exposure, exposure);
+		retval = 1;
+	}
+
+	if (retval)
+		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
+			gain, exposure);
+	return retval;
+}
+EXPORT_SYMBOL(gspca_coarse_grained_expo_autogain);
diff --git a/drivers/media/video/gspca/autogain_functions.h b/drivers/media/video/gspca/autogain_functions.h
index 46777eee678b..d625eafe63eb 100644
--- a/drivers/media/video/gspca/autogain_functions.h
+++ b/drivers/media/video/gspca/autogain_functions.h
@@ -18,6 +18,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#ifdef WANT_REGULAR_AUTOGAIN
 /* auto gain and exposure algorithm based on the knee algorithm described here:
    http://ytse.tricolour.net/docs/LowLightOptimization.html
 
@@ -91,7 +92,9 @@ static inline int auto_gain_n_exposure(
 			gain, exposure);
 	return retval;
 }
+#endif
 
+#ifdef WANT_COARSE_EXPO_AUTOGAIN
 /* Autogain + exposure algorithm for cameras with a coarse exposure control
    (usually this means we can only control the clockdiv to change exposure)
    As changing the clockdiv so that the fps drops from 30 to 15 fps for
@@ -103,7 +106,7 @@ static inline int auto_gain_n_exposure(
    which leads to oscilating as one exposure step is huge.
 
    Note this assumes that the sd struct for the cam in question has
-   exp_too_high_cnt and exp_too_high_cnt int members for use by this function.
+   exp_too_low_cnt and exp_too_high_cnt int members for use by this function.
 
    Returns 0 if no changes were made, 1 if the gain and or exposure settings
    where changed. */
@@ -177,3 +180,4 @@ static inline int coarse_grained_expo_autogain(
 			gain, exposure);
 	return retval;
 }
+#endif
diff --git a/drivers/media/video/gspca/conex.c b/drivers/media/video/gspca/conex.c
index ea17b5d94ea4..f39fee0fd10f 100644
--- a/drivers/media/video/gspca/conex.c
+++ b/drivers/media/video/gspca/conex.c
@@ -306,7 +306,7 @@ static void cx_sensor(struct gspca_dev*gspca_dev)
 
 	reg_w(gspca_dev, 0x0020, reg20, 8);
 	reg_w(gspca_dev, 0x0028, reg28, 8);
-	reg_w(gspca_dev, 0x0010, reg10, 8);
+	reg_w(gspca_dev, 0x0010, reg10, 2);
 	reg_w_val(gspca_dev, 0x0092, 0x03);
 
 	switch (gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv) {
@@ -326,7 +326,7 @@ static void cx_sensor(struct gspca_dev*gspca_dev)
 	}
 	reg_w(gspca_dev, 0x007b, reg7b, 6);
 	reg_w_val(gspca_dev, 0x00f8, 0x00);
-	reg_w(gspca_dev, 0x0010, reg10, 8);
+	reg_w(gspca_dev, 0x0010, reg10, 2);
 	reg_w_val(gspca_dev, 0x0098, 0x41);
 	for (i = 0; i < 11; i++) {
 		if (i == 3 || i == 5 || i == 8)
diff --git a/drivers/media/video/gspca/finepix.c b/drivers/media/video/gspca/finepix.c
index 0107513cd728..6e26c93b4656 100644
--- a/drivers/media/video/gspca/finepix.c
+++ b/drivers/media/video/gspca/finepix.c
@@ -94,7 +94,11 @@ static void dostream(struct work_struct *work)
 
 	/* loop reading a frame */
 again:
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 
 		/* request a frame */
 		mutex_lock(&gspca_dev->usb_lock);
@@ -102,7 +106,11 @@ again:
 		mutex_unlock(&gspca_dev->usb_lock);
 		if (ret < 0)
 			break;
-		if (!gspca_dev->present || !gspca_dev->streaming)
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
+		if (!gspca_dev->dev || !gspca_dev->streaming)
 			break;
 
 		/* the frame comes in parts */
@@ -117,7 +125,11 @@ again:
 				 * error. Just restart. */
 				goto again;
 			}
-			if (!gspca_dev->present || !gspca_dev->streaming)
+#ifdef CONFIG_PM
+			if (gspca_dev->frozen)
+				goto out;
+#endif
+			if (!gspca_dev->dev || !gspca_dev->streaming)
 				goto out;
 			if (len < FPIX_MAX_TRANSFER ||
 				(data[len - 2] == 0xff &&
diff --git a/drivers/media/video/gspca/gl860/gl860.c b/drivers/media/video/gspca/gl860/gl860.c
index c84e26006fc3..c549574c1c7e 100644
--- a/drivers/media/video/gspca/gl860/gl860.c
+++ b/drivers/media/video/gspca/gl860/gl860.c
@@ -405,6 +405,9 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
+	if (!sd->gspca_dev.present)
+		return;
+
 	return sd->dev_post_unset_alt(gspca_dev);
 }
 
diff --git a/drivers/media/video/gspca/gspca.c b/drivers/media/video/gspca/gspca.c
index ca5a2b139d0b..137166d73945 100644
--- a/drivers/media/video/gspca/gspca.c
+++ b/drivers/media/video/gspca/gspca.c
@@ -38,6 +38,9 @@
 #include <linux/uaccess.h>
 #include <linux/ktime.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 
 #include "gspca.h"
 
@@ -592,16 +595,13 @@ static int gspca_set_alt0(struct gspca_dev *gspca_dev)
 static void gspca_stream_off(struct gspca_dev *gspca_dev)
 {
 	gspca_dev->streaming = 0;
-	if (gspca_dev->present) {
-		if (gspca_dev->sd_desc->stopN)
-			gspca_dev->sd_desc->stopN(gspca_dev);
-		destroy_urbs(gspca_dev);
-		gspca_input_destroy_urb(gspca_dev);
-		gspca_set_alt0(gspca_dev);
-		gspca_input_create_urb(gspca_dev);
-	}
-
-	/* always call stop0 to free the subdriver's resources */
+	gspca_dev->usb_err = 0;
+	if (gspca_dev->sd_desc->stopN)
+		gspca_dev->sd_desc->stopN(gspca_dev);
+	destroy_urbs(gspca_dev);
+	gspca_input_destroy_urb(gspca_dev);
+	gspca_set_alt0(gspca_dev);
+	gspca_input_create_urb(gspca_dev);
 	if (gspca_dev->sd_desc->stop0)
 		gspca_dev->sd_desc->stop0(gspca_dev);
 	PDEBUG(D_STREAM, "stream off OK");
@@ -847,14 +847,6 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 	struct ep_tb_s ep_tb[MAX_ALT];
 	int n, ret, xfer, alt, alt_idx;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto unlock;
-	}
-
 	/* reset the streaming variables */
 	gspca_dev->image = NULL;
 	gspca_dev->image_len = 0;
@@ -869,7 +861,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 	if (gspca_dev->sd_desc->isoc_init) {
 		ret = gspca_dev->sd_desc->isoc_init(gspca_dev);
 		if (ret < 0)
-			goto unlock;
+			return ret;
 	}
 	xfer = gspca_dev->cam.bulk ? USB_ENDPOINT_XFER_BULK
 				   : USB_ENDPOINT_XFER_ISOC;
@@ -880,8 +872,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 		ep = alt_xfer(&intf->altsetting[gspca_dev->alt], xfer);
 		if (ep == NULL) {
 			pr_err("bad altsetting %d\n", gspca_dev->alt);
-			ret = -EIO;
-			goto out;
+			return -EIO;
 		}
 		ep_tb[0].alt = gspca_dev->alt;
 		alt_idx = 1;
@@ -892,8 +883,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 		alt_idx = build_isoc_ep_tb(gspca_dev, intf, ep_tb);
 		if (alt_idx <= 0) {
 			pr_err("no transfer endpoint found\n");
-			ret = -EIO;
-			goto unlock;
+			return -EIO;
 		}
 	}
 
@@ -988,8 +978,6 @@ retry:
 	}
 out:
 	gspca_input_create_urb(gspca_dev);
-unlock:
-	mutex_unlock(&gspca_dev->usb_lock);
 	return ret;
 }
 
@@ -1006,6 +994,8 @@ static void gspca_set_default_mode(struct gspca_dev *gspca_dev)
 
 	/* set the current control values to their default values
 	 * which may have changed in sd_init() */
+	/* does nothing if ctrl_handler == NULL */
+	v4l2_ctrl_handler_setup(gspca_dev->vdev.ctrl_handler);
 	ctrl = gspca_dev->cam.ctrls;
 	if (ctrl != NULL) {
 		for (i = 0;
@@ -1057,77 +1047,50 @@ static int gspca_get_mode(struct gspca_dev *gspca_dev,
 static int vidioc_g_register(struct file *file, void *priv,
 			struct v4l2_dbg_register *reg)
 {
-	int ret;
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -EINVAL;
+		return -ENOTTY;
 
 	if (!gspca_dev->sd_desc->get_register)
-		return -EINVAL;
+		return -ENOTTY;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->get_register(gspca_dev, reg);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-
-	return ret;
+	return gspca_dev->sd_desc->get_register(gspca_dev, reg);
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
 			struct v4l2_dbg_register *reg)
 {
-	int ret;
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -EINVAL;
+		return -ENOTTY;
 
 	if (!gspca_dev->sd_desc->set_register)
-		return -EINVAL;
+		return -ENOTTY;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->set_register(gspca_dev, reg);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-
-	return ret;
+	return gspca_dev->sd_desc->set_register(gspca_dev, reg);
 }
 #endif
 
 static int vidioc_g_chip_ident(struct file *file, void *priv,
 			struct v4l2_dbg_chip_ident *chip)
 {
-	int ret;
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -EINVAL;
+		return -ENOTTY;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->get_chip_ident(gspca_dev, chip);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-
-	return ret;
+	return gspca_dev->sd_desc->get_chip_ident(gspca_dev, chip);
 }
 
 static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 				struct v4l2_fmtdesc *fmtdesc)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int i, j, index;
 	__u32 fmt_tb[8];
 
@@ -1169,7 +1132,7 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 			    struct v4l2_format *fmt)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int mode;
 
 	mode = gspca_dev->curr_mode;
@@ -1214,7 +1177,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file,
 			      void *priv,
 			      struct v4l2_format *fmt)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	ret = try_fmt_vid_cap(gspca_dev, fmt);
@@ -1226,7 +1189,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file,
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 			    struct v4l2_format *fmt)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	if (mutex_lock_interruptible(&gspca_dev->queue_lock))
@@ -1265,7 +1228,7 @@ out:
 static int vidioc_enum_framesizes(struct file *file, void *priv,
 				  struct v4l2_frmsizeenum *fsize)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int i;
 	__u32 index = 0;
 
@@ -1291,7 +1254,7 @@ static int vidioc_enum_framesizes(struct file *file, void *priv,
 static int vidioc_enum_frameintervals(struct file *filp, void *priv,
 				      struct v4l2_frmivalenum *fival)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(filp);
 	int mode = wxh_to_mode(gspca_dev, fival->width, fival->height);
 	__u32 i;
 
@@ -1316,31 +1279,30 @@ static int vidioc_enum_frameintervals(struct file *filp, void *priv,
 	return -EINVAL;
 }
 
-static void gspca_release(struct video_device *vfd)
+static void gspca_release(struct v4l2_device *v4l2_device)
 {
-	struct gspca_dev *gspca_dev = container_of(vfd, struct gspca_dev, vdev);
+	struct gspca_dev *gspca_dev =
+		container_of(v4l2_device, struct gspca_dev, v4l2_dev);
 
 	PDEBUG(D_PROBE, "%s released",
 		video_device_node_name(&gspca_dev->vdev));
 
+	v4l2_ctrl_handler_free(gspca_dev->vdev.ctrl_handler);
+	v4l2_device_unregister(&gspca_dev->v4l2_dev);
 	kfree(gspca_dev->usb_buf);
 	kfree(gspca_dev);
 }
 
 static int dev_open(struct file *file)
 {
-	struct gspca_dev *gspca_dev;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	PDEBUG(D_STREAM, "[%s] open", current->comm);
-	gspca_dev = (struct gspca_dev *) video_devdata(file);
-	if (!gspca_dev->present)
-		return -ENODEV;
 
 	/* protect the subdriver against rmmod */
 	if (!try_module_get(gspca_dev->module))
 		return -ENODEV;
 
-	file->private_data = gspca_dev;
 #ifdef GSPCA_DEBUG
 	/* activate the v4l2 debug */
 	if (gspca_debug & D_V4L2)
@@ -1350,49 +1312,44 @@ static int dev_open(struct file *file)
 		gspca_dev->vdev.debug &= ~(V4L2_DEBUG_IOCTL
 					| V4L2_DEBUG_IOCTL_ARG);
 #endif
-	return 0;
+	return v4l2_fh_open(file);
 }
 
 static int dev_close(struct file *file)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	PDEBUG(D_STREAM, "[%s] close", current->comm);
-	if (mutex_lock_interruptible(&gspca_dev->queue_lock))
+
+	/* Needed for gspca_stream_off, always lock before queue_lock! */
+	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
 		return -ERESTARTSYS;
 
+	if (mutex_lock_interruptible(&gspca_dev->queue_lock)) {
+		mutex_unlock(&gspca_dev->usb_lock);
+		return -ERESTARTSYS;
+	}
+
 	/* if the file did the capture, free the streaming resources */
 	if (gspca_dev->capt_file == file) {
-		if (gspca_dev->streaming) {
-			mutex_lock(&gspca_dev->usb_lock);
-			gspca_dev->usb_err = 0;
+		if (gspca_dev->streaming)
 			gspca_stream_off(gspca_dev);
-			mutex_unlock(&gspca_dev->usb_lock);
-		}
 		frame_free(gspca_dev);
 	}
-	file->private_data = NULL;
 	module_put(gspca_dev->module);
 	mutex_unlock(&gspca_dev->queue_lock);
+	mutex_unlock(&gspca_dev->usb_lock);
 
 	PDEBUG(D_STREAM, "close done");
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
 static int vidioc_querycap(struct file *file, void  *priv,
 			   struct v4l2_capability *cap)
 {
-	struct gspca_dev *gspca_dev = priv;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
-	/* protect the access to the usb device */
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	strlcpy((char *) cap->driver, gspca_dev->sd_desc->name,
 			sizeof cap->driver);
 	if (gspca_dev->dev->product != NULL) {
@@ -1406,13 +1363,11 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	}
 	usb_make_path(gspca_dev->dev, (char *) cap->bus_info,
 			sizeof(cap->bus_info));
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE
 			  | V4L2_CAP_STREAMING
 			  | V4L2_CAP_READWRITE;
-	ret = 0;
-out:
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
 }
 
 static int get_ctrl(struct gspca_dev *gspca_dev,
@@ -1435,7 +1390,7 @@ static int get_ctrl(struct gspca_dev *gspca_dev,
 static int vidioc_queryctrl(struct file *file, void *priv,
 			   struct v4l2_queryctrl *q_ctrl)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	const struct ctrl *ctrls;
 	struct gspca_ctrl *gspca_ctrl;
 	int i, idx;
@@ -1478,10 +1433,10 @@ static int vidioc_queryctrl(struct file *file, void *priv,
 static int vidioc_s_ctrl(struct file *file, void *priv,
 			 struct v4l2_control *ctrl)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	const struct ctrl *ctrls;
 	struct gspca_ctrl *gspca_ctrl;
-	int idx, ret;
+	int idx;
 
 	idx = get_ctrl(gspca_dev, ctrl->id);
 	if (idx < 0)
@@ -1501,74 +1456,52 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 			return -ERANGE;
 	}
 	PDEBUG(D_CONF, "set ctrl [%08x] = %d", ctrl->id, ctrl->value);
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	gspca_dev->usb_err = 0;
-	if (ctrls->set != NULL) {
-		ret = ctrls->set(gspca_dev, ctrl->value);
-		goto out;
-	}
+	if (ctrls->set != NULL)
+		return ctrls->set(gspca_dev, ctrl->value);
 	if (gspca_ctrl != NULL) {
 		gspca_ctrl->val = ctrl->value;
 		if (ctrls->set_control != NULL
 		 && gspca_dev->streaming)
 			ctrls->set_control(gspca_dev);
 	}
-	ret = gspca_dev->usb_err;
-out:
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return gspca_dev->usb_err;
 }
 
 static int vidioc_g_ctrl(struct file *file, void *priv,
 			 struct v4l2_control *ctrl)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	const struct ctrl *ctrls;
-	int idx, ret;
+	int idx;
 
 	idx = get_ctrl(gspca_dev, ctrl->id);
 	if (idx < 0)
 		return -EINVAL;
 	ctrls = &gspca_dev->sd_desc->ctrls[idx];
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	gspca_dev->usb_err = 0;
-	if (ctrls->get != NULL) {
-		ret = ctrls->get(gspca_dev, &ctrl->value);
-		goto out;
-	}
+	if (ctrls->get != NULL)
+		return ctrls->get(gspca_dev, &ctrl->value);
 	if (gspca_dev->cam.ctrls != NULL)
 		ctrl->value = gspca_dev->cam.ctrls[idx].val;
-	ret = 0;
-out:
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return 0;
 }
 
 static int vidioc_querymenu(struct file *file, void *priv,
 			    struct v4l2_querymenu *qmenu)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->querymenu)
-		return -EINVAL;
+		return -ENOTTY;
 	return gspca_dev->sd_desc->querymenu(gspca_dev, qmenu);
 }
 
 static int vidioc_enum_input(struct file *file, void *priv,
 				struct v4l2_input *input)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (input->index != 0)
 		return -EINVAL;
@@ -1595,7 +1528,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *rb)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int i, ret = 0, streaming;
 
 	i = rb->memory;			/* (avoid compilation warning) */
@@ -1635,10 +1568,7 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 	/* stop streaming */
 	streaming = gspca_dev->streaming;
 	if (streaming) {
-		mutex_lock(&gspca_dev->usb_lock);
-		gspca_dev->usb_err = 0;
 		gspca_stream_off(gspca_dev);
-		mutex_unlock(&gspca_dev->usb_lock);
 
 		/* Don't restart the stream when switching from read
 		 * to mmap mode */
@@ -1666,7 +1596,7 @@ out:
 static int vidioc_querybuf(struct file *file, void *priv,
 			   struct v4l2_buffer *v4l2_buf)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 
 	if (v4l2_buf->index < 0
@@ -1681,7 +1611,7 @@ static int vidioc_querybuf(struct file *file, void *priv,
 static int vidioc_streamon(struct file *file, void *priv,
 			   enum v4l2_buf_type buf_type)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	if (buf_type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -1722,7 +1652,7 @@ out:
 static int vidioc_streamoff(struct file *file, void *priv,
 				enum v4l2_buf_type buf_type)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	if (buf_type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -1743,13 +1673,7 @@ static int vidioc_streamoff(struct file *file, void *priv,
 	}
 
 	/* stop streaming */
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock)) {
-		ret = -ERESTARTSYS;
-		goto out;
-	}
-	gspca_dev->usb_err = 0;
 	gspca_stream_off(gspca_dev);
-	mutex_unlock(&gspca_dev->usb_lock);
 	/* In case another thread is waiting in dqbuf */
 	wake_up_interruptible(&gspca_dev->wq);
 
@@ -1766,71 +1690,44 @@ out:
 static int vidioc_g_jpegcomp(struct file *file, void *priv,
 			struct v4l2_jpegcompression *jpegcomp)
 {
-	struct gspca_dev *gspca_dev = priv;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_jcomp)
-		return -EINVAL;
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
+		return -ENOTTY;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->get_jcomp(gspca_dev, jpegcomp);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return gspca_dev->sd_desc->get_jcomp(gspca_dev, jpegcomp);
 }
 
 static int vidioc_s_jpegcomp(struct file *file, void *priv,
 			struct v4l2_jpegcompression *jpegcomp)
 {
-	struct gspca_dev *gspca_dev = priv;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->set_jcomp)
-		return -EINVAL;
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
+		return -ENOTTY;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->set_jcomp(gspca_dev, jpegcomp);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return gspca_dev->sd_desc->set_jcomp(gspca_dev, jpegcomp);
 }
 
 static int vidioc_g_parm(struct file *filp, void *priv,
 			struct v4l2_streamparm *parm)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(filp);
 
 	parm->parm.capture.readbuffers = gspca_dev->nbufread;
 
 	if (gspca_dev->sd_desc->get_streamparm) {
-		int ret;
-
-		if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-			return -ERESTARTSYS;
-		if (gspca_dev->present) {
-			gspca_dev->usb_err = 0;
-			gspca_dev->sd_desc->get_streamparm(gspca_dev, parm);
-			ret = gspca_dev->usb_err;
-		} else {
-			ret = -ENODEV;
-		}
-		mutex_unlock(&gspca_dev->usb_lock);
-		return ret;
+		gspca_dev->usb_err = 0;
+		gspca_dev->sd_desc->get_streamparm(gspca_dev, parm);
+		return gspca_dev->usb_err;
 	}
-
 	return 0;
 }
 
 static int vidioc_s_parm(struct file *filp, void *priv,
 			struct v4l2_streamparm *parm)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(filp);
 	int n;
 
 	n = parm->parm.capture.readbuffers;
@@ -1840,19 +1737,9 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 		gspca_dev->nbufread = n;
 
 	if (gspca_dev->sd_desc->set_streamparm) {
-		int ret;
-
-		if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-			return -ERESTARTSYS;
-		if (gspca_dev->present) {
-			gspca_dev->usb_err = 0;
-			gspca_dev->sd_desc->set_streamparm(gspca_dev, parm);
-			ret = gspca_dev->usb_err;
-		} else {
-			ret = -ENODEV;
-		}
-		mutex_unlock(&gspca_dev->usb_lock);
-		return ret;
+		gspca_dev->usb_err = 0;
+		gspca_dev->sd_desc->set_streamparm(gspca_dev, parm);
+		return gspca_dev->usb_err;
 	}
 
 	return 0;
@@ -1860,7 +1747,7 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 
 static int dev_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	struct page *page;
 	unsigned long addr, start, size;
@@ -1872,10 +1759,6 @@ static int dev_mmap(struct file *file, struct vm_area_struct *vma)
 
 	if (mutex_lock_interruptible(&gspca_dev->queue_lock))
 		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	if (gspca_dev->capt_file != file) {
 		ret = -EINVAL;
 		goto out;
@@ -1963,7 +1846,7 @@ static int frame_ready(struct gspca_dev *gspca_dev, struct file *file,
 static int vidioc_dqbuf(struct file *file, void *priv,
 			struct v4l2_buffer *v4l2_buf)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	int i, j, ret;
 
@@ -2003,14 +1886,6 @@ static int vidioc_dqbuf(struct file *file, void *priv,
 
 	gspca_dev->fr_o = (i + 1) % GSPCA_MAX_FRAMES;
 
-	if (gspca_dev->sd_desc->dq_callback) {
-		mutex_lock(&gspca_dev->usb_lock);
-		gspca_dev->usb_err = 0;
-		if (gspca_dev->present)
-			gspca_dev->sd_desc->dq_callback(gspca_dev);
-		mutex_unlock(&gspca_dev->usb_lock);
-	}
-
 	frame->v4l2_buf.flags &= ~V4L2_BUF_FLAG_DONE;
 	memcpy(v4l2_buf, &frame->v4l2_buf, sizeof *v4l2_buf);
 	PDEBUG(D_FRAM, "dqbuf %d", j);
@@ -2027,6 +1902,15 @@ static int vidioc_dqbuf(struct file *file, void *priv,
 	}
 out:
 	mutex_unlock(&gspca_dev->queue_lock);
+
+	if (ret == 0 && gspca_dev->sd_desc->dq_callback) {
+		mutex_lock(&gspca_dev->usb_lock);
+		gspca_dev->usb_err = 0;
+		if (gspca_dev->present)
+			gspca_dev->sd_desc->dq_callback(gspca_dev);
+		mutex_unlock(&gspca_dev->usb_lock);
+	}
+
 	return ret;
 }
 
@@ -2039,7 +1923,7 @@ out:
 static int vidioc_qbuf(struct file *file, void *priv,
 			struct v4l2_buffer *v4l2_buf)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	int i, index, ret;
 
@@ -2098,6 +1982,10 @@ static int read_alloc(struct gspca_dev *gspca_dev,
 	int i, ret;
 
 	PDEBUG(D_STREAM, "read alloc");
+
+	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
+		return -ERESTARTSYS;
+
 	if (gspca_dev->nframes == 0) {
 		struct v4l2_requestbuffers rb;
 
@@ -2108,7 +1996,7 @@ static int read_alloc(struct gspca_dev *gspca_dev,
 		ret = vidioc_reqbufs(file, gspca_dev, &rb);
 		if (ret != 0) {
 			PDEBUG(D_STREAM, "read reqbuf err %d", ret);
-			return ret;
+			goto out;
 		}
 		memset(&v4l2_buf, 0, sizeof v4l2_buf);
 		v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -2118,61 +2006,69 @@ static int read_alloc(struct gspca_dev *gspca_dev,
 			ret = vidioc_qbuf(file, gspca_dev, &v4l2_buf);
 			if (ret != 0) {
 				PDEBUG(D_STREAM, "read qbuf err: %d", ret);
-				return ret;
+				goto out;
 			}
 		}
-		gspca_dev->memory = GSPCA_MEMORY_READ;
 	}
 
 	/* start streaming */
 	ret = vidioc_streamon(file, gspca_dev, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 	if (ret != 0)
 		PDEBUG(D_STREAM, "read streamon err %d", ret);
+out:
+	mutex_unlock(&gspca_dev->usb_lock);
 	return ret;
 }
 
 static unsigned int dev_poll(struct file *file, poll_table *wait)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
+	unsigned long req_events = poll_requested_events(wait);
+	int ret = 0;
 
 	PDEBUG(D_FRAM, "poll");
 
-	poll_wait(file, &gspca_dev->wq, wait);
+	if (req_events & POLLPRI)
+		ret |= v4l2_ctrl_poll(file, wait);
 
-	/* if reqbufs is not done, the user would use read() */
-	if (gspca_dev->memory == GSPCA_MEMORY_NO) {
-		ret = read_alloc(gspca_dev, file);
-		if (ret != 0)
-			return POLLERR;
-	}
+	if (req_events & (POLLIN | POLLRDNORM)) {
+		/* if reqbufs is not done, the user would use read() */
+		if (gspca_dev->memory == GSPCA_MEMORY_NO) {
+			if (read_alloc(gspca_dev, file) != 0) {
+				ret |= POLLERR;
+				goto out;
+			}
+		}
 
-	if (mutex_lock_interruptible(&gspca_dev->queue_lock) != 0)
-		return POLLERR;
+		poll_wait(file, &gspca_dev->wq, wait);
 
-	/* check if an image has been received */
-	if (gspca_dev->fr_o != atomic_read(&gspca_dev->fr_i))
-		ret = POLLIN | POLLRDNORM;	/* yes */
-	else
-		ret = 0;
-	mutex_unlock(&gspca_dev->queue_lock);
+		/* check if an image has been received */
+		if (mutex_lock_interruptible(&gspca_dev->queue_lock) != 0) {
+			ret |= POLLERR;
+			goto out;
+		}
+		if (gspca_dev->fr_o != atomic_read(&gspca_dev->fr_i))
+			ret |= POLLIN | POLLRDNORM;
+		mutex_unlock(&gspca_dev->queue_lock);
+	}
+
+out:
 	if (!gspca_dev->present)
-		return POLLHUP;
+		ret |= POLLHUP;
+
 	return ret;
 }
 
 static ssize_t dev_read(struct file *file, char __user *data,
 		    size_t count, loff_t *ppos)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	struct v4l2_buffer v4l2_buf;
 	struct timeval timestamp;
 	int n, ret, ret2;
 
 	PDEBUG(D_FRAM, "read (%zd)", count);
-	if (!gspca_dev->present)
-		return -ENODEV;
 	if (gspca_dev->memory == GSPCA_MEMORY_NO) { /* first time ? */
 		ret = read_alloc(gspca_dev, file);
 		if (ret != 0)
@@ -2266,13 +2162,15 @@ static const struct v4l2_ioctl_ops dev_ioctl_ops = {
 	.vidioc_s_register	= vidioc_s_register,
 #endif
 	.vidioc_g_chip_ident	= vidioc_g_chip_ident,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static const struct video_device gspca_template = {
 	.name = "gspca main driver",
 	.fops = &dev_fops,
 	.ioctl_ops = &dev_ioctl_ops,
-	.release = gspca_release,
+	.release = video_device_release_empty, /* We use v4l2_dev.release */
 };
 
 /* initialize the controls */
@@ -2344,9 +2242,24 @@ int gspca_dev_probe2(struct usb_interface *intf,
 		}
 	}
 
+	gspca_dev->v4l2_dev.release = gspca_release;
+	ret = v4l2_device_register(&intf->dev, &gspca_dev->v4l2_dev);
+	if (ret)
+		goto out;
 	gspca_dev->sd_desc = sd_desc;
 	gspca_dev->nbufread = 2;
 	gspca_dev->empty_packet = -1;	/* don't check the empty packets */
+	gspca_dev->vdev = gspca_template;
+	gspca_dev->vdev.v4l2_dev = &gspca_dev->v4l2_dev;
+	video_set_drvdata(&gspca_dev->vdev, gspca_dev);
+	set_bit(V4L2_FL_USE_FH_PRIO, &gspca_dev->vdev.flags);
+	gspca_dev->module = module;
+	gspca_dev->present = 1;
+
+	mutex_init(&gspca_dev->usb_lock);
+	gspca_dev->vdev.lock = &gspca_dev->usb_lock;
+	mutex_init(&gspca_dev->queue_lock);
+	init_waitqueue_head(&gspca_dev->wq);
 
 	/* configure the subdriver and initialize the USB device */
 	ret = sd_desc->config(gspca_dev, id);
@@ -2357,21 +2270,26 @@ int gspca_dev_probe2(struct usb_interface *intf,
 	ret = sd_desc->init(gspca_dev);
 	if (ret < 0)
 		goto out;
+	if (sd_desc->init_controls)
+		ret = sd_desc->init_controls(gspca_dev);
+	if (ret < 0)
+		goto out;
 	gspca_set_default_mode(gspca_dev);
 
 	ret = gspca_input_connect(gspca_dev);
 	if (ret)
 		goto out;
 
-	mutex_init(&gspca_dev->usb_lock);
-	mutex_init(&gspca_dev->queue_lock);
-	init_waitqueue_head(&gspca_dev->wq);
+	/*
+	 * Don't take usb_lock for these ioctls. This improves latency if
+	 * usb_lock is taken for a long time, e.g. when changing a control
+	 * value, and a new frame is ready to be dequeued.
+	 */
+	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_DQBUF);
+	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_QBUF);
+	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_QUERYBUF);
 
 	/* init video stuff */
-	memcpy(&gspca_dev->vdev, &gspca_template, sizeof gspca_template);
-	gspca_dev->vdev.parent = &intf->dev;
-	gspca_dev->module = module;
-	gspca_dev->present = 1;
 	ret = video_register_device(&gspca_dev->vdev,
 				  VFL_TYPE_GRABBER,
 				  -1);
@@ -2391,6 +2309,7 @@ out:
 	if (gspca_dev->input_dev)
 		input_unregister_device(gspca_dev->input_dev);
 #endif
+	v4l2_ctrl_handler_free(gspca_dev->vdev.ctrl_handler);
 	kfree(gspca_dev->usb_buf);
 	kfree(gspca_dev);
 	return ret;
@@ -2437,11 +2356,12 @@ void gspca_disconnect(struct usb_interface *intf)
 
 	PDEBUG(D_PROBE, "%s disconnect",
 		video_device_node_name(&gspca_dev->vdev));
+
 	mutex_lock(&gspca_dev->usb_lock);
 
+	usb_set_intfdata(intf, NULL);
+	gspca_dev->dev = NULL;
 	gspca_dev->present = 0;
-	wake_up_interruptible(&gspca_dev->wq);
-
 	destroy_urbs(gspca_dev);
 
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
@@ -2452,18 +2372,19 @@ void gspca_disconnect(struct usb_interface *intf)
 		input_unregister_device(input_dev);
 	}
 #endif
+	/* Free subdriver's streaming resources / stop sd workqueue(s) */
+	if (gspca_dev->sd_desc->stop0 && gspca_dev->streaming)
+		gspca_dev->sd_desc->stop0(gspca_dev);
+	gspca_dev->streaming = 0;
+	wake_up_interruptible(&gspca_dev->wq);
 
-	/* the device is freed at exit of this function */
-	gspca_dev->dev = NULL;
-	mutex_unlock(&gspca_dev->usb_lock);
+	v4l2_device_disconnect(&gspca_dev->v4l2_dev);
+	video_unregister_device(&gspca_dev->vdev);
 
-	usb_set_intfdata(intf, NULL);
+	mutex_unlock(&gspca_dev->usb_lock);
 
-	/* release the device */
 	/* (this will call gspca_release() immediately or on last close) */
-	video_unregister_device(&gspca_dev->vdev);
-
-/*	PDEBUG(D_PROBE, "disconnect complete"); */
+	v4l2_device_put(&gspca_dev->v4l2_dev);
 }
 EXPORT_SYMBOL(gspca_disconnect);
 
@@ -2474,7 +2395,9 @@ int gspca_suspend(struct usb_interface *intf, pm_message_t message)
 
 	if (!gspca_dev->streaming)
 		return 0;
+	mutex_lock(&gspca_dev->usb_lock);
 	gspca_dev->frozen = 1;		/* avoid urb error messages */
+	gspca_dev->usb_err = 0;
 	if (gspca_dev->sd_desc->stopN)
 		gspca_dev->sd_desc->stopN(gspca_dev);
 	destroy_urbs(gspca_dev);
@@ -2482,6 +2405,7 @@ int gspca_suspend(struct usb_interface *intf, pm_message_t message)
 	gspca_set_alt0(gspca_dev);
 	if (gspca_dev->sd_desc->stop0)
 		gspca_dev->sd_desc->stop0(gspca_dev);
+	mutex_unlock(&gspca_dev->usb_lock);
 	return 0;
 }
 EXPORT_SYMBOL(gspca_suspend);
@@ -2489,105 +2413,28 @@ EXPORT_SYMBOL(gspca_suspend);
 int gspca_resume(struct usb_interface *intf)
 {
 	struct gspca_dev *gspca_dev = usb_get_intfdata(intf);
+	int streaming, ret = 0;
 
+	mutex_lock(&gspca_dev->usb_lock);
 	gspca_dev->frozen = 0;
+	gspca_dev->usb_err = 0;
 	gspca_dev->sd_desc->init(gspca_dev);
 	gspca_input_create_urb(gspca_dev);
-	if (gspca_dev->streaming)
-		return gspca_init_transfer(gspca_dev);
-	return 0;
+	/*
+	 * Most subdrivers send all ctrl values on sd_start and thus
+	 * only write to the device registers on s_ctrl when streaming ->
+	 * Clear streaming to avoid setting all ctrls twice.
+	 */
+	streaming = gspca_dev->streaming;
+	gspca_dev->streaming = 0;
+	v4l2_ctrl_handler_setup(gspca_dev->vdev.ctrl_handler);
+	if (streaming)
+		ret = gspca_init_transfer(gspca_dev);
+	mutex_unlock(&gspca_dev->usb_lock);
+	return ret;
 }
 EXPORT_SYMBOL(gspca_resume);
 #endif
-/* -- cam driver utility functions -- */
-
-/* auto gain and exposure algorithm based on the knee algorithm described here:
-   http://ytse.tricolour.net/docs/LowLightOptimization.html
-
-   Returns 0 if no changes were made, 1 if the gain and or exposure settings
-   where changed. */
-int gspca_auto_gain_n_exposure(struct gspca_dev *gspca_dev, int avg_lum,
-	int desired_avg_lum, int deadzone, int gain_knee, int exposure_knee)
-{
-	int i, steps, gain, orig_gain, exposure, orig_exposure, autogain;
-	const struct ctrl *gain_ctrl = NULL;
-	const struct ctrl *exposure_ctrl = NULL;
-	const struct ctrl *autogain_ctrl = NULL;
-	int retval = 0;
-
-	for (i = 0; i < gspca_dev->sd_desc->nctrls; i++) {
-		if (gspca_dev->ctrl_dis & (1 << i))
-			continue;
-		if (gspca_dev->sd_desc->ctrls[i].qctrl.id == V4L2_CID_GAIN)
-			gain_ctrl = &gspca_dev->sd_desc->ctrls[i];
-		if (gspca_dev->sd_desc->ctrls[i].qctrl.id == V4L2_CID_EXPOSURE)
-			exposure_ctrl = &gspca_dev->sd_desc->ctrls[i];
-		if (gspca_dev->sd_desc->ctrls[i].qctrl.id == V4L2_CID_AUTOGAIN)
-			autogain_ctrl = &gspca_dev->sd_desc->ctrls[i];
-	}
-	if (!gain_ctrl || !exposure_ctrl || !autogain_ctrl) {
-		PDEBUG(D_ERR, "Error: gspca_auto_gain_n_exposure called "
-			"on cam without (auto)gain/exposure");
-		return 0;
-	}
-
-	if (gain_ctrl->get(gspca_dev, &gain) ||
-			exposure_ctrl->get(gspca_dev, &exposure) ||
-			autogain_ctrl->get(gspca_dev, &autogain) || !autogain)
-		return 0;
-
-	orig_gain = gain;
-	orig_exposure = exposure;
-
-	/* If we are of a multiple of deadzone, do multiple steps to reach the
-	   desired lumination fast (with the risc of a slight overshoot) */
-	steps = abs(desired_avg_lum - avg_lum) / deadzone;
-
-	PDEBUG(D_FRAM, "autogain: lum: %d, desired: %d, steps: %d",
-		avg_lum, desired_avg_lum, steps);
-
-	for (i = 0; i < steps; i++) {
-		if (avg_lum > desired_avg_lum) {
-			if (gain > gain_knee)
-				gain--;
-			else if (exposure > exposure_knee)
-				exposure--;
-			else if (gain > gain_ctrl->qctrl.default_value)
-				gain--;
-			else if (exposure > exposure_ctrl->qctrl.minimum)
-				exposure--;
-			else if (gain > gain_ctrl->qctrl.minimum)
-				gain--;
-			else
-				break;
-		} else {
-			if (gain < gain_ctrl->qctrl.default_value)
-				gain++;
-			else if (exposure < exposure_knee)
-				exposure++;
-			else if (gain < gain_knee)
-				gain++;
-			else if (exposure < exposure_ctrl->qctrl.maximum)
-				exposure++;
-			else if (gain < gain_ctrl->qctrl.maximum)
-				gain++;
-			else
-				break;
-		}
-	}
-
-	if (gain != orig_gain) {
-		gain_ctrl->set(gspca_dev, gain);
-		retval = 1;
-	}
-	if (exposure != orig_exposure) {
-		exposure_ctrl->set(gspca_dev, exposure);
-		retval = 1;
-	}
-
-	return retval;
-}
-EXPORT_SYMBOL(gspca_auto_gain_n_exposure);
 
 /* -- module insert / remove -- */
 static int __init gspca_init(void)
diff --git a/drivers/media/video/gspca/gspca.h b/drivers/media/video/gspca/gspca.h
index 589009f4496f..dc688c7f5e48 100644
--- a/drivers/media/video/gspca/gspca.h
+++ b/drivers/media/video/gspca/gspca.h
@@ -6,6 +6,8 @@
 #include <linux/usb.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
 #include <linux/mutex.h>
 
 /* compilation option */
@@ -115,6 +117,7 @@ struct sd_desc {
 /* mandatory operations */
 	cam_cf_op config;	/* called on probe */
 	cam_op init;		/* called on probe and resume */
+	cam_op init_controls;	/* called on probe */
 	cam_op start;		/* called on stream on after URBs creation */
 	cam_pkt_op pkt_scan;
 /* optional operations */
@@ -158,8 +161,10 @@ struct gspca_frame {
 struct gspca_dev {
 	struct video_device vdev;	/* !! must be the first item */
 	struct module *module;		/* subdriver handling the device */
+	struct v4l2_device v4l2_dev;
 	struct usb_device *dev;
 	struct file *capt_file;		/* file doing video capture */
+					/* protected by queue_lock */
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 	struct input_dev *input_dev;
 	char phys[64];			/* physical device path */
@@ -169,6 +174,16 @@ struct gspca_dev {
 	const struct sd_desc *sd_desc;		/* subdriver description */
 	unsigned ctrl_dis;		/* disabled controls (bit map) */
 	unsigned ctrl_inac;		/* inactive controls (bit map) */
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* autogain and exposure or gain control cluster, these are global as
+	   the autogain/exposure functions in autogain_functions.c use them */
+	struct {
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+		int exp_too_low_cnt, exp_too_high_cnt;
+	};
 
 #define USB_BUF_SZ 64
 	__u8 *usb_buf;				/* buffer for USB exchanges */
@@ -189,7 +204,7 @@ struct gspca_dev {
 	u8 fr_o;				/* next frame to dequeue */
 	__u8 last_packet_type;
 	__s8 empty_packet;		/* if (-1) don't check empty packets */
-	__u8 streaming;
+	__u8 streaming;			/* protected by both mutexes (*) */
 
 	__u8 curr_mode;			/* current camera mode */
 	__u32 pixfmt;			/* current mode parameters */
@@ -211,6 +226,10 @@ struct gspca_dev {
 	__u8 iface;			/* USB interface number */
 	__u8 alt;			/* USB alternate setting */
 	u8 audio;			/* presence of audio device */
+
+	/* (*) These variables are proteced by both usb_lock and queue_lock,
+	   that is any code setting them is holding *both*, which means that
+	   any code getting them needs to hold at least one of them */
 };
 
 int gspca_dev_probe(struct usb_interface *intf,
@@ -232,6 +251,9 @@ void gspca_frame_add(struct gspca_dev *gspca_dev,
 int gspca_suspend(struct usb_interface *intf, pm_message_t message);
 int gspca_resume(struct usb_interface *intf);
 #endif
-int gspca_auto_gain_n_exposure(struct gspca_dev *gspca_dev, int avg_lum,
+int gspca_expo_autogain(struct gspca_dev *gspca_dev, int avg_lum,
 	int desired_avg_lum, int deadzone, int gain_knee, int exposure_knee);
+int gspca_coarse_grained_expo_autogain(struct gspca_dev *gspca_dev,
+        int avg_lum, int desired_avg_lum, int deadzone);
+
 #endif /* GSPCAV2_H */
diff --git a/drivers/media/video/gspca/jl2005bcd.c b/drivers/media/video/gspca/jl2005bcd.c
index 53f58ef367cf..9c591c7c6f54 100644
--- a/drivers/media/video/gspca/jl2005bcd.c
+++ b/drivers/media/video/gspca/jl2005bcd.c
@@ -335,7 +335,11 @@ static void jl2005c_dostream(struct work_struct *work)
 		goto quit_stream;
 	}
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		/* Check if this is a new frame. If so, start the frame first */
 		if (!header_read) {
 			mutex_lock(&gspca_dev->usb_lock);
@@ -367,7 +371,7 @@ static void jl2005c_dostream(struct work_struct *work)
 					buffer, act_len);
 			header_read = 1;
 		}
-		while (bytes_left > 0 && gspca_dev->present) {
+		while (bytes_left > 0 && gspca_dev->dev) {
 			data_len = bytes_left > JL2005C_MAX_TRANSFER ?
 				JL2005C_MAX_TRANSFER : bytes_left;
 			ret = usb_bulk_msg(gspca_dev->dev,
@@ -390,7 +394,7 @@ static void jl2005c_dostream(struct work_struct *work)
 		}
 	}
 quit_stream:
-	if (gspca_dev->present) {
+	if (gspca_dev->dev) {
 		mutex_lock(&gspca_dev->usb_lock);
 		jl2005c_stop(gspca_dev);
 		mutex_unlock(&gspca_dev->usb_lock);
diff --git a/drivers/media/video/gspca/mars.c b/drivers/media/video/gspca/mars.c
index b0231465afae..ec7b21ee79fb 100644
--- a/drivers/media/video/gspca/mars.c
+++ b/drivers/media/video/gspca/mars.c
@@ -30,22 +30,19 @@ MODULE_AUTHOR("Michel Xhaard <mxhaard@users.sourceforge.net>");
 MODULE_DESCRIPTION("GSPCA/Mars USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	COLORS,
-	GAMMA,
-	SHARPNESS,
-	ILLUM_TOP,
-	ILLUM_BOT,
-	NCTRLS		/* number of controls */
-};
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *gamma;
+	struct { /* illuminator control cluster */
+		struct v4l2_ctrl *illum_top;
+		struct v4l2_ctrl *illum_bottom;
+	};
+	struct v4l2_ctrl *jpegqual;
 
 	u8 quality;
 #define QUALITY_MIN 40
@@ -56,89 +53,10 @@ struct sd {
 };
 
 /* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
-static void setgamma(struct gspca_dev *gspca_dev);
-static void setsharpness(struct gspca_dev *gspca_dev);
-static int sd_setilluminator1(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_setilluminator2(struct gspca_dev *gspca_dev, __s32 val);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 30,
-		.step    = 1,
-		.default_value = 15,
-	    },
-	    .set_control = setbrightness
-	},
-[COLORS] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 1,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 200,
-	    },
-	    .set_control = setcolors
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setgamma
-	},
-[SHARPNESS] = {
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 2,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setsharpness
-	},
-[ILLUM_TOP] = {
-	    {
-		.id	 = V4L2_CID_ILLUMINATORS_1,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Top illuminator",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-		.flags = V4L2_CTRL_FLAG_UPDATE,
-	    },
-	    .set = sd_setilluminator1
-	},
-[ILLUM_BOT] = {
-	    {
-		.id	 = V4L2_CID_ILLUMINATORS_2,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Bottom illuminator",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-		.flags = V4L2_CTRL_FLAG_UPDATE,
-	    },
-	    .set = sd_setilluminator2
-	},
-};
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val);
+static void setcolors(struct gspca_dev *gspca_dev, s32 val);
+static void setgamma(struct gspca_dev *gspca_dev, s32 val);
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val);
 
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
@@ -198,59 +116,130 @@ static void mi_w(struct gspca_dev *gspca_dev,
 	reg_w(gspca_dev, 4);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->usb_buf[0] = 0x61;
-	gspca_dev->usb_buf[1] = sd->ctrls[BRIGHTNESS].val;
+	gspca_dev->usb_buf[1] = val;
 	reg_w(gspca_dev, 2);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	s16 val;
-
-	val = sd->ctrls[COLORS].val;
 	gspca_dev->usb_buf[0] = 0x5f;
 	gspca_dev->usb_buf[1] = val << 3;
 	gspca_dev->usb_buf[2] = ((val >> 2) & 0xf8) | 0x04;
 	reg_w(gspca_dev, 3);
 }
 
-static void setgamma(struct gspca_dev *gspca_dev)
+static void setgamma(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->usb_buf[0] = 0x06;
-	gspca_dev->usb_buf[1] = sd->ctrls[GAMMA].val * 0x40;
+	gspca_dev->usb_buf[1] = val * 0x40;
 	reg_w(gspca_dev, 2);
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->usb_buf[0] = 0x67;
-	gspca_dev->usb_buf[1] = sd->ctrls[SHARPNESS].val * 4 + 3;
+	gspca_dev->usb_buf[1] = val * 4 + 3;
 	reg_w(gspca_dev, 2);
 }
 
-static void setilluminators(struct gspca_dev *gspca_dev)
+static void setilluminators(struct gspca_dev *gspca_dev, bool top, bool bottom)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
+	/* both are off if not streaming */
 	gspca_dev->usb_buf[0] = 0x22;
-	if (sd->ctrls[ILLUM_TOP].val)
+	if (top)
 		gspca_dev->usb_buf[1] = 0x76;
-	else if (sd->ctrls[ILLUM_BOT].val)
+	else if (bottom)
 		gspca_dev->usb_buf[1] = 0x7a;
 	else
 		gspca_dev->usb_buf[1] = 0x7e;
 	reg_w(gspca_dev, 2);
 }
 
+static int mars_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_ILLUMINATORS_1) {
+		/* only one can be on at a time */
+		if (ctrl->is_new && ctrl->val)
+			sd->illum_bottom->val = 0;
+		if (sd->illum_bottom->is_new && sd->illum_bottom->val)
+			sd->illum_top->val = 0;
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAMMA:
+		setgamma(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_ILLUMINATORS_1:
+		setilluminators(gspca_dev, sd->illum_top->val,
+					   sd->illum_bottom->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		jpeg_set_qual(sd->jpeg_hdr, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops mars_ctrl_ops = {
+	.s_ctrl = mars_s_ctrl,
+};
+
+/* this function is called at probe time */
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 7);
+	sd->brightness = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 30, 1, 15);
+	sd->saturation = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 200);
+	sd->gamma = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_GAMMA, 0, 3, 1, 1);
+	sd->sharpness = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 2, 1, 1);
+	sd->illum_top = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_ILLUMINATORS_1, 0, 1, 1, 0);
+	sd->illum_top->flags |= V4L2_CTRL_FLAG_UPDATE;
+	sd->illum_bottom = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_ILLUMINATORS_2, 0, 1, 1, 0);
+	sd->illum_bottom->flags |= V4L2_CTRL_FLAG_UPDATE;
+	sd->jpegqual = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY,
+			QUALITY_MIN, QUALITY_MAX, 1, QUALITY_DEF);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_cluster(2, &sd->illum_top);
+	return 0;
+}
+
 /* this function is called at probe time */
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
@@ -261,7 +250,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam = &gspca_dev->cam;
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
-	cam->ctrls = sd->ctrls;
 	sd->quality = QUALITY_DEF;
 	return 0;
 }
@@ -269,7 +257,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 /* this function is called at probe and resume time */
 static int sd_init(struct gspca_dev *gspca_dev)
 {
-	gspca_dev->ctrl_inac = (1 << ILLUM_TOP) | (1 << ILLUM_BOT);
 	return 0;
 }
 
@@ -282,7 +269,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* create the JPEG header */
 	jpeg_define(sd->jpeg_hdr, gspca_dev->height, gspca_dev->width,
 			0x21);		/* JPEG 422 */
-	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	jpeg_set_qual(sd->jpeg_hdr, v4l2_ctrl_g_ctrl(sd->jpegqual));
 
 	data = gspca_dev->usb_buf;
 
@@ -301,7 +288,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	data[5] = 0x30;		/* reg 4, MI, PAS5101 :
 				 *	0x30 for 24mhz , 0x28 for 12mhz */
 	data[6] = 0x02;		/* reg 5, H start - was 0x04 */
-	data[7] = sd->ctrls[GAMMA].val * 0x40;	/* reg 0x06: gamma */
+	data[7] = v4l2_ctrl_g_ctrl(sd->gamma) * 0x40;	/* reg 0x06: gamma */
 	data[8] = 0x01;		/* reg 7, V start - was 0x03 */
 /*	if (h_size == 320 ) */
 /*		data[9]= 0x56;	 * reg 8, 24MHz, 2:1 scale down */
@@ -333,16 +320,16 @@ static int sd_start(struct gspca_dev *gspca_dev)
 				/* reg 0x5f/0x60 (LE) = saturation */
 				/* h (60): xxxx x100
 				 * l (5f): xxxx x000 */
-	data[2] = sd->ctrls[COLORS].val << 3;
-	data[3] = ((sd->ctrls[COLORS].val >> 2) & 0xf8) | 0x04;
-	data[4] = sd->ctrls[BRIGHTNESS].val; /* reg 0x61 = brightness */
+	data[2] = v4l2_ctrl_g_ctrl(sd->saturation) << 3;
+	data[3] = ((v4l2_ctrl_g_ctrl(sd->saturation) >> 2) & 0xf8) | 0x04;
+	data[4] = v4l2_ctrl_g_ctrl(sd->brightness); /* reg 0x61 = brightness */
 	data[5] = 0x00;
 
 	reg_w(gspca_dev, 6);
 
 	data[0] = 0x67;
 /*jfm: from win trace*/
-	data[1] = sd->ctrls[SHARPNESS].val * 4 + 3;
+	data[1] = v4l2_ctrl_g_ctrl(sd->sharpness) * 4 + 3;
 	data[2] = 0x14;
 	reg_w(gspca_dev, 3);
 
@@ -365,7 +352,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	data[1] = 0x4d;		/* ISOC transferring enable... */
 	reg_w(gspca_dev, 2);
 
-	gspca_dev->ctrl_inac = 0; /* activate the illuminator controls */
+	setilluminators(gspca_dev, v4l2_ctrl_g_ctrl(sd->illum_top),
+				   v4l2_ctrl_g_ctrl(sd->illum_bottom));
+
 	return gspca_dev->usb_err;
 }
 
@@ -373,11 +362,9 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	gspca_dev->ctrl_inac = (1 << ILLUM_TOP) | (1 << ILLUM_BOT);
-	if (sd->ctrls[ILLUM_TOP].val || sd->ctrls[ILLUM_BOT].val) {
-		sd->ctrls[ILLUM_TOP].val = 0;
-		sd->ctrls[ILLUM_BOT].val = 0;
-		setilluminators(gspca_dev);
+	if (v4l2_ctrl_g_ctrl(sd->illum_top) ||
+	    v4l2_ctrl_g_ctrl(sd->illum_bottom)) {
+		setilluminators(gspca_dev, false, false);
 		msleep(20);
 	}
 
@@ -424,43 +411,16 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_setilluminator1(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	/* only one illuminator may be on */
-	sd->ctrls[ILLUM_TOP].val = val;
-	if (val)
-		sd->ctrls[ILLUM_BOT].val = 0;
-	setilluminators(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_setilluminator2(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	/* only one illuminator may be on */
-	sd->ctrls[ILLUM_BOT].val = val;
-	if (val)
-		sd->ctrls[ILLUM_TOP].val = 0;
-	setilluminators(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
 static int sd_set_jcomp(struct gspca_dev *gspca_dev,
 			struct v4l2_jpegcompression *jcomp)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	int ret;
 
-	if (jcomp->quality < QUALITY_MIN)
-		sd->quality = QUALITY_MIN;
-	else if (jcomp->quality > QUALITY_MAX)
-		sd->quality = QUALITY_MAX;
-	else
-		sd->quality = jcomp->quality;
-	if (gspca_dev->streaming)
-		jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	ret = v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
+	if (ret)
+		return ret;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	return 0;
 }
 
@@ -470,7 +430,7 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
 			| V4L2_JPEG_MARKER_DQT;
 	return 0;
@@ -479,10 +439,9 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = NCTRLS,
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -513,6 +472,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/nw80x.c b/drivers/media/video/gspca/nw80x.c
index 7167cac7359c..42e021931e60 100644
--- a/drivers/media/video/gspca/nw80x.c
+++ b/drivers/media/video/gspca/nw80x.c
@@ -2001,6 +2001,8 @@ static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
 	return gspca_dev->usb_err;
 }
 
+#define WANT_REGULAR_AUTOGAIN
+#define WANT_COARSE_EXPO_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
diff --git a/drivers/media/video/gspca/ov519.c b/drivers/media/video/gspca/ov519.c
index 739e8a2a2d30..183457c5cfdb 100644
--- a/drivers/media/video/gspca/ov519.c
+++ b/drivers/media/video/gspca/ov519.c
@@ -2804,7 +2804,7 @@ static void ov7xx0_configure(struct sd *sd)
 	/* add OV7670 here
 	 * it appears to be wrongly detected as a 7610 by default */
 	if (rc < 0) {
-		PDEBUG(D_ERR, "Error detecting sensor type");
+		pr_err("Error detecting sensor type\n");
 		return;
 	}
 	if ((rc & 3) == 3) {
@@ -2832,12 +2832,12 @@ static void ov7xx0_configure(struct sd *sd)
 		/* try to read product id registers */
 		high = i2c_r(sd, 0x0a);
 		if (high < 0) {
-			PDEBUG(D_ERR, "Error detecting camera chip PID");
+			pr_err("Error detecting camera chip PID\n");
 			return;
 		}
 		low = i2c_r(sd, 0x0b);
 		if (low < 0) {
-			PDEBUG(D_ERR, "Error detecting camera chip VER");
+			pr_err("Error detecting camera chip VER\n");
 			return;
 		}
 		if (high == 0x76) {
@@ -2863,7 +2863,7 @@ static void ov7xx0_configure(struct sd *sd)
 				sd->sensor = SEN_OV7660;
 				break;
 			default:
-				PDEBUG(D_PROBE, "Unknown sensor: 0x76%x", low);
+				pr_err("Unknown sensor: 0x76%02x\n", low);
 				return;
 			}
 		} else {
@@ -2884,7 +2884,7 @@ static void ov6xx0_configure(struct sd *sd)
 	/* Detect sensor (sub)type */
 	rc = i2c_r(sd, OV7610_REG_COM_I);
 	if (rc < 0) {
-		PDEBUG(D_ERR, "Error detecting sensor type");
+		pr_err("Error detecting sensor type\n");
 		return;
 	}
 
diff --git a/drivers/media/video/gspca/ov534.c b/drivers/media/video/gspca/ov534.c
index 04753391de3e..b5acb1e4b4e7 100644
--- a/drivers/media/video/gspca/ov534.c
+++ b/drivers/media/video/gspca/ov534.c
@@ -34,6 +34,8 @@
 
 #include "gspca.h"
 
+#include <linux/fixp-arith.h>
+
 #define OV534_REG_ADDRESS	0xf1	/* sensor address */
 #define OV534_REG_SUBADDR	0xf2
 #define OV534_REG_WRITE		0xf3
@@ -53,6 +55,8 @@ MODULE_LICENSE("GPL");
 
 /* controls */
 enum e_ctrl {
+	HUE,
+	SATURATION,
 	BRIGHTNESS,
 	CONTRAST,
 	GAIN,
@@ -63,7 +67,6 @@ enum e_ctrl {
 	SHARPNESS,
 	HFLIP,
 	VFLIP,
-	COLORS,
 	LIGHTFREQ,
 	NCTRLS		/* number of controls */
 };
@@ -87,6 +90,8 @@ enum sensors {
 };
 
 /* V4L2 controls supported by the driver */
+static void sethue(struct gspca_dev *gspca_dev);
+static void setsaturation(struct gspca_dev *gspca_dev);
 static void setbrightness(struct gspca_dev *gspca_dev);
 static void setcontrast(struct gspca_dev *gspca_dev);
 static void setgain(struct gspca_dev *gspca_dev);
@@ -96,13 +101,36 @@ static void setawb(struct gspca_dev *gspca_dev);
 static void setaec(struct gspca_dev *gspca_dev);
 static void setsharpness(struct gspca_dev *gspca_dev);
 static void sethvflip(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
 static void setlightfreq(struct gspca_dev *gspca_dev);
 
 static int sd_start(struct gspca_dev *gspca_dev);
 static void sd_stopN(struct gspca_dev *gspca_dev);
 
 static const struct ctrl sd_ctrls[] = {
+[HUE] = {
+		{
+			.id      = V4L2_CID_HUE,
+			.type    = V4L2_CTRL_TYPE_INTEGER,
+			.name    = "Hue",
+			.minimum = -90,
+			.maximum = 90,
+			.step    = 1,
+			.default_value = 0,
+		},
+		.set_control = sethue
+	},
+[SATURATION] = {
+		{
+			.id      = V4L2_CID_SATURATION,
+			.type    = V4L2_CTRL_TYPE_INTEGER,
+			.name    = "Saturation",
+			.minimum = 0,
+			.maximum = 255,
+			.step    = 1,
+			.default_value = 64,
+		},
+		.set_control = setsaturation
+	},
 [BRIGHTNESS] = {
 		{
 			.id      = V4L2_CID_BRIGHTNESS,
@@ -223,18 +251,6 @@ static const struct ctrl sd_ctrls[] = {
 		},
 		.set_control = sethvflip
 	},
-[COLORS] = {
-		{
-			.id      = V4L2_CID_SATURATION,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Saturation",
-			.minimum = 0,
-			.maximum = 6,
-			.step    = 1,
-			.default_value = 3,
-		},
-		.set_control = setcolors
-	},
 [LIGHTFREQ] = {
 		{
 			.id      = V4L2_CID_POWER_LINE_FREQUENCY,
@@ -684,7 +700,7 @@ static const u8 sensor_init_772x[][2] = {
 	{ 0x9c, 0x20 },
 	{ 0x9e, 0x81 },
 
-	{ 0xa6, 0x04 },
+	{ 0xa6, 0x07 },
 	{ 0x7e, 0x0c },
 	{ 0x7f, 0x16 },
 	{ 0x80, 0x2a },
@@ -955,6 +971,74 @@ static void set_frame_rate(struct gspca_dev *gspca_dev)
 	PDEBUG(D_PROBE, "frame_rate: %d", r->fps);
 }
 
+static void sethue(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int val;
+
+	val = sd->ctrls[HUE].val;
+	if (sd->sensor == SENSOR_OV767x) {
+		/* TBD */
+	} else {
+		s16 huesin;
+		s16 huecos;
+
+		/* fixp_sin and fixp_cos accept only positive values, while
+		 * our val is between -90 and 90
+		 */
+		val += 360;
+
+		/* According to the datasheet the registers expect HUESIN and
+		 * HUECOS to be the result of the trigonometric functions,
+		 * scaled by 0x80.
+		 *
+		 * The 0x100 here represents the maximun absolute value
+		 * returned byt fixp_sin and fixp_cos, so the scaling will
+		 * consider the result like in the interval [-1.0, 1.0].
+		 */
+		huesin = fixp_sin(val) * 0x80 / 0x100;
+		huecos = fixp_cos(val) * 0x80 / 0x100;
+
+		if (huesin < 0) {
+			sccb_reg_write(gspca_dev, 0xab,
+				sccb_reg_read(gspca_dev, 0xab) | 0x2);
+			huesin = -huesin;
+		} else {
+			sccb_reg_write(gspca_dev, 0xab,
+				sccb_reg_read(gspca_dev, 0xab) & ~0x2);
+
+		}
+		sccb_reg_write(gspca_dev, 0xa9, (u8)huecos);
+		sccb_reg_write(gspca_dev, 0xaa, (u8)huesin);
+	}
+}
+
+static void setsaturation(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int val;
+
+	val = sd->ctrls[SATURATION].val;
+	if (sd->sensor == SENSOR_OV767x) {
+		int i;
+		static u8 color_tb[][6] = {
+			{0x42, 0x42, 0x00, 0x11, 0x30, 0x41},
+			{0x52, 0x52, 0x00, 0x16, 0x3c, 0x52},
+			{0x66, 0x66, 0x00, 0x1b, 0x4b, 0x66},
+			{0x80, 0x80, 0x00, 0x22, 0x5e, 0x80},
+			{0x9a, 0x9a, 0x00, 0x29, 0x71, 0x9a},
+			{0xb8, 0xb8, 0x00, 0x31, 0x87, 0xb8},
+			{0xdd, 0xdd, 0x00, 0x3b, 0xa2, 0xdd},
+		};
+
+		for (i = 0; i < ARRAY_SIZE(color_tb[0]); i++)
+			sccb_reg_write(gspca_dev, 0x4f + i, color_tb[val][i]);
+	} else {
+		sccb_reg_write(gspca_dev, 0xa7, val); /* U saturation */
+		sccb_reg_write(gspca_dev, 0xa8, val); /* V saturation */
+	}
+}
+
 static void setbrightness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -1132,26 +1216,6 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
-	int i;
-	static u8 color_tb[][6] = {
-		{0x42, 0x42, 0x00, 0x11, 0x30, 0x41},
-		{0x52, 0x52, 0x00, 0x16, 0x3c, 0x52},
-		{0x66, 0x66, 0x00, 0x1b, 0x4b, 0x66},
-		{0x80, 0x80, 0x00, 0x22, 0x5e, 0x80},
-		{0x9a, 0x9a, 0x00, 0x29, 0x71, 0x9a},
-		{0xb8, 0xb8, 0x00, 0x31, 0x87, 0xb8},
-		{0xdd, 0xdd, 0x00, 0x3b, 0xa2, 0xdd},
-	};
-
-	val = sd->ctrls[COLORS].val;
-	for (i = 0; i < ARRAY_SIZE(color_tb[0]); i++)
-		sccb_reg_write(gspca_dev, 0x4f + i, color_tb[val][i]);
-}
-
 static void setlightfreq(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -1225,9 +1289,13 @@ static int sd_init(struct gspca_dev *gspca_dev)
 
 	if ((sensor_id & 0xfff0) == 0x7670) {
 		sd->sensor = SENSOR_OV767x;
-		gspca_dev->ctrl_dis = (1 << GAIN) |
+		gspca_dev->ctrl_dis = (1 << HUE) |
+					(1 << GAIN) |
 					(1 << AGC) |
 					(1 << SHARPNESS);	/* auto */
+		sd->ctrls[SATURATION].min = 0,
+		sd->ctrls[SATURATION].max = 6,
+		sd->ctrls[SATURATION].def = 3,
 		sd->ctrls[BRIGHTNESS].min = -127;
 		sd->ctrls[BRIGHTNESS].max = 127;
 		sd->ctrls[BRIGHTNESS].def = 0;
@@ -1243,7 +1311,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		gspca_dev->cam.nmodes = ARRAY_SIZE(ov767x_mode);
 	} else {
 		sd->sensor = SENSOR_OV772x;
-		gspca_dev->ctrl_dis = (1 << COLORS);
 		gspca_dev->cam.bulk = 1;
 		gspca_dev->cam.bulk_size = 16384;
 		gspca_dev->cam.bulk_nurbs = 2;
@@ -1302,6 +1369,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	set_frame_rate(gspca_dev);
 
+	if (!(gspca_dev->ctrl_dis & (1 << HUE)))
+		sethue(gspca_dev);
+	setsaturation(gspca_dev);
 	if (!(gspca_dev->ctrl_dis & (1 << AGC)))
 		setagc(gspca_dev);
 	setawb(gspca_dev);
@@ -1314,8 +1384,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	if (!(gspca_dev->ctrl_dis & (1 << SHARPNESS)))
 		setsharpness(gspca_dev);
 	sethvflip(gspca_dev);
-	if (!(gspca_dev->ctrl_dis & (1 << COLORS)))
-		setcolors(gspca_dev);
 	setlightfreq(gspca_dev);
 
 	ov534_set_led(gspca_dev, 1);
diff --git a/drivers/media/video/gspca/pac207.c b/drivers/media/video/gspca/pac207.c
index 3844c49f269c..fa661c6d6d55 100644
--- a/drivers/media/video/gspca/pac207.c
+++ b/drivers/media/video/gspca/pac207.c
@@ -29,6 +29,8 @@
 
 #include <linux/input.h>
 #include "gspca.h"
+/* Include pac common sof detection functions */
+#include "pac_common.h"
 
 MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
 MODULE_DESCRIPTION("Pixart PAC207");
@@ -39,16 +41,17 @@ MODULE_LICENSE("GPL");
 #define PAC207_BRIGHTNESS_MIN		0
 #define PAC207_BRIGHTNESS_MAX		255
 #define PAC207_BRIGHTNESS_DEFAULT	46
+#define PAC207_BRIGHTNESS_REG		0x08
 
 #define PAC207_EXPOSURE_MIN		3
 #define PAC207_EXPOSURE_MAX		90 /* 1 sec expo time / 1 fps */
 #define PAC207_EXPOSURE_DEFAULT		5 /* power on default: 3 */
-#define PAC207_EXPOSURE_KNEE		9 /* fps: 90 / exposure -> 9: 10 fps */
+#define PAC207_EXPOSURE_REG		0x02
 
 #define PAC207_GAIN_MIN			0
 #define PAC207_GAIN_MAX			31
 #define PAC207_GAIN_DEFAULT		7 /* power on default: 9 */
-#define PAC207_GAIN_KNEE		15
+#define PAC207_GAIN_REG			0x0e
 
 #define PAC207_AUTOGAIN_DEADZONE	30
 
@@ -56,13 +59,9 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	u8 mode;
-
-	u8 brightness;
-	u8 exposure;
-	u8 autogain;
-	u8 gain;
+	struct v4l2_ctrl *brightness;
 
+	u8 mode;
 	u8 sof_read;
 	u8 header_read;
 	u8 autogain_ignore_frames;
@@ -70,80 +69,6 @@ struct sd {
 	atomic_t avg_lum;
 };
 
-/* V4L2 controls supported by the driver */
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-#define SD_BRIGHTNESS 0
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = PAC207_BRIGHTNESS_MIN,
-		.maximum = PAC207_BRIGHTNESS_MAX,
-		.step = 1,
-		.default_value = PAC207_BRIGHTNESS_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define SD_EXPOSURE 1
-	{
-	    {
-		.id = V4L2_CID_EXPOSURE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Exposure",
-		.minimum = PAC207_EXPOSURE_MIN,
-		.maximum = PAC207_EXPOSURE_MAX,
-		.step = 1,
-		.default_value = PAC207_EXPOSURE_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setexposure,
-	    .get = sd_getexposure,
-	},
-#define SD_AUTOGAIN 2
-	{
-	    {
-		.id	  = V4L2_CID_AUTOGAIN,
-		.type	= V4L2_CTRL_TYPE_BOOLEAN,
-		.name	= "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step	= 1,
-#define AUTOGAIN_DEF 1
-		.default_value = AUTOGAIN_DEF,
-		.flags = 0,
-	    },
-	    .set = sd_setautogain,
-	    .get = sd_getautogain,
-	},
-#define SD_GAIN 3
-	{
-	    {
-		.id = V4L2_CID_GAIN,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Gain",
-		.minimum = PAC207_GAIN_MIN,
-		.maximum = PAC207_GAIN_MAX,
-		.step = 1,
-		.default_value = PAC207_GAIN_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setgain,
-	    .get = sd_getgain,
-	},
-};
-
 static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_PAC207, V4L2_FIELD_NONE,
 		.bytesperline = 176,
@@ -167,39 +92,44 @@ static const __u8 pac207_sensor_init[][8] = {
 	{0x32, 0x00, 0x96, 0x00, 0xa2, 0x02, 0xaf, 0x00},
 };
 
-static int pac207_write_regs(struct gspca_dev *gspca_dev, u16 index,
+static void pac207_write_regs(struct gspca_dev *gspca_dev, u16 index,
 	const u8 *buffer, u16 length)
 {
 	struct usb_device *udev = gspca_dev->dev;
 	int err;
 
+	if (gspca_dev->usb_err < 0)
+		return;
+
 	memcpy(gspca_dev->usb_buf, buffer, length);
 
 	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x01,
 			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
 			0x00, index,
 			gspca_dev->usb_buf, length, PAC207_CTRL_TIMEOUT);
-	if (err < 0)
+	if (err < 0) {
 		pr_err("Failed to write registers to index 0x%04X, error %d\n",
 		       index, err);
-
-	return err;
+		gspca_dev->usb_err = err;
+	}
 }
 
-
-static int pac207_write_reg(struct gspca_dev *gspca_dev, u16 index, u16 value)
+static void pac207_write_reg(struct gspca_dev *gspca_dev, u16 index, u16 value)
 {
 	struct usb_device *udev = gspca_dev->dev;
 	int err;
 
+	if (gspca_dev->usb_err < 0)
+		return;
+
 	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00,
 			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
 			value, index, NULL, 0, PAC207_CTRL_TIMEOUT);
-	if (err)
+	if (err) {
 		pr_err("Failed to write a register (index 0x%04X, value 0x%02X, error %d)\n",
 		       index, value, err);
-
-	return err;
+		gspca_dev->usb_err = err;
+	}
 }
 
 static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
@@ -207,6 +137,9 @@ static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
 	struct usb_device *udev = gspca_dev->dev;
 	int res;
 
+	if (gspca_dev->usb_err < 0)
+		return 0;
+
 	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00,
 			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
 			0x00, index,
@@ -214,7 +147,8 @@ static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
 	if (res < 0) {
 		pr_err("Failed to read a register (index 0x%04X, error %d)\n",
 		       index, res);
-		return res;
+		gspca_dev->usb_err = res;
+		return 0;
 	}
 
 	return gspca_dev->usb_buf[0];
@@ -224,7 +158,6 @@ static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
 	u8 idreg[2];
 
@@ -247,10 +180,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam = &gspca_dev->cam;
 	cam->cam_mode = sif_mode;
 	cam->nmodes = ARRAY_SIZE(sif_mode);
-	sd->brightness = PAC207_BRIGHTNESS_DEFAULT;
-	sd->exposure = PAC207_EXPOSURE_DEFAULT;
-	sd->gain = PAC207_GAIN_DEFAULT;
-	sd->autogain = AUTOGAIN_DEF;
 
 	return 0;
 }
@@ -264,6 +193,87 @@ static int sd_init(struct gspca_dev *gspca_dev)
 				 * Bit_2=Compression test mode enable */
 	pac207_write_reg(gspca_dev, 0x0f, 0x00); /* Power Control */
 
+	return gspca_dev->usb_err;
+}
+
+static void setcontrol(struct gspca_dev *gspca_dev, u16 reg, u16 val)
+{
+	pac207_write_reg(gspca_dev, reg, val);
+	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
+	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
+}
+
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_AUTOGAIN && ctrl->is_new && ctrl->val) {
+		/* when switching to autogain set defaults to make sure
+		   we are on a valid point of the autogain gain /
+		   exposure knee graph, and give this change time to
+		   take effect before doing autogain. */
+		gspca_dev->exposure->val    = PAC207_EXPOSURE_DEFAULT;
+		gspca_dev->gain->val        = PAC207_GAIN_DEFAULT;
+		sd->autogain_ignore_frames  = PAC_AUTOGAIN_IGNORE_FRAMES;
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setcontrol(gspca_dev, PAC207_BRIGHTNESS_REG, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (gspca_dev->exposure->is_new || (ctrl->is_new && ctrl->val))
+			setcontrol(gspca_dev, PAC207_EXPOSURE_REG,
+				   gspca_dev->exposure->val);
+		if (gspca_dev->gain->is_new || (ctrl->is_new && ctrl->val))
+			setcontrol(gspca_dev, PAC207_GAIN_REG,
+				   gspca_dev->gain->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+/* this function is called at probe time */
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_BRIGHTNESS,
+				PAC207_BRIGHTNESS_MIN, PAC207_BRIGHTNESS_MAX,
+				1, PAC207_BRIGHTNESS_DEFAULT);
+	gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_EXPOSURE,
+				PAC207_EXPOSURE_MIN, PAC207_EXPOSURE_MAX,
+				1, PAC207_EXPOSURE_DEFAULT);
+	gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_GAIN,
+				PAC207_GAIN_MIN, PAC207_GAIN_MAX,
+				1, PAC207_GAIN_DEFAULT);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
 	return 0;
 }
 
@@ -285,11 +295,13 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	else
 		pac207_write_reg(gspca_dev, 0x4a, 0x30);
 	pac207_write_reg(gspca_dev, 0x4b, 0x00); /* Sram test value */
-	pac207_write_reg(gspca_dev, 0x08, sd->brightness);
+	pac207_write_reg(gspca_dev, 0x08, v4l2_ctrl_g_ctrl(sd->brightness));
 
 	/* PGA global gain (Bit 4-0) */
-	pac207_write_reg(gspca_dev, 0x0e, sd->gain);
-	pac207_write_reg(gspca_dev, 0x02, sd->exposure); /* PXCK = 12MHz /n */
+	pac207_write_reg(gspca_dev, 0x0e,
+		v4l2_ctrl_g_ctrl(gspca_dev->gain));
+	pac207_write_reg(gspca_dev, 0x02,
+		v4l2_ctrl_g_ctrl(gspca_dev->exposure)); /* PXCK = 12MHz /n */
 
 	mode = 0x02; /* Image Format (Bit 0), LED (1), Compr. test mode (2) */
 	if (gspca_dev->width == 176) {	/* 176x144 */
@@ -308,7 +320,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	sd->sof_read = 0;
 	sd->autogain_ignore_frames = 0;
 	atomic_set(&sd->avg_lum, -1);
-	return 0;
+	return gspca_dev->usb_err;
 }
 
 static void sd_stopN(struct gspca_dev *gspca_dev)
@@ -318,8 +330,6 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 	pac207_write_reg(gspca_dev, 0x0f, 0x00); /* Power Control */
 }
 
-/* Include pac common sof detection functions */
-#include "pac_common.h"
 
 static void pac207_do_auto_gain(struct gspca_dev *gspca_dev)
 {
@@ -331,9 +341,8 @@ static void pac207_do_auto_gain(struct gspca_dev *gspca_dev)
 
 	if (sd->autogain_ignore_frames > 0)
 		sd->autogain_ignore_frames--;
-	else if (gspca_auto_gain_n_exposure(gspca_dev, avg_lum,
-			90, PAC207_AUTOGAIN_DEADZONE,
-			PAC207_GAIN_KNEE, PAC207_EXPOSURE_KNEE))
+	else if (gspca_coarse_grained_expo_autogain(gspca_dev, avg_lum,
+			90, PAC207_AUTOGAIN_DEADZONE))
 		sd->autogain_ignore_frames = PAC_AUTOGAIN_IGNORE_FRAMES;
 }
 
@@ -384,118 +393,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	pac207_write_reg(gspca_dev, 0x08, sd->brightness);
-	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
-	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
-}
-
-static void setexposure(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	pac207_write_reg(gspca_dev, 0x02, sd->exposure);
-	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
-	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
-}
-
-static void setgain(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	pac207_write_reg(gspca_dev, 0x0e, sd->gain);
-	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
-	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
-}
-
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->brightness = val;
-	if (gspca_dev->streaming)
-		setbrightness(gspca_dev);
-	return 0;
-}
-
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->brightness;
-	return 0;
-}
-
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->exposure = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return 0;
-}
-
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->exposure;
-	return 0;
-}
-
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		setgain(gspca_dev);
-	return 0;
-}
-
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gain;
-	return 0;
-}
-
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->autogain = val;
-	/* when switching to autogain set defaults to make sure
-	   we are on a valid point of the autogain gain /
-	   exposure knee graph, and give this change time to
-	   take effect before doing autogain. */
-	if (sd->autogain) {
-		sd->exposure = PAC207_EXPOSURE_DEFAULT;
-		sd->gain = PAC207_GAIN_DEFAULT;
-		if (gspca_dev->streaming) {
-			sd->autogain_ignore_frames =
-				PAC_AUTOGAIN_IGNORE_FRAMES;
-			setexposure(gspca_dev);
-			setgain(gspca_dev);
-		}
-	}
-
-	return 0;
-}
-
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
-	return 0;
-}
-
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 			u8 *data,		/* interrupt packet data */
@@ -518,10 +415,9 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.dq_callback = pac207_do_auto_gain,
diff --git a/drivers/media/video/gspca/pac7302.c b/drivers/media/video/gspca/pac7302.c
index 30662fccb0cf..a0369a58c4bb 100644
--- a/drivers/media/video/gspca/pac7302.c
+++ b/drivers/media/video/gspca/pac7302.c
@@ -23,43 +23,58 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-/* Some documentation about various registers as determined by trial and error.
-
-   Register page 1:
-
-   Address	Description
-   0x78		Global control, bit 6 controls the LED (inverted)
-
-   Register page 3:
-
-   Address	Description
-   0x02		Clock divider 3-63, fps = 90 / val. Must be a multiple of 3 on
-		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
-   0x03		Variable framerate ctrl reg2==3: 0 -> ~30 fps, 255 -> ~22fps
-   0x04		Another var framerate ctrl reg2==3, reg3==0: 0 -> ~30 fps,
-		63 -> ~27 fps, the 2 msb's must always be 1 !!
-   0x05		Another var framerate ctrl reg2==3, reg3==0, reg4==0xc0:
-		1 -> ~30 fps, 2 -> ~20 fps
-   0x0e		Exposure bits 0-7, 0-448, 0 = use full frame time
-   0x0f		Exposure bit 8, 0-448, 448 = no exposure at all
-   0x10		Master gain 0-31
-   0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
-
-   The registers are accessed in the following functions:
-
-   Page | Register   | Function
-   -----+------------+---------------------------------------------------
-    0   | 0x0f..0x20 | setcolors()
-    0   | 0xa2..0xab | setbrightcont()
-    0   | 0xc5       | setredbalance()
-    0   | 0xc6       | setwhitebalance()
-    0   | 0xc7       | setbluebalance()
-    0   | 0xdc       | setbrightcont(), setcolors()
-    3   | 0x02       | setexposure()
-    3   | 0x10       | setgain()
-    3   | 0x11       | setcolors(), setgain(), setexposure(), sethvflip()
-    3   | 0x21       | sethvflip()
-*/
+/*
+ * Some documentation about various registers as determined by trial and error.
+ *
+ * Register page 1:
+ *
+ * Address	Description
+ * 0x78		Global control, bit 6 controls the LED (inverted)
+ * 0x80		Compression balance, 2 interesting settings:
+ *		0x0f Default
+ *		0x50 Values >= this switch the camera to a lower compression,
+ *		     using the same table for both luminance and chrominance.
+ *		     This gives a sharper picture. Only usable when running
+ *		     at < 15 fps! Note currently the driver does not use this
+ *		     as the quality gain is small and the generated JPG-s are
+ *		     only understood by v4l-utils >= 0.8.9
+ *
+ * Register page 3:
+ *
+ * Address	Description
+ * 0x02		Clock divider 3-63, fps = 90 / val. Must be a multiple of 3 on
+ *		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
+ * 0x03		Variable framerate ctrl reg2==3: 0 -> ~30 fps, 255 -> ~22fps
+ * 0x04		Another var framerate ctrl reg2==3, reg3==0: 0 -> ~30 fps,
+ *		63 -> ~27 fps, the 2 msb's must always be 1 !!
+ * 0x05		Another var framerate ctrl reg2==3, reg3==0, reg4==0xc0:
+ *		1 -> ~30 fps, 2 -> ~20 fps
+ * 0x0e		Exposure bits 0-7, 0-448, 0 = use full frame time
+ * 0x0f		Exposure bit 8, 0-448, 448 = no exposure at all
+ * 0x10		Gain 0-31
+ * 0x12		Another gain 0-31, unlike 0x10 this one seems to start with an
+ *		amplification value of 1 rather then 0 at its lowest setting
+ * 0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
+ * 0x80		Another framerate control, best left at 1, moving it from 1 to
+ *		2 causes the framerate to become 3/4th of what it was, and
+ *		also seems to cause pixel averaging, resulting in an effective
+ *		resolution of 320x240 and thus a much blockier image
+ *
+ * The registers are accessed in the following functions:
+ *
+ * Page | Register   | Function
+ * -----+------------+---------------------------------------------------
+ *  0   | 0x0f..0x20 | setcolors()
+ *  0   | 0xa2..0xab | setbrightcont()
+ *  0   | 0xc5       | setredbalance()
+ *  0   | 0xc6       | setwhitebalance()
+ *  0   | 0xc7       | setbluebalance()
+ *  0   | 0xdc       | setbrightcont(), setcolors()
+ *  3   | 0x02       | setexposure()
+ *  3   | 0x10, 0x12 | setgain()
+ *  3   | 0x11       | setcolors(), setgain(), setexposure(), sethvflip()
+ *  3   | 0x21       | sethvflip()
+ */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
@@ -89,7 +104,6 @@ enum e_ctrl {
 	NCTRLS		/* number of controls */
 };
 
-/* specific webcam descriptor for pac7302 */
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
@@ -198,10 +212,10 @@ static const struct ctrl sd_ctrls[] = {
 		.type    = V4L2_CTRL_TYPE_INTEGER,
 		.name    = "Gain",
 		.minimum = 0,
-		.maximum = 255,
+		.maximum = 62,
 		.step    = 1,
-#define GAIN_DEF 127
-#define GAIN_KNEE 255 /* Gain seems to cause little noise on the pac73xx */
+#define GAIN_DEF 15
+#define GAIN_KNEE 46
 		.default_value = GAIN_DEF,
 	    },
 	    .set_control = setgain
@@ -270,7 +284,6 @@ static const struct v4l2_pix_format vga_mode[] = {
 #define LOAD_PAGE3		255
 #define END_OF_SEQUENCE		0
 
-/* pac 7302 */
 static const u8 init_7302[] = {
 /*	index,value */
 	0xff, 0x01,		/* page 1 */
@@ -509,7 +522,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	return 0;
 }
 
-/* This function is used by pac7302 only */
 static void setbrightcont(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -536,7 +548,6 @@ static void setbrightcont(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0xdc, 0x01);
 }
 
-/* This function is used by pac7302 only */
 static void setcolors(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -590,9 +601,19 @@ static void setbluebalance(struct gspca_dev *gspca_dev)
 static void setgain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	u8 reg10, reg12;
+
+	if (sd->ctrls[GAIN].val < 32) {
+		reg10 = sd->ctrls[GAIN].val;
+		reg12 = 0;
+	} else {
+		reg10 = 31;
+		reg12 = sd->ctrls[GAIN].val - 31;
+	}
 
 	reg_w(gspca_dev, 0xff, 0x03);			/* page 3 */
-	reg_w(gspca_dev, 0x10, sd->ctrls[GAIN].val >> 3);
+	reg_w(gspca_dev, 0x10, reg10);
+	reg_w(gspca_dev, 0x12, reg12);
 
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
@@ -604,28 +625,36 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	u8 clockdiv;
 	u16 exposure;
 
-	/* register 2 of frame 3 contains the clock divider configuring the
-	   no fps according to the formula: 90 / reg. sd->exposure is the
-	   desired exposure time in 0.5 ms. */
+	/*
+	 * Register 2 of frame 3 contains the clock divider configuring the
+	 * no fps according to the formula: 90 / reg. sd->exposure is the
+	 * desired exposure time in 0.5 ms.
+	 */
 	clockdiv = (90 * sd->ctrls[EXPOSURE].val + 1999) / 2000;
 
-	/* Note clockdiv = 3 also works, but when running at 30 fps, depending
-	   on the scene being recorded, the camera switches to another
-	   quantization table for certain JPEG blocks, and we don't know how
-	   to decompress these blocks. So we cap the framerate at 15 fps */
+	/*
+	 * Note clockdiv = 3 also works, but when running at 30 fps, depending
+	 * on the scene being recorded, the camera switches to another
+	 * quantization table for certain JPEG blocks, and we don't know how
+	 * to decompress these blocks. So we cap the framerate at 15 fps.
+	 */
 	if (clockdiv < 6)
 		clockdiv = 6;
 	else if (clockdiv > 63)
 		clockdiv = 63;
 
-	/* reg2 MUST be a multiple of 3, except when between 6 and 12?
-	   Always round up, otherwise we cannot get the desired frametime
-	   using the partial frame time exposure control */
+	/*
+	 * Register 2 MUST be a multiple of 3, except when between 6 and 12?
+	 * Always round up, otherwise we cannot get the desired frametime
+	 * using the partial frame time exposure control.
+	 */
 	if (clockdiv < 6 || clockdiv > 12)
 		clockdiv = ((clockdiv + 2) / 3) * 3;
 
-	/* frame exposure time in ms = 1000 * clockdiv / 90    ->
-	exposure = (sd->exposure / 2) * 448 / (1000 * clockdiv / 90) */
+	/*
+	 * frame exposure time in ms = 1000 * clockdiv / 90    ->
+	 * exposure = (sd->exposure / 2) * 448 / (1000 * clockdiv / 90)
+	 */
 	exposure = (sd->ctrls[EXPOSURE].val * 45 * 448) / (1000 * clockdiv);
 	/* 0 = use full frametime, 448 = no exposure, reverse it */
 	exposure = 448 - exposure;
@@ -643,10 +672,12 @@ static void setautogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	/* when switching to autogain set defaults to make sure
-	   we are on a valid point of the autogain gain /
-	   exposure knee graph, and give this change time to
-	   take effect before doing autogain. */
+	/*
+	 * When switching to autogain set defaults to make sure
+	 * we are on a valid point of the autogain gain /
+	 * exposure knee graph, and give this change time to
+	 * take effect before doing autogain.
+	 */
 	if (sd->ctrls[AUTOGAIN].val) {
 		sd->ctrls[EXPOSURE].val = EXPOSURE_DEF;
 		sd->ctrls[GAIN].val = GAIN_DEF;
@@ -700,8 +731,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	setautogain(gspca_dev);
 	sethvflip(gspca_dev);
 
-	/* only resolution 640x480 is supported for pac7302 */
-
 	sd->sof_read = 0;
 	atomic_set(&sd->avg_lum, 270 + sd->ctrls[BRIGHTNESS].val);
 
@@ -729,9 +758,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x78, 0x40);
 }
 
-/* !! coarse_grained_expo_autogain is not used !! */
-#define exp_too_low_cnt flags
-#define exp_too_high_cnt sof_read
+#define WANT_REGULAR_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
@@ -792,10 +819,12 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	if (sof) {
 		int n, lum_offset, footer_length;
 
-		/* 6 bytes after the FF D9 EOF marker a number of lumination
-		   bytes are send corresponding to different parts of the
-		   image, the 14th and 15th byte after the EOF seem to
-		   correspond to the center of the image */
+		/*
+		 * 6 bytes after the FF D9 EOF marker a number of lumination
+		 * bytes are send corresponding to different parts of the
+		 * image, the 14th and 15th byte after the EOF seem to
+		 * correspond to the center of the image.
+		 */
 		lum_offset = 61 + sizeof pac_sof_marker;
 		footer_length = 74;
 
@@ -839,9 +868,10 @@ static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
 	u8 index;
 	u8 value;
 
-	/* reg->reg: bit0..15: reserved for register index (wIndex is 16bit
-			       long on the USB bus)
-	*/
+	/*
+	 * reg->reg: bit0..15: reserved for register index (wIndex is 16bit
+	 *		       long on the USB bus)
+	 */
 	if (reg->match.type == V4L2_CHIP_MATCH_HOST &&
 	    reg->match.addr == 0 &&
 	    (reg->reg < 0x000000ff) &&
@@ -852,9 +882,11 @@ static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
 		index = reg->reg;
 		value = reg->val;
 
-		/* Note that there shall be no access to other page
-		   by any other function between the page swith and
-		   the actual register write */
+		/*
+		 * Note that there shall be no access to other page
+		 * by any other function between the page switch and
+		 * the actual register write.
+		 */
 		reg_w(gspca_dev, 0xff, 0x00);		/* page 0 */
 		reg_w(gspca_dev, index, value);
 
@@ -940,6 +972,7 @@ static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x093a, 0x2624), .driver_info = FL_VFLIP},
 	{USB_DEVICE(0x093a, 0x2625)},
 	{USB_DEVICE(0x093a, 0x2626)},
+	{USB_DEVICE(0x093a, 0x2627), .driver_info = FL_VFLIP},
 	{USB_DEVICE(0x093a, 0x2628)},
 	{USB_DEVICE(0x093a, 0x2629), .driver_info = FL_VFLIP},
 	{USB_DEVICE(0x093a, 0x262a)},
diff --git a/drivers/media/video/gspca/pac7311.c b/drivers/media/video/gspca/pac7311.c
index 1ac111176ffa..2cb7d95f7be7 100644
--- a/drivers/media/video/gspca/pac7311.c
+++ b/drivers/media/video/gspca/pac7311.c
@@ -20,34 +20,42 @@
  */
 
 /* Some documentation about various registers as determined by trial and error.
-   When the register addresses differ between the 7202 and the 7311 the 2
-   different addresses are written as 7302addr/7311addr, when one of the 2
-   addresses is a - sign that register description is not valid for the
-   matching IC.
-
-   Register page 1:
-
-   Address	Description
-   -/0x08	Unknown compressor related, must always be 8 except when not
-		in 640x480 resolution and page 4 reg 2 <= 3 then set it to 9 !
-   -/0x1b	Auto white balance related, bit 0 is AWB enable (inverted)
-		bits 345 seem to toggle per color gains on/off (inverted)
-   0x78		Global control, bit 6 controls the LED (inverted)
-   -/0x80	JPEG compression ratio ? Best not touched
-
-   Register page 3/4:
-
-   Address	Description
-   0x02		Clock divider 2-63, fps =~ 60 / val. Must be a multiple of 3 on
-		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
-   -/0x0f	Master gain 1-245, low value = high gain
-   0x10/-	Master gain 0-31
-   -/0x10	Another gain 0-15, limited influence (1-2x gain I guess)
-   0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
-   -/0x27	Seems to toggle various gains on / off, Setting bit 7 seems to
-		completely disable the analog amplification block. Set to 0x68
-		for max gain, 0x14 for minimal gain.
-*/
+ *
+ * Register page 1:
+ *
+ * Address	Description
+ * 0x08		Unknown compressor related, must always be 8 except when not
+ *		in 640x480 resolution and page 4 reg 2 <= 3 then set it to 9 !
+ * 0x1b		Auto white balance related, bit 0 is AWB enable (inverted)
+ *		bits 345 seem to toggle per color gains on/off (inverted)
+ * 0x78		Global control, bit 6 controls the LED (inverted)
+ * 0x80		Compression balance, interesting settings:
+ *		0x01 Use this to allow the camera to switch to higher compr.
+ *		     on the fly. Needed to stay within bandwidth @ 640x480@30
+ *		0x1c From usb captures under Windows for 640x480
+ *		0x2a Values >= this switch the camera to a lower compression,
+ *		     using the same table for both luminance and chrominance.
+ *		     This gives a sharper picture. Usable only at 640x480@ <
+ *		     15 fps or 320x240 / 160x120. Note currently the driver
+ *		     does not use this as the quality gain is small and the
+ *		     generated JPG-s are only understood by v4l-utils >= 0.8.9
+ *		0x3f From usb captures under Windows for 320x240
+ *		0x69 From usb captures under Windows for 160x120
+ *
+ * Register page 4:
+ *
+ * Address	Description
+ * 0x02		Clock divider 2-63, fps =~ 60 / val. Must be a multiple of 3 on
+ *		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
+ * 0x0f		Master gain 1-245, low value = high gain
+ * 0x10		Another gain 0-15, limited influence (1-2x gain I guess)
+ * 0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
+ *		Note setting vflip disabled leads to a much lower image quality,
+ *		so we always vflip, and tell userspace to flip it back
+ * 0x27		Seems to toggle various gains on / off, Setting bit 7 seems to
+ *		completely disable the analog amplification block. Set to 0x68
+ *		for max gain, 0x14 for minimal gain.
+ */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
@@ -55,21 +63,21 @@
 
 #include <linux/input.h>
 #include "gspca.h"
+/* Include pac common sof detection functions */
+#include "pac_common.h"
+
+#define PAC7311_GAIN_DEFAULT     122
+#define PAC7311_EXPOSURE_DEFAULT   3 /* 20 fps, avoid using high compr. */
 
 MODULE_AUTHOR("Thomas Kaiser thomas@kaiser-linux.li");
 MODULE_DESCRIPTION("Pixart PAC7311");
 MODULE_LICENSE("GPL");
 
-/* specific webcam descriptor for pac7311 */
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	unsigned char contrast;
-	unsigned char gain;
-	unsigned char exposure;
-	unsigned char autogain;
-	__u8 hflip;
-	__u8 vflip;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hflip;
 
 	u8 sof_read;
 	u8 autogain_ignore_frames;
@@ -77,114 +85,6 @@ struct sd {
 	atomic_t avg_lum;
 };
 
-/* V4L2 controls supported by the driver */
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_sethflip(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_gethflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-/* This control is for both the 7302 and the 7311 */
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-#define CONTRAST_MAX 255
-		.maximum = CONTRAST_MAX,
-		.step    = 1,
-#define CONTRAST_DEF 127
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-/* All controls below are for both the 7302 and the 7311 */
-	{
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-#define GAIN_MAX 255
-		.maximum = GAIN_MAX,
-		.step    = 1,
-#define GAIN_DEF 127
-#define GAIN_KNEE 255 /* Gain seems to cause little noise on the pac73xx */
-		.default_value = GAIN_DEF,
-	    },
-	    .set = sd_setgain,
-	    .get = sd_getgain,
-	},
-	{
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-#define EXPOSURE_MAX 255
-		.maximum = EXPOSURE_MAX,
-		.step    = 1,
-#define EXPOSURE_DEF  16 /*  32 ms / 30 fps */
-#define EXPOSURE_KNEE 50 /* 100 ms / 10 fps */
-		.default_value = EXPOSURE_DEF,
-	    },
-	    .set = sd_setexposure,
-	    .get = sd_getexposure,
-	},
-	{
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define AUTOGAIN_DEF 1
-		.default_value = AUTOGAIN_DEF,
-	    },
-	    .set = sd_setautogain,
-	    .get = sd_getautogain,
-	},
-	{
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define HFLIP_DEF 0
-		.default_value = HFLIP_DEF,
-	    },
-	    .set = sd_sethflip,
-	    .get = sd_gethflip,
-	},
-	{
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vflip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define VFLIP_DEF 0
-		.default_value = VFLIP_DEF,
-	    },
-	    .set = sd_setvflip,
-	    .get = sd_getvflip,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_PJPG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -206,8 +106,8 @@ static const struct v4l2_pix_format vga_mode[] = {
 #define LOAD_PAGE4		254
 #define END_OF_SEQUENCE		0
 
-/* pac 7311 */
 static const __u8 init_7311[] = {
+	0xff, 0x01,
 	0x78, 0x40,	/* Bit_0=start stream, Bit_6=LED */
 	0x78, 0x40,	/* Bit_0=start stream, Bit_6=LED */
 	0x78, 0x44,	/* Bit_0=start stream, Bit_6=LED */
@@ -387,90 +287,73 @@ static void reg_w_var(struct gspca_dev *gspca_dev,
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct cam *cam;
+	struct cam *cam = &gspca_dev->cam;
 
-	cam = &gspca_dev->cam;
-
-	PDEBUG(D_CONF, "Find Sensor PAC7311");
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
+	cam->input_flags = V4L2_IN_ST_VFLIP;
 
-	sd->contrast = CONTRAST_DEF;
-	sd->gain = GAIN_DEF;
-	sd->exposure = EXPOSURE_DEF;
-	sd->autogain = AUTOGAIN_DEF;
-	sd->hflip = HFLIP_DEF;
-	sd->vflip = VFLIP_DEF;
 	return 0;
 }
 
-/* This function is used by pac7311 only */
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	reg_w(gspca_dev, 0xff, 0x04);
-	reg_w(gspca_dev, 0x10, sd->contrast >> 4);
+	reg_w(gspca_dev, 0x10, val);
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
 }
 
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int gain = GAIN_MAX - sd->gain;
-
-	if (gain < 1)
-		gain = 1;
-	else if (gain > 245)
-		gain = 245;
 	reg_w(gspca_dev, 0xff, 0x04);			/* page 4 */
 	reg_w(gspca_dev, 0x0e, 0x00);
-	reg_w(gspca_dev, 0x0f, gain);
+	reg_w(gspca_dev, 0x0f, gspca_dev->gain->maximum - val + 1);
 
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	__u8 reg;
-
-	/* register 2 of frame 3/4 contains the clock divider configuring the
-	   no fps according to the formula: 60 / reg. sd->exposure is the
-	   desired exposure time in ms. */
-	reg = 120 * sd->exposure / 1000;
-	if (reg < 2)
-		reg = 2;
-	else if (reg > 63)
-		reg = 63;
-
 	reg_w(gspca_dev, 0xff, 0x04);			/* page 4 */
-	reg_w(gspca_dev, 0x02, reg);
+	reg_w(gspca_dev, 0x02, val);
 
-	/* Page 1 register 8 must always be 0x08 except when not in
-	   640x480 mode and Page3/4 reg 2 <= 3 then it must be 9 */
+	/* load registers to sensor (Bit 0, auto clear) */
+	reg_w(gspca_dev, 0x11, 0x01);
+
+	/*
+	 * Page 1 register 8 must always be 0x08 except when not in
+	 *  640x480 mode and page 4 reg 2 <= 3 then it must be 9
+	 */
 	reg_w(gspca_dev, 0xff, 0x01);
-	if (gspca_dev->cam.cam_mode[(int)gspca_dev->curr_mode].priv &&
-			reg <= 3) {
+	if (gspca_dev->width != 640 && val <= 3)
 		reg_w(gspca_dev, 0x08, 0x09);
-	} else {
+	else
 		reg_w(gspca_dev, 0x08, 0x08);
-	}
+
+	/*
+	 * Page1 register 80 sets the compression balance, normally we
+	 * want / use 0x1c, but for 640x480@30fps we must allow the
+	 * camera to use higher compression or we may run out of
+	 * bandwidth.
+	 */
+	if (gspca_dev->width == 640 && val == 2)
+		reg_w(gspca_dev, 0x80, 0x01);
+	else
+		reg_w(gspca_dev, 0x80, 0x1c);
 
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
 }
 
-static void sethvflip(struct gspca_dev *gspca_dev)
+static void sethvflip(struct gspca_dev *gspca_dev, s32 hflip, s32 vflip)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	__u8 data;
 
 	reg_w(gspca_dev, 0xff, 0x04);			/* page 4 */
-	data = (sd->hflip ? 0x04 : 0x00) | (sd->vflip ? 0x08 : 0x00);
+	data = (hflip ? 0x04 : 0x00) |
+	       (vflip ? 0x08 : 0x00);
 	reg_w(gspca_dev, 0x21, data);
 
 	/* load registers to sensor (Bit 0, auto clear) */
@@ -484,6 +367,82 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return gspca_dev->usb_err;
 }
 
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_AUTOGAIN && ctrl->is_new && ctrl->val) {
+		/* when switching to autogain set defaults to make sure
+		   we are on a valid point of the autogain gain /
+		   exposure knee graph, and give this change time to
+		   take effect before doing autogain. */
+		gspca_dev->exposure->val    = PAC7311_EXPOSURE_DEFAULT;
+		gspca_dev->gain->val        = PAC7311_GAIN_DEFAULT;
+		sd->autogain_ignore_frames  = PAC_AUTOGAIN_IGNORE_FRAMES;
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (gspca_dev->exposure->is_new || (ctrl->is_new && ctrl->val))
+			setexposure(gspca_dev, gspca_dev->exposure->val);
+		if (gspca_dev->gain->is_new || (ctrl->is_new && ctrl->val))
+			setgain(gspca_dev, gspca_dev->gain->val);
+		break;
+	case V4L2_CID_HFLIP:
+		sethvflip(gspca_dev, sd->hflip->val, 1);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+/* this function is called at probe time */
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_CONTRAST, 0, 15, 1, 7);
+	gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_EXPOSURE, 2, 63, 1,
+					PAC7311_EXPOSURE_DEFAULT);
+	gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_GAIN, 0, 244, 1,
+					PAC7311_GAIN_DEFAULT);
+	sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
+	return 0;
+}
+
+/* -- start the camera -- */
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -492,19 +451,19 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	reg_w_var(gspca_dev, start_7311,
 		page4_7311, sizeof(page4_7311));
-	setcontrast(gspca_dev);
-	setgain(gspca_dev);
-	setexposure(gspca_dev);
-	sethvflip(gspca_dev);
+	setcontrast(gspca_dev, v4l2_ctrl_g_ctrl(sd->contrast));
+	setgain(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->gain));
+	setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure));
+	sethvflip(gspca_dev, v4l2_ctrl_g_ctrl(sd->hflip), 1);
 
 	/* set correct resolution */
 	switch (gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv) {
-	case 2:					/* 160x120 pac7311 */
+	case 2:					/* 160x120 */
 		reg_w(gspca_dev, 0xff, 0x01);
 		reg_w(gspca_dev, 0x17, 0x20);
 		reg_w(gspca_dev, 0x87, 0x10);
 		break;
-	case 1:					/* 320x240 pac7311 */
+	case 1:					/* 320x240 */
 		reg_w(gspca_dev, 0xff, 0x01);
 		reg_w(gspca_dev, 0x17, 0x30);
 		reg_w(gspca_dev, 0x87, 0x11);
@@ -541,14 +500,6 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x78, 0x44); /* Bit_0=start stream, Bit_6=LED */
 }
 
-/* called on streamoff with alt 0 and on disconnect for 7311 */
-static void sd_stop0(struct gspca_dev *gspca_dev)
-{
-}
-
-/* Include pac common sof detection functions */
-#include "pac_common.h"
-
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -558,13 +509,13 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	if (avg_lum == -1)
 		return;
 
-	desired_lum = 200;
+	desired_lum = 170;
 	deadzone = 20;
 
 	if (sd->autogain_ignore_frames > 0)
 		sd->autogain_ignore_frames--;
-	else if (gspca_auto_gain_n_exposure(gspca_dev, avg_lum, desired_lum,
-			deadzone, GAIN_KNEE, EXPOSURE_KNEE))
+	else if (gspca_coarse_grained_expo_autogain(gspca_dev, avg_lum,
+						    desired_lum, deadzone))
 		sd->autogain_ignore_frames = PAC_AUTOGAIN_IGNORE_FRAMES;
 }
 
@@ -628,10 +579,12 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	if (sof) {
 		int n, lum_offset, footer_length;
 
-		/* 6 bytes after the FF D9 EOF marker a number of lumination
-		   bytes are send corresponding to different parts of the
-		   image, the 14th and 15th byte after the EOF seem to
-		   correspond to the center of the image */
+		/*
+		 * 6 bytes after the FF D9 EOF marker a number of lumination
+		 * bytes are send corresponding to different parts of the
+		 * image, the 14th and 15th byte after the EOF seem to
+		 * correspond to the center of the image.
+		 */
 		lum_offset = 24 + sizeof pac_sof_marker;
 		footer_length = 26;
 
@@ -668,127 +621,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->contrast = val;
-	if (gspca_dev->streaming)
-		setcontrast(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->contrast;
-	return 0;
-}
-
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		setgain(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gain;
-	return 0;
-}
-
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->exposure = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->exposure;
-	return 0;
-}
-
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->autogain = val;
-	/* when switching to autogain set defaults to make sure
-	   we are on a valid point of the autogain gain /
-	   exposure knee graph, and give this change time to
-	   take effect before doing autogain. */
-	if (sd->autogain) {
-		sd->exposure = EXPOSURE_DEF;
-		sd->gain = GAIN_DEF;
-		if (gspca_dev->streaming) {
-			sd->autogain_ignore_frames =
-				PAC_AUTOGAIN_IGNORE_FRAMES;
-			setexposure(gspca_dev);
-			setgain(gspca_dev);
-		}
-	}
-
-	return gspca_dev->usb_err;
-}
-
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
-	return 0;
-}
-
-static int sd_sethflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->hflip = val;
-	if (gspca_dev->streaming)
-		sethvflip(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_gethflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->hflip;
-	return 0;
-}
-
-static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->vflip = val;
-	if (gspca_dev->streaming)
-		sethvflip(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->vflip;
-	return 0;
-}
-
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 			u8 *data,		/* interrupt packet data */
@@ -820,16 +652,13 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 }
 #endif
 
-/* sub-driver description for pac7311 */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
-	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
 	.dq_callback = do_autogain,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
diff --git a/drivers/media/video/gspca/sn9c20x.c b/drivers/media/video/gspca/sn9c20x.c
index 7e71aa2d2522..ad098202d7f0 100644
--- a/drivers/media/video/gspca/sn9c20x.c
+++ b/drivers/media/video/gspca/sn9c20x.c
@@ -59,35 +59,38 @@ MODULE_LICENSE("GPL");
 #define SENSOR_MT9M111	9
 #define SENSOR_MT9M112  10
 #define SENSOR_HV7131R	11
-#define SENSOR_MT9VPRB	20
+#define SENSOR_MT9VPRB	12
 
 /* camera flags */
 #define HAS_NO_BUTTON	0x1
 #define LED_REVERSE	0x2 /* some cameras unset gpio to turn on leds */
 #define FLIP_DETECT	0x4
 
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	SATURATION,
-	HUE,
-	GAMMA,
-	BLUE,
-	RED,
-	VFLIP,
-	HFLIP,
-	EXPOSURE,
-	GAIN,
-	AUTOGAIN,
-	QUALITY,
-	NCTRLS		/* number of controls */
-};
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;
 
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct { /* color control cluster */
+		struct v4l2_ctrl *brightness;
+		struct v4l2_ctrl *contrast;
+		struct v4l2_ctrl *saturation;
+		struct v4l2_ctrl *hue;
+	};
+	struct { /* blue/red balance control cluster */
+		struct v4l2_ctrl *blue;
+		struct v4l2_ctrl *red;
+	};
+	struct { /* h/vflip control cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+	struct v4l2_ctrl *gamma;
+	struct { /* autogain and exposure or gain control cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *jpegqual;
 
 	struct work_struct work;
 	struct workqueue_struct *work_thread;
@@ -105,6 +108,7 @@ struct sd {
 	u8 exposure_step;
 
 	u8 i2c_addr;
+	u8 i2c_intf;
 	u8 sensor;
 	u8 hstart;
 	u8 vstart;
@@ -166,175 +170,6 @@ static const struct dmi_system_id flip_dmi_table[] = {
 	{}
 };
 
-static void set_cmatrix(struct gspca_dev *gspca_dev);
-static void set_gamma(struct gspca_dev *gspca_dev);
-static void set_redblue(struct gspca_dev *gspca_dev);
-static void set_hvflip(struct gspca_dev *gspca_dev);
-static void set_exposure(struct gspca_dev *gspca_dev);
-static void set_gain(struct gspca_dev *gspca_dev);
-static void set_quality(struct gspca_dev *gspca_dev);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x7f
-	    },
-	    .set_control = set_cmatrix
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x7f
-	    },
-	    .set_control = set_cmatrix
-	},
-[SATURATION] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x7f
-	    },
-	    .set_control = set_cmatrix
-	},
-[HUE] = {
-	    {
-		.id      = V4L2_CID_HUE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Hue",
-		.minimum = -180,
-		.maximum = 180,
-		.step    = 1,
-		.default_value = 0
-	    },
-	    .set_control = set_cmatrix
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x10
-	    },
-	    .set_control = set_gamma
-	},
-[BLUE] = {
-	    {
-		.id	 = V4L2_CID_BLUE_BALANCE,
-		.type	 = V4L2_CTRL_TYPE_INTEGER,
-		.name	 = "Blue Balance",
-		.minimum = 0,
-		.maximum = 0x7f,
-		.step	 = 1,
-		.default_value = 0x28
-	    },
-	    .set_control = set_redblue
-	},
-[RED] = {
-	    {
-		.id	 = V4L2_CID_RED_BALANCE,
-		.type	 = V4L2_CTRL_TYPE_INTEGER,
-		.name	 = "Red Balance",
-		.minimum = 0,
-		.maximum = 0x7f,
-		.step	 = 1,
-		.default_value = 0x28
-	    },
-	    .set_control = set_redblue
-	},
-[HFLIP] = {
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Horizontal Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = set_hvflip
-	},
-[VFLIP] = {
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vertical Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = set_hvflip
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-		.maximum = 0x1780,
-		.step    = 1,
-		.default_value = 0x33,
-	    },
-	    .set_control = set_exposure
-	},
-[GAIN] = {
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-		.maximum = 28,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = set_gain
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Exposure",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	},
-[QUALITY] = {
-	    {
-		.id      = V4L2_CID_JPEG_COMPRESSION_QUALITY,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Compression Quality",
-#define QUALITY_MIN 50
-#define QUALITY_MAX 90
-#define QUALITY_DEF 80
-		.minimum = QUALITY_MIN,
-		.maximum = QUALITY_MAX,
-		.step    = 1,
-		.default_value = QUALITY_DEF,
-	    },
-	    .set_control = set_quality
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -747,7 +582,7 @@ static const s16 hsv_blue_y[] = {
 	4,   2,   0,  -1,  -3,  -5,  -7,  -9, -11
 };
 
-static u16 i2c_ident[] = {
+static const u16 i2c_ident[] = {
 	V4L2_IDENT_OV9650,
 	V4L2_IDENT_OV9655,
 	V4L2_IDENT_SOI968,
@@ -760,9 +595,10 @@ static u16 i2c_ident[] = {
 	V4L2_IDENT_MT9M111,
 	V4L2_IDENT_MT9M112,
 	V4L2_IDENT_HV7131R,
+[SENSOR_MT9VPRB] = V4L2_IDENT_UNKNOWN,
 };
 
-static u16 bridge_init[][2] = {
+static const u16 bridge_init[][2] = {
 	{0x1000, 0x78}, {0x1001, 0x40}, {0x1002, 0x1c},
 	{0x1020, 0x80}, {0x1061, 0x01}, {0x1067, 0x40},
 	{0x1068, 0x30}, {0x1069, 0x20},	{0x106a, 0x10},
@@ -786,7 +622,7 @@ static u16 bridge_init[][2] = {
 };
 
 /* Gain = (bit[3:0] / 16 + 1) * (bit[4] + 1) * (bit[5] + 1) * (bit[6] + 1) */
-static u8 ov_gain[] = {
+static const u8 ov_gain[] = {
 	0x00 /* 1x */, 0x04 /* 1.25x */, 0x08 /* 1.5x */, 0x0c /* 1.75x */,
 	0x10 /* 2x */, 0x12 /* 2.25x */, 0x14 /* 2.5x */, 0x16 /* 2.75x */,
 	0x18 /* 3x */, 0x1a /* 3.25x */, 0x1c /* 3.5x */, 0x1e /* 3.75x */,
@@ -798,7 +634,7 @@ static u8 ov_gain[] = {
 };
 
 /* Gain = (bit[8] + 1) * (bit[7] + 1) * (bit[6:0] * 0.03125) */
-static u16 micron1_gain[] = {
+static const u16 micron1_gain[] = {
 	/* 1x   1.25x   1.5x    1.75x */
 	0x0020, 0x0028, 0x0030, 0x0038,
 	/* 2x   2.25x   2.5x    2.75x */
@@ -819,7 +655,7 @@ static u16 micron1_gain[] = {
 
 /* mt9m001 sensor uses a different gain formula then other micron sensors */
 /* Gain = (bit[6] + 1) * (bit[5-0] * 0.125) */
-static u16 micron2_gain[] = {
+static const u16 micron2_gain[] = {
 	/* 1x   1.25x   1.5x    1.75x */
 	0x0008, 0x000a, 0x000c, 0x000e,
 	/* 2x   2.25x   2.5x    2.75x */
@@ -839,7 +675,7 @@ static u16 micron2_gain[] = {
 };
 
 /* Gain = .5 + bit[7:0] / 16 */
-static u8 hv7131r_gain[] = {
+static const u8 hv7131r_gain[] = {
 	0x08 /* 1x */, 0x0c /* 1.25x */, 0x10 /* 1.5x */, 0x14 /* 1.75x */,
 	0x18 /* 2x */, 0x1c /* 2.25x */, 0x20 /* 2.5x */, 0x24 /* 2.75x */,
 	0x28 /* 3x */, 0x2c /* 3.25x */, 0x30 /* 3.5x */, 0x34 /* 3.75x */,
@@ -850,7 +686,7 @@ static u8 hv7131r_gain[] = {
 	0x78 /* 8x */
 };
 
-static struct i2c_reg_u8 soi968_init[] = {
+static const struct i2c_reg_u8 soi968_init[] = {
 	{0x0c, 0x00}, {0x0f, 0x1f},
 	{0x11, 0x80}, {0x38, 0x52}, {0x1e, 0x00},
 	{0x33, 0x08}, {0x35, 0x8c}, {0x36, 0x0c},
@@ -864,7 +700,7 @@ static struct i2c_reg_u8 soi968_init[] = {
 	{0x00, 0x00}, {0x01, 0x80}, {0x02, 0x80},
 };
 
-static struct i2c_reg_u8 ov7660_init[] = {
+static const struct i2c_reg_u8 ov7660_init[] = {
 	{0x0e, 0x80}, {0x0d, 0x08}, {0x0f, 0xc3},
 	{0x04, 0xc3}, {0x10, 0x40}, {0x11, 0x40},
 	{0x12, 0x05}, {0x13, 0xba}, {0x14, 0x2a},
@@ -872,11 +708,11 @@ static struct i2c_reg_u8 ov7660_init[] = {
 	   0x10, 0x61 and sd->hstart, vstart = 3, fixes ugly colored borders */
 	{0x17, 0x10}, {0x18, 0x61},
 	{0x37, 0x0f}, {0x38, 0x02}, {0x39, 0x43},
-	{0x3a, 0x00}, {0x69, 0x90}, {0x2d, 0xf6},
-	{0x2e, 0x0b}, {0x01, 0x78}, {0x02, 0x50},
+	{0x3a, 0x00}, {0x69, 0x90}, {0x2d, 0x00},
+	{0x2e, 0x00}, {0x01, 0x78}, {0x02, 0x50},
 };
 
-static struct i2c_reg_u8 ov7670_init[] = {
+static const struct i2c_reg_u8 ov7670_init[] = {
 	{0x11, 0x80}, {0x3a, 0x04}, {0x12, 0x01},
 	{0x32, 0xb6}, {0x03, 0x0a}, {0x0c, 0x00}, {0x3e, 0x00},
 	{0x70, 0x3a}, {0x71, 0x35}, {0x72, 0x11}, {0x73, 0xf0},
@@ -933,7 +769,7 @@ static struct i2c_reg_u8 ov7670_init[] = {
 	{0x93, 0x00},
 };
 
-static struct i2c_reg_u8 ov9650_init[] = {
+static const struct i2c_reg_u8 ov9650_init[] = {
 	{0x00, 0x00}, {0x01, 0x78},
 	{0x02, 0x78}, {0x03, 0x36}, {0x04, 0x03},
 	{0x05, 0x00}, {0x06, 0x00}, {0x08, 0x00},
@@ -963,7 +799,7 @@ static struct i2c_reg_u8 ov9650_init[] = {
 	{0xaa, 0x92}, {0xab, 0x0a},
 };
 
-static struct i2c_reg_u8 ov9655_init[] = {
+static const struct i2c_reg_u8 ov9655_init[] = {
 	{0x0e, 0x61}, {0x11, 0x80}, {0x13, 0xba},
 	{0x14, 0x2e}, {0x16, 0x24}, {0x1e, 0x04}, {0x27, 0x08},
 	{0x28, 0x08}, {0x29, 0x15}, {0x2c, 0x08}, {0x34, 0x3d},
@@ -990,7 +826,7 @@ static struct i2c_reg_u8 ov9655_init[] = {
 	{0x04, 0x03}, {0x00, 0x13},
 };
 
-static struct i2c_reg_u16 mt9v112_init[] = {
+static const struct i2c_reg_u16 mt9v112_init[] = {
 	{0xf0, 0x0000}, {0x0d, 0x0021}, {0x0d, 0x0020},
 	{0x34, 0xc019}, {0x0a, 0x0011}, {0x0b, 0x000b},
 	{0x20, 0x0703}, {0x35, 0x2022}, {0xf0, 0x0001},
@@ -1009,7 +845,7 @@ static struct i2c_reg_u16 mt9v112_init[] = {
 	{0x2c, 0x00ae}, {0x2d, 0x00ae}, {0x2e, 0x00ae},
 };
 
-static struct i2c_reg_u16 mt9v111_init[] = {
+static const struct i2c_reg_u16 mt9v111_init[] = {
 	{0x01, 0x0004}, {0x0d, 0x0001}, {0x0d, 0x0000},
 	{0x01, 0x0001}, {0x05, 0x0004}, {0x2d, 0xe0a0},
 	{0x2e, 0x0c64},	{0x2f, 0x0064}, {0x06, 0x600e},
@@ -1019,7 +855,7 @@ static struct i2c_reg_u16 mt9v111_init[] = {
 	{0x0e, 0x0008}, {0x20, 0x0000}
 };
 
-static struct i2c_reg_u16 mt9v011_init[] = {
+static const struct i2c_reg_u16 mt9v011_init[] = {
 	{0x07, 0x0002},	{0x0d, 0x0001},	{0x0d, 0x0000},
 	{0x01, 0x0008},	{0x02, 0x0016},	{0x03, 0x01e1},
 	{0x04, 0x0281},	{0x05, 0x0083},	{0x06, 0x0006},
@@ -1046,7 +882,7 @@ static struct i2c_reg_u16 mt9v011_init[] = {
 	{0x06, 0x0029},	{0x05, 0x0009},
 };
 
-static struct i2c_reg_u16 mt9m001_init[] = {
+static const struct i2c_reg_u16 mt9m001_init[] = {
 	{0x0d, 0x0001},
 	{0x0d, 0x0000},
 	{0x04, 0x0500},		/* hres = 1280 */
@@ -1062,21 +898,21 @@ static struct i2c_reg_u16 mt9m001_init[] = {
 	{0x35, 0x0057},
 };
 
-static struct i2c_reg_u16 mt9m111_init[] = {
+static const struct i2c_reg_u16 mt9m111_init[] = {
 	{0xf0, 0x0000}, {0x0d, 0x0021}, {0x0d, 0x0008},
 	{0xf0, 0x0001}, {0x3a, 0x4300}, {0x9b, 0x4300},
 	{0x06, 0x708e}, {0xf0, 0x0002}, {0x2e, 0x0a1e},
 	{0xf0, 0x0000},
 };
 
-static struct i2c_reg_u16 mt9m112_init[] = {
+static const struct i2c_reg_u16 mt9m112_init[] = {
 	{0xf0, 0x0000}, {0x0d, 0x0021}, {0x0d, 0x0008},
 	{0xf0, 0x0001}, {0x3a, 0x4300}, {0x9b, 0x4300},
 	{0x06, 0x708e}, {0xf0, 0x0002}, {0x2e, 0x0a1e},
 	{0xf0, 0x0000},
 };
 
-static struct i2c_reg_u8 hv7131r_init[] = {
+static const struct i2c_reg_u8 hv7131r_init[] = {
 	{0x02, 0x08}, {0x02, 0x00}, {0x01, 0x08},
 	{0x02, 0x00}, {0x20, 0x00}, {0x21, 0xd0},
 	{0x22, 0x00}, {0x23, 0x09}, {0x01, 0x08},
@@ -1167,7 +1003,7 @@ static void i2c_w1(struct gspca_dev *gspca_dev, u8 reg, u8 val)
 	 * from the point of view of the bridge, the length
 	 * includes the address
 	 */
-	row[0] = 0x81 | (2 << 4);
+	row[0] = sd->i2c_intf | (2 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = val;
@@ -1180,7 +1016,7 @@ static void i2c_w1(struct gspca_dev *gspca_dev, u8 reg, u8 val)
 }
 
 static void i2c_w1_buf(struct gspca_dev *gspca_dev,
-			struct i2c_reg_u8 *buf, int sz)
+			const struct i2c_reg_u8 *buf, int sz)
 {
 	while (--sz >= 0) {
 		i2c_w1(gspca_dev, buf->reg, buf->val);
@@ -1197,7 +1033,7 @@ static void i2c_w2(struct gspca_dev *gspca_dev, u8 reg, u16 val)
 	 * from the point of view of the bridge, the length
 	 * includes the address
 	 */
-	row[0] = 0x81 | (3 << 4);
+	row[0] = sd->i2c_intf | (3 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = val >> 8;
@@ -1210,7 +1046,7 @@ static void i2c_w2(struct gspca_dev *gspca_dev, u8 reg, u16 val)
 }
 
 static void i2c_w2_buf(struct gspca_dev *gspca_dev,
-			struct i2c_reg_u16 *buf, int sz)
+			const struct i2c_reg_u16 *buf, int sz)
 {
 	while (--sz >= 0) {
 		i2c_w2(gspca_dev, buf->reg, buf->val);
@@ -1223,7 +1059,7 @@ static void i2c_r1(struct gspca_dev *gspca_dev, u8 reg, u8 *val)
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 row[8];
 
-	row[0] = 0x81 | (1 << 4);
+	row[0] = sd->i2c_intf | (1 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = 0;
@@ -1232,7 +1068,7 @@ static void i2c_r1(struct gspca_dev *gspca_dev, u8 reg, u8 *val)
 	row[6] = 0;
 	row[7] = 0x10;
 	i2c_w(gspca_dev, row);
-	row[0] = 0x81 | (1 << 4) | 0x02;
+	row[0] = sd->i2c_intf | (1 << 4) | 0x02;
 	row[2] = 0;
 	i2c_w(gspca_dev, row);
 	reg_r(gspca_dev, 0x10c2, 5);
@@ -1244,7 +1080,7 @@ static void i2c_r2(struct gspca_dev *gspca_dev, u8 reg, u16 *val)
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 row[8];
 
-	row[0] = 0x81 | (1 << 4);
+	row[0] = sd->i2c_intf | (1 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = 0;
@@ -1253,7 +1089,7 @@ static void i2c_r2(struct gspca_dev *gspca_dev, u8 reg, u16 *val)
 	row[6] = 0;
 	row[7] = 0x10;
 	i2c_w(gspca_dev, row);
-	row[0] = 0x81 | (2 << 4) | 0x02;
+	row[0] = sd->i2c_intf | (2 << 4) | 0x02;
 	row[2] = 0;
 	i2c_w(gspca_dev, row);
 	reg_r(gspca_dev, 0x10c2, 5);
@@ -1294,8 +1130,6 @@ static void ov9655_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("OV9655 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP);
 	sd->hstart = 1;
 	sd->vstart = 2;
 }
@@ -1310,9 +1144,6 @@ static void soi968_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("SOI968 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP)
-				| (1 << EXPOSURE);
 	sd->hstart = 60;
 	sd->vstart = 11;
 }
@@ -1340,8 +1171,6 @@ static void ov7670_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("OV7670 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP);
 	sd->hstart = 0;
 	sd->vstart = 1;
 }
@@ -1378,9 +1207,6 @@ static void mt9v_init_sensor(struct gspca_dev *gspca_dev)
 			pr_err("MT9V111 sensor initialization failed\n");
 			return;
 		}
-		gspca_dev->ctrl_dis = (1 << EXPOSURE)
-					| (1 << AUTOGAIN)
-					| (1 << GAIN);
 		sd->hstart = 2;
 		sd->vstart = 2;
 		sd->sensor = SENSOR_MT9V111;
@@ -1422,8 +1248,6 @@ static void mt9m112_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("MT9M112 sensor initialization failed\n");
 
-	gspca_dev->ctrl_dis = (1 << EXPOSURE) | (1 << AUTOGAIN)
-				| (1 << GAIN);
 	sd->hstart = 0;
 	sd->vstart = 2;
 }
@@ -1436,8 +1260,6 @@ static void mt9m111_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("MT9M111 sensor initialization failed\n");
 
-	gspca_dev->ctrl_dis = (1 << EXPOSURE) | (1 << AUTOGAIN)
-				| (1 << GAIN);
 	sd->hstart = 0;
 	sd->vstart = 2;
 }
@@ -1470,8 +1292,6 @@ static void mt9m001_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("MT9M001 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP);
 	sd->hstart = 1;
 	sd->vstart = 1;
 }
@@ -1488,20 +1308,18 @@ static void hv7131r_init_sensor(struct gspca_dev *gspca_dev)
 	sd->vstart = 1;
 }
 
-static void set_cmatrix(struct gspca_dev *gspca_dev)
+static void set_cmatrix(struct gspca_dev *gspca_dev,
+		s32 brightness, s32 contrast, s32 satur, s32 hue)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int satur;
-	s32 hue_coord, hue_index = 180 + sd->ctrls[HUE].val;
+	s32 hue_coord, hue_index = 180 + hue;
 	u8 cmatrix[21];
 
 	memset(cmatrix, 0, sizeof cmatrix);
-	cmatrix[2] = (sd->ctrls[CONTRAST].val * 0x25 / 0x100) + 0x26;
+	cmatrix[2] = (contrast * 0x25 / 0x100) + 0x26;
 	cmatrix[0] = 0x13 + (cmatrix[2] - 0x26) * 0x13 / 0x25;
 	cmatrix[4] = 0x07 + (cmatrix[2] - 0x26) * 0x07 / 0x25;
-	cmatrix[18] = sd->ctrls[BRIGHTNESS].val - 0x80;
+	cmatrix[18] = brightness - 0x80;
 
-	satur = sd->ctrls[SATURATION].val;
 	hue_coord = (hsv_red_x[hue_index] * satur) >> 8;
 	cmatrix[6] = hue_coord;
 	cmatrix[7] = (hue_coord >> 8) & 0x0f;
@@ -1529,11 +1347,10 @@ static void set_cmatrix(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x10e1, cmatrix, 21);
 }
 
-static void set_gamma(struct gspca_dev *gspca_dev)
+static void set_gamma(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 gamma[17];
-	u8 gval = sd->ctrls[GAMMA].val * 0xb8 / 0x100;
+	u8 gval = val * 0xb8 / 0x100;
 
 	gamma[0] = 0x0a;
 	gamma[1] = 0x13 + (gval * (0xcb - 0x13) / 0xb8);
@@ -1556,26 +1373,21 @@ static void set_gamma(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x1190, gamma, 17);
 }
 
-static void set_redblue(struct gspca_dev *gspca_dev)
+static void set_redblue(struct gspca_dev *gspca_dev, s32 blue, s32 red)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_w1(gspca_dev, 0x118c, sd->ctrls[RED].val);
-	reg_w1(gspca_dev, 0x118f, sd->ctrls[BLUE].val);
+	reg_w1(gspca_dev, 0x118c, red);
+	reg_w1(gspca_dev, 0x118f, blue);
 }
 
-static void set_hvflip(struct gspca_dev *gspca_dev)
+static void set_hvflip(struct gspca_dev *gspca_dev, s32 hflip, s32 vflip)
 {
-	u8 value, tslb, hflip, vflip;
+	u8 value, tslb;
 	u16 value2;
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if ((sd->flags & FLIP_DETECT) && dmi_check_system(flip_dmi_table)) {
-		hflip = !sd->ctrls[HFLIP].val;
-		vflip = !sd->ctrls[VFLIP].val;
-	} else {
-		hflip = sd->ctrls[HFLIP].val;
-		vflip = sd->ctrls[VFLIP].val;
+		hflip = !hflip;
+		vflip = !vflip;
 	}
 
 	switch (sd->sensor) {
@@ -1638,20 +1450,38 @@ static void set_hvflip(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void set_exposure(struct gspca_dev *gspca_dev)
+static void set_exposure(struct gspca_dev *gspca_dev, s32 expo)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 exp[8] = {0x81, sd->i2c_addr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e};
-	int expo;
+	u8 exp[8] = {sd->i2c_intf, sd->i2c_addr,
+				0x00, 0x00, 0x00, 0x00, 0x00, 0x10};
+	int expo2;
+
+	if (gspca_dev->streaming)
+		exp[7] = 0x1e;
 
-	expo = sd->ctrls[EXPOSURE].val;
 	switch (sd->sensor) {
 	case SENSOR_OV7660:
 	case SENSOR_OV7670:
 	case SENSOR_OV9655:
 	case SENSOR_OV9650:
+		if (expo > 547)
+			expo2 = 547;
+		else
+			expo2 = expo;
+		exp[0] |= (2 << 4);
+		exp[2] = 0x10;			/* AECH */
+		exp[3] = expo2 >> 2;
+		exp[7] = 0x10;
+		i2c_w(gspca_dev, exp);
+		exp[2] = 0x04;			/* COM1 */
+		exp[3] = expo2 & 0x0003;
+		exp[7] = 0x10;
+		i2c_w(gspca_dev, exp);
+		expo -= expo2;
+		exp[7] = 0x1e;
 		exp[0] |= (3 << 4);
-		exp[2] = 0x2d;
+		exp[2] = 0x2d;			/* ADVFL & ADVFH */
 		exp[3] = expo;
 		exp[4] = expo >> 8;
 		break;
@@ -1676,13 +1506,15 @@ static void set_exposure(struct gspca_dev *gspca_dev)
 	i2c_w(gspca_dev, exp);
 }
 
-static void set_gain(struct gspca_dev *gspca_dev)
+static void set_gain(struct gspca_dev *gspca_dev, s32 g)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 gain[8] = {0x81, sd->i2c_addr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d};
-	int g;
+	u8 gain[8] = {sd->i2c_intf, sd->i2c_addr,
+				0x00, 0x00, 0x00, 0x00, 0x00, 0x10};
+
+	if (gspca_dev->streaming)
+		gain[7] = 0x15;		/* or 1d ? */
 
-	g = sd->ctrls[GAIN].val;
 	switch (sd->sensor) {
 	case SENSOR_OV7660:
 	case SENSOR_OV7670:
@@ -1721,11 +1553,11 @@ static void set_gain(struct gspca_dev *gspca_dev)
 	i2c_w(gspca_dev, gain);
 }
 
-static void set_quality(struct gspca_dev *gspca_dev)
+static void set_quality(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	jpeg_set_qual(sd->jpeg_hdr, sd->ctrls[QUALITY].val);
+	jpeg_set_qual(sd->jpeg_hdr, val);
 	reg_w1(gspca_dev, 0x1061, 0x01);	/* stop transfer */
 	reg_w1(gspca_dev, 0x10e0, sd->fmt | 0x20); /* write QTAB */
 	reg_w(gspca_dev, 0x1100, &sd->jpeg_hdr[JPEG_QT0_OFFSET], 64);
@@ -1827,6 +1659,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	sd->sensor = id->driver_info >> 8;
 	sd->i2c_addr = id->driver_info;
 	sd->flags = id->driver_info >> 16;
+	sd->i2c_intf = 0x80;			/* i2c 100 Kb/s */
 
 	switch (sd->sensor) {
 	case SENSOR_MT9M112:
@@ -1840,6 +1673,9 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->cam_mode = mono_mode;
 		cam->nmodes = ARRAY_SIZE(mono_mode);
 		break;
+	case SENSOR_HV7131R:
+		sd->i2c_intf = 0x81;			/* i2c 400 Kb/s */
+		/* fall thru */
 	default:
 		cam->cam_mode = vga_mode;
 		cam->nmodes = ARRAY_SIZE(vga_mode);
@@ -1850,13 +1686,133 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	sd->older_step = 0;
 	sd->exposure_step = 16;
 
-	gspca_dev->cam.ctrls = sd->ctrls;
-
 	INIT_WORK(&sd->work, qual_upd);
 
 	return 0;
 }
 
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	/* color control cluster */
+	case V4L2_CID_BRIGHTNESS:
+		set_cmatrix(gspca_dev, sd->brightness->val,
+			sd->contrast->val, sd->saturation->val, sd->hue->val);
+		break;
+	case V4L2_CID_GAMMA:
+		set_gamma(gspca_dev, ctrl->val);
+		break;
+	/* blue/red balance cluster */
+	case V4L2_CID_BLUE_BALANCE:
+		set_redblue(gspca_dev, sd->blue->val, sd->red->val);
+		break;
+	/* h/vflip cluster */
+	case V4L2_CID_HFLIP:
+		set_hvflip(gspca_dev, sd->hflip->val, sd->vflip->val);
+		break;
+	/* standalone exposure control */
+	case V4L2_CID_EXPOSURE:
+		set_exposure(gspca_dev, ctrl->val);
+		break;
+	/* standalone gain control */
+	case V4L2_CID_GAIN:
+		set_gain(gspca_dev, ctrl->val);
+		break;
+	/* autogain + exposure or gain control cluster */
+	case V4L2_CID_AUTOGAIN:
+		if (sd->sensor == SENSOR_SOI968)
+			set_gain(gspca_dev, sd->gain->val);
+		else
+			set_exposure(gspca_dev, sd->exposure->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		set_quality(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 13);
+
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 127);
+	sd->saturation = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 127);
+	sd->hue = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HUE, -180, 180, 1, 0);
+	v4l2_ctrl_cluster(4, &sd->brightness);
+
+	sd->gamma = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAMMA, 0, 255, 1, 0x10);
+
+	sd->blue = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 0, 127, 1, 0x28);
+	sd->red = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 0, 127, 1, 0x28);
+	v4l2_ctrl_cluster(2, &sd->blue);
+
+	if (sd->sensor != SENSOR_OV9655 && sd->sensor != SENSOR_SOI968 &&
+	    sd->sensor != SENSOR_OV7670 && sd->sensor != SENSOR_MT9M001 &&
+	    sd->sensor != SENSOR_MT9VPRB) {
+		sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+		sd->vflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+		v4l2_ctrl_cluster(2, &sd->hflip);
+	}
+
+	if (sd->sensor != SENSOR_SOI968 && sd->sensor != SENSOR_MT9VPRB &&
+	    sd->sensor != SENSOR_MT9M112 && sd->sensor != SENSOR_MT9M111 &&
+	    sd->sensor != SENSOR_MT9V111)
+		sd->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 0x1780, 1, 0x33);
+
+	if (sd->sensor != SENSOR_MT9VPRB && sd->sensor != SENSOR_MT9M112 &&
+	    sd->sensor != SENSOR_MT9M111 && sd->sensor != SENSOR_MT9V111) {
+		sd->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 28, 1, 0);
+		sd->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		if (sd->sensor == SENSOR_SOI968)
+			/* this sensor doesn't have the exposure control and
+			   autogain is clustered with gain instead. This works
+			   because sd->exposure == NULL. */
+			v4l2_ctrl_auto_cluster(3, &sd->autogain, 0, false);
+		else
+			/* Otherwise autogain is clustered with exposure. */
+			v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, false);
+	}
+
+	sd->jpegqual = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 50, 90, 1, 80);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	return 0;
+}
+
 static int sd_init(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -1949,7 +1905,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		pr_err("Unsupported sensor\n");
 		gspca_dev->usb_err = -ENODEV;
 	}
-
 	return gspca_dev->usb_err;
 }
 
@@ -2025,8 +1980,8 @@ static int sd_isoc_init(struct gspca_dev *gspca_dev)
 
 		if (intf->num_altsetting != 9) {
 			pr_warn("sn9c20x camera with unknown number of alt "
-			        "settings (%d), please report!\n",
-			        intf->num_altsetting);
+				"settings (%d), please report!\n",
+				intf->num_altsetting);
 			gspca_dev->alt = intf->num_altsetting;
 			return 0;
 		}
@@ -2067,7 +2022,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	jpeg_define(sd->jpeg_hdr, height, width,
 			0x21);
-	jpeg_set_qual(sd->jpeg_hdr, sd->ctrls[QUALITY].val);
+	jpeg_set_qual(sd->jpeg_hdr, v4l2_ctrl_g_ctrl(sd->jpegqual));
 
 	if (mode & MODE_RAW)
 		fmt = 0x2d;
@@ -2104,12 +2059,17 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	reg_w1(gspca_dev, 0x1189, scale);
 	reg_w1(gspca_dev, 0x10e0, fmt);
 
-	set_cmatrix(gspca_dev);
-	set_gamma(gspca_dev);
-	set_redblue(gspca_dev);
-	set_gain(gspca_dev);
-	set_exposure(gspca_dev);
-	set_hvflip(gspca_dev);
+	set_cmatrix(gspca_dev, v4l2_ctrl_g_ctrl(sd->brightness),
+			v4l2_ctrl_g_ctrl(sd->contrast),
+			v4l2_ctrl_g_ctrl(sd->saturation),
+			v4l2_ctrl_g_ctrl(sd->hue));
+	set_gamma(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma));
+	set_redblue(gspca_dev, v4l2_ctrl_g_ctrl(sd->blue),
+			v4l2_ctrl_g_ctrl(sd->red));
+	set_gain(gspca_dev, v4l2_ctrl_g_ctrl(sd->gain));
+	set_exposure(gspca_dev, v4l2_ctrl_g_ctrl(sd->exposure));
+	set_hvflip(gspca_dev, v4l2_ctrl_g_ctrl(sd->hflip),
+			v4l2_ctrl_g_ctrl(sd->vflip));
 
 	reg_w1(gspca_dev, 0x1007, 0x20);
 	reg_w1(gspca_dev, 0x1061, 0x03);
@@ -2148,6 +2108,9 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	s32 cur_exp = v4l2_ctrl_g_ctrl(sd->exposure);
+	s32 max = sd->exposure->maximum - sd->exposure_step;
+	s32 min = sd->exposure->minimum + sd->exposure_step;
 	s16 new_exp;
 
 	/*
@@ -2156,16 +2119,15 @@ static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 	 * and exposure steps
 	 */
 	if (avg_lum < MIN_AVG_LUM) {
-		if (sd->ctrls[EXPOSURE].val > 0x1770)
+		if (cur_exp > max)
 			return;
 
-		new_exp = sd->ctrls[EXPOSURE].val + sd->exposure_step;
-		if (new_exp > 0x1770)
-			new_exp = 0x1770;
-		if (new_exp < 0x10)
-			new_exp = 0x10;
-		sd->ctrls[EXPOSURE].val = new_exp;
-		set_exposure(gspca_dev);
+		new_exp = cur_exp + sd->exposure_step;
+		if (new_exp > max)
+			new_exp = max;
+		if (new_exp < min)
+			new_exp = min;
+		v4l2_ctrl_s_ctrl(sd->exposure, new_exp);
 
 		sd->older_step = sd->old_step;
 		sd->old_step = 1;
@@ -2176,15 +2138,14 @@ static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 			sd->exposure_step += 2;
 	}
 	if (avg_lum > MAX_AVG_LUM) {
-		if (sd->ctrls[EXPOSURE].val < 0x10)
+		if (cur_exp < min)
 			return;
-		new_exp = sd->ctrls[EXPOSURE].val - sd->exposure_step;
-		if (new_exp > 0x1700)
-			new_exp = 0x1770;
-		if (new_exp < 0x10)
-			new_exp = 0x10;
-		sd->ctrls[EXPOSURE].val = new_exp;
-		set_exposure(gspca_dev);
+		new_exp = cur_exp - sd->exposure_step;
+		if (new_exp > max)
+			new_exp = max;
+		if (new_exp < min)
+			new_exp = min;
+		v4l2_ctrl_s_ctrl(sd->exposure, new_exp);
 		sd->older_step = sd->old_step;
 		sd->old_step = 0;
 
@@ -2198,19 +2159,12 @@ static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 static void do_autogain(struct gspca_dev *gspca_dev, u16 avg_lum)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	s32 cur_gain = v4l2_ctrl_g_ctrl(sd->gain);
 
-	if (avg_lum < MIN_AVG_LUM) {
-		if (sd->ctrls[GAIN].val + 1 <= 28) {
-			sd->ctrls[GAIN].val++;
-			set_gain(gspca_dev);
-		}
-	}
-	if (avg_lum > MAX_AVG_LUM) {
-		if (sd->ctrls[GAIN].val > 0) {
-			sd->ctrls[GAIN].val--;
-			set_gain(gspca_dev);
-		}
-	}
+	if (avg_lum < MIN_AVG_LUM && cur_gain < sd->gain->maximum)
+		v4l2_ctrl_s_ctrl(sd->gain, cur_gain + 1);
+	if (avg_lum > MAX_AVG_LUM && cur_gain > sd->gain->minimum)
+		v4l2_ctrl_s_ctrl(sd->gain, cur_gain - 1);
 }
 
 static void sd_dqcallback(struct gspca_dev *gspca_dev)
@@ -2218,7 +2172,7 @@ static void sd_dqcallback(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	int avg_lum;
 
-	if (!sd->ctrls[AUTOGAIN].val)
+	if (!v4l2_ctrl_g_ctrl(sd->autogain))
 		return;
 
 	avg_lum = atomic_read(&sd->avg_lum);
@@ -2234,10 +2188,11 @@ static void qual_upd(struct work_struct *work)
 {
 	struct sd *sd = container_of(work, struct sd, work);
 	struct gspca_dev *gspca_dev = &sd->gspca_dev;
+	s32 qual = v4l2_ctrl_g_ctrl(sd->jpegqual);
 
 	mutex_lock(&gspca_dev->usb_lock);
-	PDEBUG(D_STREAM, "qual_upd %d%%", sd->ctrls[QUALITY].val);
-	set_quality(gspca_dev);
+	PDEBUG(D_STREAM, "qual_upd %d%%", qual);
+	set_quality(gspca_dev, qual);
 	mutex_unlock(&gspca_dev->usb_lock);
 }
 
@@ -2286,14 +2241,18 @@ static void transfer_check(struct gspca_dev *gspca_dev,
 	if (new_qual != 0) {
 		sd->nchg += new_qual;
 		if (sd->nchg < -6 || sd->nchg >= 12) {
+			/* Note: we are in interrupt context, so we can't
+			   use v4l2_ctrl_g/s_ctrl here. Access the value
+			   directly instead. */
+			s32 curqual = sd->jpegqual->cur.val;
 			sd->nchg = 0;
-			new_qual += sd->ctrls[QUALITY].val;
-			if (new_qual < QUALITY_MIN)
-				new_qual = QUALITY_MIN;
-			else if (new_qual > QUALITY_MAX)
-				new_qual = QUALITY_MAX;
-			if (new_qual != sd->ctrls[QUALITY].val) {
-				sd->ctrls[QUALITY].val = new_qual;
+			new_qual += curqual;
+			if (new_qual < sd->jpegqual->minimum)
+				new_qual = sd->jpegqual->minimum;
+			else if (new_qual > sd->jpegqual->maximum)
+				new_qual = sd->jpegqual->maximum;
+			if (new_qual != curqual) {
+				sd->jpegqual->cur.val = new_qual;
 				queue_work(sd->work_thread, &sd->work);
 			}
 		}
@@ -2309,7 +2268,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int avg_lum, is_jpeg;
-	static u8 frame_header[] =
+	static const u8 frame_header[] =
 		{0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96};
 
 	is_jpeg = (sd->fmt & 0x03) == 0;
@@ -2373,10 +2332,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = KBUILD_MODNAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init = sd_isoc_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
diff --git a/drivers/media/video/gspca/sonixb.c b/drivers/media/video/gspca/sonixb.c
index 6a1148d7fe92..e2bdf8f632f4 100644
--- a/drivers/media/video/gspca/sonixb.c
+++ b/drivers/media/video/gspca/sonixb.c
@@ -1000,6 +1000,8 @@ static void setfreq(struct gspca_dev *gspca_dev)
 	}
 }
 
+#define WANT_REGULAR_AUTOGAIN
+#define WANT_COARSE_EXPO_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
diff --git a/drivers/media/video/gspca/sonixj.c b/drivers/media/video/gspca/sonixj.c
index 863c755dd2b7..4d1696d1a7f4 100644
--- a/drivers/media/video/gspca/sonixj.c
+++ b/drivers/media/video/gspca/sonixj.c
@@ -2800,10 +2800,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	}
 }
 
-/* !! coarse_grained_expo_autogain is not used !! */
-#define exp_too_low_cnt bridge
-#define exp_too_high_cnt sensor
-
+#define WANT_REGULAR_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
diff --git a/drivers/media/video/gspca/sq905.c b/drivers/media/video/gspca/sq905.c
index 2fe3c29bd6b7..04f54654a026 100644
--- a/drivers/media/video/gspca/sq905.c
+++ b/drivers/media/video/gspca/sq905.c
@@ -232,7 +232,11 @@ static void sq905_dostream(struct work_struct *work)
 	frame_sz = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].sizeimage
 			+ FRAME_HEADER_LEN;
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		/* request some data and then read it until we have
 		 * a complete frame. */
 		bytes_left = frame_sz;
@@ -242,7 +246,7 @@ static void sq905_dostream(struct work_struct *work)
 		   we must finish reading an entire frame, otherwise the
 		   next time we stream we start reading in the middle of a
 		   frame. */
-		while (bytes_left > 0 && gspca_dev->present) {
+		while (bytes_left > 0 && gspca_dev->dev) {
 			data_len = bytes_left > SQ905_MAX_TRANSFER ?
 				SQ905_MAX_TRANSFER : bytes_left;
 			ret = sq905_read_data(gspca_dev, buffer, data_len, 1);
@@ -274,7 +278,7 @@ static void sq905_dostream(struct work_struct *work)
 				gspca_frame_add(gspca_dev, LAST_PACKET,
 						NULL, 0);
 		}
-		if (gspca_dev->present) {
+		if (gspca_dev->dev) {
 			/* acknowledge the frame */
 			mutex_lock(&gspca_dev->usb_lock);
 			ret = sq905_ack_frame(gspca_dev);
@@ -284,7 +288,7 @@ static void sq905_dostream(struct work_struct *work)
 		}
 	}
 quit_stream:
-	if (gspca_dev->present) {
+	if (gspca_dev->dev) {
 		mutex_lock(&gspca_dev->usb_lock);
 		sq905_command(gspca_dev, SQ905_CLEAR);
 		mutex_unlock(&gspca_dev->usb_lock);
diff --git a/drivers/media/video/gspca/sq905c.c b/drivers/media/video/gspca/sq905c.c
index ae783634712f..f34ddb0570c8 100644
--- a/drivers/media/video/gspca/sq905c.c
+++ b/drivers/media/video/gspca/sq905c.c
@@ -150,7 +150,11 @@ static void sq905c_dostream(struct work_struct *work)
 		goto quit_stream;
 	}
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		/* Request the header, which tells the size to download */
 		ret = usb_bulk_msg(gspca_dev->dev,
 				usb_rcvbulkpipe(gspca_dev->dev, 0x81),
@@ -169,7 +173,7 @@ static void sq905c_dostream(struct work_struct *work)
 		packet_type = FIRST_PACKET;
 		gspca_frame_add(gspca_dev, packet_type,
 				buffer, FRAME_HEADER_LEN);
-		while (bytes_left > 0 && gspca_dev->present) {
+		while (bytes_left > 0 && gspca_dev->dev) {
 			data_len = bytes_left > SQ905C_MAX_TRANSFER ?
 				SQ905C_MAX_TRANSFER : bytes_left;
 			ret = usb_bulk_msg(gspca_dev->dev,
@@ -191,7 +195,7 @@ static void sq905c_dostream(struct work_struct *work)
 		}
 	}
 quit_stream:
-	if (gspca_dev->present) {
+	if (gspca_dev->dev) {
 		mutex_lock(&gspca_dev->usb_lock);
 		sq905c_command(gspca_dev, SQ905C_CLEAR, 0);
 		mutex_unlock(&gspca_dev->usb_lock);
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx.c b/drivers/media/video/gspca/stv06xx/stv06xx.c
index 91d99b4cc57b..999ec7764449 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx.c
@@ -261,6 +261,17 @@ static int stv06xx_init(struct gspca_dev *gspca_dev)
 	return (err < 0) ? err : 0;
 }
 
+/* this function is called at probe time */
+static int stv06xx_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	PDEBUG(D_PROBE, "Initializing controls");
+
+	gspca_dev->vdev.ctrl_handler = &gspca_dev->ctrl_handler;
+	return sd->sensor->init_controls(sd);
+}
+
 /* Start the camera */
 static int stv06xx_start(struct gspca_dev *gspca_dev)
 {
@@ -512,6 +523,7 @@ static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
 	.config = stv06xx_config,
 	.init = stv06xx_init,
+	.init_controls = stv06xx_init_controls,
 	.start = stv06xx_start,
 	.stopN = stv06xx_stopN,
 	.pkt_scan = stv06xx_pkt_scan,
@@ -530,9 +542,8 @@ static int stv06xx_config(struct gspca_dev *gspca_dev,
 
 	PDEBUG(D_PROBE, "Configuring camera");
 
-	sd->desc = sd_desc;
 	sd->bridge = id->driver_info;
-	gspca_dev->sd_desc = &sd->desc;
+	gspca_dev->sd_desc = &sd_desc;
 
 	if (dump_bridge)
 		stv06xx_dump_bridge(sd);
@@ -594,11 +605,12 @@ static void sd_disconnect(struct usb_interface *intf)
 {
 	struct gspca_dev *gspca_dev = usb_get_intfdata(intf);
 	struct sd *sd = (struct sd *) gspca_dev;
+	void *priv = sd->sensor_priv;
 	PDEBUG(D_PROBE, "Disconnecting the stv06xx device");
 
-	if (sd->sensor->disconnect)
-		sd->sensor->disconnect(sd);
+	sd->sensor = NULL;
 	gspca_disconnect(intf);
+	kfree(priv);
 }
 
 static struct usb_driver sd_driver = {
@@ -609,6 +621,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx.h b/drivers/media/video/gspca/stv06xx/stv06xx.h
index d270a5981afe..34957a4ec150 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx.h
@@ -89,9 +89,6 @@ struct sd {
 	/* A pointer to the currently connected sensor */
 	const struct stv06xx_sensor *sensor;
 
-	/* A pointer to the sd_desc struct */
-	struct sd_desc desc;
-
 	/* Sensor private data */
 	void *sensor_priv;
 
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
index a8698b7a7566..06fa54c5efb2 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
@@ -32,36 +32,6 @@
 
 #include "stv06xx_hdcs.h"
 
-static const struct ctrl hdcs1x00_ctrl[] = {
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_EXPOSURE,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_exposure,
-		.get = hdcs_get_exposure
-	}, {
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_gain,
-		.get = hdcs_get_gain
-	}
-};
-
 static struct v4l2_pix_format hdcs1x00_mode[] = {
 	{
 		HDCS_1X00_DEF_WIDTH,
@@ -76,36 +46,6 @@ static struct v4l2_pix_format hdcs1x00_mode[] = {
 	}
 };
 
-static const struct ctrl hdcs1020_ctrl[] = {
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0xffff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_EXPOSURE,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_exposure,
-		.get = hdcs_get_exposure
-	}, {
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_gain,
-		.get = hdcs_get_gain
-	}
-};
-
 static struct v4l2_pix_format hdcs1020_mode[] = {
 	{
 		HDCS_1020_DEF_WIDTH,
@@ -150,7 +90,6 @@ struct hdcs {
 	} exp;
 
 	int psmp;
-	u8 exp_cache, gain_cache;
 };
 
 static int hdcs_reg_write_seq(struct sd *sd, u8 reg, u8 *vals, u8 len)
@@ -232,16 +171,6 @@ static int hdcs_reset(struct sd *sd)
 	return err;
 }
 
-static int hdcs_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct hdcs *hdcs = sd->sensor_priv;
-
-	*val = hdcs->exp_cache;
-
-	return 0;
-}
-
 static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -260,9 +189,6 @@ static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	int cycles, err;
 	u8 exp[14];
 
-	val &= 0xff;
-	hdcs->exp_cache = val;
-
 	cycles = val * HDCS_CLK_FREQ_MHZ * 257;
 
 	ct = hdcs->exp.cto + hdcs->psmp + (HDCS_ADC_START_SIG_DUR + 2);
@@ -336,12 +262,9 @@ static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 
 static int hdcs_set_gains(struct sd *sd, u8 g)
 {
-	struct hdcs *hdcs = sd->sensor_priv;
 	int err;
 	u8 gains[4];
 
-	hdcs->gain_cache = g;
-
 	/* the voltage gain Av = (1 + 19 * val / 127) * (1 + bit7) */
 	if (g > 127)
 		g = 0x80 | (g / 2);
@@ -352,17 +275,7 @@ static int hdcs_set_gains(struct sd *sd, u8 g)
 	gains[3] = g;
 
 	err = hdcs_reg_write_seq(sd, HDCS_ERECPGA, gains, 4);
-		return err;
-}
-
-static int hdcs_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct hdcs *hdcs = sd->sensor_priv;
-
-	*val = hdcs->gain_cache;
-
-	return 0;
+	return err;
 }
 
 static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -420,6 +333,39 @@ static int hdcs_set_size(struct sd *sd,
 	return err;
 }
 
+static int hdcs_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	int err = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		err = hdcs_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = hdcs_set_exposure(gspca_dev, ctrl->val);
+		break;
+	}
+	return err;
+}
+
+static const struct v4l2_ctrl_ops hdcs_ctrl_ops = {
+	.s_ctrl = hdcs_s_ctrl,
+};
+
+static int hdcs_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	v4l2_ctrl_handler_init(hdl, 2);
+	v4l2_ctrl_new_std(hdl, &hdcs_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 0xff, 1, HDCS_DEFAULT_EXPOSURE);
+	v4l2_ctrl_new_std(hdl, &hdcs_ctrl_ops,
+			V4L2_CID_GAIN, 0, 0xff, 1, HDCS_DEFAULT_GAIN);
+	return hdl->error;
+}
+
 static int hdcs_probe_1x00(struct sd *sd)
 {
 	struct hdcs *hdcs;
@@ -434,8 +380,6 @@ static int hdcs_probe_1x00(struct sd *sd)
 
 	sd->gspca_dev.cam.cam_mode = hdcs1x00_mode;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(hdcs1x00_mode);
-	sd->desc.ctrls = hdcs1x00_ctrl;
-	sd->desc.nctrls = ARRAY_SIZE(hdcs1x00_ctrl);
 
 	hdcs = kmalloc(sizeof(struct hdcs), GFP_KERNEL);
 	if (!hdcs)
@@ -493,8 +437,6 @@ static int hdcs_probe_1020(struct sd *sd)
 
 	sd->gspca_dev.cam.cam_mode = hdcs1020_mode;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(hdcs1020_mode);
-	sd->desc.ctrls = hdcs1020_ctrl;
-	sd->desc.nctrls = ARRAY_SIZE(hdcs1020_ctrl);
 
 	hdcs = kmalloc(sizeof(struct hdcs), GFP_KERNEL);
 	if (!hdcs)
@@ -537,12 +479,6 @@ static int hdcs_stop(struct sd *sd)
 	return hdcs_set_state(sd, HDCS_STATE_SLEEP);
 }
 
-static void hdcs_disconnect(struct sd *sd)
-{
-	PDEBUG(D_PROBE, "Disconnecting the sensor");
-	kfree(sd->sensor_priv);
-}
-
 static int hdcs_init(struct sd *sd)
 {
 	struct hdcs *hdcs = sd->sensor_priv;
@@ -587,16 +523,7 @@ static int hdcs_init(struct sd *sd)
 	if (err < 0)
 		return err;
 
-	err = hdcs_set_gains(sd, HDCS_DEFAULT_GAIN);
-	if (err < 0)
-		return err;
-
-	err = hdcs_set_size(sd, hdcs->array.width, hdcs->array.height);
-	if (err < 0)
-		return err;
-
-	err = hdcs_set_exposure(&sd->gspca_dev, HDCS_DEFAULT_EXPOSURE);
-	return err;
+	return hdcs_set_size(sd, hdcs->array.width, hdcs->array.height);
 }
 
 static int hdcs_dump(struct sd *sd)
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
index a14a84a5079b..1ba9158d0102 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
@@ -131,14 +131,12 @@ static int hdcs_probe_1x00(struct sd *sd);
 static int hdcs_probe_1020(struct sd *sd);
 static int hdcs_start(struct sd *sd);
 static int hdcs_init(struct sd *sd);
+static int hdcs_init_controls(struct sd *sd);
 static int hdcs_stop(struct sd *sd);
 static int hdcs_dump(struct sd *sd);
-static void hdcs_disconnect(struct sd *sd);
 
-static int hdcs_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
 static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int hdcs_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
 
 const struct stv06xx_sensor stv06xx_sensor_hdcs1x00 = {
 	.name = "HP HDCS-1000/1100",
@@ -152,10 +150,10 @@ const struct stv06xx_sensor stv06xx_sensor_hdcs1x00 = {
 	.max_packet_size = { 847 },
 
 	.init = hdcs_init,
+	.init_controls = hdcs_init_controls,
 	.probe = hdcs_probe_1x00,
 	.start = hdcs_start,
 	.stop = hdcs_stop,
-	.disconnect = hdcs_disconnect,
 	.dump = hdcs_dump,
 };
 
@@ -171,6 +169,7 @@ const struct stv06xx_sensor stv06xx_sensor_hdcs1020 = {
 	.max_packet_size = { 847 },
 
 	.init = hdcs_init,
+	.init_controls = hdcs_init_controls,
 	.probe = hdcs_probe_1020,
 	.start = hdcs_start,
 	.stop = hdcs_stop,
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
index 26f14fc4a135..cdfc3d05ab6b 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
@@ -48,105 +48,16 @@
 
 #include "stv06xx_pb0100.h"
 
-static const struct ctrl pb0100_ctrl[] = {
-#define GAIN_IDX 0
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Gain",
-			.minimum	= 0,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 128
-		},
-		.set = pb0100_set_gain,
-		.get = pb0100_get_gain
-	},
-#define RED_BALANCE_IDX 1
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Red Balance",
-			.minimum	= -255,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 0
-		},
-		.set = pb0100_set_red_balance,
-		.get = pb0100_get_red_balance
-	},
-#define BLUE_BALANCE_IDX 2
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Blue Balance",
-			.minimum	= -255,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 0
-		},
-		.set = pb0100_set_blue_balance,
-		.get = pb0100_get_blue_balance
-	},
-#define EXPOSURE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Exposure",
-			.minimum	= 0,
-			.maximum	= 511,
-			.step		= 1,
-			.default_value  = 12
-		},
-		.set = pb0100_set_exposure,
-		.get = pb0100_get_exposure
-	},
-#define AUTOGAIN_IDX 4
-	{
-		{
-			.id		= V4L2_CID_AUTOGAIN,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "Automatic Gain and Exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value  = 1
-		},
-		.set = pb0100_set_autogain,
-		.get = pb0100_get_autogain
-	},
-#define AUTOGAIN_TARGET_IDX 5
-	{
-		{
-			.id		= V4L2_CTRL_CLASS_USER + 0x1000,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Automatic Gain Target",
-			.minimum	= 0,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 128
-		},
-		.set = pb0100_set_autogain_target,
-		.get = pb0100_get_autogain_target
-	},
-#define NATURAL_IDX 6
-	{
-		{
-			.id		= V4L2_CTRL_CLASS_USER + 0x1001,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "Natural Light Source",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value  = 1
-		},
-		.set = pb0100_set_natural,
-		.get = pb0100_get_natural
-	}
+struct pb0100_ctrls {
+	struct { /* one big happy control cluster... */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *red;
+		struct v4l2_ctrl *blue;
+		struct v4l2_ctrl *natural;
+	};
+	struct v4l2_ctrl *target;
 };
 
 static struct v4l2_pix_format pb0100_mode[] = {
@@ -174,38 +85,104 @@ static struct v4l2_pix_format pb0100_mode[] = {
 	}
 };
 
+static int pb0100_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
+	int err = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		err = pb0100_set_autogain(gspca_dev, ctrl->val);
+		if (err)
+			break;
+		if (ctrl->val)
+			break;
+		err = pb0100_set_gain(gspca_dev, ctrls->gain->val);
+		if (err)
+			break;
+		err = pb0100_set_exposure(gspca_dev, ctrls->exposure->val);
+		break;
+	case V4L2_CTRL_CLASS_USER + 0x1001:
+		err = pb0100_set_autogain_target(gspca_dev, ctrl->val);
+		break;
+	}
+	return err;
+}
+
+static const struct v4l2_ctrl_ops pb0100_ctrl_ops = {
+	.s_ctrl = pb0100_s_ctrl,
+};
+
+static int pb0100_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+	struct pb0100_ctrls *ctrls;
+	static const struct v4l2_ctrl_config autogain_target = {
+		.ops = &pb0100_ctrl_ops,
+		.id = V4L2_CTRL_CLASS_USER + 0x1000,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Automatic Gain Target",
+		.max = 255,
+		.step = 1,
+		.def = 128,
+	};
+	static const struct v4l2_ctrl_config natural_light = {
+		.ops = &pb0100_ctrl_ops,
+		.id = V4L2_CTRL_CLASS_USER + 0x1001,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Natural Light Source",
+		.max = 1,
+		.step = 1,
+		.def = 1,
+	};
+
+	ctrls = kzalloc(sizeof(*ctrls), GFP_KERNEL);
+	if (!ctrls)
+		return -ENOMEM;
+
+	v4l2_ctrl_handler_init(hdl, 6);
+	ctrls->autogain = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 511, 1, 12);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 128);
+	ctrls->red = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_RED_BALANCE, -255, 255, 1, 0);
+	ctrls->blue = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, -255, 255, 1, 0);
+	ctrls->natural = v4l2_ctrl_new_custom(hdl, &natural_light, NULL);
+	ctrls->target = v4l2_ctrl_new_custom(hdl, &autogain_target, NULL);
+	if (hdl->error) {
+		kfree(ctrls);
+		return hdl->error;
+	}
+	sd->sensor_priv = ctrls;
+	v4l2_ctrl_auto_cluster(5, &ctrls->autogain, 0, false);
+	return 0;
+}
+
 static int pb0100_probe(struct sd *sd)
 {
 	u16 sensor;
-	int i, err;
-	s32 *sensor_settings;
+	int err;
 
 	err = stv06xx_read_sensor(sd, PB_IDENT, &sensor);
 
 	if (err < 0)
 		return -ENODEV;
+	if ((sensor >> 8) != 0x64)
+		return -ENODEV;
 
-	if ((sensor >> 8) == 0x64) {
-		sensor_settings = kmalloc(
-				ARRAY_SIZE(pb0100_ctrl) * sizeof(s32),
-				GFP_KERNEL);
-		if (!sensor_settings)
-			return -ENOMEM;
-
-		pr_info("Photobit pb0100 sensor detected\n");
-
-		sd->gspca_dev.cam.cam_mode = pb0100_mode;
-		sd->gspca_dev.cam.nmodes = ARRAY_SIZE(pb0100_mode);
-		sd->desc.ctrls = pb0100_ctrl;
-		sd->desc.nctrls = ARRAY_SIZE(pb0100_ctrl);
-		for (i = 0; i < sd->desc.nctrls; i++)
-			sensor_settings[i] = pb0100_ctrl[i].qctrl.default_value;
-		sd->sensor_priv = sensor_settings;
+	pr_info("Photobit pb0100 sensor detected\n");
 
-		return 0;
-	}
+	sd->gspca_dev.cam.cam_mode = pb0100_mode;
+	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(pb0100_mode);
 
-	return -ENODEV;
+	return 0;
 }
 
 static int pb0100_start(struct sd *sd)
@@ -214,7 +191,6 @@ static int pb0100_start(struct sd *sd)
 	struct usb_host_interface *alt;
 	struct usb_interface *intf;
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
 	u32 mode = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
@@ -255,13 +231,6 @@ static int pb0100_start(struct sd *sd)
 		stv06xx_write_bridge(sd, STV_SCAN_RATE, 0x20);
 	}
 
-	/* set_gain also sets red and blue balance */
-	pb0100_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	pb0100_set_exposure(&sd->gspca_dev, sensor_settings[EXPOSURE_IDX]);
-	pb0100_set_autogain_target(&sd->gspca_dev,
-				   sensor_settings[AUTOGAIN_TARGET_IDX]);
-	pb0100_set_autogain(&sd->gspca_dev, sensor_settings[AUTOGAIN_IDX]);
-
 	err = stv06xx_write_sensor(sd, PB_CONTROL, BIT(5)|BIT(3)|BIT(1));
 	PDEBUG(D_STREAM, "Started stream, status: %d", err);
 
@@ -285,12 +254,6 @@ out:
 	return (err < 0) ? err : 0;
 }
 
-static void pb0100_disconnect(struct sd *sd)
-{
-	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
 /* FIXME: Sort the init commands out and put them into tables,
 	  this is only for getting the camera to work */
 /* FIXME: No error handling for now,
@@ -362,62 +325,32 @@ static int pb0100_dump(struct sd *sd)
 	return 0;
 }
 
-static int pb0100_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GAIN_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
-
-	sensor_settings[GAIN_IDX] = val;
 	err = stv06xx_write_sensor(sd, PB_G1GAIN, val);
 	if (!err)
 		err = stv06xx_write_sensor(sd, PB_G2GAIN, val);
 	PDEBUG(D_V4L2, "Set green gain to %d, status: %d", val, err);
 
 	if (!err)
-		err = pb0100_set_red_balance(gspca_dev,
-					     sensor_settings[RED_BALANCE_IDX]);
+		err = pb0100_set_red_balance(gspca_dev, ctrls->red->val);
 	if (!err)
-		err = pb0100_set_blue_balance(gspca_dev,
-					    sensor_settings[BLUE_BALANCE_IDX]);
+		err = pb0100_set_blue_balance(gspca_dev, ctrls->blue->val);
 
 	return err;
 }
 
-static int pb0100_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[RED_BALANCE_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
-
-	sensor_settings[RED_BALANCE_IDX] = val;
-	val += sensor_settings[GAIN_IDX];
+	val += ctrls->gain->val;
 	if (val < 0)
 		val = 0;
 	else if (val > 255)
@@ -429,27 +362,13 @@ static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int pb0100_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[BLUE_BALANCE_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
-
-	sensor_settings[BLUE_BALANCE_IDX] = val;
-	val += sensor_settings[GAIN_IDX];
+	val += ctrls->gain->val;
 	if (val < 0)
 		val = 0;
 	else if (val > 255)
@@ -461,51 +380,25 @@ static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int pb0100_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[EXPOSURE_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
-	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
+	int err;
 
-	sensor_settings[EXPOSURE_IDX] = val;
 	err = stv06xx_write_sensor(sd, PB_RINTTIME, val);
 	PDEBUG(D_V4L2, "Set exposure to %d, status: %d", val, err);
 
 	return err;
 }
 
-static int pb0100_get_autogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTOGAIN_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	sensor_settings[AUTOGAIN_IDX] = val;
-	if (sensor_settings[AUTOGAIN_IDX]) {
-		if (sensor_settings[NATURAL_IDX])
+	if (val) {
+		if (ctrls->natural->val)
 			val = BIT(6)|BIT(4)|BIT(0);
 		else
 			val = BIT(4)|BIT(0);
@@ -514,29 +407,15 @@ static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val)
 
 	err = stv06xx_write_sensor(sd, PB_EXPGAIN, val);
 	PDEBUG(D_V4L2, "Set autogain to %d (natural: %d), status: %d",
-	       sensor_settings[AUTOGAIN_IDX], sensor_settings[NATURAL_IDX],
-	       err);
+	       val, ctrls->natural->val, err);
 
 	return err;
 }
 
-static int pb0100_get_autogain_target(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTOGAIN_TARGET_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err, totalpixels, brightpixels, darkpixels;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	sensor_settings[AUTOGAIN_TARGET_IDX] = val;
 
 	/* Number of pixels counted by the sensor when subsampling the pixels.
 	 * Slightly larger than the real value to avoid oscillation */
@@ -553,23 +432,3 @@ static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val)
 
 	return err;
 }
-
-static int pb0100_get_natural(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[NATURAL_IDX];
-
-	return 0;
-}
-
-static int pb0100_set_natural(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	sensor_settings[NATURAL_IDX] = val;
-
-	return pb0100_set_autogain(gspca_dev, sensor_settings[AUTOGAIN_IDX]);
-}
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
index 757de246dc75..5071e5353fd3 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
@@ -112,25 +112,17 @@
 static int pb0100_probe(struct sd *sd);
 static int pb0100_start(struct sd *sd);
 static int pb0100_init(struct sd *sd);
+static int pb0100_init_controls(struct sd *sd);
 static int pb0100_stop(struct sd *sd);
 static int pb0100_dump(struct sd *sd);
-static void pb0100_disconnect(struct sd *sd);
 
 /* V4L2 controls supported by the driver */
-static int pb0100_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_autogain(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_autogain_target(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_natural(struct gspca_dev *gspca_dev, __s32 *val);
-static int pb0100_set_natural(struct gspca_dev *gspca_dev, __s32 val);
 
 const struct stv06xx_sensor stv06xx_sensor_pb0100 = {
 	.name = "PB-0100",
@@ -142,11 +134,11 @@ const struct stv06xx_sensor stv06xx_sensor_pb0100 = {
 	.max_packet_size = { 847, 923 },
 
 	.init = pb0100_init,
+	.init_controls = pb0100_init_controls,
 	.probe = pb0100_probe,
 	.start = pb0100_start,
 	.stop = pb0100_stop,
 	.dump = pb0100_dump,
-	.disconnect = pb0100_disconnect,
 };
 
 #endif
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h b/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
index fb229d8ded58..3a498c2495c6 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
@@ -63,8 +63,8 @@ struct stv06xx_sensor {
 	/* Performs a initialization sequence */
 	int (*init)(struct sd *sd);
 
-	/* Executed at device disconnect */
-	void (*disconnect)(struct sd *sd);
+	/* Initializes the controls */
+	int (*init_controls)(struct sd *sd);
 
 	/* Reads a sensor register */
 	int (*read_sensor)(struct sd *sd, const u8 address,
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
index 9940e035b3ab..8a57990dfe0f 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
@@ -30,20 +30,6 @@
 
 #include "stv06xx_st6422.h"
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	GAIN,
-	EXPOSURE,
-	NCTRLS		/* number of controls */
-};
-
-/* sensor settings */
-struct st6422_settings {
-	struct gspca_ctrl ctrls[NCTRLS];
-};
-
 static struct v4l2_pix_format st6422_mode[] = {
 	/* Note we actually get 124 lines of data, of which we skip the 4st
 	   4 as they are garbage */
@@ -74,83 +60,70 @@ static struct v4l2_pix_format st6422_mode[] = {
 };
 
 /* V4L2 controls supported by the driver */
-static void st6422_set_brightness(struct gspca_dev *gspca_dev);
-static void st6422_set_contrast(struct gspca_dev *gspca_dev);
-static void st6422_set_gain(struct gspca_dev *gspca_dev);
-static void st6422_set_exposure(struct gspca_dev *gspca_dev);
-
-static const struct ctrl st6422_ctrl[NCTRLS] = {
-[BRIGHTNESS] = {
-		{
-			.id		= V4L2_CID_BRIGHTNESS,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Brightness",
-			.minimum	= 0,
-			.maximum	= 31,
-			.step		= 1,
-			.default_value  = 3
-		},
-		.set_control = st6422_set_brightness
-	},
-[CONTRAST] = {
-		{
-			.id		= V4L2_CID_CONTRAST,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Contrast",
-			.minimum	= 0,
-			.maximum	= 15,
-			.step		= 1,
-			.default_value  = 11
-		},
-		.set_control = st6422_set_contrast
-	},
-[GAIN] = {
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Gain",
-			.minimum	= 0,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 64
-		},
-		.set_control = st6422_set_gain
-	},
-[EXPOSURE] = {
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Exposure",
-			.minimum	= 0,
-#define EXPOSURE_MAX 1023
-			.maximum	= EXPOSURE_MAX,
-			.step		= 1,
-			.default_value  = 256
-		},
-		.set_control = st6422_set_exposure
-	},
+static int setbrightness(struct sd *sd, s32 val);
+static int setcontrast(struct sd *sd, s32 val);
+static int setgain(struct sd *sd, u8 gain);
+static int setexposure(struct sd *sd, s16 expo);
+
+static int st6422_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+	int err = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		err = setbrightness(sd, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		err = setcontrast(sd, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = setgain(sd, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = setexposure(sd, ctrl->val);
+		break;
+	}
+
+	/* commit settings */
+	if (err >= 0)
+		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
+	sd->gspca_dev.usb_err = err;
+	return err;
+}
+
+static const struct v4l2_ctrl_ops st6422_ctrl_ops = {
+	.s_ctrl = st6422_s_ctrl,
 };
 
-static int st6422_probe(struct sd *sd)
+static int st6422_init_controls(struct sd *sd)
 {
-	struct st6422_settings *sensor_settings;
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 31, 1, 3);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 15, 1, 11);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 1023, 1, 256);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 64);
+
+	return hdl->error;
+}
 
+static int st6422_probe(struct sd *sd)
+{
 	if (sd->bridge != BRIDGE_ST6422)
 		return -ENODEV;
 
 	pr_info("st6422 sensor detected\n");
 
-	sensor_settings = kmalloc(sizeof *sensor_settings, GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = st6422_mode;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(st6422_mode);
-	sd->gspca_dev.cam.ctrls = sensor_settings->ctrls;
-	sd->desc.ctrls = st6422_ctrl;
-	sd->desc.nctrls = ARRAY_SIZE(st6422_ctrl);
-	sd->sensor_priv = sensor_settings;
-
 	return 0;
 }
 
@@ -239,38 +212,22 @@ static int st6422_init(struct sd *sd)
 	return err;
 }
 
-static void st6422_disconnect(struct sd *sd)
-{
-	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int setbrightness(struct sd *sd)
+static int setbrightness(struct sd *sd, s32 val)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-
 	/* val goes from 0 -> 31 */
-	return stv06xx_write_bridge(sd, 0x1432,
-			sensor_settings->ctrls[BRIGHTNESS].val);
+	return stv06xx_write_bridge(sd, 0x1432, val);
 }
 
-static int setcontrast(struct sd *sd)
+static int setcontrast(struct sd *sd, s32 val)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-
 	/* Val goes from 0 -> 15 */
-	return stv06xx_write_bridge(sd, 0x143a,
-			sensor_settings->ctrls[CONTRAST].val | 0xf0);
+	return stv06xx_write_bridge(sd, 0x143a, val | 0xf0);
 }
 
-static int setgain(struct sd *sd)
+static int setgain(struct sd *sd, u8 gain)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-	u8 gain;
 	int err;
 
-	gain = sensor_settings->ctrls[GAIN].val;
-
 	/* Set red, green, blue, gain */
 	err = stv06xx_write_bridge(sd, 0x0509, gain);
 	if (err < 0)
@@ -292,13 +249,10 @@ static int setgain(struct sd *sd)
 	return stv06xx_write_bridge(sd, 0x050d, 0x01);
 }
 
-static int setexposure(struct sd *sd)
+static int setexposure(struct sd *sd, s16 expo)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-	u16 expo;
 	int err;
 
-	expo = sensor_settings->ctrls[EXPOSURE].val;
 	err = stv06xx_write_bridge(sd, 0x143d, expo & 0xff);
 	if (err < 0)
 		return err;
@@ -318,22 +272,6 @@ static int st6422_start(struct sd *sd)
 	if (err < 0)
 		return err;
 
-	err = setbrightness(sd);
-	if (err < 0)
-		return err;
-
-	err = setcontrast(sd);
-	if (err < 0)
-		return err;
-
-	err = setexposure(sd);
-	if (err < 0)
-		return err;
-
-	err = setgain(sd);
-	if (err < 0)
-		return err;
-
 	/* commit settings */
 	err = stv06xx_write_bridge(sd, 0x143f, 0x01);
 	return (err < 0) ? err : 0;
@@ -345,59 +283,3 @@ static int st6422_stop(struct sd *sd)
 
 	return 0;
 }
-
-static void st6422_set_brightness(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setbrightness(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
-
-static void st6422_set_contrast(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setcontrast(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
-
-static void st6422_set_gain(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setgain(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
-
-static void st6422_set_exposure(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setexposure(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
index d7498e06432b..8f20fbf30f33 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
@@ -34,8 +34,8 @@
 static int st6422_probe(struct sd *sd);
 static int st6422_start(struct sd *sd);
 static int st6422_init(struct sd *sd);
+static int st6422_init_controls(struct sd *sd);
 static int st6422_stop(struct sd *sd);
-static void st6422_disconnect(struct sd *sd);
 
 const struct stv06xx_sensor stv06xx_sensor_st6422 = {
 	.name = "ST6422",
@@ -43,10 +43,10 @@ const struct stv06xx_sensor stv06xx_sensor_st6422 = {
 	.min_packet_size = { 300, 847 },
 	.max_packet_size = { 300, 847 },
 	.init = st6422_init,
+	.init_controls = st6422_init_controls,
 	.probe = st6422_probe,
 	.start = st6422_start,
 	.stop = st6422_stop,
-	.disconnect = st6422_disconnect,
 };
 
 #endif
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
index a5c69d9ebdd4..748e1421d6d8 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
@@ -44,130 +44,83 @@ static struct v4l2_pix_format vv6410_mode[] = {
 	}
 };
 
-static const struct ctrl vv6410_ctrl[] = {
-#define HFLIP_IDX 0
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = vv6410_set_hflip,
-		.get = vv6410_get_hflip
-	},
-#define VFLIP_IDX 1
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = vv6410_set_vflip,
-		.get = vv6410_get_vflip
-	},
-#define GAIN_IDX 2
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "analog gain",
-			.minimum	= 0,
-			.maximum	= 15,
-			.step		= 1,
-			.default_value  = 10
-		},
-		.set = vv6410_set_analog_gain,
-		.get = vv6410_get_analog_gain
-	},
-#define EXPOSURE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0,
-			.maximum	= 32768,
-			.step		= 1,
-			.default_value  = 20000
-		},
-		.set = vv6410_set_exposure,
-		.get = vv6410_get_exposure
+static int vv6410_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	int err = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		err = vv6410_set_hflip(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		err = vv6410_set_vflip(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = vv6410_set_analog_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = vv6410_set_exposure(gspca_dev, ctrl->val);
+		break;
 	}
-	};
+	return err;
+}
+
+static const struct v4l2_ctrl_ops vv6410_ctrl_ops = {
+	.s_ctrl = vv6410_s_ctrl,
+};
 
 static int vv6410_probe(struct sd *sd)
 {
 	u16 data;
-	int err, i;
-	s32 *sensor_settings;
+	int err;
 
 	err = stv06xx_read_sensor(sd, VV6410_DEVICEH, &data);
 	if (err < 0)
 		return -ENODEV;
 
-	if (data == 0x19) {
-		pr_info("vv6410 sensor detected\n");
+	if (data != 0x19)
+		return -ENODEV;
 
-		sensor_settings = kmalloc(ARRAY_SIZE(vv6410_ctrl) * sizeof(s32),
-					  GFP_KERNEL);
-		if (!sensor_settings)
-			return -ENOMEM;
+	pr_info("vv6410 sensor detected\n");
 
-		sd->gspca_dev.cam.cam_mode = vv6410_mode;
-		sd->gspca_dev.cam.nmodes = ARRAY_SIZE(vv6410_mode);
-		sd->desc.ctrls = vv6410_ctrl;
-		sd->desc.nctrls = ARRAY_SIZE(vv6410_ctrl);
+	sd->gspca_dev.cam.cam_mode = vv6410_mode;
+	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(vv6410_mode);
+	return 0;
+}
 
-		for (i = 0; i < sd->desc.nctrls; i++)
-			sensor_settings[i] = vv6410_ctrl[i].qctrl.default_value;
-		sd->sensor_priv = sensor_settings;
-		return 0;
-	}
-	return -ENODEV;
+static int vv6410_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 32768, 1, 20000);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_GAIN, 0, 15, 1, 10);
+	return hdl->error;
 }
 
 static int vv6410_init(struct sd *sd)
 {
 	int err = 0, i;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	for (i = 0; i < ARRAY_SIZE(stv_bridge_init); i++) {
+	for (i = 0; i < ARRAY_SIZE(stv_bridge_init); i++)
 		stv06xx_write_bridge(sd, stv_bridge_init[i].addr, stv_bridge_init[i].data);
-	}
 
 	if (err < 0)
 		return err;
 
 	err = stv06xx_write_sensor_bytes(sd, (u8 *) vv6410_sensor_init,
 					 ARRAY_SIZE(vv6410_sensor_init));
-	if (err < 0)
-		return err;
-
-	err = vv6410_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = vv6410_set_analog_gain(&sd->gspca_dev,
-				      sensor_settings[GAIN_IDX]);
-
 	return (err < 0) ? err : 0;
 }
 
-static void vv6410_disconnect(struct sd *sd)
-{
-	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
 static int vv6410_start(struct sd *sd)
 {
 	int err;
@@ -233,25 +186,12 @@ static int vv6410_dump(struct sd *sd)
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[HFLIP_IDX];
-	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
-
-	return 0;
-}
-
 static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u16 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[HFLIP_IDX] = val;
 	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
 	if (err < 0)
 		return err;
@@ -267,25 +207,12 @@ static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
-
-	return 0;
-}
-
 static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u16 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[VFLIP_IDX] = val;
 	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
 	if (err < 0)
 		return err;
@@ -301,52 +228,23 @@ static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_get_analog_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GAIN_IDX];
-
-	PDEBUG(D_V4L2, "Read analog gain %d", *val);
-
-	return 0;
-}
-
 static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[GAIN_IDX] = val;
 	PDEBUG(D_V4L2, "Set analog gain to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_ANALOGGAIN, 0xf0 | (val & 0xf));
 
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[EXPOSURE_IDX];
-
-	PDEBUG(D_V4L2, "Read exposure %d", *val);
-
-	return 0;
-}
-
 static int vv6410_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	unsigned int fine, coarse;
 
-	sensor_settings[EXPOSURE_IDX] = val;
-
 	val = (val * val >> 14) + val / 4;
 
 	fine = val % VV6410_CIF_LINELENGTH;
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
index a25b8873f2e6..53e67b40ca05 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
@@ -178,18 +178,14 @@
 static int vv6410_probe(struct sd *sd);
 static int vv6410_start(struct sd *sd);
 static int vv6410_init(struct sd *sd);
+static int vv6410_init_controls(struct sd *sd);
 static int vv6410_stop(struct sd *sd);
 static int vv6410_dump(struct sd *sd);
-static void vv6410_disconnect(struct sd *sd);
 
 /* V4L2 controls supported by the driver */
-static int vv6410_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int vv6410_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int vv6410_get_analog_gain(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int vv6410_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
 
 const struct stv06xx_sensor stv06xx_sensor_vv6410 = {
@@ -202,11 +198,11 @@ const struct stv06xx_sensor stv06xx_sensor_vv6410 = {
 	.min_packet_size = { 1023 },
 	.max_packet_size = { 1023 },
 	.init = vv6410_init,
+	.init_controls = vv6410_init_controls,
 	.probe = vv6410_probe,
 	.start = vv6410_start,
 	.stop = vv6410_stop,
 	.dump = vv6410_dump,
-	.disconnect = vv6410_disconnect,
 };
 
 /* If NULL, only single value to write, stored in len */
diff --git a/drivers/media/video/gspca/topro.c b/drivers/media/video/gspca/topro.c
index 444d3c5b9079..c6326d177a3d 100644
--- a/drivers/media/video/gspca/topro.c
+++ b/drivers/media/video/gspca/topro.c
@@ -4675,11 +4675,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 /* -- do autogain -- */
 /* gain setting is done in setexposure() for tp6810 */
 static void setgain(struct gspca_dev *gspca_dev) {}
-/* !! coarse_grained_expo_autogain is not used !! */
-#define exp_too_low_cnt bridge
-#define exp_too_high_cnt sensor
-
+#define WANT_REGULAR_AUTOGAIN
 #include "autogain_functions.h"
+
 static void sd_dq_callback(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
diff --git a/drivers/media/video/gspca/vicam.c b/drivers/media/video/gspca/vicam.c
index 911152e169d6..15a30f7a4b2a 100644
--- a/drivers/media/video/gspca/vicam.c
+++ b/drivers/media/video/gspca/vicam.c
@@ -37,9 +37,12 @@
 #include <linux/ihex.h>
 #include "gspca.h"
 
+#define VICAM_FIRMWARE "vicam/firmware.fw"
+
 MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
 MODULE_DESCRIPTION("GSPCA ViCam USB Camera Driver");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(VICAM_FIRMWARE);
 
 enum e_ctrl {
 	GAIN,
@@ -222,7 +225,11 @@ static void vicam_dostream(struct work_struct *work)
 		goto exit;
 	}
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		ret = vicam_read_frame(gspca_dev, buffer, frame_sz);
 		if (ret < 0)
 			break;
@@ -268,7 +275,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	const struct firmware *uninitialized_var(fw);
 	u8 *firmware_buf;
 
-	ret = request_ihex_firmware(&fw, "vicam/firmware.fw",
+	ret = request_ihex_firmware(&fw, VICAM_FIRMWARE,
 				    &gspca_dev->dev->dev);
 	if (ret) {
 		pr_err("Failed to load \"vicam/firmware.fw\": %d\n", ret);
@@ -324,7 +331,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	dev->work_thread = NULL;
 	mutex_lock(&gspca_dev->usb_lock);
 
-	if (gspca_dev->present)
+	if (gspca_dev->dev)
 		vicam_set_camera_power(gspca_dev, 0);
 }
 
diff --git a/drivers/media/video/gspca/zc3xx.c b/drivers/media/video/gspca/zc3xx.c
index 7d9a4f1be9dc..f0bacee33ef9 100644
--- a/drivers/media/video/gspca/zc3xx.c
+++ b/drivers/media/video/gspca/zc3xx.c
@@ -32,29 +32,25 @@ MODULE_LICENSE("GPL");
 
 static int force_sensor = -1;
 
-#define REG08_DEF 3		/* default JPEG compression (70%) */
+#define REG08_DEF 3		/* default JPEG compression (75%) */
 #include "zc3xx-reg.h"
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	EXPOSURE,
-	GAMMA,
-	AUTOGAIN,
-	LIGHTFREQ,
-	SHARPNESS,
-	QUALITY,
-	NCTRLS		/* number of controls */
-};
-
-#define AUTOGAIN_DEF 1
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct { /* gamma/brightness/contrast control cluster */
+		struct v4l2_ctrl *gamma;
+		struct v4l2_ctrl *brightness;
+		struct v4l2_ctrl *contrast;
+	};
+	struct { /* autogain/exposure control cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *exposure;
+	};
+	struct v4l2_ctrl *plfreq;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *jpegqual;
 
 	struct work_struct work;
 	struct workqueue_struct *work_thread;
@@ -94,114 +90,6 @@ enum sensors {
 	SENSOR_MAX
 };
 
-/* V4L2 controls supported by the driver */
-static void setcontrast(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static void setlightfreq(struct gspca_dev *gspca_dev);
-static void setsharpness(struct gspca_dev *gspca_dev);
-static int sd_setquality(struct gspca_dev *gspca_dev, __s32 val);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 128,
-	    },
-	    .set_control = setcontrast
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 128,
-	    },
-	    .set_control = setcontrast
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0x30d,
-		.maximum	= 0x493e,
-		.step		= 1,
-		.default_value  = 0x927
-	    },
-	    .set_control = setexposure
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 1,
-		.maximum = 6,
-		.step    = 1,
-		.default_value = 4,
-	    },
-	    .set_control = setcontrast
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = AUTOGAIN_DEF,
-		.flags   = V4L2_CTRL_FLAG_UPDATE
-	    },
-	    .set = sd_setautogain
-	},
-[LIGHTFREQ] = {
-	    {
-		.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type    = V4L2_CTRL_TYPE_MENU,
-		.name    = "Light frequency filter",
-		.minimum = 0,
-		.maximum = 2,	/* 0: 0, 1: 50Hz, 2:60Hz */
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = setlightfreq
-	},
-[SHARPNESS] = {
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 2,
-	    },
-	    .set_control = setsharpness
-	},
-[QUALITY] = {
-	    {
-		.id	 = V4L2_CID_JPEG_COMPRESSION_QUALITY,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Compression Quality",
-		.minimum = 40,
-		.maximum = 70,
-		.step    = 1,
-		.default_value = 70	/* updated in sd_init() */
-	    },
-	    .set = sd_setquality
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -241,8 +129,11 @@ static const struct v4l2_pix_format sif_mode[] = {
 		.priv = 0},
 };
 
-/* bridge reg08 -> JPEG quality conversion table */
-static u8 jpeg_qual[] = {40, 50, 60, 70, /*80*/};
+/*
+ * Bridge reg08 bits 1-2 -> JPEG quality conversion table. Note the highest
+ * quality setting is not usable as USB 1 does not have enough bandwidth.
+ */
+static u8 jpeg_qual[] = {50, 75, 87, /* 94 */};
 
 /* usb exchanges */
 struct usb_action {
@@ -5818,10 +5709,8 @@ static void setmatrix(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, matrix[i], 0x010a + i);
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int sharpness;
 	static const u8 sharpness_tb[][2] = {
 		{0x02, 0x03},
 		{0x04, 0x07},
@@ -5829,19 +5718,18 @@ static void setsharpness(struct gspca_dev *gspca_dev)
 		{0x10, 0x1e}
 	};
 
-	sharpness = sd->ctrls[SHARPNESS].val;
-	reg_w(gspca_dev, sharpness_tb[sharpness][0], 0x01c6);
+	reg_w(gspca_dev, sharpness_tb[val][0], 0x01c6);
 	reg_r(gspca_dev, 0x01c8);
 	reg_r(gspca_dev, 0x01c9);
 	reg_r(gspca_dev, 0x01ca);
-	reg_w(gspca_dev, sharpness_tb[sharpness][1], 0x01cb);
+	reg_w(gspca_dev, sharpness_tb[val][1], 0x01cb);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev,
+		s32 gamma, s32 brightness, s32 contrast)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	const u8 *Tgamma;
-	int g, i, brightness, contrast, adj, gp1, gp2;
+	int g, i, adj, gp1, gp2;
 	u8 gr[16];
 	static const u8 delta_b[16] =		/* delta for brightness */
 		{0x50, 0x38, 0x2d, 0x28, 0x24, 0x21, 0x1e, 0x1d,
@@ -5864,10 +5752,10 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 		 0xe0, 0xeb, 0xf4, 0xff, 0xff, 0xff, 0xff, 0xff},
 	};
 
-	Tgamma = gamma_tb[sd->ctrls[GAMMA].val - 1];
+	Tgamma = gamma_tb[gamma - 1];
 
-	contrast = ((int) sd->ctrls[CONTRAST].val - 128); /* -128 / 127 */
-	brightness = ((int) sd->ctrls[BRIGHTNESS].val - 128); /* -128 / 92 */
+	contrast -= 128; /* -128 / 127 */
+	brightness -= 128; /* -128 / 92 */
 	adj = 0;
 	gp1 = gp2 = 0;
 	for (i = 0; i < 16; i++) {
@@ -5894,25 +5782,15 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, gr[i], 0x0130 + i);	/* gradient */
 }
 
-static void getexposure(struct gspca_dev *gspca_dev)
+static s32 getexposure(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (sd->sensor != SENSOR_HV7131R)
-		return;
-	sd->ctrls[EXPOSURE].val = (i2c_read(gspca_dev, 0x25) << 9)
+	return (i2c_read(gspca_dev, 0x25) << 9)
 		| (i2c_read(gspca_dev, 0x26) << 1)
 		| (i2c_read(gspca_dev, 0x27) >> 7);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
-
-	if (sd->sensor != SENSOR_HV7131R)
-		return;
-	val = sd->ctrls[EXPOSURE].val;
 	i2c_write(gspca_dev, 0x25, val >> 9, 0x00);
 	i2c_write(gspca_dev, 0x26, val >> 1, 0x00);
 	i2c_write(gspca_dev, 0x27, val << 7, 0x00);
@@ -5921,20 +5799,8 @@ static void setexposure(struct gspca_dev *gspca_dev)
 static void setquality(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s8 reg07;
-
-	reg07 = 0;
-	switch (sd->sensor) {
-	case SENSOR_OV7620:
-		reg07 = 0x30;
-		break;
-	case SENSOR_HV7131R:
-	case SENSOR_PAS202B:
-		return;			/* done by work queue */
-	}
+	jpeg_set_qual(sd->jpeg_hdr, jpeg_qual[sd->reg08 >> 1]);
 	reg_w(gspca_dev, sd->reg08, ZC3XX_R008_CLOCKSETTING);
-	if (reg07 != 0)
-		reg_w(gspca_dev, reg07, 0x0007);
 }
 
 /* Matches the sensor's internal frame rate to the lighting frequency.
@@ -5943,7 +5809,7 @@ static void setquality(struct gspca_dev *gspca_dev)
  *	60Hz, for American lighting
  *	0 = No Fliker (for outdoore usage)
  */
-static void setlightfreq(struct gspca_dev *gspca_dev)
+static void setlightfreq(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int i, mode;
@@ -6027,7 +5893,7 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 		 tas5130c_60HZ, tas5130c_60HZScale},
 	};
 
-	i = sd->ctrls[LIGHTFREQ].val * 2;
+	i = val * 2;
 	mode = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
 	if (mode)
 		i++;			/* 320x240 */
@@ -6037,14 +5903,14 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 	usb_exchange(gspca_dev, zc3_freq);
 	switch (sd->sensor) {
 	case SENSOR_GC0305:
-		if (mode				/* if 320x240 */
-		    && sd->ctrls[LIGHTFREQ].val == 1)	/* and 50Hz */
+		if (mode		/* if 320x240 */
+		    && val == 1)	/* and 50Hz */
 			reg_w(gspca_dev, 0x85, 0x018d);
 					/* win: 0x80, 0x018d */
 		break;
 	case SENSOR_OV7620:
-		if (!mode) {				/* if 640x480 */
-			if (sd->ctrls[LIGHTFREQ].val != 0) /* and filter */
+		if (!mode) {		/* if 640x480 */
+			if (val != 0)	/* and filter */
 				reg_w(gspca_dev, 0x40, 0x0002);
 			else
 				reg_w(gspca_dev, 0x44, 0x0002);
@@ -6056,22 +5922,15 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 autoval;
-
-	if (sd->ctrls[AUTOGAIN].val)
-		autoval = 0x42;
-	else
-		autoval = 0x02;
-	reg_w(gspca_dev, autoval, 0x0180);
+	reg_w(gspca_dev, val ? 0x42 : 0x02, 0x0180);
 }
 
-/* update the transfer parameters */
-/* This function is executed from a work queue. */
-/* The exact use of the bridge registers 07 and 08 is not known.
- * The following algorithm has been adapted from ms-win traces */
+/*
+ * Update the transfer parameters.
+ * This function is executed from a work queue.
+ */
 static void transfer_update(struct work_struct *work)
 {
 	struct sd *sd = container_of(work, struct sd, work);
@@ -6079,96 +5938,55 @@ static void transfer_update(struct work_struct *work)
 	int change, good;
 	u8 reg07, reg11;
 
-	/* synchronize with the main driver and initialize the registers */
-	mutex_lock(&gspca_dev->usb_lock);
-	reg07 = 0;					/* max */
-	reg_w(gspca_dev, reg07, 0x0007);
-	reg_w(gspca_dev, sd->reg08, ZC3XX_R008_CLOCKSETTING);
-	mutex_unlock(&gspca_dev->usb_lock);
+	/* reg07 gets set to 0 by sd_start before starting us */
+	reg07 = 0;
 
 	good = 0;
 	for (;;) {
 		msleep(100);
 
-		/* get the transfer status */
-		/* the bit 0 of the bridge register 11 indicates overflow */
 		mutex_lock(&gspca_dev->usb_lock);
-		if (!gspca_dev->present || !gspca_dev->streaming)
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
 			goto err;
+#endif
+		if (!gspca_dev->dev || !gspca_dev->streaming)
+			goto err;
+
+		/* Bit 0 of register 11 indicates FIFO overflow */
+		gspca_dev->usb_err = 0;
 		reg11 = reg_r(gspca_dev, 0x0011);
-		if (gspca_dev->usb_err < 0
-		 || !gspca_dev->present || !gspca_dev->streaming)
+		if (gspca_dev->usb_err)
 			goto err;
 
 		change = reg11 & 0x01;
 		if (change) {				/* overflow */
-			switch (reg07) {
-			case 0:				/* max */
-				reg07 = sd->sensor == SENSOR_HV7131R
-						? 0x30 : 0x32;
-				if (sd->reg08 != 0) {
-					change = 3;
-					sd->reg08--;
-				}
-				break;
-			case 0x32:
-				reg07 -= 4;
-				break;
-			default:
-				reg07 -= 2;
-				break;
-			case 2:
-				change = 0;		/* already min */
-				break;
-			}
 			good = 0;
+
+			if (reg07 == 0) /* Bit Rate Control not enabled? */
+				reg07 = 0x32; /* Allow 98 bytes / unit */
+			else if (reg07 > 2)
+				reg07 -= 2; /* Decrease allowed bytes / unit */
+			else
+				change = 0;
 		} else {				/* no overflow */
-			if (reg07 != 0) {		/* if not max */
-				good++;
-				if (good >= 10) {
-					good = 0;
+			good++;
+			if (good >= 10) {
+				good = 0;
+				if (reg07) { /* BRC enabled? */
 					change = 1;
-					reg07 += 2;
-					switch (reg07) {
-					case 0x30:
-						if (sd->sensor == SENSOR_PAS202B)
-							reg07 += 2;
-						break;
-					case 0x32:
-					case 0x34:
+					if (reg07 < 0x32)
+						reg07 += 2;
+					else
 						reg07 = 0;
-						break;
-					}
-				}
-			} else {			/* reg07 max */
-				if (sd->reg08 < sizeof jpeg_qual - 1) {
-					good++;
-					if (good > 10) {
-						sd->reg08++;
-						change = 2;
-					}
 				}
 			}
 		}
 		if (change) {
-			if (change & 1) {
-				reg_w(gspca_dev, reg07, 0x0007);
-				if (gspca_dev->usb_err < 0
-				 || !gspca_dev->present
-				 || !gspca_dev->streaming)
-					goto err;
-			}
-			if (change & 2) {
-				reg_w(gspca_dev, sd->reg08,
-						ZC3XX_R008_CLOCKSETTING);
-				if (gspca_dev->usb_err < 0
-				 || !gspca_dev->present
-				 || !gspca_dev->streaming)
-					goto err;
-				sd->ctrls[QUALITY].val = jpeg_qual[sd->reg08];
-				jpeg_set_qual(sd->jpeg_hdr,
-						jpeg_qual[sd->reg08]);
-			}
+			gspca_dev->usb_err = 0;
+			reg_w(gspca_dev, reg07, 0x0007);
+			if (gspca_dev->usb_err)
+				goto err;
 		}
 		mutex_unlock(&gspca_dev->usb_lock);
 	}
@@ -6503,7 +6321,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	/* define some sensors from the vendor/product */
 	sd->sensor = id->driver_info;
 
-	gspca_dev->cam.ctrls = sd->ctrls;
 	sd->reg08 = REG08_DEF;
 
 	INIT_WORK(&sd->work, transfer_update);
@@ -6511,12 +6328,87 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	return 0;
 }
 
-/* this function is called at probe and resume time */
-static int sd_init(struct gspca_dev *gspca_dev)
+static int zcxx_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct cam *cam;
-	int sensor;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		gspca_dev->usb_err = 0;
+		if (ctrl->val && sd->exposure && gspca_dev->streaming)
+			sd->exposure->val = getexposure(gspca_dev);
+		return gspca_dev->usb_err;
+	}
+	return -EINVAL;
+}
+
+static int zcxx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+	int i, qual;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_JPEG_COMPRESSION_QUALITY) {
+		qual = sd->reg08 >> 1;
+
+		for (i = 0; i < ARRAY_SIZE(jpeg_qual); i++) {
+			if (ctrl->val <= jpeg_qual[i])
+				break;
+		}
+		if (i > 0 && i == qual && ctrl->val < jpeg_qual[i])
+			i--;
+
+		/* With high quality settings we need max bandwidth */
+		if (i >= 2 && gspca_dev->streaming &&
+		    !gspca_dev->cam.needs_full_bandwidth)
+			return -EBUSY;
+
+		sd->reg08 = (i << 1) | 1;
+		ctrl->val = jpeg_qual[i];
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	/* gamma/brightness/contrast cluster */
+	case V4L2_CID_GAMMA:
+		setcontrast(gspca_dev, sd->gamma->val,
+				sd->brightness->val, sd->contrast->val);
+		break;
+	/* autogain/exposure cluster */
+	case V4L2_CID_AUTOGAIN:
+		setautogain(gspca_dev, ctrl->val);
+		if (!gspca_dev->usb_err && !ctrl->val && sd->exposure)
+			setexposure(gspca_dev, sd->exposure->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setlightfreq(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		setquality(gspca_dev);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops zcxx_ctrl_ops = {
+	.g_volatile_ctrl = zcxx_g_volatile_ctrl,
+	.s_ctrl = zcxx_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
 	static const u8 gamma[SENSOR_MAX] = {
 		[SENSOR_ADCM2700] =	4,
 		[SENSOR_CS2102] =	4,
@@ -6538,6 +6430,48 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		[SENSOR_PO2030] =	4,
 		[SENSOR_TAS5130C] =	3,
 	};
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 8);
+	sd->brightness = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	sd->gamma = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_GAMMA, 1, 6, 1, gamma[sd->sensor]);
+	if (sd->sensor == SENSOR_HV7131R)
+		sd->exposure = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0x30d, 0x493e, 1, 0x927);
+	sd->autogain = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	if (sd->sensor != SENSOR_OV7630C)
+		sd->plfreq = v4l2_ctrl_new_std_menu(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
+			V4L2_CID_POWER_LINE_FREQUENCY_DISABLED);
+	sd->sharpness = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 3, 1,
+			sd->sensor == SENSOR_PO2030 ? 0 : 2);
+	sd->jpegqual = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY,
+			jpeg_qual[0], jpeg_qual[ARRAY_SIZE(jpeg_qual) - 1], 1,
+			jpeg_qual[REG08_DEF >> 1]);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_cluster(3, &sd->gamma);
+	if (sd->sensor == SENSOR_HV7131R)
+		v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, true);
+	return 0;
+}
+
+/* this function is called at probe and resume time */
+static int sd_init(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct cam *cam;
+	int sensor;
 	static const u8 mode_tb[SENSOR_MAX] = {
 		[SENSOR_ADCM2700] =	2,
 		[SENSOR_CS2102] =	1,
@@ -6559,27 +6493,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		[SENSOR_PO2030] =	1,
 		[SENSOR_TAS5130C] =	1,
 	};
-	static const u8 reg08_tb[SENSOR_MAX] = {
-		[SENSOR_ADCM2700] =	1,
-		[SENSOR_CS2102] =	3,
-		[SENSOR_CS2102K] =	3,
-		[SENSOR_GC0303] =	2,
-		[SENSOR_GC0305] =	3,
-		[SENSOR_HDCS2020] =	1,
-		[SENSOR_HV7131B] =	3,
-		[SENSOR_HV7131R] =	3,
-		[SENSOR_ICM105A] =	3,
-		[SENSOR_MC501CB] =	3,
-		[SENSOR_MT9V111_1] =	3,
-		[SENSOR_MT9V111_3] =	3,
-		[SENSOR_OV7620] =	1,
-		[SENSOR_OV7630C] =	3,
-		[SENSOR_PAS106] =	3,
-		[SENSOR_PAS202B] =	3,
-		[SENSOR_PB0330] =	3,
-		[SENSOR_PO2030] =	2,
-		[SENSOR_TAS5130C] =	3,
-	};
 
 	sensor = zcxx_probeSensor(gspca_dev);
 	if (sensor >= 0)
@@ -6688,7 +6601,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		case 0x2030:
 			PDEBUG(D_PROBE, "Find Sensor PO2030");
 			sd->sensor = SENSOR_PO2030;
-			sd->ctrls[SHARPNESS].def = 0;	/* from win traces */
 			break;
 		case 0x7620:
 			PDEBUG(D_PROBE, "Find Sensor OV7620");
@@ -6730,36 +6642,18 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	sd->ctrls[GAMMA].def = gamma[sd->sensor];
-	sd->reg08 = reg08_tb[sd->sensor];
-	sd->ctrls[QUALITY].def = jpeg_qual[sd->reg08];
-	sd->ctrls[QUALITY].min = jpeg_qual[0];
-	sd->ctrls[QUALITY].max = jpeg_qual[ARRAY_SIZE(jpeg_qual) - 1];
-
-	switch (sd->sensor) {
-	case SENSOR_HV7131R:
-		gspca_dev->ctrl_dis = (1 << QUALITY);
-		break;
-	case SENSOR_OV7630C:
-		gspca_dev->ctrl_dis = (1 << LIGHTFREQ) | (1 << EXPOSURE);
-		break;
-	case SENSOR_PAS202B:
-		gspca_dev->ctrl_dis = (1 << QUALITY) | (1 << EXPOSURE);
-		break;
-	default:
-		gspca_dev->ctrl_dis = (1 << EXPOSURE);
-		break;
-	}
-#if AUTOGAIN_DEF
-	if (sd->ctrls[AUTOGAIN].val)
-		gspca_dev->ctrl_inac = (1 << EXPOSURE);
-#endif
-
 	/* switch off the led */
 	reg_w(gspca_dev, 0x01, 0x0000);
 	return gspca_dev->usb_err;
 }
 
+static int sd_pre_start(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	gspca_dev->cam.needs_full_bandwidth = (sd->reg08 >= 4) ? 1 : 0;
+	return 0;
+}
+
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -6864,7 +6758,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x03, 0x0008);
 		break;
 	}
-	setsharpness(gspca_dev);
+	setsharpness(gspca_dev, v4l2_ctrl_g_ctrl(sd->sharpness));
 
 	/* set the gamma tables when not set */
 	switch (sd->sensor) {
@@ -6873,7 +6767,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	case SENSOR_OV7630C:
 		break;
 	default:
-		setcontrast(gspca_dev);
+		setcontrast(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma),
+				v4l2_ctrl_g_ctrl(sd->brightness),
+				v4l2_ctrl_g_ctrl(sd->contrast));
 		break;
 	}
 	setmatrix(gspca_dev);			/* one more time? */
@@ -6885,8 +6781,10 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	}
 	setquality(gspca_dev);
-	jpeg_set_qual(sd->jpeg_hdr, jpeg_qual[sd->reg08]);
-	setlightfreq(gspca_dev);
+	/* Start with BRC disabled, transfer_update will enable it if needed */
+	reg_w(gspca_dev, 0x00, 0x0007);
+	if (sd->plfreq)
+		setlightfreq(gspca_dev, v4l2_ctrl_g_ctrl(sd->plfreq));
 
 	switch (sd->sensor) {
 	case SENSOR_ADCM2700:
@@ -6897,7 +6795,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x40, 0x0117);
 		break;
 	case SENSOR_HV7131R:
-		setexposure(gspca_dev);
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(sd->exposure));
 		reg_w(gspca_dev, 0x00, ZC3XX_R1A7_CALCGLOBALMEAN);
 		break;
 	case SENSOR_GC0305:
@@ -6921,21 +6819,16 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	setautogain(gspca_dev);
+	setautogain(gspca_dev, v4l2_ctrl_g_ctrl(sd->autogain));
 
-	/* start the transfer update thread if needed */
-	if (gspca_dev->usb_err >= 0) {
-		switch (sd->sensor) {
-		case SENSOR_HV7131R:
-		case SENSOR_PAS202B:
-			sd->work_thread =
-				create_singlethread_workqueue(KBUILD_MODNAME);
-			queue_work(sd->work_thread, &sd->work);
-			break;
-		}
-	}
+	if (gspca_dev->usb_err < 0)
+		return gspca_dev->usb_err;
 
-	return gspca_dev->usb_err;
+	/* Start the transfer parameters update thread */
+	sd->work_thread = create_singlethread_workqueue(KBUILD_MODNAME);
+	queue_work(sd->work_thread, &sd->work);
+
+	return 0;
 }
 
 /* called on streamoff with alt 0 and on disconnect */
@@ -6949,7 +6842,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 		mutex_lock(&gspca_dev->usb_lock);
 		sd->work_thread = NULL;
 	}
-	if (!gspca_dev->present)
+	if (!gspca_dev->dev)
 		return;
 	send_unknown(gspca_dev, sd->sensor);
 }
@@ -6987,72 +6880,17 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->ctrls[AUTOGAIN].val = val;
-	if (val) {
-		gspca_dev->ctrl_inac |= (1 << EXPOSURE);
-	} else {
-		gspca_dev->ctrl_inac &= ~(1 << EXPOSURE);
-		if (gspca_dev->streaming)
-			getexposure(gspca_dev);
-	}
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:		/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "NoFliker");
-			return 0;
-		case 1:		/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case 2:		/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		}
-		break;
-	}
-	return -EINVAL;
-}
-
-static int sd_setquality(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(jpeg_qual) - 1; i++) {
-		if (val <= jpeg_qual[i])
-			break;
-	}
-	if (i > 0
-	 && i == sd->reg08
-	 && val < jpeg_qual[sd->reg08])
-		i--;
-	sd->reg08 = i;
-	sd->ctrls[QUALITY].val = jpeg_qual[i];
-	if (gspca_dev->streaming)
-		jpeg_set_qual(sd->jpeg_hdr, sd->ctrls[QUALITY].val);
-	return gspca_dev->usb_err;
-}
-
 static int sd_set_jcomp(struct gspca_dev *gspca_dev,
 			struct v4l2_jpegcompression *jcomp)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	int ret;
 
-	sd_setquality(gspca_dev, jcomp->quality);
-	jcomp->quality = sd->ctrls[QUALITY].val;
-	return gspca_dev->usb_err;
+	ret = v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
+	if (ret)
+		return ret;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
+	return 0;
 }
 
 static int sd_get_jcomp(struct gspca_dev *gspca_dev,
@@ -7061,7 +6899,7 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->ctrls[QUALITY].val;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
 			| V4L2_JPEG_MARKER_DQT;
 	return 0;
@@ -7085,14 +6923,13 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 
 static const struct sd_desc sd_desc = {
 	.name = KBUILD_MODNAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
+	.isoc_init = sd_pre_start,
 	.start = sd_start,
 	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 	.get_jcomp = sd_get_jcomp,
 	.set_jcomp = sd_set_jcomp,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
@@ -7176,6 +7013,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/hdpvr/hdpvr-control.c b/drivers/media/video/hdpvr/hdpvr-control.c
index 068df4ba3f51..ae8f229d1141 100644
--- a/drivers/media/video/hdpvr/hdpvr-control.c
+++ b/drivers/media/video/hdpvr/hdpvr-control.c
@@ -113,6 +113,8 @@ int get_input_lines_info(struct hdpvr_device *dev)
 			 "get input lines info returned: %d, %s\n", ret,
 			 print_buf);
 	}
+#else
+	(void)ret;	/* suppress compiler warning */
 #endif
 	lines = dev->usbc_buf[1] << 8 | dev->usbc_buf[0];
 	mutex_unlock(&dev->usbc_mutex);
diff --git a/drivers/media/video/hdpvr/hdpvr-video.c b/drivers/media/video/hdpvr/hdpvr-video.c
index 11ffe9cc1780..0e9e156bb2aa 100644
--- a/drivers/media/video/hdpvr/hdpvr-video.c
+++ b/drivers/media/video/hdpvr/hdpvr-video.c
@@ -994,7 +994,7 @@ static int hdpvr_try_ctrl(struct v4l2_ext_control *ctrl, int ac3)
 	default:
 		return -EINVAL;
 	}
-	return 0;
+	return ret;
 }
 
 static int vidioc_try_ext_ctrls(struct file *file, void *priv,
diff --git a/drivers/media/video/hexium_gemini.c b/drivers/media/video/hexium_gemini.c
index a62322d5c0d8..366434f5647e 100644
--- a/drivers/media/video/hexium_gemini.c
+++ b/drivers/media/video/hexium_gemini.c
@@ -40,15 +40,15 @@ static int hexium_num;
 
 #define HEXIUM_INPUTS	9
 static struct v4l2_input hexium_inputs[HEXIUM_INPUTS] = {
-	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
 };
 
 #define HEXIUM_AUDIOS	0
@@ -59,11 +59,6 @@ struct hexium_data
 	u8 byte;
 };
 
-#define HEXIUM_CONTROLS	1
-static struct v4l2_queryctrl hexium_controls[] = {
-	{ V4L2_CID_PRIVATE_BASE, V4L2_CTRL_TYPE_BOOLEAN, "B/W", 0, 1, 1, 0, 0 },
-};
-
 #define HEXIUM_GEMINI_V_1_0		1
 #define HEXIUM_GEMINI_DUAL_V_1_0	2
 
@@ -76,7 +71,6 @@ struct hexium
 
 	int 		cur_input;	/* current input */
 	v4l2_std_id 	cur_std;	/* current standard */
-	int		cur_bw;		/* current black/white status */
 };
 
 /* Samsung KS0127B decoder default registers */
@@ -119,18 +113,10 @@ static struct hexium_data hexium_pal[] = {
 	{ 0x01, 0x52 }, { 0x12, 0x64 }, { 0x2D, 0x2C }, { 0x2E, 0x9B }, { -1 , 0xFF }
 };
 
-static struct hexium_data hexium_pal_bw[] = {
-	{ 0x01, 0x52 },	{ 0x12, 0x64 },	{ 0x2D, 0x2C },	{ 0x2E, 0x9B },	{ -1 , 0xFF }
-};
-
 static struct hexium_data hexium_ntsc[] = {
 	{ 0x01, 0x53 }, { 0x12, 0x04 }, { 0x2D, 0x23 }, { 0x2E, 0x81 }, { -1 , 0xFF }
 };
 
-static struct hexium_data hexium_ntsc_bw[] = {
-	{ 0x01, 0x53 }, { 0x12, 0x04 }, { 0x2D, 0x23 }, { 0x2E, 0x81 }, { -1 , 0xFF }
-};
-
 static struct hexium_data hexium_secam[] = {
 	{ 0x01, 0x52 }, { 0x12, 0x64 }, { 0x2D, 0x2C }, { 0x2E, 0x9B }, { -1 , 0xFF }
 };
@@ -264,93 +250,6 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	return 0;
 }
 
-/* the saa7146 provides some controls (brightness, contrast, saturation)
-   which gets registered *after* this function. because of this we have
-   to return with a value != 0 even if the function succeeded.. */
-static int vidioc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *qc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	int i;
-
-	for (i = HEXIUM_CONTROLS - 1; i >= 0; i--) {
-		if (hexium_controls[i].id == qc->id) {
-			*qc = hexium_controls[i];
-			DEB_D("VIDIOC_QUERYCTRL %d\n", qc->id);
-			return 0;
-		}
-	}
-	return dev->ext_vv_data->core_ops->vidioc_queryctrl(file, fh, qc);
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct hexium *hexium = (struct hexium *) dev->ext_priv;
-	int i;
-
-	for (i = HEXIUM_CONTROLS - 1; i >= 0; i--) {
-		if (hexium_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_g_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_PRIVATE_BASE) {
-		vc->value = hexium->cur_bw;
-		DEB_D("VIDIOC_G_CTRL BW:%d\n", vc->value);
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct hexium *hexium = (struct hexium *) dev->ext_priv;
-	int i = 0;
-
-	for (i = HEXIUM_CONTROLS - 1; i >= 0; i--) {
-		if (hexium_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_s_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_PRIVATE_BASE)
-		hexium->cur_bw = vc->value;
-
-	DEB_D("VIDIOC_S_CTRL BW:%d\n", hexium->cur_bw);
-
-	if (0 == hexium->cur_bw && V4L2_STD_PAL == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_pal);
-		return 0;
-	}
-	if (0 == hexium->cur_bw && V4L2_STD_NTSC == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_ntsc);
-		return 0;
-	}
-	if (0 == hexium->cur_bw && V4L2_STD_SECAM == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_secam);
-		return 0;
-	}
-	if (1 == hexium->cur_bw && V4L2_STD_PAL == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_pal_bw);
-		return 0;
-	}
-	if (1 == hexium->cur_bw && V4L2_STD_NTSC == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_ntsc_bw);
-		return 0;
-	}
-	if (1 == hexium->cur_bw && V4L2_STD_SECAM == hexium->cur_std)
-		/* fixme: is there no bw secam mode? */
-		return -EINVAL;
-
-	return -EINVAL;
-}
-
-
 static struct saa7146_ext_vv vv_data;
 
 /* this function only gets called when the probing was successful */
@@ -399,12 +298,10 @@ static int hexium_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_d
 	hexium->cur_input = 0;
 
 	saa7146_vv_init(dev, &vv_data);
-	vv_data.ops.vidioc_queryctrl = vidioc_queryctrl;
-	vv_data.ops.vidioc_g_ctrl = vidioc_g_ctrl;
-	vv_data.ops.vidioc_s_ctrl = vidioc_s_ctrl;
-	vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data.ops.vidioc_g_input = vidioc_g_input;
-	vv_data.ops.vidioc_s_input = vidioc_s_input;
+
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
 	ret = saa7146_register_device(&hexium->video_dev, dev, "hexium gemini", VFL_TYPE_GRABBER);
 	if (ret < 0) {
 		pr_err("cannot register capture v4l2 device. skipping.\n");
diff --git a/drivers/media/video/hexium_orion.c b/drivers/media/video/hexium_orion.c
index 23debc967d94..a1eb26d11070 100644
--- a/drivers/media/video/hexium_orion.c
+++ b/drivers/media/video/hexium_orion.c
@@ -41,15 +41,15 @@ static int hexium_num;
 
 #define HEXIUM_INPUTS	9
 static struct v4l2_input hexium_inputs[HEXIUM_INPUTS] = {
-	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
 };
 
 #define HEXIUM_AUDIOS	0
@@ -371,9 +371,9 @@ static int hexium_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_d
 	DEB_EE("\n");
 
 	saa7146_vv_init(dev, &vv_data);
-	vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data.ops.vidioc_g_input = vidioc_g_input;
-	vv_data.ops.vidioc_s_input = vidioc_s_input;
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
 	if (0 != saa7146_register_device(&hexium->video_dev, dev, "hexium orion", VFL_TYPE_GRABBER)) {
 		pr_err("cannot register capture v4l2 device. skipping.\n");
 		return -1;
diff --git a/drivers/media/video/ivtv/ivtv-driver.c b/drivers/media/video/ivtv/ivtv-driver.c
index 679262ed13bc..057929e165ab 100644
--- a/drivers/media/video/ivtv/ivtv-driver.c
+++ b/drivers/media/video/ivtv/ivtv-driver.c
@@ -1201,9 +1201,9 @@ static int __devinit ivtv_probe(struct pci_dev *pdev,
 		struct v4l2_ctrl_handler *hdl = itv->v4l2_dev.ctrl_handler;
 
 		itv->ctrl_pts = v4l2_ctrl_new_std(hdl, &ivtv_hdl_out_ops,
-				V4L2_CID_MPEG_VIDEO_DEC_PTS, 0, 0, 1, 0);
+				V4L2_CID_MPEG_VIDEO_DEC_PTS, 0, 0, 0, 0);
 		itv->ctrl_frame = v4l2_ctrl_new_std(hdl, &ivtv_hdl_out_ops,
-				V4L2_CID_MPEG_VIDEO_DEC_FRAME, 0, 0x7fffffff, 1, 0);
+				V4L2_CID_MPEG_VIDEO_DEC_FRAME, 0, 0, 0, 0);
 		/* Note: V4L2_MPEG_AUDIO_DEC_PLAYBACK_AUTO is not supported,
 		   mask that menu item. */
 		itv->ctrl_audio_playback =
diff --git a/drivers/media/video/ivtv/ivtv-fileops.c b/drivers/media/video/ivtv/ivtv-fileops.c
index c9663e885b9f..9ff69b5a87e2 100644
--- a/drivers/media/video/ivtv/ivtv-fileops.c
+++ b/drivers/media/video/ivtv/ivtv-fileops.c
@@ -746,8 +746,9 @@ unsigned int ivtv_v4l2_dec_poll(struct file *filp, poll_table *wait)
 	return res;
 }
 
-unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table * wait)
+unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct ivtv_open_id *id = fh2id(filp->private_data);
 	struct ivtv *itv = id->itv;
 	struct ivtv_stream *s = &itv->streams[id->type];
@@ -755,7 +756,8 @@ unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table * wait)
 	unsigned res = 0;
 
 	/* Start a capture if there is none */
-	if (!eof && !test_bit(IVTV_F_S_STREAMING, &s->s_flags)) {
+	if (!eof && !test_bit(IVTV_F_S_STREAMING, &s->s_flags) &&
+			(req_events & (POLLIN | POLLRDNORM))) {
 		int rc;
 
 		rc = ivtv_start_capture(id);
diff --git a/drivers/media/video/ivtv/ivtv-ioctl.c b/drivers/media/video/ivtv/ivtv-ioctl.c
index 989e556913ed..f7d57b3f2842 100644
--- a/drivers/media/video/ivtv/ivtv-ioctl.c
+++ b/drivers/media/video/ivtv/ivtv-ioctl.c
@@ -135,7 +135,6 @@ void ivtv_set_osd_alpha(struct ivtv *itv)
 int ivtv_set_speed(struct ivtv *itv, int speed)
 {
 	u32 data[CX2341X_MBOX_MAX_DATA];
-	struct ivtv_stream *s;
 	int single_step = (speed == 1 || speed == -1);
 	DEFINE_WAIT(wait);
 
@@ -145,8 +144,6 @@ int ivtv_set_speed(struct ivtv *itv, int speed)
 	if (speed == itv->speed && !single_step)
 		return 0;
 
-	s = &itv->streams[IVTV_DEC_STREAM_TYPE_MPG];
-
 	if (single_step && (speed < 0) == (itv->speed < 0)) {
 		/* Single step video and no need to change direction */
 		ivtv_vapi(itv, CX2341X_DEC_STEP_VIDEO, 1, 0);
@@ -1468,8 +1465,9 @@ static int ivtv_subscribe_event(struct v4l2_fh *fh, struct v4l2_event_subscripti
 	switch (sub->type) {
 	case V4L2_EVENT_VSYNC:
 	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
 	case V4L2_EVENT_CTRL:
-		return v4l2_event_subscribe(fh, sub, 0);
+		return v4l2_event_subscribe(fh, sub, 0, &v4l2_ctrl_sub_ev_ops);
 	default:
 		return -EINVAL;
 	}
@@ -1827,7 +1825,7 @@ static long ivtv_default(struct file *file, void *fh, bool valid_prio,
 		return ivtv_decoder_ioctls(file, cmd, (void *)arg);
 
 	default:
-		return -EINVAL;
+		return -ENOTTY;
 	}
 	return 0;
 }
diff --git a/drivers/media/video/ivtv/ivtv-streams.c b/drivers/media/video/ivtv/ivtv-streams.c
index 7ea5ca7f012b..6738592aa35d 100644
--- a/drivers/media/video/ivtv/ivtv-streams.c
+++ b/drivers/media/video/ivtv/ivtv-streams.c
@@ -228,6 +228,10 @@ static int ivtv_prep_dev(struct ivtv *itv, int type)
 	s->vdev->release = video_device_release;
 	s->vdev->tvnorms = V4L2_STD_ALL;
 	s->vdev->lock = &itv->serialize_lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &s->vdev->flags);
 	set_bit(V4L2_FL_USE_FH_PRIO, &s->vdev->flags);
 	ivtv_set_funcs(s->vdev);
 	return 0;
diff --git a/drivers/media/video/ivtv/ivtvfb.c b/drivers/media/video/ivtv/ivtvfb.c
index e5e7fa9e737b..05b94aa8ba32 100644
--- a/drivers/media/video/ivtv/ivtvfb.c
+++ b/drivers/media/video/ivtv/ivtvfb.c
@@ -1293,6 +1293,7 @@ static int __init ivtvfb_init(void)
 
 	drv = driver_find("ivtv", &pci_bus_type);
 	err = driver_for_each_device(drv, NULL, &registered, ivtvfb_callback_init);
+	(void)err;	/* suppress compiler warning */
 	if (!registered) {
 		printk(KERN_ERR "ivtvfb:  no cards found\n");
 		return -ENODEV;
@@ -1309,6 +1310,7 @@ static void ivtvfb_cleanup(void)
 
 	drv = driver_find("ivtv", &pci_bus_type);
 	err = driver_for_each_device(drv, NULL, NULL, ivtvfb_callback_cleanup);
+	(void)err;	/* suppress compiler warning */
 }
 
 module_init(ivtvfb_init);
diff --git a/drivers/media/video/m5mols/m5mols.h b/drivers/media/video/m5mols/m5mols.h
index 4b021e1ee5f2..bb589917b65b 100644
--- a/drivers/media/video/m5mols/m5mols.h
+++ b/drivers/media/video/m5mols/m5mols.h
@@ -21,11 +21,6 @@
 
 extern int m5mols_debug;
 
-#define to_m5mols(__sd)	container_of(__sd, struct m5mols_info, sd)
-
-#define to_sd(__ctrl) \
-	(&container_of(__ctrl->handler, struct m5mols_info, handle)->sd)
-
 enum m5mols_restype {
 	M5MOLS_RESTYPE_MONITOR,
 	M5MOLS_RESTYPE_CAPTURE,
@@ -163,21 +158,27 @@ struct m5mols_version {
  * @ffmt: current fmt according to resolution type
  * @res_type: current resolution type
  * @irq_waitq: waitqueue for the capture
- * @flags: state variable for the interrupt handler
+ * @irq_done: set to 1 in the interrupt handler
  * @handle: control handler
- * @autoexposure: Auto Exposure control
- * @exposure: Exposure control
- * @autowb: Auto White Balance control
- * @colorfx: Color effect control
- * @saturation:	Saturation control
- * @zoom: Zoom control
+ * @auto_exposure: auto/manual exposure control
+ * @exposure_bias: exposure compensation control
+ * @exposure: manual exposure control
+ * @metering: exposure metering control
+ * @auto_iso: auto/manual ISO sensitivity control
+ * @iso: manual ISO sensitivity control
+ * @auto_wb: auto white balance control
+ * @lock_3a: 3A lock control
+ * @colorfx: color effect control
+ * @saturation: saturation control
+ * @zoom: zoom control
+ * @wdr: wide dynamic range control
+ * @stabilization: image stabilization control
+ * @jpeg_quality: JPEG compression quality control
  * @ver: information of the version
  * @cap: the capture mode attributes
- * @power: current sensor's power status
  * @isp_ready: 1 when the ISP controller has completed booting
+ * @power: current sensor's power status
  * @ctrl_sync: 1 when the control handler state is restored in H/W
- * @lock_ae: true means the Auto Exposure is locked
- * @lock_awb: true means the Aut WhiteBalance is locked
  * @resolution:	register value for current resolution
  * @mode: register value for current operation mode
  * @set_power: optional power callback to the board code
@@ -193,15 +194,27 @@ struct m5mols_info {
 	atomic_t irq_done;
 
 	struct v4l2_ctrl_handler handle;
+	struct {
+		/* exposure/exposure bias/auto exposure cluster */
+		struct v4l2_ctrl *auto_exposure;
+		struct v4l2_ctrl *exposure_bias;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *metering;
+	};
+	struct {
+		/* iso/auto iso cluster */
+		struct v4l2_ctrl *auto_iso;
+		struct v4l2_ctrl *iso;
+	};
+	struct v4l2_ctrl *auto_wb;
 
-	/* Autoexposure/exposure control cluster */
-	struct v4l2_ctrl *autoexposure;
-	struct v4l2_ctrl *exposure;
-
-	struct v4l2_ctrl *autowb;
+	struct v4l2_ctrl *lock_3a;
 	struct v4l2_ctrl *colorfx;
 	struct v4l2_ctrl *saturation;
 	struct v4l2_ctrl *zoom;
+	struct v4l2_ctrl *wdr;
+	struct v4l2_ctrl *stabilization;
+	struct v4l2_ctrl *jpeg_quality;
 
 	struct m5mols_version ver;
 	struct m5mols_capture cap;
@@ -210,8 +223,6 @@ struct m5mols_info {
 	unsigned int power:1;
 	unsigned int ctrl_sync:1;
 
-	bool lock_ae;
-	bool lock_awb;
 	u8 resolution;
 	u8 mode;
 
@@ -282,7 +293,7 @@ int m5mols_busy_wait(struct v4l2_subdev *sd, u32 reg, u32 value, u32 mask,
  * The available executing order between each modes are as follows:
  *   PARAMETER <---> MONITOR <---> CAPTURE
  */
-int m5mols_mode(struct m5mols_info *info, u8 mode);
+int m5mols_set_mode(struct m5mols_info *info, u8 mode);
 
 int m5mols_enable_interrupt(struct v4l2_subdev *sd, u8 reg);
 int m5mols_wait_interrupt(struct v4l2_subdev *sd, u8 condition, u32 timeout);
@@ -291,9 +302,33 @@ int m5mols_start_capture(struct m5mols_info *info);
 int m5mols_do_scenemode(struct m5mols_info *info, u8 mode);
 int m5mols_lock_3a(struct m5mols_info *info, bool lock);
 int m5mols_set_ctrl(struct v4l2_ctrl *ctrl);
+int m5mols_init_controls(struct v4l2_subdev *sd);
 
 /* The firmware function */
 int m5mols_update_fw(struct v4l2_subdev *sd,
 		     int (*set_power)(struct m5mols_info *, bool));
 
+static inline struct m5mols_info *to_m5mols(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct m5mols_info, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	struct m5mols_info *info = container_of(ctrl->handler,
+						struct m5mols_info, handle);
+	return &info->sd;
+}
+
+static inline void m5mols_set_ctrl_mode(struct v4l2_ctrl *ctrl,
+					unsigned int mode)
+{
+	ctrl->priv = (void *)mode;
+}
+
+static inline unsigned int m5mols_get_ctrl_mode(struct v4l2_ctrl *ctrl)
+{
+	return (unsigned int)ctrl->priv;
+}
+
 #endif	/* M5MOLS_H */
diff --git a/drivers/media/video/m5mols/m5mols_capture.c b/drivers/media/video/m5mols/m5mols_capture.c
index ba25e8e2ba4c..cb243bd278ce 100644
--- a/drivers/media/video/m5mols/m5mols_capture.c
+++ b/drivers/media/video/m5mols/m5mols_capture.c
@@ -106,7 +106,6 @@ static int m5mols_capture_info(struct m5mols_info *info)
 int m5mols_start_capture(struct m5mols_info *info)
 {
 	struct v4l2_subdev *sd = &info->sd;
-	u8 resolution = info->resolution;
 	int ret;
 
 	/*
@@ -114,22 +113,18 @@ int m5mols_start_capture(struct m5mols_info *info)
 	 * format. The frame capture is initiated during switching from Monitor
 	 * to Capture mode.
 	 */
-	ret = m5mols_mode(info, REG_MONITOR);
+	ret = m5mols_set_mode(info, REG_MONITOR);
 	if (!ret)
 		ret = m5mols_restore_controls(info);
 	if (!ret)
 		ret = m5mols_write(sd, CAPP_YUVOUT_MAIN, REG_JPEG);
 	if (!ret)
-		ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, resolution);
+		ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, info->resolution);
 	if (!ret)
-		ret = m5mols_lock_3a(info, true);
-	if (!ret)
-		ret = m5mols_mode(info, REG_CAPTURE);
+		ret = m5mols_set_mode(info, REG_CAPTURE);
 	if (!ret)
 		/* Wait until a frame is captured to ISP internal memory */
 		ret = m5mols_wait_interrupt(sd, REG_INT_CAPTURE, 2000);
-	if (!ret)
-		ret = m5mols_lock_3a(info, false);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/video/m5mols/m5mols_controls.c b/drivers/media/video/m5mols/m5mols_controls.c
index d135d20d09cf..392a028730e2 100644
--- a/drivers/media/video/m5mols/m5mols_controls.c
+++ b/drivers/media/video/m5mols/m5mols_controls.c
@@ -139,7 +139,7 @@ int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
 	if (mode > REG_SCENE_CANDLE)
 		return -EINVAL;
 
-	ret = m5mols_lock_3a(info, false);
+	ret = v4l2_ctrl_s_ctrl(info->lock_3a, 0);
 	if (!ret)
 		ret = m5mols_write(sd, AE_EV_PRESET_MONITOR, mode);
 	if (!ret)
@@ -169,7 +169,7 @@ int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
 	if (!ret)
 		ret = m5mols_write(sd, AE_ISO, scenemode.iso);
 	if (!ret)
-		ret = m5mols_mode(info, REG_CAPTURE);
+		ret = m5mols_set_mode(info, REG_CAPTURE);
 	if (!ret)
 		ret = m5mols_write(sd, CAPP_WDR_EN, scenemode.wdr);
 	if (!ret)
@@ -181,119 +181,448 @@ int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
 	if (!ret)
 		ret = m5mols_write(sd, CAPC_MODE, scenemode.capt_mode);
 	if (!ret)
-		ret = m5mols_mode(info, REG_MONITOR);
+		ret = m5mols_set_mode(info, REG_MONITOR);
 
 	return ret;
 }
 
-static int m5mols_lock_ae(struct m5mols_info *info, bool lock)
+static int m5mols_3a_lock(struct m5mols_info *info, struct v4l2_ctrl *ctrl)
 {
+	bool af_lock = ctrl->val & V4L2_LOCK_FOCUS;
 	int ret = 0;
 
-	if (info->lock_ae != lock)
-		ret = m5mols_write(&info->sd, AE_LOCK,
-				lock ? REG_AE_LOCK : REG_AE_UNLOCK);
-	if (!ret)
-		info->lock_ae = lock;
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_EXPOSURE) {
+		bool ae_lock = ctrl->val & V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_write(&info->sd, AE_LOCK, ae_lock ?
+				   REG_AE_LOCK : REG_AE_UNLOCK);
+		if (ret)
+			return ret;
+	}
+
+	if (((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_WHITE_BALANCE)
+	    && info->auto_wb->val) {
+		bool awb_lock = ctrl->val & V4L2_LOCK_WHITE_BALANCE;
+
+		ret = m5mols_write(&info->sd, AWB_LOCK, awb_lock ?
+				   REG_AWB_LOCK : REG_AWB_UNLOCK);
+		if (ret)
+			return ret;
+	}
+
+	if (!info->ver.af || !af_lock)
+		return ret;
+
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_FOCUS)
+		ret = m5mols_write(&info->sd, AF_EXECUTE, REG_AF_STOP);
+
+	return ret;
+}
+
+static int m5mols_set_metering_mode(struct m5mols_info *info, int mode)
+{
+	unsigned int metering;
+
+	switch (mode) {
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+		metering = REG_AE_CENTER;
+		break;
+	case V4L2_EXPOSURE_METERING_SPOT:
+		metering = REG_AE_SPOT;
+		break;
+	default:
+		metering = REG_AE_ALL;
+		break;
+	}
+
+	return m5mols_write(&info->sd, AE_MODE, metering);
+}
+
+static int m5mols_set_exposure(struct m5mols_info *info, int exposure)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = 0;
+
+	if (exposure == V4L2_EXPOSURE_AUTO) {
+		/* Unlock auto exposure */
+		info->lock_3a->val &= ~V4L2_LOCK_EXPOSURE;
+		m5mols_3a_lock(info, info->lock_3a);
+
+		ret = m5mols_set_metering_mode(info, info->metering->val);
+		if (ret < 0)
+			return ret;
+
+		v4l2_dbg(1, m5mols_debug, sd,
+			 "%s: exposure bias: %#x, metering: %#x\n",
+			 __func__, info->exposure_bias->val,
+			 info->metering->val);
+
+		return m5mols_write(sd, AE_INDEX, info->exposure_bias->val);
+	}
+
+	if (exposure == V4L2_EXPOSURE_MANUAL) {
+		ret = m5mols_write(sd, AE_MODE, REG_AE_OFF);
+		if (ret == 0)
+			ret = m5mols_write(sd, AE_MAN_GAIN_MON,
+					   info->exposure->val);
+		if (ret == 0)
+			ret = m5mols_write(sd, AE_MAN_GAIN_CAP,
+					   info->exposure->val);
+
+		v4l2_dbg(1, m5mols_debug, sd, "%s: exposure: %#x\n",
+			 __func__, info->exposure->val);
+	}
+
+	return ret;
+}
+
+static int m5mols_set_white_balance(struct m5mols_info *info, int val)
+{
+	static const unsigned short wb[][2] = {
+		{ V4L2_WHITE_BALANCE_INCANDESCENT,  REG_AWB_INCANDESCENT },
+		{ V4L2_WHITE_BALANCE_FLUORESCENT,   REG_AWB_FLUORESCENT_1 },
+		{ V4L2_WHITE_BALANCE_FLUORESCENT_H, REG_AWB_FLUORESCENT_2 },
+		{ V4L2_WHITE_BALANCE_HORIZON,       REG_AWB_HORIZON },
+		{ V4L2_WHITE_BALANCE_DAYLIGHT,      REG_AWB_DAYLIGHT },
+		{ V4L2_WHITE_BALANCE_FLASH,         REG_AWB_LEDLIGHT },
+		{ V4L2_WHITE_BALANCE_CLOUDY,        REG_AWB_CLOUDY },
+		{ V4L2_WHITE_BALANCE_SHADE,         REG_AWB_SHADE },
+		{ V4L2_WHITE_BALANCE_AUTO,          REG_AWB_AUTO },
+	};
+	int i;
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(wb); i++) {
+		int awb;
+		if (wb[i][0] != val)
+			continue;
+
+		v4l2_dbg(1, m5mols_debug, sd,
+			 "Setting white balance to: %#x\n", wb[i][0]);
+
+		awb = wb[i][0] == V4L2_WHITE_BALANCE_AUTO;
+		ret = m5mols_write(sd, AWB_MODE, awb ? REG_AWB_AUTO :
+						 REG_AWB_PRESET);
+		if (ret < 0)
+			return ret;
+
+		if (!awb)
+			ret = m5mols_write(sd, AWB_MANUAL, wb[i][1]);
+	}
 
 	return ret;
 }
 
-static int m5mols_lock_awb(struct m5mols_info *info, bool lock)
+static int m5mols_set_saturation(struct m5mols_info *info, int val)
+{
+	int ret = m5mols_write(&info->sd, MON_CHROMA_LVL, val);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(&info->sd, MON_CHROMA_EN, REG_CHROMA_ON);
+}
+
+static int m5mols_set_color_effect(struct m5mols_info *info, int val)
 {
+	unsigned int m_effect = REG_COLOR_EFFECT_OFF;
+	unsigned int p_effect = REG_EFFECT_OFF;
+	unsigned int cfix_r = 0, cfix_b = 0;
+	struct v4l2_subdev *sd = &info->sd;
 	int ret = 0;
 
-	if (info->lock_awb != lock)
-		ret = m5mols_write(&info->sd, AWB_LOCK,
-				lock ? REG_AWB_LOCK : REG_AWB_UNLOCK);
+	switch (val) {
+	case V4L2_COLORFX_BW:
+		m_effect = REG_COLOR_EFFECT_ON;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		p_effect = REG_EFFECT_NEGA;
+		break;
+	case V4L2_COLORFX_EMBOSS:
+		p_effect = REG_EFFECT_EMBOSS;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		m_effect = REG_COLOR_EFFECT_ON;
+		cfix_r = REG_CFIXR_SEPIA;
+		cfix_b = REG_CFIXB_SEPIA;
+		break;
+	}
+
+	ret = m5mols_write(sd, PARM_EFFECT, p_effect);
 	if (!ret)
-		info->lock_awb = lock;
+		ret = m5mols_write(sd, MON_EFFECT, m_effect);
+
+	if (ret == 0 && m_effect == REG_COLOR_EFFECT_ON) {
+		ret = m5mols_write(sd, MON_CFIXR, cfix_r);
+		if (!ret)
+			ret = m5mols_write(sd, MON_CFIXB, cfix_b);
+	}
+
+	v4l2_dbg(1, m5mols_debug, sd,
+		 "p_effect: %#x, m_effect: %#x, r: %#x, b: %#x (%d)\n",
+		 p_effect, m_effect, cfix_r, cfix_b, ret);
 
 	return ret;
 }
 
-/* m5mols_lock_3a() - Lock 3A(Auto Exposure, Auto Whitebalance, Auto Focus) */
-int m5mols_lock_3a(struct m5mols_info *info, bool lock)
+static int m5mols_set_iso(struct m5mols_info *info, int auto_iso)
+{
+	u32 iso = auto_iso ? 0 : info->iso->val + 1;
+
+	return m5mols_write(&info->sd, AE_ISO, iso);
+}
+
+static int m5mols_set_wdr(struct m5mols_info *info, int wdr)
 {
 	int ret;
 
-	ret = m5mols_lock_ae(info, lock);
-	if (!ret)
-		ret = m5mols_lock_awb(info, lock);
-	/* Don't need to handle unlocking AF */
-	if (!ret && is_available_af(info) && lock)
-		ret = m5mols_write(&info->sd, AF_EXECUTE, REG_AF_STOP);
+	ret = m5mols_write(&info->sd, MON_TONE_CTL, wdr ? 9 : 5);
+	if (ret < 0)
+		return ret;
+
+	ret = m5mols_set_mode(info, REG_CAPTURE);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(&info->sd, CAPP_WDR_EN, wdr);
+}
+
+static int m5mols_set_stabilization(struct m5mols_info *info, int val)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	unsigned int evp = val ? 0xe : 0x0;
+	int ret;
+
+	ret = m5mols_write(sd, AE_EV_PRESET_MONITOR, evp);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(sd, AE_EV_PRESET_CAPTURE, evp);
+}
+
+static int m5mols_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct m5mols_info *info = to_m5mols(sd);
+	int ret = 0;
+	u8 status;
+
+	v4l2_dbg(1, m5mols_debug, sd, "%s: ctrl: %s (%d)\n",
+		 __func__, ctrl->name, info->isp_ready);
+
+	if (!info->isp_ready)
+		return -EBUSY;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		ret = m5mols_read_u8(sd, AE_ISO, &status);
+		if (ret == 0)
+			ctrl->val = !status;
+		if (status != REG_ISO_AUTO)
+			info->iso->val = status - 1;
+		break;
+
+	case V4L2_CID_3A_LOCK:
+		ctrl->val &= ~0x7;
+
+		ret = m5mols_read_u8(sd, AE_LOCK, &status);
+		if (ret)
+			return ret;
+		if (status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_read_u8(sd, AWB_LOCK, &status);
+		if (ret)
+			return ret;
+		if (status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_read_u8(sd, AF_EXECUTE, &status);
+		if (!status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+		break;
+	}
 
 	return ret;
 }
 
-/* m5mols_set_ctrl() - The main s_ctrl function called by m5mols_set_ctrl() */
-int m5mols_set_ctrl(struct v4l2_ctrl *ctrl)
+static int m5mols_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	unsigned int ctrl_mode = m5mols_get_ctrl_mode(ctrl);
 	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct m5mols_info *info = to_m5mols(sd);
-	int ret;
+	int last_mode = info->mode;
+	int ret = 0;
+
+	/*
+	 * If needed, defer restoring the controls until
+	 * the device is fully initialized.
+	 */
+	if (!info->isp_ready) {
+		info->ctrl_sync = 0;
+		return 0;
+	}
+
+	v4l2_dbg(1, m5mols_debug, sd, "%s: %s, val: %d, priv: %#x\n",
+		 __func__, ctrl->name, ctrl->val, (int)ctrl->priv);
+
+	if (ctrl_mode && ctrl_mode != info->mode) {
+		ret = m5mols_set_mode(info, ctrl_mode);
+		if (ret < 0)
+			return ret;
+	}
 
 	switch (ctrl->id) {
+	case V4L2_CID_3A_LOCK:
+		ret = m5mols_3a_lock(info, ctrl);
+		break;
+
 	case V4L2_CID_ZOOM_ABSOLUTE:
-		return m5mols_write(sd, MON_ZOOM, ctrl->val);
+		ret = m5mols_write(sd, MON_ZOOM, ctrl->val);
+		break;
 
 	case V4L2_CID_EXPOSURE_AUTO:
-		ret = m5mols_lock_ae(info,
-			ctrl->val == V4L2_EXPOSURE_AUTO ? false : true);
-		if (!ret && ctrl->val == V4L2_EXPOSURE_AUTO)
-			ret = m5mols_write(sd, AE_MODE, REG_AE_ALL);
-		if (!ret && ctrl->val == V4L2_EXPOSURE_MANUAL) {
-			int val = info->exposure->val;
-			ret = m5mols_write(sd, AE_MODE, REG_AE_OFF);
-			if (!ret)
-				ret = m5mols_write(sd, AE_MAN_GAIN_MON, val);
-			if (!ret)
-				ret = m5mols_write(sd, AE_MAN_GAIN_CAP, val);
-		}
-		return ret;
+		ret = m5mols_set_exposure(info, ctrl->val);
+		break;
 
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		ret = m5mols_lock_awb(info, ctrl->val ? false : true);
-		if (!ret)
-			ret = m5mols_write(sd, AWB_MODE, ctrl->val ?
-				REG_AWB_AUTO : REG_AWB_PRESET);
-		return ret;
+	case V4L2_CID_ISO_SENSITIVITY:
+		ret = m5mols_set_iso(info, ctrl->val);
+		break;
+
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ret = m5mols_set_white_balance(info, ctrl->val);
+		break;
 
 	case V4L2_CID_SATURATION:
-		ret = m5mols_write(sd, MON_CHROMA_LVL, ctrl->val);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CHROMA_EN, REG_CHROMA_ON);
-		return ret;
+		ret = m5mols_set_saturation(info, ctrl->val);
+		break;
 
 	case V4L2_CID_COLORFX:
-		/*
-		 * This control uses two kinds of registers: normal & color.
-		 * The normal effect belongs to category 1, while the color
-		 * one belongs to category 2.
-		 *
-		 * The normal effect uses one register: CAT1_EFFECT.
-		 * The color effect uses three registers:
-		 * CAT2_COLOR_EFFECT, CAT2_CFIXR, CAT2_CFIXB.
-		 */
-		ret = m5mols_write(sd, PARM_EFFECT,
-			ctrl->val == V4L2_COLORFX_NEGATIVE ? REG_EFFECT_NEGA :
-			ctrl->val == V4L2_COLORFX_EMBOSS ? REG_EFFECT_EMBOSS :
-			REG_EFFECT_OFF);
-		if (!ret)
-			ret = m5mols_write(sd, MON_EFFECT,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_COLOR_EFFECT_ON : REG_COLOR_EFFECT_OFF);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CFIXR,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_CFIXR_SEPIA : 0);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CFIXB,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_CFIXB_SEPIA : 0);
+		ret = m5mols_set_color_effect(info, ctrl->val);
+		break;
+
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:
+		ret = m5mols_set_wdr(info, ctrl->val);
+		break;
+
+	case V4L2_CID_IMAGE_STABILIZATION:
+		ret = m5mols_set_stabilization(info, ctrl->val);
+		break;
+
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		ret = m5mols_write(sd, CAPP_JPEG_RATIO, ctrl->val);
+		break;
+	}
+
+	if (ret == 0 && info->mode != last_mode)
+		ret = m5mols_set_mode(info, last_mode);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops m5mols_ctrl_ops = {
+	.g_volatile_ctrl	= m5mols_g_volatile_ctrl,
+	.s_ctrl			= m5mols_s_ctrl,
+};
+
+/* Supported manual ISO values */
+static const s64 iso_qmenu[] = {
+	/* AE_ISO: 0x01...0x07 */
+	50, 100, 200, 400, 800, 1600, 3200
+};
+
+/* Supported Exposure Bias values, -2.0EV...+2.0EV */
+static const s64 ev_bias_qmenu[] = {
+	/* AE_INDEX: 0x00...0x08 */
+	-2000, -1500, -1000, -500, 0, 500, 1000, 1500, 2000
+};
+
+int m5mols_init_controls(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	u16 exposure_max;
+	u16 zoom_step;
+	int ret;
+
+	/* Determine the firmware dependant control range and step values */
+	ret = m5mols_read_u16(sd, AE_MAX_GAIN_MON, &exposure_max);
+	if (ret < 0)
+		return ret;
+
+	zoom_step = is_manufacturer(info, REG_SAMSUNG_OPTICS) ? 31 : 1;
+	v4l2_ctrl_handler_init(&info->handle, 20);
+
+	info->auto_wb = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+			9, ~0x3fe, V4L2_WHITE_BALANCE_AUTO);
+
+	/* Exposure control cluster */
+	info->auto_exposure = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
+			1, ~0x03, V4L2_EXPOSURE_AUTO);
+
+	info->exposure = v4l2_ctrl_new_std(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE,
+			0, exposure_max, 1, exposure_max / 2);
+
+	info->exposure_bias = v4l2_ctrl_new_int_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
+			ARRAY_SIZE(ev_bias_qmenu) - 1,
+			ARRAY_SIZE(ev_bias_qmenu)/2 - 1,
+			ev_bias_qmenu);
+
+	info->metering = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_METERING,
+			2, ~0x7, V4L2_EXPOSURE_METERING_AVERAGE);
+
+	/* ISO control cluster */
+	info->auto_iso = v4l2_ctrl_new_std_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ISO_SENSITIVITY_AUTO, 1, ~0x03, 1);
+
+	info->iso = v4l2_ctrl_new_int_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ISO_SENSITIVITY, ARRAY_SIZE(iso_qmenu) - 1,
+			ARRAY_SIZE(iso_qmenu)/2 - 1, iso_qmenu);
+
+	info->saturation = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_SATURATION, 1, 5, 1, 3);
+
+	info->zoom = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ZOOM_ABSOLUTE, 1, 70, zoom_step, 1);
+
+	info->colorfx = v4l2_ctrl_new_std_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_COLORFX, 4, 0, V4L2_COLORFX_NONE);
+
+	info->wdr = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_WIDE_DYNAMIC_RANGE, 0, 1, 1, 0);
+
+	info->stabilization = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_IMAGE_STABILIZATION, 0, 1, 1, 0);
+
+	info->jpeg_quality = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 1, 100, 1, 80);
+
+	info->lock_3a = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_3A_LOCK, 0, 0x7, 0, 0);
+
+	if (info->handle.error) {
+		int ret = info->handle.error;
+		v4l2_err(sd, "Failed to initialize controls: %d\n", ret);
+		v4l2_ctrl_handler_free(&info->handle);
 		return ret;
 	}
 
-	return -EINVAL;
+	v4l2_ctrl_auto_cluster(4, &info->auto_exposure, 1, false);
+	info->auto_iso->flags |= V4L2_CTRL_FLAG_VOLATILE |
+				V4L2_CTRL_FLAG_UPDATE;
+	v4l2_ctrl_auto_cluster(2, &info->auto_iso, 0, false);
+
+	info->lock_3a->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	m5mols_set_ctrl_mode(info->auto_exposure, REG_PARAMETER);
+	m5mols_set_ctrl_mode(info->auto_wb, REG_PARAMETER);
+	m5mols_set_ctrl_mode(info->colorfx, REG_MONITOR);
+
+	sd->ctrl_handler = &info->handle;
+
+	return 0;
 }
diff --git a/drivers/media/video/m5mols/m5mols_core.c b/drivers/media/video/m5mols/m5mols_core.c
index d718aee01c77..ac7d28b6ddf2 100644
--- a/drivers/media/video/m5mols/m5mols_core.c
+++ b/drivers/media/video/m5mols/m5mols_core.c
@@ -362,14 +362,14 @@ static int m5mols_reg_mode(struct v4l2_subdev *sd, u8 mode)
 }
 
 /**
- * m5mols_mode - manage the M-5MOLS's mode
+ * m5mols_set_mode - set the M-5MOLS controller mode
  * @mode: the required operation mode
  *
  * The commands of M-5MOLS are grouped into specific modes. Each functionality
- * can be guaranteed only when the sensor is operating in mode which which
- * a command belongs to.
+ * can be guaranteed only when the sensor is operating in mode which a command
+ * belongs to.
  */
-int m5mols_mode(struct m5mols_info *info, u8 mode)
+int m5mols_set_mode(struct m5mols_info *info, u8 mode)
 {
 	struct v4l2_subdev *sd = &info->sd;
 	int ret = -EINVAL;
@@ -645,13 +645,13 @@ static int m5mols_start_monitor(struct m5mols_info *info)
 	struct v4l2_subdev *sd = &info->sd;
 	int ret;
 
-	ret = m5mols_mode(info, REG_PARAMETER);
+	ret = m5mols_set_mode(info, REG_PARAMETER);
 	if (!ret)
 		ret = m5mols_write(sd, PARM_MON_SIZE, info->resolution);
 	if (!ret)
 		ret = m5mols_write(sd, PARM_MON_FPS, REG_FPS_30);
 	if (!ret)
-		ret = m5mols_mode(info, REG_MONITOR);
+		ret = m5mols_set_mode(info, REG_MONITOR);
 	if (!ret)
 		ret = m5mols_restore_controls(info);
 
@@ -674,42 +674,13 @@ static int m5mols_s_stream(struct v4l2_subdev *sd, int enable)
 		return ret;
 	}
 
-	return m5mols_mode(info, REG_PARAMETER);
+	return m5mols_set_mode(info, REG_PARAMETER);
 }
 
 static const struct v4l2_subdev_video_ops m5mols_video_ops = {
 	.s_stream	= m5mols_s_stream,
 };
 
-static int m5mols_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct v4l2_subdev *sd = to_sd(ctrl);
-	struct m5mols_info *info = to_m5mols(sd);
-	int ispstate = info->mode;
-	int ret;
-
-	/*
-	 * If needed, defer restoring the controls until
-	 * the device is fully initialized.
-	 */
-	if (!info->isp_ready) {
-		info->ctrl_sync = 0;
-		return 0;
-	}
-
-	ret = m5mols_mode(info, REG_PARAMETER);
-	if (ret < 0)
-		return ret;
-	ret = m5mols_set_ctrl(ctrl);
-	if (ret < 0)
-		return ret;
-	return m5mols_mode(info, ispstate);
-}
-
-static const struct v4l2_ctrl_ops m5mols_ctrl_ops = {
-	.s_ctrl	= m5mols_s_ctrl,
-};
-
 static int m5mols_sensor_power(struct m5mols_info *info, bool enable)
 {
 	struct v4l2_subdev *sd = &info->sd;
@@ -802,52 +773,6 @@ static int m5mols_fw_start(struct v4l2_subdev *sd)
 	return ret;
 }
 
-static int m5mols_init_controls(struct m5mols_info *info)
-{
-	struct v4l2_subdev *sd = &info->sd;
-	u16 max_exposure;
-	u16 step_zoom;
-	int ret;
-
-	/* Determine value's range & step of controls for various FW version */
-	ret = m5mols_read_u16(sd, AE_MAX_GAIN_MON, &max_exposure);
-	if (!ret)
-		step_zoom = is_manufacturer(info, REG_SAMSUNG_OPTICS) ? 31 : 1;
-	if (ret)
-		return ret;
-
-	v4l2_ctrl_handler_init(&info->handle, 6);
-	info->autowb = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_AUTO_WHITE_BALANCE,
-			0, 1, 1, 0);
-	info->saturation = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_SATURATION,
-			1, 5, 1, 3);
-	info->zoom = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_ZOOM_ABSOLUTE,
-			1, 70, step_zoom, 1);
-	info->exposure = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE,
-			0, max_exposure, 1, (int)max_exposure/2);
-	info->colorfx = v4l2_ctrl_new_std_menu(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_COLORFX,
-			4, (1 << V4L2_COLORFX_BW), V4L2_COLORFX_NONE);
-	info->autoexposure = v4l2_ctrl_new_std_menu(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
-			1, 0, V4L2_EXPOSURE_AUTO);
-
-	sd->ctrl_handler = &info->handle;
-	if (info->handle.error) {
-		v4l2_err(sd, "Failed to initialize controls: %d\n", ret);
-		v4l2_ctrl_handler_free(&info->handle);
-		return info->handle.error;
-	}
-
-	v4l2_ctrl_cluster(2, &info->autoexposure);
-
-	return 0;
-}
-
 /**
  * m5mols_s_power - Main sensor power control function
  *
@@ -868,7 +793,7 @@ static int m5mols_s_power(struct v4l2_subdev *sd, int on)
 	}
 
 	if (is_manufacturer(info, REG_SAMSUNG_TECHWIN)) {
-		ret = m5mols_mode(info, REG_MONITOR);
+		ret = m5mols_set_mode(info, REG_MONITOR);
 		if (!ret)
 			ret = m5mols_write(sd, AF_EXECUTE, REG_AF_STOP);
 		if (!ret)
@@ -1010,7 +935,7 @@ static int __devinit m5mols_probe(struct i2c_client *client,
 
 	ret = m5mols_fw_start(sd);
 	if (!ret)
-		ret = m5mols_init_controls(info);
+		ret = m5mols_init_controls(sd);
 
 	m5mols_sensor_power(info, false);
 	if (!ret)
diff --git a/drivers/media/video/m5mols/m5mols_reg.h b/drivers/media/video/m5mols/m5mols_reg.h
index ae4aced0f9b2..14d4be72aeff 100644
--- a/drivers/media/video/m5mols/m5mols_reg.h
+++ b/drivers/media/video/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_RATIO		I2C_REG(CAT_CAPT_PARM, 0x17, 1)
 
 #define CAPP_MCC_MODE		I2C_REG(CAT_CAPT_PARM, 0x1d, 1)
 #define REG_MCC_OFF		0x00
diff --git a/drivers/media/video/marvell-ccic/mcam-core.c b/drivers/media/video/marvell-ccic/mcam-core.c
index 996ac34d9a89..ce2b7b4788d6 100644
--- a/drivers/media/video/marvell-ccic/mcam-core.c
+++ b/drivers/media/video/marvell-ccic/mcam-core.c
@@ -1356,7 +1356,6 @@ static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
 			goto out;
 	}
 	mcam_set_config_needed(cam, 1);
-	ret = 0;
 out:
 	mutex_unlock(&cam->s_mutex);
 	return ret;
diff --git a/drivers/media/video/mem2mem_testdev.c b/drivers/media/video/mem2mem_testdev.c
index 12897e8a3314..d2dec585e61b 100644
--- a/drivers/media/video/mem2mem_testdev.c
+++ b/drivers/media/video/mem2mem_testdev.c
@@ -40,7 +40,7 @@ MODULE_VERSION("0.1.1");
 #define MIN_H 32
 #define MAX_W 640
 #define MAX_H 480
-#define DIM_ALIGN_MASK 0x08 /* 8-alignment for dimensions */
+#define DIM_ALIGN_MASK 7 /* 8-byte alignment for line length */
 
 /* Flags that indicate a format can be used for capture/output */
 #define MEM2MEM_CAPTURE	(1 << 0)
@@ -958,6 +958,10 @@ static int m2mtest_probe(struct platform_device *pdev)
 	}
 
 	*vfd = m2mtest_videodev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &dev->dev_mutex;
 
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
diff --git a/drivers/media/video/meye.c b/drivers/media/video/meye.c
index b09a3c80a15e..7bc775219f97 100644
--- a/drivers/media/video/meye.c
+++ b/drivers/media/video/meye.c
@@ -1570,7 +1570,7 @@ static long vidioc_default(struct file *file, void *fh, bool valid_prio,
 		return meyeioc_stilljcapt((int *) arg);
 
 	default:
-		return -EINVAL;
+		return -ENOTTY;
 	}
 
 }
diff --git a/drivers/media/video/msp3400-driver.c b/drivers/media/video/msp3400-driver.c
index 82ce50721de3..aeb22be7dcbd 100644
--- a/drivers/media/video/msp3400-driver.c
+++ b/drivers/media/video/msp3400-driver.c
@@ -597,19 +597,23 @@ static int msp_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int msp_suspend(struct i2c_client *client, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int msp_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	v4l_dbg(1, msp_debug, client, "suspend\n");
 	msp_reset(client);
 	return 0;
 }
 
-static int msp_resume(struct i2c_client *client)
+static int msp_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	v4l_dbg(1, msp_debug, client, "resume\n");
 	msp_wake_thread(client);
 	return 0;
 }
+#endif
 
 /* ----------------------------------------------------------------------- */
 
@@ -863,6 +867,10 @@ static int msp_remove(struct i2c_client *client)
 
 /* ----------------------------------------------------------------------- */
 
+static const struct dev_pm_ops msp3400_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(msp_suspend, msp_resume)
+};
+
 static const struct i2c_device_id msp_id[] = {
 	{ "msp3400", 0 },
 	{ }
@@ -873,11 +881,10 @@ static struct i2c_driver msp_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= "msp3400",
+		.pm	= &msp3400_pm_ops,
 	},
 	.probe		= msp_probe,
 	.remove		= msp_remove,
-	.suspend	= msp_suspend,
-	.resume		= msp_resume,
 	.id_table	= msp_id,
 };
 
diff --git a/drivers/media/video/mt9m032.c b/drivers/media/video/mt9m032.c
index 645973c5feb0..3c1e626139b7 100644
--- a/drivers/media/video/mt9m032.c
+++ b/drivers/media/video/mt9m032.c
@@ -838,9 +838,9 @@ static int mt9m032_remove(struct i2c_client *client)
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct mt9m032 *sensor = to_mt9m032(subdev);
 
-	v4l2_device_unregister_subdev(&sensor->subdev);
+	v4l2_device_unregister_subdev(subdev);
 	v4l2_ctrl_handler_free(&sensor->ctrls);
-	media_entity_cleanup(&sensor->subdev.entity);
+	media_entity_cleanup(&subdev->entity);
 	mutex_destroy(&sensor->lock);
 	kfree(sensor);
 	return 0;
diff --git a/drivers/media/video/mt9p031.c b/drivers/media/video/mt9p031.c
index c81eaf4fbe01..8f061d9ac443 100644
--- a/drivers/media/video/mt9p031.c
+++ b/drivers/media/video/mt9p031.c
@@ -14,6 +14,7 @@
 
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/gpio.h>
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/log2.h>
@@ -90,7 +91,14 @@
 #define		MT9P031_GLOBAL_GAIN_MAX			1024
 #define		MT9P031_GLOBAL_GAIN_DEF			8
 #define		MT9P031_GLOBAL_GAIN_MULT		(1 << 6)
+#define MT9P031_ROW_BLACK_TARGET			0x49
 #define MT9P031_ROW_BLACK_DEF_OFFSET			0x4b
+#define MT9P031_GREEN1_OFFSET				0x60
+#define MT9P031_GREEN2_OFFSET				0x61
+#define MT9P031_BLACK_LEVEL_CALIBRATION			0x62
+#define		MT9P031_BLC_MANUAL_BLC			(1 << 0)
+#define MT9P031_RED_OFFSET				0x63
+#define MT9P031_BLUE_OFFSET				0x64
 #define MT9P031_TEST_PATTERN				0xa0
 #define		MT9P031_TEST_PATTERN_SHIFT		3
 #define		MT9P031_TEST_PATTERN_ENABLE		(1 << 0)
@@ -99,17 +107,27 @@
 #define MT9P031_TEST_PATTERN_RED			0xa2
 #define MT9P031_TEST_PATTERN_BLUE			0xa3
 
+enum mt9p031_model {
+	MT9P031_MODEL_COLOR,
+	MT9P031_MODEL_MONOCHROME,
+};
+
 struct mt9p031 {
 	struct v4l2_subdev subdev;
 	struct media_pad pad;
 	struct v4l2_rect crop;  /* Sensor window */
 	struct v4l2_mbus_framefmt format;
-	struct v4l2_ctrl_handler ctrls;
 	struct mt9p031_platform_data *pdata;
 	struct mutex power_lock; /* lock to protect power_count */
 	int power_count;
 
+	enum mt9p031_model model;
 	struct aptina_pll pll;
+	int reset;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *blc_auto;
+	struct v4l2_ctrl *blc_offset;
 
 	/* Registers cache */
 	u16 output_control;
@@ -241,8 +259,8 @@ static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
 static int mt9p031_power_on(struct mt9p031 *mt9p031)
 {
 	/* Ensure RESET_BAR is low */
-	if (mt9p031->pdata->reset) {
-		mt9p031->pdata->reset(&mt9p031->subdev, 1);
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 0);
 		usleep_range(1000, 2000);
 	}
 
@@ -252,8 +270,8 @@ static int mt9p031_power_on(struct mt9p031 *mt9p031)
 					 mt9p031->pdata->ext_freq);
 
 	/* Now RESET_BAR must be high */
-	if (mt9p031->pdata->reset) {
-		mt9p031->pdata->reset(&mt9p031->subdev, 0);
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 1);
 		usleep_range(1000, 2000);
 	}
 
@@ -262,8 +280,8 @@ static int mt9p031_power_on(struct mt9p031 *mt9p031)
 
 static void mt9p031_power_off(struct mt9p031 *mt9p031)
 {
-	if (mt9p031->pdata->reset) {
-		mt9p031->pdata->reset(&mt9p031->subdev, 1);
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 0);
 		usleep_range(1000, 2000);
 	}
 
@@ -557,6 +575,10 @@ static int mt9p031_set_crop(struct v4l2_subdev *subdev,
  */
 
 #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)
+#define V4L2_CID_BLC_DIGITAL_OFFSET	(V4L2_CID_USER_BASE | 0x1005)
 
 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 {
@@ -621,11 +643,17 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	case V4L2_CID_TEST_PATTERN:
 		if (!ctrl->val) {
-			ret = mt9p031_set_mode2(mt9p031,
-					0, MT9P031_READ_MODE_2_ROW_BLC);
-			if (ret < 0)
-				return ret;
-
+			/* Restore the black level compensation settings. */
+			if (mt9p031->blc_auto->cur.val != 0) {
+				ret = mt9p031_s_ctrl(mt9p031->blc_auto);
+				if (ret < 0)
+					return ret;
+			}
+			if (mt9p031->blc_offset->cur.val != 0) {
+				ret = mt9p031_s_ctrl(mt9p031->blc_offset);
+				if (ret < 0)
+					return ret;
+			}
 			return mt9p031_write(client, MT9P031_TEST_PATTERN,
 					     MT9P031_TEST_PATTERN_DISABLE);
 		}
@@ -640,10 +668,14 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 		if (ret < 0)
 			return ret;
 
+		/* Disable digital black level compensation when using a test
+		 * pattern.
+		 */
 		ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
 					0);
 		if (ret < 0)
 			return ret;
+
 		ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
 		if (ret < 0)
 			return ret;
@@ -651,7 +683,40 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 		return mt9p031_write(client, MT9P031_TEST_PATTERN,
 				((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
 				| MT9P031_TEST_PATTERN_ENABLE);
+
+	case V4L2_CID_BLC_AUTO:
+		ret = mt9p031_set_mode2(mt9p031,
+				ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
+				ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
+				     ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
+
+	case V4L2_CID_BLC_TARGET_LEVEL:
+		return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
+				     ctrl->val);
+
+	case V4L2_CID_BLC_ANALOG_OFFSET:
+		data = ctrl->val & ((1 << 9) - 1);
+
+		ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
+
+	case V4L2_CID_BLC_DIGITAL_OFFSET:
+		return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
+				     ctrl->val & ((1 << 12) - 1));
 	}
+
 	return 0;
 }
 
@@ -685,6 +750,46 @@ static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
 		.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",
+		.min		= 0,
+		.max		= 1,
+		.step		= 1,
+		.def		= 1,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_TARGET_LEVEL,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Target Level",
+		.min		= 0,
+		.max		= 4095,
+		.step		= 1,
+		.def		= 168,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_ANALOG_OFFSET,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Analog Offset",
+		.min		= -255,
+		.max		= 255,
+		.step		= 1,
+		.def		= 32,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_DIGITAL_OFFSET,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Digital Offset",
+		.min		= -2048,
+		.max		= 2047,
+		.step		= 1,
+		.def		= 40,
+		.flags		= 0,
 	}
 };
 
@@ -764,7 +869,7 @@ static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 
 	format = v4l2_subdev_get_try_format(fh, 0);
 
-	if (mt9p031->pdata->version == MT9P031_MONOCHROME_VERSION)
+	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 		format->code = V4L2_MBUS_FMT_Y12_1X12;
 	else
 		format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
@@ -842,6 +947,8 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->pdata = pdata;
 	mt9p031->output_control	= MT9P031_OUTPUT_CONTROL_DEF;
 	mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
+	mt9p031->model = did->driver_data;
+	mt9p031->reset = -1;
 
 	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 4);
 
@@ -862,9 +969,16 @@ static int mt9p031_probe(struct i2c_client *client,
 
 	mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
 
-	if (mt9p031->ctrls.error)
+	if (mt9p031->ctrls.error) {
 		printk(KERN_INFO "%s: control initialization error %d\n",
 		       __func__, mt9p031->ctrls.error);
+		ret = mt9p031->ctrls.error;
+		goto done;
+	}
+
+	mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
+	mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
+					     V4L2_CID_BLC_DIGITAL_OFFSET);
 
 	mutex_init(&mt9p031->power_lock);
 	v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
@@ -882,7 +996,7 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
 	mt9p031->crop.top = MT9P031_ROW_START_DEF;
 
-	if (mt9p031->pdata->version == MT9P031_MONOCHROME_VERSION)
+	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 		mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
 	else
 		mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
@@ -892,10 +1006,22 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->format.field = V4L2_FIELD_NONE;
 	mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
 
+	if (pdata->reset != -1) {
+		ret = gpio_request_one(pdata->reset, GPIOF_OUT_INIT_LOW,
+				       "mt9p031_rst");
+		if (ret < 0)
+			goto done;
+
+		mt9p031->reset = pdata->reset;
+	}
+
 	ret = mt9p031_pll_setup(mt9p031);
 
 done:
 	if (ret < 0) {
+		if (mt9p031->reset != -1)
+			gpio_free(mt9p031->reset);
+
 		v4l2_ctrl_handler_free(&mt9p031->ctrls);
 		media_entity_cleanup(&mt9p031->subdev.entity);
 		kfree(mt9p031);
@@ -912,13 +1038,16 @@ static int mt9p031_remove(struct i2c_client *client)
 	v4l2_ctrl_handler_free(&mt9p031->ctrls);
 	v4l2_device_unregister_subdev(subdev);
 	media_entity_cleanup(&subdev->entity);
+	if (mt9p031->reset != -1)
+		gpio_free(mt9p031->reset);
 	kfree(mt9p031);
 
 	return 0;
 }
 
 static const struct i2c_device_id mt9p031_id[] = {
-	{ "mt9p031", 0 },
+	{ "mt9p031", MT9P031_MODEL_COLOR },
+	{ "mt9p031m", MT9P031_MODEL_MONOCHROME },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
diff --git a/drivers/media/video/mt9t112.c b/drivers/media/video/mt9t112.c
index 8d1445f12708..e1ae46a7ee96 100644
--- a/drivers/media/video/mt9t112.c
+++ b/drivers/media/video/mt9t112.c
@@ -453,6 +453,7 @@ static int mt9t112_init_pll(const struct i2c_client *client)
 	 * I2C Master Clock Divider
 	 */
 	mt9t112_reg_write(ret, client, 0x0014, 0x3046);
+	mt9t112_reg_write(ret, client, 0x0016, 0x0400); /* JPEG initialization workaround */
 	mt9t112_reg_write(ret, client, 0x0022, 0x0190);
 	mt9t112_reg_write(ret, client, 0x3B84, 0x0212);
 
diff --git a/drivers/media/video/mt9v032.c b/drivers/media/video/mt9v032.c
index 75e253a343c5..4ba4884c016e 100644
--- a/drivers/media/video/mt9v032.c
+++ b/drivers/media/video/mt9v032.c
@@ -481,7 +481,7 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	case V4L2_CID_EXPOSURE_AUTO:
 		return mt9v032_update_aec_agc(mt9v032, MT9V032_AEC_ENABLE,
-					      ctrl->val);
+					      !ctrl->val);
 
 	case V4L2_CID_EXPOSURE:
 		return mt9v032_write(client, MT9V032_TOTAL_SHUTTER_WIDTH,
diff --git a/drivers/media/video/mx1_camera.c b/drivers/media/video/mx1_camera.c
index 055d11ddb038..4296a8350298 100644
--- a/drivers/media/video/mx1_camera.c
+++ b/drivers/media/video/mx1_camera.c
@@ -126,13 +126,8 @@ static int mx1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 			      unsigned int *size)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	*size = bytes_per_line * icd->user_height;
+	*size = icd->sizeimage;
 
 	if (!*count)
 		*count = 32;
@@ -171,11 +166,6 @@ static int mx1_videobuf_prepare(struct videobuf_queue *vq,
 	struct soc_camera_device *icd = vq->priv_data;
 	struct mx1_buffer *buf = container_of(vb, struct mx1_buffer, vb);
 	int ret;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
 	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 		vb, vb->baddr, vb->bsize);
@@ -202,7 +192,7 @@ static int mx1_videobuf_prepare(struct videobuf_queue *vq,
 		vb->state	= VIDEOBUF_NEEDS_INIT;
 	}
 
-	vb->size = bytes_per_line * vb->height;
+	vb->size = icd->sizeimage;
 	if (0 != vb->baddr && vb->bsize < vb->size) {
 		ret = -EINVAL;
 		goto out;
diff --git a/drivers/media/video/mx2_camera.c b/drivers/media/video/mx2_camera.c
index 18afaeeadb7b..ded26b7286fa 100644
--- a/drivers/media/video/mx2_camera.c
+++ b/drivers/media/video/mx2_camera.c
@@ -22,6 +22,7 @@
 #include <linux/gcd.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/math64.h>
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
 #include <linux/time.h>
@@ -344,6 +345,19 @@ static struct mx2_fmt_cfg mx27_emma_prp_table[] = {
 					PRP_INTR_CH2OVF,
 		}
 	},
+	{
+		.in_fmt		= V4L2_MBUS_FMT_UYVY8_2X8,
+		.out_fmt	= V4L2_PIX_FMT_YUV420,
+		.cfg		= {
+			.channel	= 2,
+			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
+			.out_fmt	= PRP_CNTL_CH2_OUT_YUV420,
+			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
+			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH2WERR |
+					PRP_INTR_CH2FC | PRP_INTR_LBOVF |
+					PRP_INTR_CH2OVF,
+		}
+	},
 };
 
 static struct mx2_fmt_cfg *mx27_emma_prp_get_format(
@@ -525,8 +539,6 @@ static int mx2_videobuf_setup(struct vb2_queue *vq,
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx2_camera_dev *pcdev = ici->priv;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 
 	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, sizes[0]);
 
@@ -534,12 +546,9 @@ static int mx2_videobuf_setup(struct vb2_queue *vq,
 	if (fmt != NULL)
 		return -ENOTTY;
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
 	alloc_ctxs[0] = pcdev->alloc_ctx;
 
-	sizes[0] = bytes_per_line * icd->user_height;
+	sizes[0] = icd->sizeimage;
 
 	if (0 == *count)
 		*count = 32;
@@ -555,16 +564,11 @@ static int mx2_videobuf_setup(struct vb2_queue *vq,
 static int mx2_videobuf_prepare(struct vb2_buffer *vb)
 {
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 	int ret = 0;
 
 	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
 		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
 #ifdef DEBUG
 	/*
 	 * This can be useful if you want to see if we actually fill
@@ -574,7 +578,7 @@ static int mx2_videobuf_prepare(struct vb2_buffer *vb)
 	       0xaa, vb2_get_plane_payload(vb, 0));
 #endif
 
-	vb2_set_plane_payload(vb, 0, bytes_per_line * icd->user_height);
+	vb2_set_plane_payload(vb, 0, icd->sizeimage);
 	if (vb2_plane_vaddr(vb, 0) &&
 	    vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
 		ret = -EINVAL;
@@ -980,6 +984,7 @@ static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx2_camera_dev *pcdev = ici->priv;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	const struct soc_camera_format_xlate *xlate;
 	unsigned long common_flags;
 	int ret;
 	int bytesperline;
@@ -1024,14 +1029,31 @@ static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 		return ret;
 	}
 
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	if (xlate->code == V4L2_MBUS_FMT_YUYV8_2X8) {
+		csicr1 |= CSICR1_PACK_DIR;
+		csicr1 &= ~CSICR1_SWAP16_EN;
+		dev_dbg(icd->parent, "already yuyv format, don't convert\n");
+	} else if (xlate->code == V4L2_MBUS_FMT_UYVY8_2X8) {
+		csicr1 &= ~CSICR1_PACK_DIR;
+		csicr1 |= CSICR1_SWAP16_EN;
+		dev_dbg(icd->parent, "convert uyvy mbus format into yuyv\n");
+	} else {
+		dev_warn(icd->parent, "mbus format not supported\n");
+		return -EINVAL;
+	}
+
 	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
 		csicr1 |= CSICR1_REDGE;
 	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
 		csicr1 |= CSICR1_SOF_POL;
 	if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
 		csicr1 |= CSICR1_HSYNC_POL;
-	if (pcdev->platform_flags & MX2_CAMERA_SWAP16)
-		csicr1 |= CSICR1_SWAP16_EN;
 	if (pcdev->platform_flags & MX2_CAMERA_EXT_VSYNC)
 		csicr1 |= CSICR1_EXT_VSYNC;
 	if (pcdev->platform_flags & MX2_CAMERA_CCIR)
@@ -1042,8 +1064,6 @@ static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 		csicr1 |= CSICR1_GCLK_MODE;
 	if (pcdev->platform_flags & MX2_CAMERA_INV_DATA)
 		csicr1 |= CSICR1_INV_DATA;
-	if (pcdev->platform_flags & MX2_CAMERA_PACK_DIR_MSB)
-		csicr1 |= CSICR1_PACK_DIR;
 
 	pcdev->csicr1 = csicr1;
 
@@ -1118,7 +1138,8 @@ static int mx2_camera_get_formats(struct soc_camera_device *icd,
 		return 0;
 	}
 
-	if (code == V4L2_MBUS_FMT_YUYV8_2X8) {
+	if (code == V4L2_MBUS_FMT_YUYV8_2X8 ||
+	    code == V4L2_MBUS_FMT_UYVY8_2X8) {
 		formats++;
 		if (xlate) {
 			/*
@@ -1134,6 +1155,18 @@ static int mx2_camera_get_formats(struct soc_camera_device *icd,
 		}
 	}
 
+	if (code == V4L2_MBUS_FMT_UYVY8_2X8) {
+		formats++;
+		if (xlate) {
+			xlate->host_fmt =
+				soc_mbus_get_fmtdesc(V4L2_MBUS_FMT_YUYV8_2X8);
+			xlate->code	= code;
+			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
+				xlate->host_fmt->name, code);
+			xlate++;
+		}
+	}
+
 	/* Generic pass-trough */
 	formats++;
 	if (xlate) {
@@ -1363,17 +1396,20 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 				xlate->host_fmt);
 		if (pix->bytesperline < 0)
 			return pix->bytesperline;
-		pix->sizeimage = pix->height * pix->bytesperline;
+		pix->sizeimage = soc_mbus_image_size(xlate->host_fmt,
+						pix->bytesperline, pix->height);
 		/* Check against the CSIRXCNT limit */
 		if (pix->sizeimage > 4 * 0x3ffff) {
 			/* Adjust geometry, preserve aspect ratio */
-			unsigned int new_height = int_sqrt(4 * 0x3ffff *
-					pix->height / pix->bytesperline);
+			unsigned int new_height = int_sqrt(div_u64(0x3ffffULL *
+					4 * pix->height, pix->bytesperline));
 			pix->width = new_height * pix->width / pix->height;
 			pix->height = new_height;
 			pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
 							xlate->host_fmt);
 			BUG_ON(pix->bytesperline < 0);
+			pix->sizeimage = soc_mbus_image_size(xlate->host_fmt,
+						pix->bytesperline, pix->height);
 		}
 	}
 
@@ -1752,6 +1788,8 @@ static int __devinit mx2_camera_probe(struct platform_device *pdev)
 	pcdev->soc_host.priv		= pcdev;
 	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
 	pcdev->soc_host.nr		= pdev->id;
+	if (cpu_is_mx25())
+		pcdev->soc_host.capabilities = SOCAM_HOST_CAP_STRIDE;
 
 	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(pcdev->alloc_ctx)) {
diff --git a/drivers/media/video/mx2_emmaprp.c b/drivers/media/video/mx2_emmaprp.c
index ba89a7401c8c..0bd5815de369 100644
--- a/drivers/media/video/mx2_emmaprp.c
+++ b/drivers/media/video/mx2_emmaprp.c
@@ -755,7 +755,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 
 	memset(src_vq, 0, sizeof(*src_vq));
 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
-	src_vq->io_modes = VB2_MMAP;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 	src_vq->drv_priv = ctx;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &emmaprp_qops;
@@ -767,7 +767,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 
 	memset(dst_vq, 0, sizeof(*dst_vq));
 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	dst_vq->io_modes = VB2_MMAP;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 	dst_vq->drv_priv = ctx;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &emmaprp_qops;
@@ -904,6 +904,10 @@ static int emmaprp_probe(struct platform_device *pdev)
 	}
 
 	*vfd = emmaprp_videodev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &pcdev->dev_mutex;
 
 	video_set_drvdata(vfd, pcdev);
diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index 93c35ef5f0ad..02d54a057b60 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -199,8 +199,6 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
-	int bytes_per_line;
-	unsigned int height;
 
 	if (!mx3_cam->idmac_channel[0])
 		return -EINVAL;
@@ -208,21 +206,29 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
 	if (fmt) {
 		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
 								fmt->fmt.pix.pixelformat);
+		unsigned int bytes_per_line;
+		int ret;
+
 		if (!xlate)
 			return -EINVAL;
-		bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
-							 xlate->host_fmt);
-		height = fmt->fmt.pix.height;
+
+		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+					      xlate->host_fmt);
+		if (ret < 0)
+			return ret;
+
+		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
+
+		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
+					  fmt->fmt.pix.height);
+		if (ret < 0)
+			return ret;
+
+		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
 	} else {
 		/* Called from VIDIOC_REQBUFS or in compatibility mode */
-		bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-		height = icd->user_height;
+		sizes[0] = icd->sizeimage;
 	}
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	sizes[0] = bytes_per_line * height;
 
 	alloc_ctxs[0] = mx3_cam->alloc_ctx;
 
@@ -267,14 +273,11 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
 	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
 	struct idmac_video_param *video = &ichan->params.video;
 	const struct soc_mbus_pixelfmt *host_fmt = icd->current_fmt->host_fmt;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width, host_fmt);
 	unsigned long flags;
 	dma_cookie_t cookie;
 	size_t new_size;
 
-	BUG_ON(bytes_per_line <= 0);
-
-	new_size = bytes_per_line * icd->user_height;
+	new_size = icd->sizeimage;
 
 	if (vb2_plane_size(vb, 0) < new_size) {
 		dev_err(icd->parent, "Buffer #%d too small (%lu < %zu)\n",
@@ -314,9 +317,9 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
 		 * horizontal parameters in this case are expressed in bytes,
 		 * not in pixels.
 		 */
-		video->out_width	= bytes_per_line;
+		video->out_width	= icd->bytesperline;
 		video->out_height	= icd->user_height;
-		video->out_stride	= bytes_per_line;
+		video->out_stride	= icd->bytesperline;
 	} else {
 		/*
 		 * For IPU known formats the pixel unit will be managed
@@ -642,12 +645,14 @@ static const struct soc_mbus_pixelfmt mx3_camera_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_GREY,
 		.name			= "Monochrome 8 bit",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 };
 
diff --git a/drivers/media/video/mxb.c b/drivers/media/video/mxb.c
index 2e4131748438..b520a45cb3f3 100644
--- a/drivers/media/video/mxb.c
+++ b/drivers/media/video/mxb.c
@@ -31,10 +31,11 @@
 #include <media/saa7115.h>
 #include <linux/module.h>
 
-#include "mxb.h"
 #include "tea6415c.h"
 #include "tea6420.h"
 
+#define MXB_AUDIOS	6
+
 #define I2C_SAA7111A  0x24
 #define	I2C_TDA9840   0x42
 #define	I2C_TEA6415C  0x43
@@ -62,10 +63,14 @@ MODULE_PARM_DESC(debug, "Turn on/off device debugging (default:off).");
 enum { TUNER, AUX1, AUX3, AUX3_YC };
 
 static struct v4l2_input mxb_inputs[MXB_INPUTS] = {
-	{ TUNER,	"Tuner",		V4L2_INPUT_TYPE_TUNER,	1, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ AUX1,		"AUX1",			V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ AUX3,		"AUX3 Composite",	V4L2_INPUT_TYPE_CAMERA,	4, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ AUX3_YC,	"AUX3 S-Video",		V4L2_INPUT_TYPE_CAMERA,	4, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+	{ TUNER,   "Tuner",          V4L2_INPUT_TYPE_TUNER,  0x3f, 0,
+		V4L2_STD_PAL_BG | V4L2_STD_PAL_I, 0, V4L2_IN_CAP_STD },
+	{ AUX1,	   "AUX1",           V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ AUX3,	   "AUX3 Composite", V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ AUX3_YC, "AUX3 S-Video",   V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
 };
 
 /* this array holds the information, which port of the saa7146 each
@@ -90,6 +95,36 @@ struct mxb_routing {
 	u32 output;
 };
 
+/* these are the available audio sources, which can switched
+   to the line- and cd-output individually */
+static struct v4l2_audio mxb_audios[MXB_AUDIOS] = {
+	    {
+		.index	= 0,
+		.name	= "Tuner",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 1,
+		.name	= "AUX1",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 2,
+		.name	= "AUX2",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 3,
+		.name	= "AUX3",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 4,
+		.name	= "Radio (X9)",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 5,
+		.name	= "CD-ROM (X10)",
+		.capability = V4L2_AUDCAP_STEREO,
+	}
+};
+
 /* These are the necessary input-output-pins for bringing one audio source
    (see above) to the CD-output. Note that gain is set to 0 in this table. */
 static struct mxb_routing TEA6420_cd[MXB_AUDIOS + 1][2] = {
@@ -114,11 +149,6 @@ static struct mxb_routing TEA6420_line[MXB_AUDIOS + 1][2] = {
 	{ { 6, 3 }, { 6, 2 } }	/* Mute */
 };
 
-#define MAXCONTROLS	1
-static struct v4l2_queryctrl mxb_controls[] = {
-	{ V4L2_CID_AUDIO_MUTE, V4L2_CTRL_TYPE_BOOLEAN, "Mute", 0, 1, 1, 0, 0 },
-};
-
 struct mxb
 {
 	struct video_device	*video_dev;
@@ -135,6 +165,7 @@ struct mxb
 
 	int	cur_mode;	/* current audio mode (mono, stereo, ...) */
 	int	cur_input;	/* current input */
+	int	cur_audinput;	/* current audio input */
 	int	cur_mute;	/* current mute status */
 	struct v4l2_frequency	cur_freq;	/* current frequency the tuner is tuned to */
 };
@@ -150,16 +181,21 @@ struct mxb
 #define call_all(dev, o, f, args...) \
 	v4l2_device_call_until_err(&dev->v4l2_dev, 0, o, f, ##args)
 
-static inline void tea6420_route_cd(struct mxb *mxb, int idx)
+static void mxb_update_audmode(struct mxb *mxb)
+{
+	struct v4l2_tuner t = {
+		.audmode = mxb->cur_mode,
+	};
+
+	tda9840_call(mxb, tuner, s_tuner, &t);
+}
+
+static inline void tea6420_route(struct mxb *mxb, int idx)
 {
 	v4l2_subdev_call(mxb->tea6420_1, audio, s_routing,
 		TEA6420_cd[idx][0].input, TEA6420_cd[idx][0].output, 0);
 	v4l2_subdev_call(mxb->tea6420_2, audio, s_routing,
 		TEA6420_cd[idx][1].input, TEA6420_cd[idx][1].output, 0);
-}
-
-static inline void tea6420_route_line(struct mxb *mxb, int idx)
-{
 	v4l2_subdev_call(mxb->tea6420_1, audio, s_routing,
 		TEA6420_line[idx][0].input, TEA6420_line[idx][0].output, 0);
 	v4l2_subdev_call(mxb->tea6420_2, audio, s_routing,
@@ -168,16 +204,45 @@ static inline void tea6420_route_line(struct mxb *mxb, int idx)
 
 static struct saa7146_extension extension;
 
+static int mxb_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct saa7146_dev *dev = container_of(ctrl->handler,
+				struct saa7146_dev, ctrl_handler);
+	struct mxb *mxb = dev->ext_priv;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		mxb->cur_mute = ctrl->val;
+		/* switch the audio-source */
+		tea6420_route(mxb, ctrl->val ? 6 :
+				video_audio_connect[mxb->cur_input]);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mxb_ctrl_ops = {
+	.s_ctrl = mxb_s_ctrl,
+};
+
 static int mxb_probe(struct saa7146_dev *dev)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
 	struct mxb *mxb = NULL;
 
+	v4l2_ctrl_new_std(hdl, &mxb_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	if (hdl->error)
+		return hdl->error;
 	mxb = kzalloc(sizeof(struct mxb), GFP_KERNEL);
 	if (mxb == NULL) {
 		DEB_D("not enough kernel memory\n");
 		return -ENOMEM;
 	}
 
+
 	snprintf(mxb->i2c_adapter.name, sizeof(mxb->i2c_adapter.name), "mxb%d", mxb_num);
 
 	saa7146_i2c_adapter_prepare(dev, &mxb->i2c_adapter, SAA7146_I2C_BUS_BIT_RATE_480);
@@ -214,6 +279,8 @@ static int mxb_probe(struct saa7146_dev *dev)
 	/* we store the pointer in our private data field */
 	dev->ext_priv = mxb;
 
+	v4l2_ctrl_handler_setup(hdl);
+
 	return 0;
 }
 
@@ -286,6 +353,9 @@ static int mxb_init_done(struct saa7146_dev* dev)
 
 	int i = 0, err = 0;
 
+	/* mute audio on tea6420s */
+	tea6420_route(mxb, 6);
+
 	/* select video mode in saa7111a */
 	saa7111a_call(mxb, core, s_std, std);
 
@@ -306,12 +376,12 @@ static int mxb_init_done(struct saa7146_dev* dev)
 	tuner_call(mxb, tuner, s_frequency, &mxb->cur_freq);
 
 	/* set a default video standard */
+	/* These two gpio calls set the GPIO pins that control the tda9820 */
+	saa7146_write(dev, GPIO_CTRL, 0x00404050);
+	saa7111a_call(mxb, core, s_gpio, 1);
+	saa7111a_call(mxb, core, s_std, std);
 	tuner_call(mxb, core, s_std, std);
 
-	/* mute audio on tea6420s */
-	tea6420_route_line(mxb, 6);
-	tea6420_route_cd(mxb, 6);
-
 	/* switch to tuner-channel on tea6415c */
 	tea6415c_call(mxb, video, s_routing, 3, 17, 0);
 
@@ -320,9 +390,11 @@ static int mxb_init_done(struct saa7146_dev* dev)
 
 	/* the rest for mxb */
 	mxb->cur_input = 0;
+	mxb->cur_audinput = video_audio_connect[mxb->cur_input];
 	mxb->cur_mute = 1;
 
 	mxb->cur_mode = V4L2_TUNER_MODE_STEREO;
+	mxb_update_audmode(mxb);
 
 	/* check if the saa7740 (aka 'sound arena module') is present
 	   on the mxb. if so, we must initialize it. due to lack of
@@ -385,69 +457,6 @@ void mxb_irq_bh(struct saa7146_dev* dev, u32* irq_mask)
 }
 */
 
-static int vidioc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *qc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	int i;
-
-	for (i = MAXCONTROLS - 1; i >= 0; i--) {
-		if (mxb_controls[i].id == qc->id) {
-			*qc = mxb_controls[i];
-			DEB_D("VIDIOC_QUERYCTRL %d\n", qc->id);
-			return 0;
-		}
-	}
-	return dev->ext_vv_data->core_ops->vidioc_queryctrl(file, fh, qc);
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct mxb *mxb = (struct mxb *)dev->ext_priv;
-	int i;
-
-	for (i = MAXCONTROLS - 1; i >= 0; i--) {
-		if (mxb_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_g_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_AUDIO_MUTE) {
-		vc->value = mxb->cur_mute;
-		DEB_D("VIDIOC_G_CTRL V4L2_CID_AUDIO_MUTE:%d\n", vc->value);
-		return 0;
-	}
-
-	DEB_EE("VIDIOC_G_CTRL V4L2_CID_AUDIO_MUTE:%d\n", vc->value);
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct mxb *mxb = (struct mxb *)dev->ext_priv;
-	int i = 0;
-
-	for (i = MAXCONTROLS - 1; i >= 0; i--) {
-		if (mxb_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_s_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_AUDIO_MUTE) {
-		mxb->cur_mute = vc->value;
-		/* switch the audio-source */
-		tea6420_route_line(mxb, vc->value ? 6 :
-				video_audio_connect[mxb->cur_input]);
-		DEB_EE("VIDIOC_S_CTRL, V4L2_CID_AUDIO_MUTE: %d\n", vc->value);
-	}
-	return 0;
-}
-
 static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
 {
 	DEB_EE("VIDIOC_ENUMINPUT %d\n", i->index);
@@ -519,9 +528,12 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	if (saa7111a_call(mxb, video, s_routing, i, SAA7111_FMT_CCIR, 0))
 		pr_err("VIDIOC_S_INPUT: could not address saa7111a\n");
 
+	mxb->cur_audinput = video_audio_connect[input];
 	/* switch the audio-source only if necessary */
 	if (0 == mxb->cur_mute)
-		tea6420_route_line(mxb, video_audio_connect[input]);
+		tea6420_route(mxb, mxb->cur_audinput);
+	if (mxb->cur_audinput == 0)
+		mxb_update_audmode(mxb);
 
 	return 0;
 }
@@ -563,17 +575,20 @@ static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
 	return call_all(dev, tuner, s_tuner, t);
 }
 
+static int vidioc_querystd(struct file *file, void *fh, v4l2_std_id *norm)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+
+	return call_all(dev, video, querystd, norm);
+}
+
 static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
 
-	if (mxb->cur_input) {
-		DEB_D("VIDIOC_G_FREQ: channel %d does not have a tuner!\n",
-		      mxb->cur_input);
+	if (f->tuner)
 		return -EINVAL;
-	}
-
 	*f = mxb->cur_freq;
 
 	DEB_EE("VIDIOC_G_FREQ: freq:0x%08x\n", mxb->cur_freq.frequency);
@@ -592,17 +607,18 @@ static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency
 	if (V4L2_TUNER_ANALOG_TV != f->type)
 		return -EINVAL;
 
-	if (mxb->cur_input) {
-		DEB_D("VIDIOC_S_FREQ: channel %d does not have a tuner!\n",
-		      mxb->cur_input);
-		return -EINVAL;
-	}
-
-	mxb->cur_freq = *f;
 	DEB_EE("VIDIOC_S_FREQUENCY: freq:0x%08x\n", mxb->cur_freq.frequency);
 
 	/* tune in desired frequency */
-	tuner_call(mxb, tuner, s_frequency, &mxb->cur_freq);
+	tuner_call(mxb, tuner, s_frequency, f);
+	/* let the tuner subdev clamp the frequency to the tuner range */
+	tuner_call(mxb, tuner, g_frequency, f);
+	mxb->cur_freq = *f;
+	if (mxb->cur_audinput == 0)
+		mxb_update_audmode(mxb);
+
+	if (mxb->cur_input)
+		return 0;
 
 	/* hack: changing the frequency should invalidate the vbi-counter (=> alevt) */
 	spin_lock(&dev->slock);
@@ -612,25 +628,40 @@ static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency
 	return 0;
 }
 
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	if (a->index >= MXB_AUDIOS)
+		return -EINVAL;
+	*a = mxb_audios[a->index];
+	return 0;
+}
+
 static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
 
-	if (a->index > MXB_INPUTS) {
-		DEB_D("VIDIOC_G_AUDIO %d out of range\n", a->index);
-		return -EINVAL;
-	}
-
-	DEB_EE("VIDIOC_G_AUDIO %d\n", a->index);
-	memcpy(a, &mxb_audios[video_audio_connect[mxb->cur_input]], sizeof(struct v4l2_audio));
+	DEB_EE("VIDIOC_G_AUDIO\n");
+	*a = mxb_audios[mxb->cur_audinput];
 	return 0;
 }
 
 static int vidioc_s_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
 	DEB_D("VIDIOC_S_AUDIO %d\n", a->index);
-	return 0;
+	if (mxb_inputs[mxb->cur_input].audioset & (1 << a->index)) {
+		if (mxb->cur_audinput != a->index) {
+			mxb->cur_audinput = a->index;
+			tea6420_route(mxb, a->index);
+			if (mxb->cur_audinput == 0)
+				mxb_update_audmode(mxb);
+		}
+		return 0;
+	}
+	return -EINVAL;
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -638,60 +669,31 @@ static int vidioc_g_register(struct file *file, void *fh, struct v4l2_dbg_regist
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
-	return call_all(dev, core, g_register, reg);
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	if (v4l2_chip_match_host(&reg->match)) {
+		reg->val = saa7146_read(dev, reg->reg);
+		reg->size = 4;
+		return 0;
+	}
+	call_all(dev, core, g_register, reg);
+	return 0;
 }
 
 static int vidioc_s_register(struct file *file, void *fh, struct v4l2_dbg_register *reg)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
-	return call_all(dev, core, s_register, reg);
-}
-#endif
-
-static long vidioc_default(struct file *file, void *fh, bool valid_prio,
-							int cmd, void *arg)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct mxb *mxb = (struct mxb *)dev->ext_priv;
-
-	switch (cmd) {
-	case MXB_S_AUDIO_CD:
-	{
-		int i = *(int *)arg;
-
-		if (i < 0 || i >= MXB_AUDIOS) {
-			DEB_D("invalid argument to MXB_S_AUDIO_CD: i:%d\n", i);
-			return -EINVAL;
-		}
-
-		DEB_EE("MXB_S_AUDIO_CD: i:%d\n", i);
-
-		tea6420_route_cd(mxb, i);
-		return 0;
-	}
-	case MXB_S_AUDIO_LINE:
-	{
-		int i = *(int *)arg;
-
-		if (i < 0 || i >= MXB_AUDIOS) {
-			DEB_D("invalid argument to MXB_S_AUDIO_LINE: i:%d\n",
-			      i);
-			return -EINVAL;
-		}
-
-		DEB_EE("MXB_S_AUDIO_LINE: i:%d\n", i);
-		tea6420_route_line(mxb, i);
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	if (v4l2_chip_match_host(&reg->match)) {
+		saa7146_write(dev, reg->reg, reg->val);
+		reg->size = 4;
 		return 0;
 	}
-	default:
-/*
-		DEB2(pr_err("does not handle this ioctl\n"));
-*/
-		return -ENOIOCTLCMD;
-	}
-	return 0;
+	return call_all(dev, core, s_register, reg);
 }
+#endif
 
 static struct saa7146_ext_vv vv_data;
 
@@ -709,23 +711,21 @@ static int mxb_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data
 	}
 	mxb = (struct mxb *)dev->ext_priv;
 
-	vv_data.ops.vidioc_queryctrl = vidioc_queryctrl;
-	vv_data.ops.vidioc_g_ctrl = vidioc_g_ctrl;
-	vv_data.ops.vidioc_s_ctrl = vidioc_s_ctrl;
-	vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data.ops.vidioc_g_input = vidioc_g_input;
-	vv_data.ops.vidioc_s_input = vidioc_s_input;
-	vv_data.ops.vidioc_g_tuner = vidioc_g_tuner;
-	vv_data.ops.vidioc_s_tuner = vidioc_s_tuner;
-	vv_data.ops.vidioc_g_frequency = vidioc_g_frequency;
-	vv_data.ops.vidioc_s_frequency = vidioc_s_frequency;
-	vv_data.ops.vidioc_g_audio = vidioc_g_audio;
-	vv_data.ops.vidioc_s_audio = vidioc_s_audio;
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
+	vv_data.vid_ops.vidioc_querystd = vidioc_querystd;
+	vv_data.vid_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data.vid_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data.vid_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data.vid_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data.vid_ops.vidioc_enumaudio = vidioc_enumaudio;
+	vv_data.vid_ops.vidioc_g_audio = vidioc_g_audio;
+	vv_data.vid_ops.vidioc_s_audio = vidioc_s_audio;
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	vv_data.ops.vidioc_g_register = vidioc_g_register;
-	vv_data.ops.vidioc_s_register = vidioc_s_register;
+	vv_data.vid_ops.vidioc_g_register = vidioc_g_register;
+	vv_data.vid_ops.vidioc_s_register = vidioc_s_register;
 #endif
-	vv_data.ops.vidioc_default = vidioc_default;
 	if (saa7146_register_device(&mxb->video_dev, dev, "mxb", VFL_TYPE_GRABBER)) {
 		ERR("cannot register capture v4l2 device. skipping.\n");
 		saa7146_vv_release(dev);
@@ -752,6 +752,9 @@ static int mxb_detach(struct saa7146_dev *dev)
 
 	DEB_EE("dev:%p\n", dev);
 
+	/* mute audio on tea6420s */
+	tea6420_route(mxb, 6);
+
 	saa7146_unregister_device(&mxb->video_dev,dev);
 	if (MXB_BOARD_CAN_DO_VBI(dev))
 		saa7146_unregister_device(&mxb->vbi_dev, dev);
@@ -773,20 +776,24 @@ static int std_callback(struct saa7146_dev *dev, struct saa7146_standard *standa
 		v4l2_std_id std = V4L2_STD_PAL_I;
 
 		DEB_D("VIDIOC_S_STD: setting mxb for PAL_I\n");
-		/* set the 7146 gpio register -- I don't know what this does exactly */
+		/* These two gpio calls set the GPIO pins that control the tda9820 */
 		saa7146_write(dev, GPIO_CTRL, 0x00404050);
-		/* unset the 7111 gpio register -- I don't know what this does exactly */
 		saa7111a_call(mxb, core, s_gpio, 0);
-		tuner_call(mxb, core, s_std, std);
+		saa7111a_call(mxb, core, s_std, std);
+		if (mxb->cur_input == 0)
+			tuner_call(mxb, core, s_std, std);
 	} else {
 		v4l2_std_id std = V4L2_STD_PAL_BG;
 
+		if (mxb->cur_input)
+			std = standard->id;
 		DEB_D("VIDIOC_S_STD: setting mxb for PAL/NTSC/SECAM\n");
-		/* set the 7146 gpio register -- I don't know what this does exactly */
+		/* These two gpio calls set the GPIO pins that control the tda9820 */
 		saa7146_write(dev, GPIO_CTRL, 0x00404050);
-		/* set the 7111 gpio register -- I don't know what this does exactly */
 		saa7111a_call(mxb, core, s_gpio, 1);
-		tuner_call(mxb, core, s_std, std);
+		saa7111a_call(mxb, core, s_std, std);
+		if (mxb->cur_input == 0)
+			tuner_call(mxb, core, s_std, std);
 	}
 	return 0;
 }
@@ -836,14 +843,14 @@ MODULE_DEVICE_TABLE(pci, pci_tbl);
 
 static struct saa7146_ext_vv vv_data = {
 	.inputs		= MXB_INPUTS,
-	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_VBI_CAPTURE,
+	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_VBI_CAPTURE | V4L2_CAP_AUDIO,
 	.stds		= &standard[0],
 	.num_stds	= sizeof(standard)/sizeof(struct saa7146_standard),
 	.std_callback	= &std_callback,
 };
 
 static struct saa7146_extension extension = {
-	.name		= MXB_IDENTIFIER,
+	.name		= "Multimedia eXtension Board",
 	.flags		= SAA7146_USE_I2C_IRQ,
 
 	.pci_tbl	= &pci_tbl[0],
diff --git a/drivers/media/video/mxb.h b/drivers/media/video/mxb.h
deleted file mode 100644
index 400a57ba62ec..000000000000
--- a/drivers/media/video/mxb.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef __MXB__
-#define __MXB__
-
-#define BASE_VIDIOC_MXB 10
-
-#define MXB_S_AUDIO_CD		_IOW  ('V', BASE_VIDIOC_PRIVATE+BASE_VIDIOC_MXB+0, int)
-#define MXB_S_AUDIO_LINE	_IOW  ('V', BASE_VIDIOC_PRIVATE+BASE_VIDIOC_MXB+1, int)
-
-#define MXB_IDENTIFIER "Multimedia eXtension Board"
-
-#define MXB_AUDIOS	6
-
-/* these are the available audio sources, which can switched
-   to the line- and cd-output individually */
-static struct v4l2_audio mxb_audios[MXB_AUDIOS] = {
-	    {
-		.index	= 0,
-		.name	= "Tuner",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 1,
-		.name	= "AUX1",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 2,
-		.name	= "AUX2",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 3,
-		.name	= "AUX3",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 4,
-		.name	= "Radio (X9)",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 5,
-		.name	= "CD-ROM (X10)",
-		.capability = V4L2_AUDCAP_STEREO,
-	}
-};
-#endif
diff --git a/drivers/media/video/omap1_camera.c b/drivers/media/video/omap1_camera.c
index c20f5ecd6790..c7e41145041f 100644
--- a/drivers/media/video/omap1_camera.c
+++ b/drivers/media/video/omap1_camera.c
@@ -206,15 +206,10 @@ static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 		unsigned int *size)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct omap1_cam_dev *pcdev = ici->priv;
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	*size = bytes_per_line * icd->user_height;
+	*size = icd->sizeimage;
 
 	if (!*count || *count < OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode))
 		*count = OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode);
@@ -256,15 +251,10 @@ static int omap1_videobuf_prepare(struct videobuf_queue *vq,
 {
 	struct soc_camera_device *icd = vq->priv_data;
 	struct omap1_cam_buf *buf = container_of(vb, struct omap1_cam_buf, vb);
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct omap1_cam_dev *pcdev = ici->priv;
 	int ret;
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
 	WARN_ON(!list_empty(&vb->queue));
 
 	BUG_ON(NULL == icd->current_fmt);
@@ -281,7 +271,7 @@ static int omap1_videobuf_prepare(struct videobuf_queue *vq,
 		vb->state  = VIDEOBUF_NEEDS_INIT;
 	}
 
-	vb->size = bytes_per_line * vb->height;
+	vb->size = icd->sizeimage;
 
 	if (vb->baddr && vb->bsize < vb->size) {
 		ret = -EINVAL;
@@ -999,6 +989,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_VYUY8_2X8,
@@ -1008,6 +999,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YUYV8_2X8,
@@ -1017,6 +1009,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_2X8,
@@ -1026,6 +1019,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
@@ -1035,6 +1029,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
@@ -1044,6 +1039,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_BE,
@@ -1053,6 +1049,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_LE,
@@ -1062,6 +1059,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 },
 };
diff --git a/drivers/media/video/omap24xxcam-dma.c b/drivers/media/video/omap24xxcam-dma.c
index 3ea38a8def8e..b5ae170de4a5 100644
--- a/drivers/media/video/omap24xxcam-dma.c
+++ b/drivers/media/video/omap24xxcam-dma.c
@@ -38,7 +38,7 @@
  */
 
 /* Ack all interrupt on CSR and IRQSTATUS_L0 */
-static void omap24xxcam_dmahw_ack_all(unsigned long base)
+static void omap24xxcam_dmahw_ack_all(void __iomem *base)
 {
 	u32 csr;
 	int i;
@@ -52,7 +52,7 @@ static void omap24xxcam_dmahw_ack_all(unsigned long base)
 }
 
 /* Ack dmach on CSR and IRQSTATUS_L0 */
-static u32 omap24xxcam_dmahw_ack_ch(unsigned long base, int dmach)
+static u32 omap24xxcam_dmahw_ack_ch(void __iomem *base, int dmach)
 {
 	u32 csr;
 
@@ -65,12 +65,12 @@ static u32 omap24xxcam_dmahw_ack_ch(unsigned long base, int dmach)
 	return csr;
 }
 
-static int omap24xxcam_dmahw_running(unsigned long base, int dmach)
+static int omap24xxcam_dmahw_running(void __iomem *base, int dmach)
 {
 	return omap24xxcam_reg_in(base, CAMDMA_CCR(dmach)) & CAMDMA_CCR_ENABLE;
 }
 
-static void omap24xxcam_dmahw_transfer_setup(unsigned long base, int dmach,
+static void omap24xxcam_dmahw_transfer_setup(void __iomem *base, int dmach,
 					     dma_addr_t start, u32 len)
 {
 	omap24xxcam_reg_out(base, CAMDMA_CCR(dmach),
@@ -112,7 +112,7 @@ static void omap24xxcam_dmahw_transfer_setup(unsigned long base, int dmach,
 			    | CAMDMA_CICR_DROP_IE);
 }
 
-static void omap24xxcam_dmahw_transfer_start(unsigned long base, int dmach)
+static void omap24xxcam_dmahw_transfer_start(void __iomem *base, int dmach)
 {
 	omap24xxcam_reg_out(base, CAMDMA_CCR(dmach),
 			    CAMDMA_CCR_SEL_SRC_DST_SYNC
@@ -124,7 +124,7 @@ static void omap24xxcam_dmahw_transfer_start(unsigned long base, int dmach)
 			    | CAMDMA_CCR_SYNCHRO_CAMERA);
 }
 
-static void omap24xxcam_dmahw_transfer_chain(unsigned long base, int dmach,
+static void omap24xxcam_dmahw_transfer_chain(void __iomem *base, int dmach,
 					     int free_dmach)
 {
 	int prev_dmach, ch;
@@ -160,7 +160,7 @@ static void omap24xxcam_dmahw_transfer_chain(unsigned long base, int dmach,
  * controller may not be idle after this routine completes, because
  * the completion routines might start new transfers.
  */
-static void omap24xxcam_dmahw_abort_ch(unsigned long base, int dmach)
+static void omap24xxcam_dmahw_abort_ch(void __iomem *base, int dmach)
 {
 	/* mask all interrupts from this channel */
 	omap24xxcam_reg_out(base, CAMDMA_CICR(dmach), 0);
@@ -171,7 +171,7 @@ static void omap24xxcam_dmahw_abort_ch(unsigned long base, int dmach)
 	omap24xxcam_reg_merge(base, CAMDMA_CCR(dmach), 0, CAMDMA_CCR_ENABLE);
 }
 
-static void omap24xxcam_dmahw_init(unsigned long base)
+static void omap24xxcam_dmahw_init(void __iomem *base)
 {
 	omap24xxcam_reg_out(base, CAMDMA_OCP_SYSCONFIG,
 			    CAMDMA_OCP_SYSCONFIG_MIDLEMODE_FSTANDBY
@@ -362,7 +362,7 @@ void omap24xxcam_dma_hwinit(struct omap24xxcam_dma *dma)
 }
 
 static void omap24xxcam_dma_init(struct omap24xxcam_dma *dma,
-				 unsigned long base)
+				 void __iomem *base)
 {
 	int ch;
 
@@ -577,7 +577,7 @@ void omap24xxcam_sgdma_sync(struct omap24xxcam_sgdma *sgdma)
 }
 
 void omap24xxcam_sgdma_init(struct omap24xxcam_sgdma *sgdma,
-			    unsigned long base,
+			    void __iomem *base,
 			    void (*reset_callback)(unsigned long data),
 			    unsigned long reset_callback_data)
 {
diff --git a/drivers/media/video/omap24xxcam.c b/drivers/media/video/omap24xxcam.c
index 7d3864144368..e5015b0d5508 100644
--- a/drivers/media/video/omap24xxcam.c
+++ b/drivers/media/video/omap24xxcam.c
@@ -1776,8 +1776,7 @@ static int __devinit omap24xxcam_probe(struct platform_device *pdev)
 	cam->mmio_size = resource_size(mem);
 
 	/* map the region */
-	cam->mmio_base = (unsigned long)
-		ioremap_nocache(cam->mmio_base_phys, cam->mmio_size);
+	cam->mmio_base = ioremap_nocache(cam->mmio_base_phys, cam->mmio_size);
 	if (!cam->mmio_base) {
 		dev_err(cam->dev, "cannot map camera register I/O region\n");
 		goto err;
diff --git a/drivers/media/video/omap24xxcam.h b/drivers/media/video/omap24xxcam.h
index 2ce67f5a48d5..d59727afe894 100644
--- a/drivers/media/video/omap24xxcam.h
+++ b/drivers/media/video/omap24xxcam.h
@@ -429,7 +429,7 @@ struct sgdma_state {
 struct omap24xxcam_dma {
 	spinlock_t lock;	/* Lock for the whole structure. */
 
-	unsigned long base;	/* base address for dma controller */
+	void __iomem *base;	/* base address for dma controller */
 
 	/* While dma_stop!=0, an attempt to start a new DMA transfer will
 	 * fail.
@@ -491,7 +491,7 @@ struct omap24xxcam_device {
 
 	/*** hardware resources ***/
 	unsigned int irq;
-	unsigned long mmio_base;
+	void __iomem *mmio_base;
 	unsigned long mmio_base_phys;
 	unsigned long mmio_size;
 
@@ -544,22 +544,22 @@ struct omap24xxcam_fh {
  *
  */
 
-static inline u32 omap24xxcam_reg_in(unsigned long base, u32 offset)
+static inline u32 omap24xxcam_reg_in(u32 __iomem *base, u32 offset)
 {
 	return readl(base + offset);
 }
 
-static inline u32 omap24xxcam_reg_out(unsigned long base, u32 offset,
+static inline u32 omap24xxcam_reg_out(u32 __iomem *base, u32 offset,
 					  u32 val)
 {
 	writel(val, base + offset);
 	return val;
 }
 
-static inline u32 omap24xxcam_reg_merge(unsigned long base, u32 offset,
+static inline u32 omap24xxcam_reg_merge(u32 __iomem *base, u32 offset,
 					    u32 val, u32 mask)
 {
-	u32 addr = base + offset;
+	u32 __iomem *addr = base + offset;
 	u32 new_val = (readl(addr) & ~mask) | (val & mask);
 
 	writel(new_val, addr);
@@ -585,7 +585,7 @@ int omap24xxcam_sgdma_queue(struct omap24xxcam_sgdma *sgdma,
 			    int len, sgdma_callback_t callback, void *arg);
 void omap24xxcam_sgdma_sync(struct omap24xxcam_sgdma *sgdma);
 void omap24xxcam_sgdma_init(struct omap24xxcam_sgdma *sgdma,
-			    unsigned long base,
+			    void __iomem *base,
 			    void (*reset_callback)(unsigned long data),
 			    unsigned long reset_callback_data);
 void omap24xxcam_sgdma_exit(struct omap24xxcam_sgdma *sgdma);
diff --git a/drivers/media/video/omap3isp/isp.c b/drivers/media/video/omap3isp/isp.c
index 12d5f923e1d0..1c347633e663 100644
--- a/drivers/media/video/omap3isp/isp.c
+++ b/drivers/media/video/omap3isp/isp.c
@@ -329,19 +329,6 @@ void omap3isp_configure_bridge(struct isp_device *isp,
 	isp_reg_writel(isp, ispctrl_val, OMAP3_ISP_IOMEM_MAIN, ISP_CTRL);
 }
 
-/**
- * isp_set_pixel_clock - Configures the ISP pixel clock
- * @isp: OMAP3 ISP device
- * @pixelclk: Average pixel clock in Hz
- *
- * Set the average pixel clock required by the sensor. The ISP will use the
- * lowest possible memory bandwidth settings compatible with the clock.
- **/
-static void isp_set_pixel_clock(struct isp_device *isp, unsigned int pixelclk)
-{
-	isp->isp_ccdc.vpcfg.pixelclk = pixelclk;
-}
-
 void omap3isp_hist_dma_done(struct isp_device *isp)
 {
 	if (omap3isp_ccdc_busy(&isp->isp_ccdc) ||
@@ -739,6 +726,17 @@ static int isp_pipeline_enable(struct isp_pipeline *pipe,
 	unsigned long flags;
 	int ret;
 
+	/* If the preview engine crashed it might not respond to read/write
+	 * operations on the L4 bus. This would result in a bus fault and a
+	 * kernel oops. Refuse to start streaming in that case. This check must
+	 * be performed before the loop below to avoid starting entities if the
+	 * pipeline won't start anyway (those entities would then likely fail to
+	 * stop, making the problem worse).
+	 */
+	if ((pipe->entities & isp->crashed) &
+	    (1U << isp->isp_prev.subdev.entity.id))
+		return -EIO;
+
 	spin_lock_irqsave(&pipe->lock, flags);
 	pipe->state &= ~(ISP_PIPELINE_IDLE_INPUT | ISP_PIPELINE_IDLE_OUTPUT);
 	spin_unlock_irqrestore(&pipe->lock, flags);
@@ -774,14 +772,6 @@ static int isp_pipeline_enable(struct isp_pipeline *pipe,
 		}
 	}
 
-	/* Frame number propagation. In continuous streaming mode the number
-	 * is incremented in the frame start ISR. In mem-to-mem mode
-	 * singleshot is used and frame start IRQs are not available.
-	 * Thus we have to increment the number here.
-	 */
-	if (pipe->do_propagation && mode == ISP_PIPELINE_STREAM_SINGLESHOT)
-		atomic_inc(&pipe->frame_number);
-
 	return 0;
 }
 
@@ -879,13 +869,15 @@ static int isp_pipeline_disable(struct isp_pipeline *pipe)
 
 		if (ret) {
 			dev_info(isp->dev, "Unable to stop %s\n", subdev->name);
+			/* If the entity failed to stopped, assume it has
+			 * crashed. Mark it as such, the ISP will be reset when
+			 * applications will release it.
+			 */
+			isp->crashed |= 1U << subdev->entity.id;
 			failure = -ETIMEDOUT;
 		}
 	}
 
-	if (failure < 0)
-		isp->needs_reset = true;
-
 	return failure;
 }
 
@@ -1069,6 +1061,7 @@ static int isp_reset(struct isp_device *isp)
 		udelay(1);
 	}
 
+	isp->crashed = 0;
 	return 0;
 }
 
@@ -1495,11 +1488,13 @@ void omap3isp_put(struct isp_device *isp)
 	BUG_ON(isp->ref_count == 0);
 	if (--isp->ref_count == 0) {
 		isp_disable_interrupts(isp);
-		isp_save_ctx(isp);
-		if (isp->needs_reset) {
+		if (isp->domain)
+			isp_save_ctx(isp);
+		/* Reset the ISP if an entity has failed to stop. This is the
+		 * only way to recover from such conditions.
+		 */
+		if (isp->crashed)
 			isp_reset(isp);
-			isp->needs_reset = false;
-		}
 		isp_disable_clocks(isp);
 	}
 	mutex_unlock(&isp->isp_mutex);
@@ -1970,7 +1965,7 @@ error_csiphy:
  *
  * Always returns 0.
  */
-static int isp_remove(struct platform_device *pdev)
+static int __devexit isp_remove(struct platform_device *pdev)
 {
 	struct isp_device *isp = platform_get_drvdata(pdev);
 	int i;
@@ -1981,6 +1976,7 @@ static int isp_remove(struct platform_device *pdev)
 	omap3isp_get(isp);
 	iommu_detach_device(isp->domain, &pdev->dev);
 	iommu_domain_free(isp->domain);
+	isp->domain = NULL;
 	omap3isp_put(isp);
 
 	free_irq(isp->irq_num, isp);
@@ -2050,7 +2046,7 @@ static int isp_map_mem_resource(struct platform_device *pdev,
  *   -EINVAL if couldn't install ISR,
  *   or clk_get return error value.
  */
-static int isp_probe(struct platform_device *pdev)
+static int __devinit isp_probe(struct platform_device *pdev)
 {
 	struct isp_platform_data *pdata = pdev->dev.platform_data;
 	struct isp_device *isp;
@@ -2068,7 +2064,6 @@ static int isp_probe(struct platform_device *pdev)
 
 	isp->autoidle = autoidle;
 	isp->platform_cb.set_xclk = isp_set_xclk;
-	isp->platform_cb.set_pixel_clock = isp_set_pixel_clock;
 
 	mutex_init(&isp->isp_mutex);
 	spin_lock_init(&isp->stat_lock);
@@ -2218,7 +2213,7 @@ MODULE_DEVICE_TABLE(platform, omap3isp_id_table);
 
 static struct platform_driver omap3isp_driver = {
 	.probe = isp_probe,
-	.remove = isp_remove,
+	.remove = __devexit_p(isp_remove),
 	.id_table = omap3isp_id_table,
 	.driver = {
 		.owner = THIS_MODULE,
diff --git a/drivers/media/video/omap3isp/isp.h b/drivers/media/video/omap3isp/isp.h
index d96603eb0d17..fc7af3e32efd 100644
--- a/drivers/media/video/omap3isp/isp.h
+++ b/drivers/media/video/omap3isp/isp.h
@@ -129,7 +129,6 @@ struct isp_platform_callback {
 	int (*csiphy_config)(struct isp_csiphy *phy,
 			     struct isp_csiphy_dphy_cfg *dphy,
 			     struct isp_csiphy_lanes_cfg *lanes);
-	void (*set_pixel_clock)(struct isp_device *isp, unsigned int pixelclk);
 };
 
 /*
@@ -145,6 +144,7 @@ struct isp_platform_callback {
  * @raw_dmamask: Raw DMA mask
  * @stat_lock: Spinlock for handling statistics
  * @isp_mutex: Mutex for serializing requests to ISP.
+ * @crashed: Bitmask of crashed entities (indexed by entity ID)
  * @has_context: Context has been saved at least once and can be restored.
  * @ref_count: Reference count for handling multiple ISP requests.
  * @cam_ick: Pointer to camera interface clock structure.
@@ -184,7 +184,7 @@ struct isp_device {
 	/* ISP Obj */
 	spinlock_t stat_lock;	/* common lock for statistic drivers */
 	struct mutex isp_mutex;	/* For handling ref_count field */
-	bool needs_reset;
+	u32 crashed;
 	int has_context;
 	int ref_count;
 	unsigned int autoidle;
@@ -237,10 +237,6 @@ void omap3isp_configure_bridge(struct isp_device *isp,
 			       const struct isp_parallel_platform_data *pdata,
 			       unsigned int shift);
 
-#define ISP_XCLK_NONE			0
-#define ISP_XCLK_A			1
-#define ISP_XCLK_B			2
-
 struct isp_device *omap3isp_get(struct isp_device *isp);
 void omap3isp_put(struct isp_device *isp);
 
diff --git a/drivers/media/video/omap3isp/ispccdc.c b/drivers/media/video/omap3isp/ispccdc.c
index eaabc27f0fa2..7e32331b60fb 100644
--- a/drivers/media/video/omap3isp/ispccdc.c
+++ b/drivers/media/video/omap3isp/ispccdc.c
@@ -38,6 +38,9 @@
 #include "ispreg.h"
 #include "ispccdc.h"
 
+#define CCDC_MIN_WIDTH		32
+#define CCDC_MIN_HEIGHT		32
+
 static struct v4l2_mbus_framefmt *
 __ccdc_get_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		  unsigned int pad, enum v4l2_subdev_format_whence which);
@@ -836,8 +839,8 @@ static void ccdc_config_vp(struct isp_ccdc_device *ccdc)
 
 	if (pipe->input)
 		div = DIV_ROUND_UP(l3_ick, pipe->max_rate);
-	else if (ccdc->vpcfg.pixelclk)
-		div = l3_ick / ccdc->vpcfg.pixelclk;
+	else if (pipe->external_rate)
+		div = l3_ick / pipe->external_rate;
 
 	div = clamp(div, 2U, max_div);
 	fmtcfg_vp |= (div - 2) << ISPCCDC_FMTCFG_VPIF_FRQ_SHIFT;
@@ -1118,6 +1121,7 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 	struct isp_parallel_platform_data *pdata = NULL;
 	struct v4l2_subdev *sensor;
 	struct v4l2_mbus_framefmt *format;
+	const struct v4l2_rect *crop;
 	const struct isp_format_info *fmt_info;
 	struct v4l2_subdev_format fmt_src;
 	unsigned int depth_out;
@@ -1211,14 +1215,14 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VDINT);
 
 	/* CCDC_PAD_SOURCE_OF */
-	format = &ccdc->formats[CCDC_PAD_SOURCE_OF];
+	crop = &ccdc->crop;
 
-	isp_reg_writel(isp, (0 << ISPCCDC_HORZ_INFO_SPH_SHIFT) |
-		       ((format->width - 1) << ISPCCDC_HORZ_INFO_NPH_SHIFT),
+	isp_reg_writel(isp, (crop->left << ISPCCDC_HORZ_INFO_SPH_SHIFT) |
+		       ((crop->width - 1) << ISPCCDC_HORZ_INFO_NPH_SHIFT),
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_HORZ_INFO);
-	isp_reg_writel(isp, 0 << ISPCCDC_VERT_START_SLV0_SHIFT,
+	isp_reg_writel(isp, crop->top << ISPCCDC_VERT_START_SLV0_SHIFT,
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_START);
-	isp_reg_writel(isp, (format->height - 1)
+	isp_reg_writel(isp, (crop->height - 1)
 			<< ISPCCDC_VERT_LINES_NLV_SHIFT,
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_LINES);
 
@@ -1410,6 +1414,9 @@ static void ccdc_hs_vs_isr(struct isp_ccdc_device *ccdc)
 	struct video_device *vdev = ccdc->subdev.devnode;
 	struct v4l2_event event;
 
+	/* Frame number propagation */
+	atomic_inc(&pipe->frame_number);
+
 	memset(&event, 0, sizeof(event));
 	event.type = V4L2_EVENT_FRAME_SYNC;
 	event.u.frame_sync.frame_sequence = atomic_read(&pipe->frame_number);
@@ -1703,7 +1710,7 @@ static int ccdc_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
 	if (sub->id != 0)
 		return -EINVAL;
 
-	return v4l2_event_subscribe(fh, sub, OMAP3ISP_CCDC_NEVENTS);
+	return v4l2_event_subscribe(fh, sub, OMAP3ISP_CCDC_NEVENTS, NULL);
 }
 
 static int ccdc_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
@@ -1790,6 +1797,16 @@ __ccdc_get_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		return &ccdc->formats[pad];
 }
 
+static struct v4l2_rect *
+__ccdc_get_crop(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(fh, CCDC_PAD_SOURCE_OF);
+	else
+		return &ccdc->crop;
+}
+
 /*
  * ccdc_try_format - Try video format on a pad
  * @ccdc: ISP CCDC device
@@ -1806,6 +1823,7 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 	const struct isp_format_info *info;
 	unsigned int width = fmt->width;
 	unsigned int height = fmt->height;
+	struct v4l2_rect *crop;
 	unsigned int i;
 
 	switch (pad) {
@@ -1831,14 +1849,10 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SINK, which);
 		memcpy(fmt, format, sizeof(*fmt));
 
-		/* The data formatter truncates the number of horizontal output
-		 * pixels to a multiple of 16. To avoid clipping data, allow
-		 * callers to request an output size bigger than the input size
-		 * up to the nearest multiple of 16.
-		 */
-		fmt->width = clamp_t(u32, width, 32, fmt->width + 15);
-		fmt->width &= ~15;
-		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		/* Hardcode the output size to the crop rectangle size. */
+		crop = __ccdc_get_crop(ccdc, fh, which);
+		fmt->width = crop->width;
+		fmt->height = crop->height;
 		break;
 
 	case CCDC_PAD_SOURCE_VP:
@@ -1866,6 +1880,49 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 }
 
 /*
+ * ccdc_try_crop - Validate a crop rectangle
+ * @ccdc: ISP CCDC device
+ * @sink: format on the sink pad
+ * @crop: crop rectangle to be validated
+ */
+static void ccdc_try_crop(struct isp_ccdc_device *ccdc,
+			  const struct v4l2_mbus_framefmt *sink,
+			  struct v4l2_rect *crop)
+{
+	const struct isp_format_info *info;
+	unsigned int max_width;
+
+	/* For Bayer formats, restrict left/top and width/height to even values
+	 * to keep the Bayer pattern.
+	 */
+	info = omap3isp_video_format_info(sink->code);
+	if (info->flavor != V4L2_MBUS_FMT_Y8_1X8) {
+		crop->left &= ~1;
+		crop->top &= ~1;
+	}
+
+	crop->left = clamp_t(u32, crop->left, 0, sink->width - CCDC_MIN_WIDTH);
+	crop->top = clamp_t(u32, crop->top, 0, sink->height - CCDC_MIN_HEIGHT);
+
+	/* The data formatter truncates the number of horizontal output pixels
+	 * to a multiple of 16. To avoid clipping data, allow callers to request
+	 * an output size bigger than the input size up to the nearest multiple
+	 * of 16.
+	 */
+	max_width = (sink->width - crop->left + 15) & ~15;
+	crop->width = clamp_t(u32, crop->width, CCDC_MIN_WIDTH, max_width)
+		    & ~15;
+	crop->height = clamp_t(u32, crop->height, CCDC_MIN_HEIGHT,
+			       sink->height - crop->top);
+
+	/* Odd width/height values don't make sense for Bayer formats. */
+	if (info->flavor != V4L2_MBUS_FMT_Y8_1X8) {
+		crop->width &= ~1;
+		crop->height &= ~1;
+	}
+}
+
+/*
  * ccdc_enum_mbus_code - Handle pixel format enumeration
  * @sd     : pointer to v4l2 subdev structure
  * @fh : V4L2 subdev file handle
@@ -1937,6 +1994,93 @@ static int ccdc_enum_frame_size(struct v4l2_subdev *sd,
 }
 
 /*
+ * ccdc_get_selection - Retrieve a selection rectangle on a pad
+ * @sd: ISP CCDC V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the output formatter
+ * source pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int ccdc_get_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->pad != CCDC_PAD_SOURCE_OF)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SINK, sel->which);
+		ccdc_try_crop(ccdc, format, &sel->r);
+		break;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		sel->r = *__ccdc_get_crop(ccdc, fh, sel->which);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * ccdc_set_selection - Set a selection rectangle on a pad
+ * @sd: ISP CCDC V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the output
+ * formatter source pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int ccdc_set_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != CCDC_PAD_SOURCE_OF)
+		return -EINVAL;
+
+	/* The crop rectangle can't be changed while streaming. */
+	if (ccdc->state != ISP_PIPELINE_STREAM_STOPPED)
+		return -EBUSY;
+
+	/* Modifying the crop rectangle always changes the format on the source
+	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
+	 * rectangle.
+	 */
+	if (sel->flags & V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG) {
+		sel->r = *__ccdc_get_crop(ccdc, fh, sel->which);
+		return 0;
+	}
+
+	format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SINK, sel->which);
+	ccdc_try_crop(ccdc, format, &sel->r);
+	*__ccdc_get_crop(ccdc, fh, sel->which) = sel->r;
+
+	/* Update the source format. */
+	format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SOURCE_OF, sel->which);
+	ccdc_try_format(ccdc, fh, CCDC_PAD_SOURCE_OF, format, sel->which);
+
+	return 0;
+}
+
+/*
  * ccdc_get_format - Retrieve the video format on a pad
  * @sd : ISP CCDC V4L2 subdevice
  * @fh : V4L2 subdev file handle
@@ -1973,6 +2117,7 @@ static int ccdc_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 {
 	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
 
 	format = __ccdc_get_format(ccdc, fh, fmt->pad, fmt->which);
 	if (format == NULL)
@@ -1983,6 +2128,16 @@ static int ccdc_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 
 	/* Propagate the format from sink to source */
 	if (fmt->pad == CCDC_PAD_SINK) {
+		/* Reset the crop rectangle. */
+		crop = __ccdc_get_crop(ccdc, fh, fmt->which);
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = fmt->format.width;
+		crop->height = fmt->format.height;
+
+		ccdc_try_crop(ccdc, &fmt->format, crop);
+
+		/* Update the source formats. */
 		format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SOURCE_OF,
 					   fmt->which);
 		*format = fmt->format;
@@ -2000,6 +2155,69 @@ static int ccdc_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 }
 
 /*
+ * Decide whether desired output pixel code can be obtained with
+ * the lane shifter by shifting the input pixel code.
+ * @in: input pixelcode to shifter
+ * @out: output pixelcode from shifter
+ * @additional_shift: # of bits the sensor's LSB is offset from CAMEXT[0]
+ *
+ * return true if the combination is possible
+ * return false otherwise
+ */
+static bool ccdc_is_shiftable(enum v4l2_mbus_pixelcode in,
+			      enum v4l2_mbus_pixelcode out,
+			      unsigned int additional_shift)
+{
+	const struct isp_format_info *in_info, *out_info;
+
+	if (in == out)
+		return true;
+
+	in_info = omap3isp_video_format_info(in);
+	out_info = omap3isp_video_format_info(out);
+
+	if ((in_info->flavor == 0) || (out_info->flavor == 0))
+		return false;
+
+	if (in_info->flavor != out_info->flavor)
+		return false;
+
+	return in_info->bpp - out_info->bpp + additional_shift <= 6;
+}
+
+static int ccdc_link_validate(struct v4l2_subdev *sd,
+			      struct media_link *link,
+			      struct v4l2_subdev_format *source_fmt,
+			      struct v4l2_subdev_format *sink_fmt)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	unsigned long parallel_shift;
+
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	/* We've got a parallel sensor here. */
+	if (ccdc->input == CCDC_INPUT_PARALLEL) {
+		struct isp_parallel_platform_data *pdata =
+			&((struct isp_v4l2_subdevs_group *)
+			  media_entity_to_v4l2_subdev(link->source->entity)
+			  ->host_priv)->bus.parallel;
+		parallel_shift = pdata->data_lane_shift * 2;
+	} else {
+		parallel_shift = 0;
+	}
+
+	/* Lane shifter may be used to drop bits on CCDC sink pad */
+	if (!ccdc_is_shiftable(source_fmt->format.code,
+			       sink_fmt->format.code, parallel_shift))
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
  * ccdc_init_formats - Initialize formats on all pads
  * @sd: ISP CCDC V4L2 subdevice
  * @fh: V4L2 subdev file handle
@@ -2041,6 +2259,9 @@ static const struct v4l2_subdev_pad_ops ccdc_v4l2_pad_ops = {
 	.enum_frame_size = ccdc_enum_frame_size,
 	.get_fmt = ccdc_get_format,
 	.set_fmt = ccdc_set_format,
+	.get_selection = ccdc_get_selection,
+	.set_selection = ccdc_set_selection,
+	.link_validate = ccdc_link_validate,
 };
 
 /* V4L2 subdev operations */
@@ -2150,6 +2371,7 @@ static int ccdc_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations ccdc_media_ops = {
 	.link_setup = ccdc_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc)
@@ -2276,8 +2498,6 @@ int omap3isp_ccdc_init(struct isp_device *isp)
 	ccdc->clamp.oblen = 0;
 	ccdc->clamp.dcsubval = 0;
 
-	ccdc->vpcfg.pixelclk = 0;
-
 	ccdc->update = OMAP3ISP_CCDC_BLCLAMP;
 	ccdc_apply_controls(ccdc);
 
diff --git a/drivers/media/video/omap3isp/ispccdc.h b/drivers/media/video/omap3isp/ispccdc.h
index 6d0264bab75b..890f6b3a68fd 100644
--- a/drivers/media/video/omap3isp/ispccdc.h
+++ b/drivers/media/video/omap3isp/ispccdc.h
@@ -80,14 +80,6 @@ struct ispccdc_syncif {
 	u8 bt_r656_en;
 };
 
-/*
- * struct ispccdc_vp - Structure for Video Port parameters
- * @pixelclk: Input pixel clock in Hz
- */
-struct ispccdc_vp {
-	unsigned int pixelclk;
-};
-
 enum ispccdc_lsc_state {
 	LSC_STATE_STOPPED = 0,
 	LSC_STATE_STOPPING = 1,
@@ -147,6 +139,7 @@ struct ispccdc_lsc {
  * @subdev: V4L2 subdevice
  * @pads: Sink and source media entity pads
  * @formats: Active video formats
+ * @crop: Active crop rectangle on the OF source pad
  * @input: Active input
  * @output: Active outputs
  * @video_out: Output video node
@@ -161,7 +154,6 @@ struct ispccdc_lsc {
  * @update: Bitmask of controls to update during the next interrupt
  * @shadow_update: Controls update in progress by userspace
  * @syncif: Interface synchronization configuration
- * @vpcfg: Video port configuration
  * @underrun: A buffer underrun occurred and a new buffer has been queued
  * @state: Streaming state
  * @lock: Serializes shadow_update with interrupt handler
@@ -173,6 +165,7 @@ struct isp_ccdc_device {
 	struct v4l2_subdev subdev;
 	struct media_pad pads[CCDC_PADS_NUM];
 	struct v4l2_mbus_framefmt formats[CCDC_PADS_NUM];
+	struct v4l2_rect crop;
 
 	enum ccdc_input_entity input;
 	unsigned int output;
@@ -190,7 +183,6 @@ struct isp_ccdc_device {
 	unsigned int shadow_update;
 
 	struct ispccdc_syncif syncif;
-	struct ispccdc_vp vpcfg;
 
 	unsigned int underrun:1;
 	enum isp_pipeline_stream_state state;
diff --git a/drivers/media/video/omap3isp/ispccp2.c b/drivers/media/video/omap3isp/ispccp2.c
index 70ddbf35b223..85f0de85f37c 100644
--- a/drivers/media/video/omap3isp/ispccp2.c
+++ b/drivers/media/video/omap3isp/ispccp2.c
@@ -161,7 +161,6 @@ static void ccp2_pwr_cfg(struct isp_ccp2_device *ccp2)
 static void ccp2_if_enable(struct isp_ccp2_device *ccp2, u8 enable)
 {
 	struct isp_device *isp = to_isp_device(ccp2);
-	struct isp_pipeline *pipe = to_isp_pipeline(&ccp2->subdev.entity);
 	int i;
 
 	if (enable && ccp2->vdds_csib)
@@ -178,19 +177,6 @@ static void ccp2_if_enable(struct isp_ccp2_device *ccp2, u8 enable)
 			ISPCCP2_CTRL_MODE | ISPCCP2_CTRL_IF_EN,
 			enable ? (ISPCCP2_CTRL_MODE | ISPCCP2_CTRL_IF_EN) : 0);
 
-	/* For frame count propagation */
-	if (pipe->do_propagation) {
-		/* We may want the Frame Start IRQ from LC0 */
-		if (enable)
-			isp_reg_set(isp, OMAP3_ISP_IOMEM_CCP2,
-				    ISPCCP2_LC01_IRQENABLE,
-				    ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ);
-		else
-			isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCP2,
-				    ISPCCP2_LC01_IRQENABLE,
-				    ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ);
-	}
-
 	if (!enable && ccp2->vdds_csib)
 		regulator_disable(ccp2->vdds_csib);
 }
@@ -350,7 +336,6 @@ static void ccp2_lcx_config(struct isp_ccp2_device *ccp2,
 	      ISPCCP2_LC01_IRQSTATUS_LC0_CRC_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_FSP_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_FW_IRQ |
-	      ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_FSC_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_SSC_IRQ;
 
@@ -613,14 +598,6 @@ void omap3isp_ccp2_isr(struct isp_ccp2_device *ccp2)
 	if (omap3isp_module_sync_is_stopping(&ccp2->wait, &ccp2->stopping))
 		return;
 
-	/* Frame number propagation */
-	if (lcx_irqstatus & ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ) {
-		struct isp_pipeline *pipe =
-			to_isp_pipeline(&ccp2->subdev.entity);
-		if (pipe->do_propagation)
-			atomic_inc(&pipe->frame_number);
-	}
-
 	/* Handle queued buffers on frame end interrupts */
 	if (lcm_irqstatus & ISPCCP2_LCM_IRQSTATUS_EOF_IRQ)
 		ccp2_isr_buffer(ccp2);
@@ -1021,6 +998,7 @@ static int ccp2_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations ccp2_media_ops = {
 	.link_setup = ccp2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 /*
diff --git a/drivers/media/video/omap3isp/ispcsi2.c b/drivers/media/video/omap3isp/ispcsi2.c
index fcb5168996a7..a1724362b6d5 100644
--- a/drivers/media/video/omap3isp/ispcsi2.c
+++ b/drivers/media/video/omap3isp/ispcsi2.c
@@ -378,21 +378,17 @@ static void csi2_timing_config(struct isp_device *isp,
 static void csi2_irq_ctx_set(struct isp_device *isp,
 			     struct isp_csi2_device *csi2, int enable)
 {
-	u32 reg = ISPCSI2_CTX_IRQSTATUS_FE_IRQ;
 	int i;
 
-	if (csi2->use_fs_irq)
-		reg |= ISPCSI2_CTX_IRQSTATUS_FS_IRQ;
-
 	for (i = 0; i < 8; i++) {
-		isp_reg_writel(isp, reg, csi2->regs1,
+		isp_reg_writel(isp, ISPCSI2_CTX_IRQSTATUS_FE_IRQ, csi2->regs1,
 			       ISPCSI2_CTX_IRQSTATUS(i));
 		if (enable)
 			isp_reg_set(isp, csi2->regs1, ISPCSI2_CTX_IRQENABLE(i),
-				    reg);
+				    ISPCSI2_CTX_IRQSTATUS_FE_IRQ);
 		else
 			isp_reg_clr(isp, csi2->regs1, ISPCSI2_CTX_IRQENABLE(i),
-				    reg);
+				    ISPCSI2_CTX_IRQSTATUS_FE_IRQ);
 	}
 }
 
@@ -690,14 +686,6 @@ static void csi2_isr_ctx(struct isp_csi2_device *csi2,
 	status = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTX_IRQSTATUS(n));
 	isp_reg_writel(isp, status, csi2->regs1, ISPCSI2_CTX_IRQSTATUS(n));
 
-	/* Propagate frame number */
-	if (status & ISPCSI2_CTX_IRQSTATUS_FS_IRQ) {
-		struct isp_pipeline *pipe =
-				     to_isp_pipeline(&csi2->subdev.entity);
-		if (pipe->do_propagation)
-			atomic_inc(&pipe->frame_number);
-	}
-
 	if (!(status & ISPCSI2_CTX_IRQSTATUS_FE_IRQ))
 		return;
 
@@ -1047,14 +1035,12 @@ static int csi2_set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
 	struct isp_device *isp = csi2->isp;
-	struct isp_pipeline *pipe = to_isp_pipeline(&csi2->subdev.entity);
 	struct isp_video *video_out = &csi2->video_out;
 
 	switch (enable) {
 	case ISP_PIPELINE_STREAM_CONTINUOUS:
 		if (omap3isp_csiphy_acquire(csi2->phy) < 0)
 			return -ENODEV;
-		csi2->use_fs_irq = pipe->do_propagation;
 		if (csi2->output & CSI2_OUTPUT_MEMORY)
 			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_CSI2A_WRITE);
 		csi2_configure(csi2);
@@ -1181,6 +1167,7 @@ static int csi2_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations csi2_media_ops = {
 	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2)
diff --git a/drivers/media/video/omap3isp/ispcsi2.h b/drivers/media/video/omap3isp/ispcsi2.h
index 885ad79a7678..c57729b7e86e 100644
--- a/drivers/media/video/omap3isp/ispcsi2.h
+++ b/drivers/media/video/omap3isp/ispcsi2.h
@@ -145,7 +145,6 @@ struct isp_csi2_device {
 	u32 output; /* output to CCDC, memory or both? */
 	bool dpcm_decompress;
 	unsigned int frame_skip;
-	bool use_fs_irq;
 
 	struct isp_csiphy *phy;
 	struct isp_csi2_ctx_cfg contexts[ISP_CSI2_MAX_CTX_NUM + 1];
diff --git a/drivers/media/video/omap3isp/ispcsiphy.c b/drivers/media/video/omap3isp/ispcsiphy.c
index 5be37ce7d0c2..348f67ebbbc9 100644
--- a/drivers/media/video/omap3isp/ispcsiphy.c
+++ b/drivers/media/video/omap3isp/ispcsiphy.c
@@ -186,7 +186,9 @@ int omap3isp_csiphy_acquire(struct isp_csiphy *phy)
 	if (rval < 0)
 		goto done;
 
-	omap3isp_csi2_reset(phy->csi2);
+	rval = omap3isp_csi2_reset(phy->csi2);
+	if (rval < 0)
+		goto done;
 
 	csiphy_dphy_config(phy);
 	csiphy_lanes_config(phy);
diff --git a/drivers/media/video/omap3isp/ispcsiphy.h b/drivers/media/video/omap3isp/ispcsiphy.h
index 9596dc6830a6..e93a661e65d9 100644
--- a/drivers/media/video/omap3isp/ispcsiphy.h
+++ b/drivers/media/video/omap3isp/ispcsiphy.h
@@ -27,22 +27,11 @@
 #ifndef OMAP3_ISP_CSI_PHY_H
 #define OMAP3_ISP_CSI_PHY_H
 
+#include <media/omap3isp.h>
+
 struct isp_csi2_device;
 struct regulator;
 
-struct csiphy_lane {
-	u8 pos;
-	u8 pol;
-};
-
-#define ISP_CSIPHY2_NUM_DATA_LANES	2
-#define ISP_CSIPHY1_NUM_DATA_LANES	1
-
-struct isp_csiphy_lanes_cfg {
-	struct csiphy_lane data[ISP_CSIPHY2_NUM_DATA_LANES];
-	struct csiphy_lane clk;
-};
-
 struct isp_csiphy_dphy_cfg {
 	u8 ths_term;
 	u8 ths_settle;
diff --git a/drivers/media/video/omap3isp/isppreview.c b/drivers/media/video/omap3isp/isppreview.c
index 6d0fb2c8c26d..8a4935ecc655 100644
--- a/drivers/media/video/omap3isp/isppreview.c
+++ b/drivers/media/video/omap3isp/isppreview.c
@@ -441,23 +441,6 @@ preview_enable_dcor(struct isp_prev_device *prev, u8 enable)
 }
 
 /*
- * preview_enable_cfa - Enable/Disable the CFA Interpolation.
- * @enable: 1 - Enables the CFA.
- */
-static void
-preview_enable_cfa(struct isp_prev_device *prev, u8 enable)
-{
-	struct isp_device *isp = to_isp_device(prev);
-
-	if (enable)
-		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
-			    ISPPRV_PCR_CFAEN);
-	else
-		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
-			    ISPPRV_PCR_CFAEN);
-}
-
-/*
  * preview_enable_gammabypass - Enables/Disables the GammaByPass
  * @enable: 1 - Bypasses Gamma - 10bit input is cropped to 8MSB.
  *          0 - Goes through Gamma Correction. input and output is 10bit.
@@ -608,12 +591,12 @@ preview_config_rgb_blending(struct isp_prev_device *prev, const void *rgb2rgb)
 }
 
 /*
- * Configures the RGB-YCbYCr conversion matrix
+ * Configures the color space conversion (RGB toYCbYCr) matrix
  * @prev_csc: Structure containing the RGB to YCbYCr matrix and the
  *            YCbCr offset.
  */
 static void
-preview_config_rgb_to_ycbcr(struct isp_prev_device *prev, const void *prev_csc)
+preview_config_csc(struct isp_prev_device *prev, const void *prev_csc)
 {
 	struct isp_device *isp = to_isp_device(prev);
 	const struct omap3isp_prev_csc *csc = prev_csc;
@@ -649,12 +632,18 @@ preview_config_rgb_to_ycbcr(struct isp_prev_device *prev, const void *prev_csc)
 static void
 preview_update_contrast(struct isp_prev_device *prev, u8 contrast)
 {
-	struct prev_params *params = &prev->params;
+	struct prev_params *params;
+	unsigned long flags;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	params = (prev->params.active & OMAP3ISP_PREV_CONTRAST)
+	       ? &prev->params.params[0] : &prev->params.params[1];
 
 	if (params->contrast != (contrast * ISPPRV_CONTRAST_UNITS)) {
 		params->contrast = contrast * ISPPRV_CONTRAST_UNITS;
-		prev->update |= PREV_CONTRAST;
+		params->update |= OMAP3ISP_PREV_CONTRAST;
 	}
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /*
@@ -681,12 +670,18 @@ preview_config_contrast(struct isp_prev_device *prev, const void *params)
 static void
 preview_update_brightness(struct isp_prev_device *prev, u8 brightness)
 {
-	struct prev_params *params = &prev->params;
+	struct prev_params *params;
+	unsigned long flags;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	params = (prev->params.active & OMAP3ISP_PREV_BRIGHTNESS)
+	       ? &prev->params.params[0] : &prev->params.params[1];
 
 	if (params->brightness != (brightness * ISPPRV_BRIGHT_UNITS)) {
 		params->brightness = brightness * ISPPRV_BRIGHT_UNITS;
-		prev->update |= PREV_BRIGHTNESS;
+		params->update |= OMAP3ISP_PREV_BRIGHTNESS;
 	}
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /*
@@ -721,159 +716,188 @@ preview_config_yc_range(struct isp_prev_device *prev, const void *yclimit)
 		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SETUP_YC);
 }
 
+static u32
+preview_params_lock(struct isp_prev_device *prev, u32 update, bool shadow)
+{
+	u32 active = prev->params.active;
+
+	if (shadow) {
+		/* Mark all shadow parameters we are going to touch as busy. */
+		prev->params.params[0].busy |= ~active & update;
+		prev->params.params[1].busy |= active & update;
+	} else {
+		/* Mark all active parameters we are going to touch as busy. */
+		update = (prev->params.params[0].update & active)
+		       | (prev->params.params[1].update & ~active);
+
+		prev->params.params[0].busy |= active & update;
+		prev->params.params[1].busy |= ~active & update;
+	}
+
+	return update;
+}
+
+static void
+preview_params_unlock(struct isp_prev_device *prev, u32 update, bool shadow)
+{
+	u32 active = prev->params.active;
+
+	if (shadow) {
+		/* Set the update flag for shadow parameters that have been
+		 * updated and clear the busy flag for all shadow parameters.
+		 */
+		prev->params.params[0].update |= (~active & update);
+		prev->params.params[1].update |= (active & update);
+		prev->params.params[0].busy &= active;
+		prev->params.params[1].busy &= ~active;
+	} else {
+		/* Clear the update flag for active parameters that have been
+		 * applied and the busy flag for all active parameters.
+		 */
+		prev->params.params[0].update &= ~(active & update);
+		prev->params.params[1].update &= ~(~active & update);
+		prev->params.params[0].busy &= ~active;
+		prev->params.params[1].busy &= active;
+	}
+}
+
+static void preview_params_switch(struct isp_prev_device *prev)
+{
+	u32 to_switch;
+
+	/* Switch active parameters with updated shadow parameters when the
+	 * shadow parameter has been updated and neither the active not the
+	 * shadow parameter is busy.
+	 */
+	to_switch = (prev->params.params[0].update & ~prev->params.active)
+		  | (prev->params.params[1].update & prev->params.active);
+	to_switch &= ~(prev->params.params[0].busy |
+		       prev->params.params[1].busy);
+	if (to_switch == 0)
+		return;
+
+	prev->params.active ^= to_switch;
+
+	/* Remove the update flag for the shadow copy of parameters we have
+	 * switched.
+	 */
+	prev->params.params[0].update &= ~(~prev->params.active & to_switch);
+	prev->params.params[1].update &= ~(prev->params.active & to_switch);
+}
+
 /* preview parameters update structure */
 struct preview_update {
-	int cfg_bit;
-	int feature_bit;
 	void (*config)(struct isp_prev_device *, const void *);
 	void (*enable)(struct isp_prev_device *, u8);
+	unsigned int param_offset;
+	unsigned int param_size;
+	unsigned int config_offset;
+	bool skip;
 };
 
-static struct preview_update update_attrs[] = {
-	{OMAP3ISP_PREV_LUMAENH, PREV_LUMA_ENHANCE,
+/* Keep the array indexed by the OMAP3ISP_PREV_* bit number. */
+static const struct preview_update update_attrs[] = {
+	/* OMAP3ISP_PREV_LUMAENH */ {
 		preview_config_luma_enhancement,
-		preview_enable_luma_enhancement},
-	{OMAP3ISP_PREV_INVALAW, PREV_INVERSE_ALAW,
+		preview_enable_luma_enhancement,
+		offsetof(struct prev_params, luma),
+		FIELD_SIZEOF(struct prev_params, luma),
+		offsetof(struct omap3isp_prev_update_config, luma),
+	}, /* OMAP3ISP_PREV_INVALAW */ {
 		NULL,
-		preview_enable_invalaw},
-	{OMAP3ISP_PREV_HRZ_MED, PREV_HORZ_MEDIAN_FILTER,
+		preview_enable_invalaw,
+	}, /* OMAP3ISP_PREV_HRZ_MED */ {
 		preview_config_hmed,
-		preview_enable_hmed},
-	{OMAP3ISP_PREV_CFA, PREV_CFA,
+		preview_enable_hmed,
+		offsetof(struct prev_params, hmed),
+		FIELD_SIZEOF(struct prev_params, hmed),
+		offsetof(struct omap3isp_prev_update_config, hmed),
+	}, /* OMAP3ISP_PREV_CFA */ {
 		preview_config_cfa,
-		preview_enable_cfa},
-	{OMAP3ISP_PREV_CHROMA_SUPP, PREV_CHROMA_SUPPRESS,
+		NULL,
+		offsetof(struct prev_params, cfa),
+		FIELD_SIZEOF(struct prev_params, cfa),
+		offsetof(struct omap3isp_prev_update_config, cfa),
+	}, /* OMAP3ISP_PREV_CHROMA_SUPP */ {
 		preview_config_chroma_suppression,
-		preview_enable_chroma_suppression},
-	{OMAP3ISP_PREV_WB, PREV_WB,
+		preview_enable_chroma_suppression,
+		offsetof(struct prev_params, csup),
+		FIELD_SIZEOF(struct prev_params, csup),
+		offsetof(struct omap3isp_prev_update_config, csup),
+	}, /* OMAP3ISP_PREV_WB */ {
 		preview_config_whitebalance,
-		NULL},
-	{OMAP3ISP_PREV_BLKADJ, PREV_BLKADJ,
+		NULL,
+		offsetof(struct prev_params, wbal),
+		FIELD_SIZEOF(struct prev_params, wbal),
+		offsetof(struct omap3isp_prev_update_config, wbal),
+	}, /* OMAP3ISP_PREV_BLKADJ */ {
 		preview_config_blkadj,
-		NULL},
-	{OMAP3ISP_PREV_RGB2RGB, PREV_RGB2RGB,
+		NULL,
+		offsetof(struct prev_params, blkadj),
+		FIELD_SIZEOF(struct prev_params, blkadj),
+		offsetof(struct omap3isp_prev_update_config, blkadj),
+	}, /* OMAP3ISP_PREV_RGB2RGB */ {
 		preview_config_rgb_blending,
-		NULL},
-	{OMAP3ISP_PREV_COLOR_CONV, PREV_COLOR_CONV,
-		preview_config_rgb_to_ycbcr,
-		NULL},
-	{OMAP3ISP_PREV_YC_LIMIT, PREV_YCLIMITS,
+		NULL,
+		offsetof(struct prev_params, rgb2rgb),
+		FIELD_SIZEOF(struct prev_params, rgb2rgb),
+		offsetof(struct omap3isp_prev_update_config, rgb2rgb),
+	}, /* OMAP3ISP_PREV_COLOR_CONV */ {
+		preview_config_csc,
+		NULL,
+		offsetof(struct prev_params, csc),
+		FIELD_SIZEOF(struct prev_params, csc),
+		offsetof(struct omap3isp_prev_update_config, csc),
+	}, /* OMAP3ISP_PREV_YC_LIMIT */ {
 		preview_config_yc_range,
-		NULL},
-	{OMAP3ISP_PREV_DEFECT_COR, PREV_DEFECT_COR,
+		NULL,
+		offsetof(struct prev_params, yclimit),
+		FIELD_SIZEOF(struct prev_params, yclimit),
+		offsetof(struct omap3isp_prev_update_config, yclimit),
+	}, /* OMAP3ISP_PREV_DEFECT_COR */ {
 		preview_config_dcor,
-		preview_enable_dcor},
-	{OMAP3ISP_PREV_GAMMABYPASS, PREV_GAMMA_BYPASS,
+		preview_enable_dcor,
+		offsetof(struct prev_params, dcor),
+		FIELD_SIZEOF(struct prev_params, dcor),
+		offsetof(struct omap3isp_prev_update_config, dcor),
+	}, /* OMAP3ISP_PREV_GAMMABYPASS */ {
 		NULL,
-		preview_enable_gammabypass},
-	{OMAP3ISP_PREV_DRK_FRM_CAPTURE, PREV_DARK_FRAME_CAPTURE,
+		preview_enable_gammabypass,
+	}, /* OMAP3ISP_PREV_DRK_FRM_CAPTURE */ {
 		NULL,
-		preview_enable_drkframe_capture},
-	{OMAP3ISP_PREV_DRK_FRM_SUBTRACT, PREV_DARK_FRAME_SUBTRACT,
+		preview_enable_drkframe_capture,
+	}, /* OMAP3ISP_PREV_DRK_FRM_SUBTRACT */ {
 		NULL,
-		preview_enable_drkframe},
-	{OMAP3ISP_PREV_LENS_SHADING, PREV_LENS_SHADING,
+		preview_enable_drkframe,
+	}, /* OMAP3ISP_PREV_LENS_SHADING */ {
 		preview_config_drkf_shadcomp,
-		preview_enable_drkframe},
-	{OMAP3ISP_PREV_NF, PREV_NOISE_FILTER,
+		preview_enable_drkframe,
+	}, /* OMAP3ISP_PREV_NF */ {
 		preview_config_noisefilter,
-		preview_enable_noisefilter},
-	{OMAP3ISP_PREV_GAMMA, PREV_GAMMA,
+		preview_enable_noisefilter,
+		offsetof(struct prev_params, nf),
+		FIELD_SIZEOF(struct prev_params, nf),
+		offsetof(struct omap3isp_prev_update_config, nf),
+	}, /* OMAP3ISP_PREV_GAMMA */ {
 		preview_config_gammacorrn,
-		NULL},
-	{-1, PREV_CONTRAST,
+		NULL,
+		offsetof(struct prev_params, gamma),
+		FIELD_SIZEOF(struct prev_params, gamma),
+		offsetof(struct omap3isp_prev_update_config, gamma),
+	}, /* OMAP3ISP_PREV_CONTRAST */ {
 		preview_config_contrast,
-		NULL},
-	{-1, PREV_BRIGHTNESS,
+		NULL,
+		offsetof(struct prev_params, contrast),
+		0, true,
+	}, /* OMAP3ISP_PREV_BRIGHTNESS */ {
 		preview_config_brightness,
-		NULL},
+		NULL,
+		offsetof(struct prev_params, brightness),
+		0, true,
+	},
 };
 
 /*
- * __preview_get_ptrs - helper function which return pointers to members
- *                         of params and config structures.
- * @params - pointer to preview_params structure.
- * @param - return pointer to appropriate structure field.
- * @configs - pointer to update config structure.
- * @config - return pointer to appropriate structure field.
- * @bit - for which feature to return pointers.
- * Return size of corresponding prev_params member
- */
-static u32
-__preview_get_ptrs(struct prev_params *params, void **param,
-		   struct omap3isp_prev_update_config *configs,
-		   void __user **config, u32 bit)
-{
-#define CHKARG(cfgs, cfg, field)				\
-	if (cfgs && cfg) {					\
-		*(cfg) = (cfgs)->field;				\
-	}
-
-	switch (bit) {
-	case PREV_HORZ_MEDIAN_FILTER:
-		*param = &params->hmed;
-		CHKARG(configs, config, hmed)
-		return sizeof(params->hmed);
-	case PREV_NOISE_FILTER:
-		*param = &params->nf;
-		CHKARG(configs, config, nf)
-		return sizeof(params->nf);
-		break;
-	case PREV_CFA:
-		*param = &params->cfa;
-		CHKARG(configs, config, cfa)
-		return sizeof(params->cfa);
-	case PREV_LUMA_ENHANCE:
-		*param = &params->luma;
-		CHKARG(configs, config, luma)
-		return sizeof(params->luma);
-	case PREV_CHROMA_SUPPRESS:
-		*param = &params->csup;
-		CHKARG(configs, config, csup)
-		return sizeof(params->csup);
-	case PREV_DEFECT_COR:
-		*param = &params->dcor;
-		CHKARG(configs, config, dcor)
-		return sizeof(params->dcor);
-	case PREV_BLKADJ:
-		*param = &params->blk_adj;
-		CHKARG(configs, config, blkadj)
-		return sizeof(params->blk_adj);
-	case PREV_YCLIMITS:
-		*param = &params->yclimit;
-		CHKARG(configs, config, yclimit)
-		return sizeof(params->yclimit);
-	case PREV_RGB2RGB:
-		*param = &params->rgb2rgb;
-		CHKARG(configs, config, rgb2rgb)
-		return sizeof(params->rgb2rgb);
-	case PREV_COLOR_CONV:
-		*param = &params->rgb2ycbcr;
-		CHKARG(configs, config, csc)
-		return sizeof(params->rgb2ycbcr);
-	case PREV_WB:
-		*param = &params->wbal;
-		CHKARG(configs, config, wbal)
-		return sizeof(params->wbal);
-	case PREV_GAMMA:
-		*param = &params->gamma;
-		CHKARG(configs, config, gamma)
-		return sizeof(params->gamma);
-	case PREV_CONTRAST:
-		*param = &params->contrast;
-		return 0;
-	case PREV_BRIGHTNESS:
-		*param = &params->brightness;
-		return 0;
-	default:
-		*param = NULL;
-		*config = NULL;
-		break;
-	}
-	return 0;
-}
-
-/*
  * preview_config - Copy and update local structure with userspace preview
  *                  configuration.
  * @prev: ISP preview engine
@@ -885,84 +909,103 @@ __preview_get_ptrs(struct prev_params *params, void **param,
 static int preview_config(struct isp_prev_device *prev,
 			  struct omap3isp_prev_update_config *cfg)
 {
-	struct prev_params *params;
-	struct preview_update *attr;
-	int i, bit, rval = 0;
+	unsigned long flags;
+	unsigned int i;
+	int rval = 0;
+	u32 update;
+	u32 active;
 
-	params = &prev->params;
+	if (cfg->update == 0)
+		return 0;
 
-	if (prev->state != ISP_PIPELINE_STREAM_STOPPED) {
-		unsigned long flags;
+	/* Mark the shadow parameters we're going to update as busy. */
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_lock(prev, cfg->update, true);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 
-		spin_lock_irqsave(&prev->lock, flags);
-		prev->shadow_update = 1;
-		spin_unlock_irqrestore(&prev->lock, flags);
-	}
+	update = 0;
 
 	for (i = 0; i < ARRAY_SIZE(update_attrs); i++) {
-		attr = &update_attrs[i];
-		bit = 0;
+		const struct preview_update *attr = &update_attrs[i];
+		struct prev_params *params;
+		unsigned int bit = 1 << i;
 
-		if (!(cfg->update & attr->cfg_bit))
+		if (attr->skip || !(cfg->update & bit))
 			continue;
 
-		bit = cfg->flag & attr->cfg_bit;
-		if (bit) {
-			void *to = NULL, __user *from = NULL;
-			unsigned long sz = 0;
+		params = &prev->params.params[!!(active & bit)];
+
+		if (cfg->flag & bit) {
+			void __user *from = *(void * __user *)
+				((void *)cfg + attr->config_offset);
+			void *to = (void *)params + attr->param_offset;
+			size_t size = attr->param_size;
 
-			sz = __preview_get_ptrs(params, &to, cfg, &from,
-						   bit);
-			if (to && from && sz) {
-				if (copy_from_user(to, from, sz)) {
+			if (to && from && size) {
+				if (copy_from_user(to, from, size)) {
 					rval = -EFAULT;
 					break;
 				}
 			}
-			params->features |= attr->feature_bit;
+			params->features |= bit;
 		} else {
-			params->features &= ~attr->feature_bit;
+			params->features &= ~bit;
 		}
 
-		prev->update |= attr->feature_bit;
+		update |= bit;
 	}
 
-	prev->shadow_update = 0;
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, true);
+	preview_params_switch(prev);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
 	return rval;
 }
 
 /*
  * preview_setup_hw - Setup preview registers and/or internal memory
  * @prev: pointer to preview private structure
+ * @update: Bitmask of parameters to setup
+ * @active: Bitmask of parameters active in set 0
  * Note: can be called from interrupt context
  * Return none
  */
-static void preview_setup_hw(struct isp_prev_device *prev)
+static void preview_setup_hw(struct isp_prev_device *prev, u32 update,
+			     u32 active)
 {
-	struct prev_params *params = &prev->params;
-	struct preview_update *attr;
-	int i, bit;
-	void *param_ptr;
+	unsigned int i;
+	u32 features;
+
+	if (update == 0)
+		return;
+
+	features = (prev->params.params[0].features & active)
+		 | (prev->params.params[1].features & ~active);
 
 	for (i = 0; i < ARRAY_SIZE(update_attrs); i++) {
-		attr = &update_attrs[i];
+		const struct preview_update *attr = &update_attrs[i];
+		struct prev_params *params;
+		unsigned int bit = 1 << i;
+		void *param_ptr;
 
-		if (!(prev->update & attr->feature_bit))
+		if (!(update & bit))
 			continue;
-		bit = params->features & attr->feature_bit;
-		if (bit) {
+
+		params = &prev->params.params[!(active & bit)];
+
+		if (params->features & bit) {
 			if (attr->config) {
-				__preview_get_ptrs(params, &param_ptr, NULL,
-						      NULL, bit);
+				param_ptr = (void *)params + attr->param_offset;
 				attr->config(prev, param_ptr);
 			}
 			if (attr->enable)
 				attr->enable(prev, 1);
-		} else
+		} else {
 			if (attr->enable)
 				attr->enable(prev, 0);
-
-		prev->update &= ~attr->feature_bit;
+		}
 	}
 }
 
@@ -1000,13 +1043,17 @@ preview_config_ycpos(struct isp_prev_device *prev,
 static void preview_config_averager(struct isp_prev_device *prev, u8 average)
 {
 	struct isp_device *isp = to_isp_device(prev);
+	struct prev_params *params;
 	int reg = 0;
 
-	if (prev->params.cfa.format == OMAP3ISP_CFAFMT_BAYER)
+	params = (prev->params.active & OMAP3ISP_PREV_CFA)
+	       ? &prev->params.params[0] : &prev->params.params[1];
+
+	if (params->cfa.format == OMAP3ISP_CFAFMT_BAYER)
 		reg = ISPPRV_AVE_EVENDIST_2 << ISPPRV_AVE_EVENDIST_SHIFT |
 		      ISPPRV_AVE_ODDDIST_2 << ISPPRV_AVE_ODDDIST_SHIFT |
 		      average;
-	else if (prev->params.cfa.format == OMAP3ISP_CFAFMT_RGBFOVEON)
+	else if (params->cfa.format == OMAP3ISP_CFAFMT_RGBFOVEON)
 		reg = ISPPRV_AVE_EVENDIST_3 << ISPPRV_AVE_EVENDIST_SHIFT |
 		      ISPPRV_AVE_ODDDIST_3 << ISPPRV_AVE_ODDDIST_SHIFT |
 		      average;
@@ -1014,6 +1061,27 @@ static void preview_config_averager(struct isp_prev_device *prev, u8 average)
 }
 
 /*
+ * preview_config_input_format - Configure the input format
+ * @prev: The preview engine
+ * @format: Format on the preview engine sink pad
+ *
+ * Enable CFA interpolation for Bayer formats and disable it for greyscale
+ * formats.
+ */
+static void preview_config_input_format(struct isp_prev_device *prev,
+					const struct v4l2_mbus_framefmt *format)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (format->code != V4L2_MBUS_FMT_Y10_1X10)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_CFAEN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_CFAEN);
+}
+
+/*
  * preview_config_input_size - Configure the input frame size
  *
  * The preview engine crops several rows and columns internally depending on
@@ -1024,32 +1092,37 @@ static void preview_config_averager(struct isp_prev_device *prev, u8 average)
  *
  * See the explanation at the PREV_MARGIN_* definitions for more details.
  */
-static void preview_config_input_size(struct isp_prev_device *prev)
+static void preview_config_input_size(struct isp_prev_device *prev, u32 active)
 {
+	const struct v4l2_mbus_framefmt *format = &prev->formats[PREV_PAD_SINK];
 	struct isp_device *isp = to_isp_device(prev);
-	struct prev_params *params = &prev->params;
 	unsigned int sph = prev->crop.left;
 	unsigned int eph = prev->crop.left + prev->crop.width - 1;
 	unsigned int slv = prev->crop.top;
 	unsigned int elv = prev->crop.top + prev->crop.height - 1;
+	u32 features;
 
-	if (params->features & PREV_CFA) {
+	if (format->code == V4L2_MBUS_FMT_Y10_1X10) {
 		sph -= 2;
 		eph += 2;
 		slv -= 2;
 		elv += 2;
 	}
-	if (params->features & (PREV_DEFECT_COR | PREV_NOISE_FILTER)) {
+
+	features = (prev->params.params[0].features & active)
+		 | (prev->params.params[1].features & ~active);
+
+	if (features & (OMAP3ISP_PREV_DEFECT_COR | OMAP3ISP_PREV_NF)) {
 		sph -= 2;
 		eph += 2;
 		slv -= 2;
 		elv += 2;
 	}
-	if (params->features & PREV_HORZ_MEDIAN_FILTER) {
+	if (features & OMAP3ISP_PREV_HRZ_MED) {
 		sph -= 2;
 		eph += 2;
 	}
-	if (params->features & (PREV_CHROMA_SUPPRESS | PREV_LUMA_ENHANCE))
+	if (features & (OMAP3ISP_PREV_CHROMA_SUPP | OMAP3ISP_PREV_LUMAENH))
 		sph -= 2;
 
 	isp_reg_writel(isp, (sph << ISPPRV_HORZ_INFO_SPH_SHIFT) | eph,
@@ -1184,8 +1257,16 @@ int omap3isp_preview_busy(struct isp_prev_device *prev)
  */
 void omap3isp_preview_restore_context(struct isp_device *isp)
 {
-	isp->isp_prev.update = PREV_FEATURES_END - 1;
-	preview_setup_hw(&isp->isp_prev);
+	struct isp_prev_device *prev = &isp->isp_prev;
+	const u32 update = OMAP3ISP_PREV_FEATURES_END - 1;
+
+	prev->params.params[0].update = prev->params.active & update;
+	prev->params.params[1].update = ~prev->params.active & update;
+
+	preview_setup_hw(prev, update, prev->params.active);
+
+	prev->params.params[0].update = 0;
+	prev->params.params[1].update = 0;
 }
 
 /*
@@ -1244,12 +1325,21 @@ static void preview_print_status(struct isp_prev_device *prev)
 /*
  * preview_init_params - init image processing parameters.
  * @prev: pointer to previewer private structure
- * return none
  */
 static void preview_init_params(struct isp_prev_device *prev)
 {
-	struct prev_params *params = &prev->params;
-	int i = 0;
+	struct prev_params *params;
+	unsigned int i;
+
+	spin_lock_init(&prev->params.lock);
+
+	prev->params.active = ~0;
+	prev->params.params[0].busy = 0;
+	prev->params.params[0].update = OMAP3ISP_PREV_FEATURES_END - 1;
+	prev->params.params[1].busy = 0;
+	prev->params.params[1].update = 0;
+
+	params = &prev->params.params[0];
 
 	/* Init values */
 	params->contrast = ISPPRV_CONTRAST_DEF * ISPPRV_CONTRAST_UNITS;
@@ -1277,22 +1367,22 @@ static void preview_init_params(struct isp_prev_device *prev)
 	params->wbal.coef1 = FLR_WBAL_COEF;
 	params->wbal.coef2 = FLR_WBAL_COEF;
 	params->wbal.coef3 = FLR_WBAL_COEF;
-	params->blk_adj.red = FLR_BLKADJ_RED;
-	params->blk_adj.green = FLR_BLKADJ_GREEN;
-	params->blk_adj.blue = FLR_BLKADJ_BLUE;
+	params->blkadj.red = FLR_BLKADJ_RED;
+	params->blkadj.green = FLR_BLKADJ_GREEN;
+	params->blkadj.blue = FLR_BLKADJ_BLUE;
 	params->rgb2rgb = flr_rgb2rgb;
-	params->rgb2ycbcr = flr_prev_csc;
+	params->csc = flr_prev_csc;
 	params->yclimit.minC = ISPPRV_YC_MIN;
 	params->yclimit.maxC = ISPPRV_YC_MAX;
 	params->yclimit.minY = ISPPRV_YC_MIN;
 	params->yclimit.maxY = ISPPRV_YC_MAX;
 
-	params->features = PREV_CFA | PREV_DEFECT_COR | PREV_NOISE_FILTER
-			 | PREV_GAMMA | PREV_BLKADJ | PREV_YCLIMITS
-			 | PREV_RGB2RGB | PREV_COLOR_CONV | PREV_WB
-			 | PREV_BRIGHTNESS | PREV_CONTRAST;
-
-	prev->update = PREV_FEATURES_END - 1;
+	params->features = OMAP3ISP_PREV_CFA | OMAP3ISP_PREV_DEFECT_COR
+			 | OMAP3ISP_PREV_NF | OMAP3ISP_PREV_GAMMA
+			 | OMAP3ISP_PREV_BLKADJ | OMAP3ISP_PREV_YC_LIMIT
+			 | OMAP3ISP_PREV_RGB2RGB | OMAP3ISP_PREV_COLOR_CONV
+			 | OMAP3ISP_PREV_WB | OMAP3ISP_PREV_BRIGHTNESS
+			 | OMAP3ISP_PREV_CONTRAST;
 }
 
 /*
@@ -1321,8 +1411,17 @@ static void preview_configure(struct isp_prev_device *prev)
 {
 	struct isp_device *isp = to_isp_device(prev);
 	struct v4l2_mbus_framefmt *format;
+	unsigned long flags;
+	u32 update;
+	u32 active;
 
-	preview_setup_hw(prev);
+	spin_lock_irqsave(&prev->params.lock, flags);
+	/* Mark all active parameters we are going to touch as busy. */
+	update = preview_params_lock(prev, 0, false);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
+	preview_setup_hw(prev, update, active);
 
 	if (prev->output & PREVIEW_OUTPUT_MEMORY)
 		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
@@ -1343,7 +1442,8 @@ static void preview_configure(struct isp_prev_device *prev)
 
 	preview_adjust_bandwidth(prev);
 
-	preview_config_input_size(prev);
+	preview_config_input_format(prev, format);
+	preview_config_input_size(prev, active);
 
 	if (prev->input == PREVIEW_INPUT_CCDC)
 		preview_config_inlineoffset(prev, 0);
@@ -1360,6 +1460,10 @@ static void preview_configure(struct isp_prev_device *prev)
 
 	preview_config_averager(prev, 0);
 	preview_config_ycpos(prev, format->code);
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, false);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /* -----------------------------------------------------------------------------
@@ -1448,25 +1552,30 @@ static void preview_isr_buffer(struct isp_prev_device *prev)
 void omap3isp_preview_isr(struct isp_prev_device *prev)
 {
 	unsigned long flags;
+	u32 update;
+	u32 active;
 
 	if (omap3isp_module_sync_is_stopping(&prev->wait, &prev->stopping))
 		return;
 
-	spin_lock_irqsave(&prev->lock, flags);
-	if (prev->shadow_update)
-		goto done;
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_switch(prev);
+	update = preview_params_lock(prev, 0, false);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 
-	preview_setup_hw(prev);
-	preview_config_input_size(prev);
-
-done:
-	spin_unlock_irqrestore(&prev->lock, flags);
+	preview_setup_hw(prev, update, active);
+	preview_config_input_size(prev, active);
 
 	if (prev->input == PREVIEW_INPUT_MEMORY ||
 	    prev->output & PREVIEW_OUTPUT_MEMORY)
 		preview_isr_buffer(prev);
 	else if (prev->state == ISP_PIPELINE_STREAM_CONTINUOUS)
 		preview_enable_oneshot(prev);
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, false);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /* -----------------------------------------------------------------------------
@@ -1552,7 +1661,6 @@ static int preview_set_stream(struct v4l2_subdev *sd, int enable)
 	struct isp_video *video_out = &prev->video_out;
 	struct isp_device *isp = to_isp_device(prev);
 	struct device *dev = to_device(prev);
-	unsigned long flags;
 
 	if (prev->state == ISP_PIPELINE_STREAM_STOPPED) {
 		if (enable == ISP_PIPELINE_STREAM_STOPPED)
@@ -1589,11 +1697,9 @@ static int preview_set_stream(struct v4l2_subdev *sd, int enable)
 		if (omap3isp_module_sync_idle(&sd->entity, &prev->wait,
 					      &prev->stopping))
 			dev_dbg(dev, "%s: stop timeout.\n", sd->name);
-		spin_lock_irqsave(&prev->lock, flags);
 		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_PREVIEW_READ);
 		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_PREVIEW_WRITE);
 		omap3isp_subclk_disable(isp, OMAP3_ISP_SUBCLK_PREVIEW);
-		spin_unlock_irqrestore(&prev->lock, flags);
 		isp_video_dmaqueue_flags_clr(video_out);
 		break;
 	}
@@ -1624,6 +1730,7 @@ __preview_get_crop(struct isp_prev_device *prev, struct v4l2_subdev_fh *fh,
 
 /* previewer format descriptions */
 static const unsigned int preview_input_fmts[] = {
+	V4L2_MBUS_FMT_Y10_1X10,
 	V4L2_MBUS_FMT_SGRBG10_1X10,
 	V4L2_MBUS_FMT_SRGGB10_1X10,
 	V4L2_MBUS_FMT_SBGGR10_1X10,
@@ -1822,55 +1929,89 @@ static int preview_enum_frame_size(struct v4l2_subdev *sd,
 }
 
 /*
- * preview_get_crop - Retrieve the crop rectangle on a pad
+ * preview_get_selection - Retrieve a selection rectangle on a pad
  * @sd: ISP preview V4L2 subdevice
  * @fh: V4L2 subdev file handle
- * @crop: crop rectangle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the sink pad.
  *
  * Return 0 on success or a negative error code otherwise.
  */
-static int preview_get_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			    struct v4l2_subdev_crop *crop)
+static int preview_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
 {
 	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->pad != PREV_PAD_SINK)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		format = __preview_get_format(prev, fh, PREV_PAD_SINK,
+					      sel->which);
+		preview_try_crop(prev, format, &sel->r);
+		break;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		sel->r = *__preview_get_crop(prev, fh, sel->which);
+		break;
 
-	/* Cropping is only supported on the sink pad. */
-	if (crop->pad != PREV_PAD_SINK)
+	default:
 		return -EINVAL;
+	}
 
-	crop->rect = *__preview_get_crop(prev, fh, crop->which);
 	return 0;
 }
 
 /*
- * preview_set_crop - Retrieve the crop rectangle on a pad
+ * preview_set_selection - Set a selection rectangle on a pad
  * @sd: ISP preview V4L2 subdevice
  * @fh: V4L2 subdev file handle
- * @crop: crop rectangle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the sink pad.
  *
  * Return 0 on success or a negative error code otherwise.
  */
-static int preview_set_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			    struct v4l2_subdev_crop *crop)
+static int preview_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
 {
 	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
 
-	/* Cropping is only supported on the sink pad. */
-	if (crop->pad != PREV_PAD_SINK)
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != PREV_PAD_SINK)
 		return -EINVAL;
 
 	/* The crop rectangle can't be changed while streaming. */
 	if (prev->state != ISP_PIPELINE_STREAM_STOPPED)
 		return -EBUSY;
 
-	format = __preview_get_format(prev, fh, PREV_PAD_SINK, crop->which);
-	preview_try_crop(prev, format, &crop->rect);
-	*__preview_get_crop(prev, fh, crop->which) = crop->rect;
+	/* Modifying the crop rectangle always changes the format on the source
+	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
+	 * rectangle.
+	 */
+	if (sel->flags & V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG) {
+		sel->r = *__preview_get_crop(prev, fh, sel->which);
+		return 0;
+	}
+
+	format = __preview_get_format(prev, fh, PREV_PAD_SINK, sel->which);
+	preview_try_crop(prev, format, &sel->r);
+	*__preview_get_crop(prev, fh, sel->which) = sel->r;
 
 	/* Update the source format. */
-	format = __preview_get_format(prev, fh, PREV_PAD_SOURCE, crop->which);
-	preview_try_format(prev, fh, PREV_PAD_SOURCE, format, crop->which);
+	format = __preview_get_format(prev, fh, PREV_PAD_SOURCE, sel->which);
+	preview_try_format(prev, fh, PREV_PAD_SOURCE, format, sel->which);
 
 	return 0;
 }
@@ -1979,8 +2120,8 @@ static const struct v4l2_subdev_pad_ops preview_v4l2_pad_ops = {
 	.enum_frame_size = preview_enum_frame_size,
 	.get_fmt = preview_get_format,
 	.set_fmt = preview_set_format,
-	.get_crop = preview_get_crop,
-	.set_crop = preview_set_crop,
+	.get_selection = preview_get_selection,
+	.set_selection = preview_set_selection,
 };
 
 /* subdev operations */
@@ -2076,6 +2217,7 @@ static int preview_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations preview_media_ops = {
 	.link_setup = preview_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_preview_unregister_entities(struct isp_prev_device *prev)
@@ -2201,7 +2343,7 @@ error_video_in:
 }
 
 /*
- * isp_preview_init - Previewer initialization.
+ * omap3isp_preview_init - Previewer initialization.
  * @dev : Pointer to ISP device
  * return -ENOMEM or zero on success
  */
@@ -2209,8 +2351,8 @@ int omap3isp_preview_init(struct isp_device *isp)
 {
 	struct isp_prev_device *prev = &isp->isp_prev;
 
-	spin_lock_init(&prev->lock);
 	init_waitqueue_head(&prev->wait);
+
 	preview_init_params(prev);
 
 	return preview_init_entities(prev);
diff --git a/drivers/media/video/omap3isp/isppreview.h b/drivers/media/video/omap3isp/isppreview.h
index 09686607973c..6663ab64e4b1 100644
--- a/drivers/media/video/omap3isp/isppreview.h
+++ b/drivers/media/video/omap3isp/isppreview.h
@@ -45,29 +45,10 @@
 #define ISPPRV_CONTRAST_HIGH		0xFF
 #define ISPPRV_CONTRAST_UNITS		0x1
 
-/* Features list */
-#define PREV_LUMA_ENHANCE		OMAP3ISP_PREV_LUMAENH
-#define PREV_INVERSE_ALAW		OMAP3ISP_PREV_INVALAW
-#define PREV_HORZ_MEDIAN_FILTER		OMAP3ISP_PREV_HRZ_MED
-#define PREV_CFA			OMAP3ISP_PREV_CFA
-#define PREV_CHROMA_SUPPRESS		OMAP3ISP_PREV_CHROMA_SUPP
-#define PREV_WB				OMAP3ISP_PREV_WB
-#define PREV_BLKADJ			OMAP3ISP_PREV_BLKADJ
-#define PREV_RGB2RGB			OMAP3ISP_PREV_RGB2RGB
-#define PREV_COLOR_CONV			OMAP3ISP_PREV_COLOR_CONV
-#define PREV_YCLIMITS			OMAP3ISP_PREV_YC_LIMIT
-#define PREV_DEFECT_COR			OMAP3ISP_PREV_DEFECT_COR
-#define PREV_GAMMA_BYPASS		OMAP3ISP_PREV_GAMMABYPASS
-#define PREV_DARK_FRAME_CAPTURE		OMAP3ISP_PREV_DRK_FRM_CAPTURE
-#define PREV_DARK_FRAME_SUBTRACT	OMAP3ISP_PREV_DRK_FRM_SUBTRACT
-#define PREV_LENS_SHADING		OMAP3ISP_PREV_LENS_SHADING
-#define PREV_NOISE_FILTER		OMAP3ISP_PREV_NF
-#define PREV_GAMMA			OMAP3ISP_PREV_GAMMA
-
-#define PREV_CONTRAST			(1 << 17)
-#define PREV_BRIGHTNESS			(1 << 18)
-#define PREV_AVERAGER			(1 << 19)
-#define PREV_FEATURES_END		(1 << 20)
+/* Additional features not listed in linux/omap3isp.h */
+#define OMAP3ISP_PREV_CONTRAST		(1 << 17)
+#define OMAP3ISP_PREV_BRIGHTNESS	(1 << 18)
+#define OMAP3ISP_PREV_FEATURES_END	(1 << 19)
 
 enum preview_input_entity {
 	PREVIEW_INPUT_NONE,
@@ -88,6 +69,8 @@ enum preview_ycpos_mode {
 
 /*
  * struct prev_params - Structure for all configuration
+ * @busy: Bitmask of busy parameters (being updated or used)
+ * @update: Bitmask of the parameters to be updated
  * @features: Set of features enabled.
  * @cfa: CFA coefficients.
  * @csup: Chroma suppression coefficients.
@@ -96,15 +79,17 @@ enum preview_ycpos_mode {
  * @dcor: Noise filter coefficients.
  * @gamma: Gamma coefficients.
  * @wbal: White Balance parameters.
- * @blk_adj: Black adjustment parameters.
+ * @blkadj: Black adjustment parameters.
  * @rgb2rgb: RGB blending parameters.
- * @rgb2ycbcr: RGB to ycbcr parameters.
+ * @csc: Color space conversion (RGB to YCbCr) parameters.
  * @hmed: Horizontal median filter.
  * @yclimit: YC limits parameters.
  * @contrast: Contrast.
  * @brightness: Brightness.
  */
 struct prev_params {
+	u32 busy;
+	u32 update;
 	u32 features;
 	struct omap3isp_prev_cfa cfa;
 	struct omap3isp_prev_csup csup;
@@ -113,35 +98,15 @@ struct prev_params {
 	struct omap3isp_prev_dcor dcor;
 	struct omap3isp_prev_gtables gamma;
 	struct omap3isp_prev_wbal wbal;
-	struct omap3isp_prev_blkadj blk_adj;
+	struct omap3isp_prev_blkadj blkadj;
 	struct omap3isp_prev_rgbtorgb rgb2rgb;
-	struct omap3isp_prev_csc rgb2ycbcr;
+	struct omap3isp_prev_csc csc;
 	struct omap3isp_prev_hmed hmed;
 	struct omap3isp_prev_yclimit yclimit;
 	u8 contrast;
 	u8 brightness;
 };
 
-/*
- * struct isptables_update - Structure for Table Configuration.
- * @update: Specifies which tables should be updated.
- * @flag: Specifies which tables should be enabled.
- * @nf: Pointer to structure for Noise Filter
- * @lsc: Pointer to LSC gain table. (currently not used)
- * @gamma: Pointer to gamma correction tables.
- * @cfa: Pointer to color filter array configuration.
- * @wbal: Pointer to colour and digital gain configuration.
- */
-struct isptables_update {
-	u32 update;
-	u32 flag;
-	struct omap3isp_prev_nf *nf;
-	u32 *lsc;
-	struct omap3isp_prev_gtables *gamma;
-	struct omap3isp_prev_cfa *cfa;
-	struct omap3isp_prev_wbal *wbal;
-};
-
 /* Sink and source previewer pads */
 #define PREV_PAD_SINK			0
 #define PREV_PAD_SOURCE			1
@@ -157,12 +122,11 @@ struct isptables_update {
  * @output: Bitmask of the active output
  * @video_in: Input video entity
  * @video_out: Output video entity
- * @params: Module configuration data
- * @shadow_update: If set, update the hardware configured in the next interrupt
+ * @params.params : Active and shadow parameters sets
+ * @params.active: Bitmask of parameters active in set 0
+ * @params.lock: Parameters lock, protects params.active and params.shadow
  * @underrun: Whether the preview entity has queued buffers on the output
  * @state: Current preview pipeline state
- * @lock: Shadow update lock
- * @update: Bitmask of the parameters to be updated
  *
  * This structure is used to store the OMAP ISP Preview module Information.
  */
@@ -179,13 +143,15 @@ struct isp_prev_device {
 	struct isp_video video_in;
 	struct isp_video video_out;
 
-	struct prev_params params;
-	unsigned int shadow_update:1;
+	struct {
+		struct prev_params params[2];
+		u32 active;
+		spinlock_t lock;
+	} params;
+
 	enum isp_pipeline_stream_state state;
 	wait_queue_head_t wait;
 	atomic_t stopping;
-	spinlock_t lock;
-	u32 update;
 };
 
 struct isp_device;
diff --git a/drivers/media/video/omap3isp/ispqueue.h b/drivers/media/video/omap3isp/ispqueue.h
index 92c5a12157d5..908dfd712e8e 100644
--- a/drivers/media/video/omap3isp/ispqueue.h
+++ b/drivers/media/video/omap3isp/ispqueue.h
@@ -90,7 +90,7 @@ struct isp_video_buffer {
 	void *vaddr;
 
 	/* For userspace buffers. */
-	unsigned long vm_flags;
+	vm_flags_t vm_flags;
 	unsigned long offset;
 	unsigned int npages;
 	struct page **pages;
diff --git a/drivers/media/video/omap3isp/ispresizer.c b/drivers/media/video/omap3isp/ispresizer.c
index 6958a9e3dc22..14041c9c8643 100644
--- a/drivers/media/video/omap3isp/ispresizer.c
+++ b/drivers/media/video/omap3isp/ispresizer.c
@@ -1188,32 +1188,6 @@ static int resizer_set_stream(struct v4l2_subdev *sd, int enable)
 }
 
 /*
- * resizer_g_crop - handle get crop subdev operation
- * @sd : pointer to v4l2 subdev structure
- * @pad : subdev pad
- * @crop : pointer to crop structure
- * @which : active or try format
- * return zero
- */
-static int resizer_g_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			  struct v4l2_subdev_crop *crop)
-{
-	struct isp_res_device *res = v4l2_get_subdevdata(sd);
-	struct v4l2_mbus_framefmt *format;
-	struct resizer_ratio ratio;
-
-	/* Only sink pad has crop capability */
-	if (crop->pad != RESZ_PAD_SINK)
-		return -EINVAL;
-
-	format = __resizer_get_format(res, fh, RESZ_PAD_SOURCE, crop->which);
-	crop->rect = *__resizer_get_crop(res, fh, crop->which);
-	resizer_calc_ratios(res, &crop->rect, format, &ratio);
-
-	return 0;
-}
-
-/*
  * resizer_try_crop - mangles crop parameters.
  */
 static void resizer_try_crop(const struct v4l2_mbus_framefmt *sink,
@@ -1223,7 +1197,7 @@ static void resizer_try_crop(const struct v4l2_mbus_framefmt *sink,
 	const unsigned int spv = DEFAULT_PHASE;
 	const unsigned int sph = DEFAULT_PHASE;
 
-	/* Crop rectangle is constrained to the output size so that zoom ratio
+	/* Crop rectangle is constrained by the output size so that zoom ratio
 	 * cannot exceed +/-4.0.
 	 */
 	unsigned int min_width =
@@ -1248,51 +1222,115 @@ static void resizer_try_crop(const struct v4l2_mbus_framefmt *sink,
 }
 
 /*
- * resizer_s_crop - handle set crop subdev operation
- * @sd : pointer to v4l2 subdev structure
- * @pad : subdev pad
- * @crop : pointer to crop structure
- * @which : active or try format
- * return -EINVAL or zero when succeed
+ * resizer_get_selection - Retrieve a selection rectangle on a pad
+ * @sd: ISP resizer V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the sink pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
  */
-static int resizer_s_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			  struct v4l2_subdev_crop *crop)
+static int resizer_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format_source;
+	struct v4l2_mbus_framefmt *format_sink;
+	struct resizer_ratio ratio;
+
+	if (sel->pad != RESZ_PAD_SINK)
+		return -EINVAL;
+
+	format_sink = __resizer_get_format(res, fh, RESZ_PAD_SINK,
+					   sel->which);
+	format_source = __resizer_get_format(res, fh, RESZ_PAD_SOURCE,
+					     sel->which);
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		resizer_try_crop(format_sink, format_source, &sel->r);
+		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
+		break;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		sel->r = *__resizer_get_crop(res, fh, sel->which);
+		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * resizer_set_selection - Set a selection rectangle on a pad
+ * @sd: ISP resizer V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the sink pad.
+ *
+ * FIXME: This function currently behaves as if the KEEP_CONFIG selection flag
+ * was always set.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int resizer_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
 {
 	struct isp_res_device *res = v4l2_get_subdevdata(sd);
 	struct isp_device *isp = to_isp_device(res);
 	struct v4l2_mbus_framefmt *format_sink, *format_source;
 	struct resizer_ratio ratio;
 
-	/* Only sink pad has crop capability */
-	if (crop->pad != RESZ_PAD_SINK)
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != RESZ_PAD_SINK)
 		return -EINVAL;
 
 	format_sink = __resizer_get_format(res, fh, RESZ_PAD_SINK,
-					   crop->which);
+					   sel->which);
 	format_source = __resizer_get_format(res, fh, RESZ_PAD_SOURCE,
-					     crop->which);
+					     sel->which);
 
 	dev_dbg(isp->dev, "%s: L=%d,T=%d,W=%d,H=%d,which=%d\n", __func__,
-		crop->rect.left, crop->rect.top, crop->rect.width,
-		crop->rect.height, crop->which);
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height,
+		sel->which);
 
 	dev_dbg(isp->dev, "%s: input=%dx%d, output=%dx%d\n", __func__,
 		format_sink->width, format_sink->height,
 		format_source->width, format_source->height);
 
-	resizer_try_crop(format_sink, format_source, &crop->rect);
-	*__resizer_get_crop(res, fh, crop->which) = crop->rect;
-	resizer_calc_ratios(res, &crop->rect, format_source, &ratio);
+	/* Clamp the crop rectangle to the bounds, and then mangle it further to
+	 * fulfill the TRM equations. Store the clamped but otherwise unmangled
+	 * rectangle to avoid cropping the input multiple times: when an
+	 * application sets the output format, the current crop rectangle is
+	 * mangled during crop rectangle computation, which would lead to a new,
+	 * smaller input crop rectangle every time the output size is set if we
+	 * stored the mangled rectangle.
+	 */
+	resizer_try_crop(format_sink, format_source, &sel->r);
+	*__resizer_get_crop(res, fh, sel->which) = sel->r;
+	resizer_calc_ratios(res, &sel->r, format_source, &ratio);
 
-	if (crop->which == V4L2_SUBDEV_FORMAT_TRY)
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
 		return 0;
 
 	res->ratio = ratio;
-	res->crop.active = crop->rect;
+	res->crop.active = sel->r;
 
 	/*
-	 * s_crop can be called while streaming is on. In this case
-	 * the crop values will be set in the next IRQ.
+	 * set_selection can be called while streaming is on. In this case the
+	 * crop values will be set in the next IRQ.
 	 */
 	if (res->state != ISP_PIPELINE_STREAM_STOPPED)
 		res->applycrop = 1;
@@ -1530,8 +1568,8 @@ static const struct v4l2_subdev_pad_ops resizer_v4l2_pad_ops = {
 	.enum_frame_size = resizer_enum_frame_size,
 	.get_fmt = resizer_get_format,
 	.set_fmt = resizer_set_format,
-	.get_crop = resizer_g_crop,
-	.set_crop = resizer_s_crop,
+	.get_selection = resizer_get_selection,
+	.set_selection = resizer_set_selection,
 };
 
 /* subdev operations */
@@ -1603,6 +1641,7 @@ static int resizer_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations resizer_media_ops = {
 	.link_setup = resizer_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_resizer_unregister_entities(struct isp_res_device *res)
diff --git a/drivers/media/video/omap3isp/ispstat.c b/drivers/media/video/omap3isp/ispstat.c
index 11871ecc6d25..b8640be692f1 100644
--- a/drivers/media/video/omap3isp/ispstat.c
+++ b/drivers/media/video/omap3isp/ispstat.c
@@ -1032,7 +1032,7 @@ int omap3isp_stat_subscribe_event(struct v4l2_subdev *subdev,
 	if (sub->type != stat->event_type)
 		return -EINVAL;
 
-	return v4l2_event_subscribe(fh, sub, STAT_NEVENTS);
+	return v4l2_event_subscribe(fh, sub, STAT_NEVENTS, NULL);
 }
 
 int omap3isp_stat_unsubscribe_event(struct v4l2_subdev *subdev,
diff --git a/drivers/media/video/omap3isp/ispvideo.c b/drivers/media/video/omap3isp/ispvideo.c
index b02070057724..b37379d39cdd 100644
--- a/drivers/media/video/omap3isp/ispvideo.c
+++ b/drivers/media/video/omap3isp/ispvideo.c
@@ -46,6 +46,10 @@
  * Helper functions
  */
 
+/*
+ * NOTE: When adding new media bus codes, always remember to add
+ * corresponding in-memory formats to the table below!!!
+ */
 static struct isp_format_info formats[] = {
 	{ V4L2_MBUS_FMT_Y8_1X8, V4L2_MBUS_FMT_Y8_1X8,
 	  V4L2_MBUS_FMT_Y8_1X8, V4L2_MBUS_FMT_Y8_1X8,
@@ -68,9 +72,18 @@ static struct isp_format_info formats[] = {
 	{ V4L2_MBUS_FMT_SRGGB8_1X8, V4L2_MBUS_FMT_SRGGB8_1X8,
 	  V4L2_MBUS_FMT_SRGGB8_1X8, V4L2_MBUS_FMT_SRGGB8_1X8,
 	  V4L2_PIX_FMT_SRGGB8, 8, },
+	{ V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8, V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8,
+	  V4L2_MBUS_FMT_SBGGR10_1X10, 0,
+	  V4L2_PIX_FMT_SBGGR10DPCM8, 8, },
+	{ V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8, V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8,
+	  V4L2_MBUS_FMT_SGBRG10_1X10, 0,
+	  V4L2_PIX_FMT_SGBRG10DPCM8, 8, },
 	{ V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8, V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8,
 	  V4L2_MBUS_FMT_SGRBG10_1X10, 0,
 	  V4L2_PIX_FMT_SGRBG10DPCM8, 8, },
+	{ V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8, V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8,
+	  V4L2_MBUS_FMT_SRGGB10_1X10, 0,
+	  V4L2_PIX_FMT_SRGGB10DPCM8, 8, },
 	{ V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_MBUS_FMT_SBGGR10_1X10,
 	  V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_MBUS_FMT_SBGGR8_1X8,
 	  V4L2_PIX_FMT_SBGGR10, 10, },
@@ -117,37 +130,6 @@ omap3isp_video_format_info(enum v4l2_mbus_pixelcode code)
 }
 
 /*
- * Decide whether desired output pixel code can be obtained with
- * the lane shifter by shifting the input pixel code.
- * @in: input pixelcode to shifter
- * @out: output pixelcode from shifter
- * @additional_shift: # of bits the sensor's LSB is offset from CAMEXT[0]
- *
- * return true if the combination is possible
- * return false otherwise
- */
-static bool isp_video_is_shiftable(enum v4l2_mbus_pixelcode in,
-		enum v4l2_mbus_pixelcode out,
-		unsigned int additional_shift)
-{
-	const struct isp_format_info *in_info, *out_info;
-
-	if (in == out)
-		return true;
-
-	in_info = omap3isp_video_format_info(in);
-	out_info = omap3isp_video_format_info(out);
-
-	if ((in_info->flavor == 0) || (out_info->flavor == 0))
-		return false;
-
-	if (in_info->flavor != out_info->flavor)
-		return false;
-
-	return in_info->bpp - out_info->bpp + additional_shift <= 6;
-}
-
-/*
  * isp_video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
  * @video: ISP video instance
  * @mbus: v4l2_mbus_framefmt format (input)
@@ -242,8 +224,8 @@ isp_video_remote_subdev(struct isp_video *video, u32 *pad)
 }
 
 /* Return a pointer to the ISP video instance at the far end of the pipeline. */
-static struct isp_video *
-isp_video_far_end(struct isp_video *video)
+static int isp_video_get_graph_data(struct isp_video *video,
+				    struct isp_pipeline *pipe)
 {
 	struct media_entity_graph graph;
 	struct media_entity *entity = &video->video.entity;
@@ -254,21 +236,38 @@ isp_video_far_end(struct isp_video *video)
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
+		struct isp_video *__video;
+
+		pipe->entities |= 1 << entity->id;
+
+		if (far_end != NULL)
+			continue;
+
 		if (entity == &video->video.entity)
 			continue;
 
 		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
 			continue;
 
-		far_end = to_isp_video(media_entity_to_video_device(entity));
-		if (far_end->type != video->type)
-			break;
-
-		far_end = NULL;
+		__video = to_isp_video(media_entity_to_video_device(entity));
+		if (__video->type != video->type)
+			far_end = __video;
 	}
 
 	mutex_unlock(&mdev->graph_mutex);
-	return far_end;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pipe->input = far_end;
+		pipe->output = video;
+	} else {
+		if (far_end == NULL)
+			return -EPIPE;
+
+		pipe->input = video;
+		pipe->output = far_end;
+	}
+
+	return 0;
 }
 
 /*
@@ -285,52 +284,24 @@ isp_video_far_end(struct isp_video *video)
 static int isp_video_validate_pipeline(struct isp_pipeline *pipe)
 {
 	struct isp_device *isp = pipe->output->isp;
-	struct v4l2_subdev_format fmt_source;
-	struct v4l2_subdev_format fmt_sink;
 	struct media_pad *pad;
 	struct v4l2_subdev *subdev;
-	int ret;
-
-	pipe->max_rate = pipe->l3_ick;
 
 	subdev = isp_video_remote_subdev(pipe->output, NULL);
 	if (subdev == NULL)
 		return -EPIPE;
 
 	while (1) {
-		unsigned int shifter_link;
 		/* Retrieve the sink format */
 		pad = &subdev->entity.pads[0];
 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
 			break;
 
-		fmt_sink.pad = pad->index;
-		fmt_sink.which = V4L2_SUBDEV_FORMAT_ACTIVE;
-		ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt_sink);
-		if (ret < 0 && ret != -ENOIOCTLCMD)
-			return -EPIPE;
-
 		/* Update the maximum frame rate */
 		if (subdev == &isp->isp_res.subdev)
 			omap3isp_resizer_max_rate(&isp->isp_res,
 						  &pipe->max_rate);
 
-		/* Check ccdc maximum data rate when data comes from sensor
-		 * TODO: Include ccdc rate in pipe->max_rate and compare the
-		 *       total pipe rate with the input data rate from sensor.
-		 */
-		if (subdev == &isp->isp_ccdc.subdev && pipe->input == NULL) {
-			unsigned int rate = UINT_MAX;
-
-			omap3isp_ccdc_max_rate(&isp->isp_ccdc, &rate);
-			if (isp->isp_ccdc.vpcfg.pixelclk > rate)
-				return -ENOSPC;
-		}
-
-		/* If sink pad is on CCDC, the link has the lane shifter
-		 * in the middle of it. */
-		shifter_link = subdev == &isp->isp_ccdc.subdev;
-
 		/* Retrieve the source format. Return an error if no source
 		 * entity can be found, and stop checking the pipeline if the
 		 * source entity isn't a subdev.
@@ -343,32 +314,6 @@ static int isp_video_validate_pipeline(struct isp_pipeline *pipe)
 			break;
 
 		subdev = media_entity_to_v4l2_subdev(pad->entity);
-
-		fmt_source.pad = pad->index;
-		fmt_source.which = V4L2_SUBDEV_FORMAT_ACTIVE;
-		ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt_source);
-		if (ret < 0 && ret != -ENOIOCTLCMD)
-			return -EPIPE;
-
-		/* Check if the two ends match */
-		if (fmt_source.format.width != fmt_sink.format.width ||
-		    fmt_source.format.height != fmt_sink.format.height)
-			return -EPIPE;
-
-		if (shifter_link) {
-			unsigned int parallel_shift = 0;
-			if (isp->isp_ccdc.input == CCDC_INPUT_PARALLEL) {
-				struct isp_parallel_platform_data *pdata =
-					&((struct isp_v4l2_subdevs_group *)
-					      subdev->host_priv)->bus.parallel;
-				parallel_shift = pdata->data_lane_shift * 2;
-			}
-			if (!isp_video_is_shiftable(fmt_source.format.code,
-						fmt_sink.format.code,
-						parallel_shift))
-				return -EPIPE;
-		} else if (fmt_source.format.code != fmt_sink.format.code)
-			return -EPIPE;
 	}
 
 	return 0;
@@ -923,6 +868,92 @@ isp_video_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
 					  file->f_flags & O_NONBLOCK);
 }
 
+static int isp_video_check_external_subdevs(struct isp_video *video,
+					    struct isp_pipeline *pipe)
+{
+	struct isp_device *isp = video->isp;
+	struct media_entity *ents[] = {
+		&isp->isp_csi2a.subdev.entity,
+		&isp->isp_csi2c.subdev.entity,
+		&isp->isp_ccp2.subdev.entity,
+		&isp->isp_ccdc.subdev.entity
+	};
+	struct media_pad *source_pad;
+	struct media_entity *source = NULL;
+	struct media_entity *sink;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_ext_controls ctrls;
+	struct v4l2_ext_control ctrl;
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; i < ARRAY_SIZE(ents); i++) {
+		/* Is the entity part of the pipeline? */
+		if (!(pipe->entities & (1 << ents[i]->id)))
+			continue;
+
+		/* ISP entities have always sink pad == 0. Find source. */
+		source_pad = media_entity_remote_source(&ents[i]->pads[0]);
+		if (source_pad == NULL)
+			continue;
+
+		source = source_pad->entity;
+		sink = ents[i];
+		break;
+	}
+
+	if (!source) {
+		dev_warn(isp->dev, "can't find source, failing now\n");
+		return ret;
+	}
+
+	if (media_entity_type(source) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return 0;
+
+	pipe->external = media_entity_to_v4l2_subdev(source);
+
+	fmt.pad = source_pad->index;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(media_entity_to_v4l2_subdev(sink),
+			       pad, get_fmt, NULL, &fmt);
+	if (unlikely(ret < 0)) {
+		dev_warn(isp->dev, "get_fmt returned null!\n");
+		return ret;
+	}
+
+	pipe->external_bpp = omap3isp_video_format_info(fmt.format.code)->bpp;
+
+	memset(&ctrls, 0, sizeof(ctrls));
+	memset(&ctrl, 0, sizeof(ctrl));
+
+	ctrl.id = V4L2_CID_PIXEL_RATE;
+
+	ctrls.count = 1;
+	ctrls.controls = &ctrl;
+
+	ret = v4l2_g_ext_ctrls(pipe->external->ctrl_handler, &ctrls);
+	if (ret < 0) {
+		dev_warn(isp->dev, "no pixel rate control in subdev %s\n",
+			 pipe->external->name);
+		return ret;
+	}
+
+	pipe->external_rate = ctrl.value64;
+
+	if (pipe->entities & (1 << isp->isp_ccdc.subdev.entity.id)) {
+		unsigned int rate = UINT_MAX;
+		/*
+		 * Check that maximum allowed CCDC pixel rate isn't
+		 * exceeded by the pixel rate.
+		 */
+		omap3isp_ccdc_max_rate(&isp->isp_ccdc, &rate);
+		if (pipe->external_rate > rate)
+			return -ENOSPC;
+	}
+
+	return 0;
+}
+
 /*
  * Stream management
  *
@@ -961,7 +992,6 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	struct isp_video *video = video_drvdata(file);
 	enum isp_pipeline_state state;
 	struct isp_pipeline *pipe;
-	struct isp_video *far_end;
 	unsigned long flags;
 	int ret;
 
@@ -980,46 +1010,45 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	 */
 	pipe = video->video.entity.pipe
 	     ? to_isp_pipeline(&video->video.entity) : &video->pipe;
-	media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+
+	pipe->entities = 0;
+
+	if (video->isp->pdata->set_constraints)
+		video->isp->pdata->set_constraints(video->isp, true);
+	pipe->l3_ick = clk_get_rate(video->isp->clock[ISP_CLK_L3_ICK]);
+	pipe->max_rate = pipe->l3_ick;
+
+	ret = media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+	if (ret < 0)
+		goto err_pipeline_start;
 
 	/* Verify that the currently configured format matches the output of
 	 * the connected subdev.
 	 */
 	ret = isp_video_check_format(video, vfh);
 	if (ret < 0)
-		goto error;
+		goto err_check_format;
 
 	video->bpl_padding = ret;
 	video->bpl_value = vfh->format.fmt.pix.bytesperline;
 
-	/* Find the ISP video node connected at the far end of the pipeline and
-	 * update the pipeline.
-	 */
-	far_end = isp_video_far_end(video);
+	ret = isp_video_get_graph_data(video, pipe);
+	if (ret < 0)
+		goto err_check_format;
 
-	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		state = ISP_PIPELINE_STREAM_OUTPUT | ISP_PIPELINE_IDLE_OUTPUT;
-		pipe->input = far_end;
-		pipe->output = video;
-	} else {
-		if (far_end == NULL) {
-			ret = -EPIPE;
-			goto error;
-		}
-
+	else
 		state = ISP_PIPELINE_STREAM_INPUT | ISP_PIPELINE_IDLE_INPUT;
-		pipe->input = video;
-		pipe->output = far_end;
-	}
 
-	if (video->isp->pdata->set_constraints)
-		video->isp->pdata->set_constraints(video->isp, true);
-	pipe->l3_ick = clk_get_rate(video->isp->clock[ISP_CLK_L3_ICK]);
+	ret = isp_video_check_external_subdevs(video, pipe);
+	if (ret < 0)
+		goto err_check_format;
 
 	/* Validate the pipeline and update its state. */
 	ret = isp_video_validate_pipeline(pipe);
 	if (ret < 0)
-		goto error;
+		goto err_check_format;
 
 	pipe->error = false;
 
@@ -1041,7 +1070,7 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 
 	ret = omap3isp_video_queue_streamon(&vfh->queue);
 	if (ret < 0)
-		goto error;
+		goto err_check_format;
 
 	/* In sensor-to-memory mode, the stream can be started synchronously
 	 * to the stream on command. In memory-to-memory mode, it will be
@@ -1051,32 +1080,34 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 		ret = omap3isp_pipeline_set_stream(pipe,
 					      ISP_PIPELINE_STREAM_CONTINUOUS);
 		if (ret < 0)
-			goto error;
+			goto err_set_stream;
 		spin_lock_irqsave(&video->queue->irqlock, flags);
 		if (list_empty(&video->dmaqueue))
 			video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_UNDERRUN;
 		spin_unlock_irqrestore(&video->queue->irqlock, flags);
 	}
 
-error:
-	if (ret < 0) {
-		omap3isp_video_queue_streamoff(&vfh->queue);
-		if (video->isp->pdata->set_constraints)
-			video->isp->pdata->set_constraints(video->isp, false);
-		media_entity_pipeline_stop(&video->video.entity);
-		/* The DMA queue must be emptied here, otherwise CCDC interrupts
-		 * that will get triggered the next time the CCDC is powered up
-		 * will try to access buffers that might have been freed but
-		 * still present in the DMA queue. This can easily get triggered
-		 * if the above omap3isp_pipeline_set_stream() call fails on a
-		 * system with a free-running sensor.
-		 */
-		INIT_LIST_HEAD(&video->dmaqueue);
-		video->queue = NULL;
-	}
+	video->streaming = 1;
+
+	mutex_unlock(&video->stream_lock);
+	return 0;
 
-	if (!ret)
-		video->streaming = 1;
+err_set_stream:
+	omap3isp_video_queue_streamoff(&vfh->queue);
+err_check_format:
+	media_entity_pipeline_stop(&video->video.entity);
+err_pipeline_start:
+	if (video->isp->pdata->set_constraints)
+		video->isp->pdata->set_constraints(video->isp, false);
+	/* The DMA queue must be emptied here, otherwise CCDC interrupts that
+	 * will get triggered the next time the CCDC is powered up will try to
+	 * access buffers that might have been freed but still present in the
+	 * DMA queue. This can easily get triggered if the above
+	 * omap3isp_pipeline_set_stream() call fails on a system with a
+	 * free-running sensor.
+	 */
+	INIT_LIST_HEAD(&video->dmaqueue);
+	video->queue = NULL;
 
 	mutex_unlock(&video->stream_lock);
 	return ret;
diff --git a/drivers/media/video/omap3isp/ispvideo.h b/drivers/media/video/omap3isp/ispvideo.h
index d91bdb919be0..5acc909500ec 100644
--- a/drivers/media/video/omap3isp/ispvideo.h
+++ b/drivers/media/video/omap3isp/ispvideo.h
@@ -88,6 +88,7 @@ enum isp_pipeline_state {
 /*
  * struct isp_pipeline - An ISP hardware pipeline
  * @error: A hardware error occurred during capture
+ * @entities: Bitmask of entities in the pipeline (indexed by entity ID)
  */
 struct isp_pipeline {
 	struct media_pipeline pipe;
@@ -96,12 +97,16 @@ struct isp_pipeline {
 	enum isp_pipeline_stream_state stream_state;
 	struct isp_video *input;
 	struct isp_video *output;
+	u32 entities;
 	unsigned long l3_ick;
 	unsigned int max_rate;
 	atomic_t frame_number;
 	bool do_propagation; /* of frame number */
 	bool error;
 	struct v4l2_fract max_timeperframe;
+	struct v4l2_subdev *external;
+	unsigned int external_rate;
+	unsigned int external_bpp;
 };
 
 #define to_isp_pipeline(__e) \
diff --git a/drivers/media/video/ov5642.c b/drivers/media/video/ov5642.c
index 80e07794ac8e..0bc93313d37a 100644
--- a/drivers/media/video/ov5642.c
+++ b/drivers/media/video/ov5642.c
@@ -1025,8 +1025,6 @@ static int ov5642_probe(struct i2c_client *client,
 	priv->crop_rect.height	= OV5642_DEFAULT_HEIGHT;
 	priv->crop_rect.left	= (OV5642_MAX_WIDTH - OV5642_DEFAULT_WIDTH) / 2;
 	priv->crop_rect.top	= (OV5642_MAX_HEIGHT - OV5642_DEFAULT_HEIGHT) / 2;
-	priv->crop_rect.width	= OV5642_DEFAULT_WIDTH;
-	priv->crop_rect.height	= OV5642_DEFAULT_HEIGHT;
 	priv->total_width = OV5642_DEFAULT_WIDTH + BLANKING_EXTRA_WIDTH;
 	priv->total_height = BLANKING_MIN_HEIGHT;
 
diff --git a/drivers/media/video/pms.c b/drivers/media/video/pms.c
index e753b5e4d2ce..af2d9086d7e8 100644
--- a/drivers/media/video/pms.c
+++ b/drivers/media/video/pms.c
@@ -30,15 +30,19 @@
 #include <linux/init.h>
 #include <linux/mutex.h>
 #include <linux/uaccess.h>
+#include <linux/isa.h>
 #include <asm/io.h>
 
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-device.h>
 
 MODULE_LICENSE("GPL");
-MODULE_VERSION("0.0.4");
+MODULE_VERSION("0.0.5");
 
 #define MOTOROLA	1
 #define PHILIPS2	2               /* SAA7191 */
@@ -55,11 +59,11 @@ struct i2c_info {
 struct pms {
 	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
 	int height;
 	int width;
 	int depth;
 	int input;
-	s32 brightness, saturation, hue, contrast;
 	struct mutex lock;
 	int i2c_count;
 	struct i2c_info i2cinfo[64];
@@ -72,8 +76,6 @@ struct pms {
 	void __iomem *mem;
 };
 
-static struct pms pms_card;
-
 /*
  *	I/O ports and Shared Memory
  */
@@ -676,8 +678,10 @@ static int pms_querycap(struct file *file, void  *priv,
 
 	strlcpy(vcap->driver, dev->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "Mediavision PMS", sizeof(vcap->card));
-	strlcpy(vcap->bus_info, "ISA", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	snprintf(vcap->bus_info, sizeof(vcap->bus_info),
+			"ISA:%s", dev->v4l2_dev.name);
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -716,11 +720,9 @@ static int pms_s_input(struct file *file, void *fh, unsigned int inp)
 	if (inp > 3)
 		return -EINVAL;
 
-	mutex_lock(&dev->lock);
 	dev->input = inp;
 	pms_videosource(dev, inp & 1);
 	pms_vcrinput(dev, inp >> 1);
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 
@@ -738,7 +740,6 @@ static int pms_s_std(struct file *file, void *fh, v4l2_std_id *std)
 	int ret = 0;
 
 	dev->std = *std;
-	mutex_lock(&dev->lock);
 	if (dev->std & V4L2_STD_NTSC) {
 		pms_framerate(dev, 30);
 		pms_secamcross(dev, 0);
@@ -762,81 +763,31 @@ static int pms_s_std(struct file *file, void *fh, v4l2_std_id *std)
 		pms_format(dev, 0);
 		break;
 	}*/
-	mutex_unlock(&dev->lock);
-	return 0;
-}
-
-static int pms_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 139);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 70);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 64);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 0);
-	}
-	return -EINVAL;
-}
-
-static int pms_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct pms *dev = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = dev->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = dev->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = dev->saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = dev->hue;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
 	return ret;
 }
 
-static int pms_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
+static int pms_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct pms *dev = video_drvdata(file);
+	struct pms *dev = container_of(ctrl->handler, struct pms, hdl);
 	int ret = 0;
 
-	mutex_lock(&dev->lock);
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		dev->brightness = ctrl->value;
-		pms_brightness(dev, dev->brightness);
+		pms_brightness(dev, ctrl->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		dev->contrast = ctrl->value;
-		pms_contrast(dev, dev->contrast);
+		pms_contrast(dev, ctrl->val);
 		break;
 	case V4L2_CID_SATURATION:
-		dev->saturation = ctrl->value;
-		pms_saturation(dev, dev->saturation);
+		pms_saturation(dev, ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		dev->hue = ctrl->value;
-		pms_hue(dev, dev->hue);
+		pms_hue(dev, ctrl->val);
 		break;
 	default:
 		ret = -EINVAL;
 		break;
 	}
-	mutex_unlock(&dev->lock);
 	return ret;
 }
 
@@ -884,13 +835,11 @@ static int pms_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fm
 
 	if (ret)
 		return ret;
-	mutex_lock(&dev->lock);
 	dev->width = pix->width;
 	dev->height = pix->height;
 	dev->depth = (pix->pixelformat == V4L2_PIX_FMT_RGB555) ? 15 : 16;
 	pms_resolution(dev, dev->width, dev->height);
 	/* Ok we figured out what to use from our wide choice */
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 
@@ -901,7 +850,7 @@ static int pms_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtdesc
 		  "RGB 5:5:5", V4L2_PIX_FMT_RGB555,
 		  { 0, 0, 0, 0 }
 		},
-		{ 0, 0, 0,
+		{ 1, 0, 0,
 		  "RGB 5:6:5", V4L2_PIX_FMT_RGB565,
 		  { 0, 0, 0, 0 }
 		},
@@ -922,32 +871,43 @@ static ssize_t pms_read(struct file *file, char __user *buf,
 	struct pms *dev = video_drvdata(file);
 	int len;
 
-	mutex_lock(&dev->lock);
 	len = pms_capture(dev, buf, (dev->depth == 15), count);
-	mutex_unlock(&dev->lock);
 	return len;
 }
 
+static unsigned int pms_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct v4l2_fh *fh = file->private_data;
+	unsigned int res = POLLIN | POLLRDNORM;
+
+	if (v4l2_event_pending(fh))
+		res |= POLLPRI;
+	poll_wait(file, &fh->wait, wait);
+	return res;
+}
+
 static const struct v4l2_file_operations pms_fops = {
 	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = v4l2_fh_release,
+	.poll           = pms_poll,
 	.unlocked_ioctl	= video_ioctl2,
 	.read           = pms_read,
 };
 
 static const struct v4l2_ioctl_ops pms_ioctl_ops = {
-	.vidioc_querycap    		    = pms_querycap,
-	.vidioc_g_input      		    = pms_g_input,
-	.vidioc_s_input      		    = pms_s_input,
-	.vidioc_enum_input   		    = pms_enum_input,
-	.vidioc_g_std 			    = pms_g_std,
-	.vidioc_s_std 			    = pms_s_std,
-	.vidioc_queryctrl 		    = pms_queryctrl,
-	.vidioc_g_ctrl  		    = pms_g_ctrl,
-	.vidioc_s_ctrl 			    = pms_s_ctrl,
-	.vidioc_enum_fmt_vid_cap 	    = pms_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap 		    = pms_g_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap  		    = pms_s_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap  	    = pms_try_fmt_vid_cap,
+	.vidioc_querycap	    = pms_querycap,
+	.vidioc_g_input		    = pms_g_input,
+	.vidioc_s_input		    = pms_s_input,
+	.vidioc_enum_input	    = pms_enum_input,
+	.vidioc_g_std		    = pms_g_std,
+	.vidioc_s_std		    = pms_s_std,
+	.vidioc_enum_fmt_vid_cap    = pms_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	    = pms_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	    = pms_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap     = pms_try_fmt_vid_cap,
+	.vidioc_subscribe_event     = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event   = v4l2_event_unsubscribe,
 };
 
 /*
@@ -956,7 +916,6 @@ static const struct v4l2_ioctl_ops pms_ioctl_ops = {
 
 static int init_mediavision(struct pms *dev)
 {
-	int id;
 	int idec, decst;
 	int i;
 	static const unsigned char i2c_defs[] = {
@@ -988,7 +947,6 @@ static int init_mediavision(struct pms *dev)
 	outb(dev->io >> 4, 0x9a01);	/* Set IO port */
 
 
-	id = mvv_read(dev, 3);
 	decst = pms_i2c_stat(dev, 0x43);
 
 	if (decst != -1)
@@ -1068,76 +1026,125 @@ static int enable;
 module_param(enable, int, 0);
 #endif
 
-static int __init pms_init(void)
+static const struct v4l2_ctrl_ops pms_ctrl_ops = {
+	.s_ctrl = pms_s_ctrl,
+};
+
+static int pms_probe(struct device *pdev, unsigned int card)
 {
-	struct pms *dev = &pms_card;
-	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
+	struct pms *dev;
+	struct v4l2_device *v4l2_dev;
+	struct v4l2_ctrl_handler *hdl;
 	int res;
 
-	strlcpy(v4l2_dev->name, "pms", sizeof(v4l2_dev->name));
-
-	v4l2_info(v4l2_dev, "Mediavision Pro Movie Studio driver 0.03\n");
-
 #ifndef MODULE
 	if (!enable) {
-		v4l2_err(v4l2_dev,
-			"PMS: not enabled, use pms.enable=1 to probe\n");
+		pr_err("PMS: not enabled, use pms.enable=1 to probe\n");
 		return -ENODEV;
 	}
 #endif
 
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL)
+		return -ENOMEM;
+
 	dev->decoder = PHILIPS2;
 	dev->io = io_port;
 	dev->data = io_port + 1;
+	v4l2_dev = &dev->v4l2_dev;
+	hdl = &dev->hdl;
 
-	if (init_mediavision(dev)) {
+	res = v4l2_device_register(pdev, v4l2_dev);
+	if (res < 0) {
+		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+		goto free_dev;
+	}
+	v4l2_info(v4l2_dev, "Mediavision Pro Movie Studio driver 0.05\n");
+
+	res = init_mediavision(dev);
+	if (res) {
 		v4l2_err(v4l2_dev, "Board not found.\n");
-		return -ENODEV;
+		goto free_io;
 	}
 
-	res = v4l2_device_register(NULL, v4l2_dev);
-	if (res < 0) {
-		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
-		return res;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 139);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 70);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 64);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_HUE, 0, 255, 1, 0);
+	if (hdl->error) {
+		res = hdl->error;
+		goto free_hdl;
 	}
 
+	mutex_init(&dev->lock);
 	strlcpy(dev->vdev.name, v4l2_dev->name, sizeof(dev->vdev.name));
 	dev->vdev.v4l2_dev = v4l2_dev;
+	dev->vdev.ctrl_handler = hdl;
 	dev->vdev.fops = &pms_fops;
 	dev->vdev.ioctl_ops = &pms_ioctl_ops;
 	dev->vdev.release = video_device_release_empty;
+	dev->vdev.lock = &dev->lock;
+	dev->vdev.tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vdev.flags);
 	video_set_drvdata(&dev->vdev, dev);
-	mutex_init(&dev->lock);
 	dev->std = V4L2_STD_NTSC_M;
 	dev->height = 240;
 	dev->width = 320;
-	dev->depth = 15;
-	dev->brightness = 139;
-	dev->contrast = 70;
-	dev->hue = 0;
-	dev->saturation = 64;
+	dev->depth = 16;
 	pms_swsense(dev, 75);
 	pms_resolution(dev, 320, 240);
 	pms_videosource(dev, 0);
 	pms_vcrinput(dev, 0);
-	if (video_register_device(&dev->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
-		v4l2_device_unregister(&dev->v4l2_dev);
-		release_region(dev->io, 3);
-		release_region(0x9a01, 1);
-		iounmap(dev->mem);
-		return -EINVAL;
-	}
-	return 0;
+	v4l2_ctrl_handler_setup(hdl);
+	res = video_register_device(&dev->vdev, VFL_TYPE_GRABBER, video_nr);
+	if (res >= 0)
+		return 0;
+
+free_hdl:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+free_io:
+	release_region(dev->io, 3);
+	release_region(0x9a01, 1);
+	iounmap(dev->mem);
+free_dev:
+	kfree(dev);
+	return res;
 }
 
-static void __exit pms_exit(void)
+static int pms_remove(struct device *pdev, unsigned int card)
 {
-	struct pms *dev = &pms_card;
+	struct pms *dev = dev_get_drvdata(pdev);
 
 	video_unregister_device(&dev->vdev);
+	v4l2_ctrl_handler_free(&dev->hdl);
 	release_region(dev->io, 3);
 	release_region(0x9a01, 1);
 	iounmap(dev->mem);
+	return 0;
+}
+
+static struct isa_driver pms_driver = {
+	.probe		= pms_probe,
+	.remove		= pms_remove,
+	.driver		= {
+		.name	= "pms",
+	},
+};
+
+static int __init pms_init(void)
+{
+	return isa_register_driver(&pms_driver, 1);
+}
+
+static void __exit pms_exit(void)
+{
+	isa_unregister_driver(&pms_driver);
 }
 
 module_init(pms_init);
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h b/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h
index 305e6aaa844a..036952f2a3cb 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h
@@ -317,18 +317,16 @@ struct pvr2_hdw {
 	v4l2_std_id std_mask_eeprom; // Hardware supported selections
 	v4l2_std_id std_mask_avail;  // Which standards we may select from
 	v4l2_std_id std_mask_cur;    // Currently selected standard(s)
-	unsigned int std_enum_cnt;   // # of enumerated standards
 	int std_enum_cur;            // selected standard enumeration value
 	int std_dirty;               // True if std_mask_cur has changed
 	struct pvr2_ctl_info std_info_enum;
 	struct pvr2_ctl_info std_info_avail;
 	struct pvr2_ctl_info std_info_cur;
-	struct v4l2_standard *std_defs;
-	const char **std_enum_names;
+	struct pvr2_ctl_info std_info_detect;
 
 	// Generated string names, one per actual V4L2 standard
 	const char *std_mask_ptrs[32];
-	char std_mask_names[32][10];
+	char std_mask_names[32][16];
 
 	int unit_number;             /* ID for driver instance */
 	unsigned long serial_number; /* ID for hardware itself */
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.c b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
index ebc2c7e39233..fb828ba1dbbe 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
@@ -334,8 +334,6 @@ static void pvr2_hdw_state_log_state(struct pvr2_hdw *);
 static int pvr2_hdw_cmd_usbstream(struct pvr2_hdw *hdw,int runFl);
 static int pvr2_hdw_commit_setup(struct pvr2_hdw *hdw);
 static int pvr2_hdw_get_eeprom_addr(struct pvr2_hdw *hdw);
-static void pvr2_hdw_internal_find_stdenum(struct pvr2_hdw *hdw);
-static void pvr2_hdw_internal_set_std_avail(struct pvr2_hdw *hdw);
 static void pvr2_hdw_quiescent_timeout(unsigned long);
 static void pvr2_hdw_decoder_stabilization_timeout(unsigned long);
 static void pvr2_hdw_encoder_wait_timeout(unsigned long);
@@ -346,7 +344,7 @@ static int pvr2_send_request_ex(struct pvr2_hdw *hdw,
 				void *write_data,unsigned int write_len,
 				void *read_data,unsigned int read_len);
 static int pvr2_hdw_check_cropcap(struct pvr2_hdw *hdw);
-
+static v4l2_std_id pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw);
 
 static void trace_stbit(const char *name,int val)
 {
@@ -840,6 +838,12 @@ static int ctrl_hsm_get(struct pvr2_ctrl *cptr,int *vp)
 	return 0;
 }
 
+static int ctrl_stddetect_get(struct pvr2_ctrl *cptr, int *vp)
+{
+	*vp = pvr2_hdw_get_detected_std(cptr->hdw);
+	return 0;
+}
+
 static int ctrl_stdavail_get(struct pvr2_ctrl *cptr,int *vp)
 {
 	*vp = cptr->hdw->std_mask_avail;
@@ -854,8 +858,7 @@ static int ctrl_stdavail_set(struct pvr2_ctrl *cptr,int m,int v)
 	ns = (ns & ~m) | (v & m);
 	if (ns == hdw->std_mask_avail) return 0;
 	hdw->std_mask_avail = ns;
-	pvr2_hdw_internal_set_std_avail(hdw);
-	pvr2_hdw_internal_find_stdenum(hdw);
+	hdw->std_info_cur.def.type_bitmask.valid_bits = hdw->std_mask_avail;
 	return 0;
 }
 
@@ -895,7 +898,6 @@ static int ctrl_stdcur_set(struct pvr2_ctrl *cptr,int m,int v)
 	if (ns == hdw->std_mask_cur) return 0;
 	hdw->std_mask_cur = ns;
 	hdw->std_dirty = !0;
-	pvr2_hdw_internal_find_stdenum(hdw);
 	return 0;
 }
 
@@ -941,40 +943,6 @@ static int ctrl_audio_modes_present_get(struct pvr2_ctrl *cptr,int *vp)
 }
 
 
-static int ctrl_stdenumcur_set(struct pvr2_ctrl *cptr,int m,int v)
-{
-	struct pvr2_hdw *hdw = cptr->hdw;
-	if (v < 0) return -EINVAL;
-	if (v > hdw->std_enum_cnt) return -EINVAL;
-	hdw->std_enum_cur = v;
-	if (!v) return 0;
-	v--;
-	if (hdw->std_mask_cur == hdw->std_defs[v].id) return 0;
-	hdw->std_mask_cur = hdw->std_defs[v].id;
-	hdw->std_dirty = !0;
-	return 0;
-}
-
-
-static int ctrl_stdenumcur_get(struct pvr2_ctrl *cptr,int *vp)
-{
-	*vp = cptr->hdw->std_enum_cur;
-	return 0;
-}
-
-
-static int ctrl_stdenumcur_is_dirty(struct pvr2_ctrl *cptr)
-{
-	return cptr->hdw->std_dirty != 0;
-}
-
-
-static void ctrl_stdenumcur_clear_dirty(struct pvr2_ctrl *cptr)
-{
-	cptr->hdw->std_dirty = 0;
-}
-
-
 #define DEFINT(vmin,vmax) \
 	.type = pvr2_ctl_int, \
 	.def.type_int.min_value = vmin, \
@@ -1293,15 +1261,14 @@ static const struct pvr2_ctl_info control_defs[] = {
 		.sym_to_val = ctrl_std_sym_to_val,
 		.type = pvr2_ctl_bitmask,
 	},{
-		.desc = "Video Standard Name",
-		.name = "video_standard",
-		.internal_id = PVR2_CID_STDENUM,
+		.desc = "Video Standards Detected Mask",
+		.name = "video_standard_mask_detected",
+		.internal_id = PVR2_CID_STDDETECT,
 		.skip_init = !0,
-		.get_value = ctrl_stdenumcur_get,
-		.set_value = ctrl_stdenumcur_set,
-		.is_dirty = ctrl_stdenumcur_is_dirty,
-		.clear_dirty = ctrl_stdenumcur_clear_dirty,
-		.type = pvr2_ctl_enum,
+		.get_value = ctrl_stddetect_get,
+		.val_to_sym = ctrl_std_val_to_sym,
+		.sym_to_val = ctrl_std_sym_to_val,
+		.type = pvr2_ctl_bitmask,
 	}
 };
 
@@ -1936,7 +1903,7 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 		hdw->std_mask_avail |= std2;
 	}
 
-	pvr2_hdw_internal_set_std_avail(hdw);
+	hdw->std_info_cur.def.type_bitmask.valid_bits = hdw->std_mask_avail;
 
 	if (std1) {
 		bcnt = pvr2_std_id_to_str(buf,sizeof(buf),std1);
@@ -1945,7 +1912,6 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 			   bcnt,buf);
 		hdw->std_mask_cur = std1;
 		hdw->std_dirty = !0;
-		pvr2_hdw_internal_find_stdenum(hdw);
 		return;
 	}
 	if (std3) {
@@ -1955,7 +1921,6 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 			   " (determined by device type): %.*s",bcnt,buf);
 		hdw->std_mask_cur = std3;
 		hdw->std_dirty = !0;
-		pvr2_hdw_internal_find_stdenum(hdw);
 		return;
 	}
 
@@ -1975,24 +1940,10 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 				   bcnt,buf);
 			hdw->std_mask_cur = std_eeprom_maps[idx].std;
 			hdw->std_dirty = !0;
-			pvr2_hdw_internal_find_stdenum(hdw);
 			return;
 		}
 	}
 
-	if (hdw->std_enum_cnt > 1) {
-		// Autoselect the first listed standard
-		hdw->std_enum_cur = 1;
-		hdw->std_mask_cur = hdw->std_defs[hdw->std_enum_cur-1].id;
-		hdw->std_dirty = !0;
-		pvr2_trace(PVR2_TRACE_STD,
-			   "Initial video standard auto-selected to %s",
-			   hdw->std_defs[hdw->std_enum_cur-1].name);
-		return;
-	}
-
-	pvr2_trace(PVR2_TRACE_ERROR_LEGS,
-		   "Unable to select a viable initial video standard");
 }
 
 
@@ -2594,14 +2545,6 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 		cptr->info = ciptr;
 	}
 
-	// Initialize video standard enum dynamic control
-	cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDENUM);
-	if (cptr) {
-		memcpy(&hdw->std_info_enum,cptr->info,
-		       sizeof(hdw->std_info_enum));
-		cptr->info = &hdw->std_info_enum;
-
-	}
 	// Initialize control data regarding video standard masks
 	valid_std_mask = pvr2_std_get_usable();
 	for (idx = 0; idx < 32; idx++) {
@@ -2629,7 +2572,17 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 		cptr->info = &hdw->std_info_cur;
 		hdw->std_info_cur.def.type_bitmask.bit_names =
 			hdw->std_mask_ptrs;
-		hdw->std_info_avail.def.type_bitmask.valid_bits =
+		hdw->std_info_cur.def.type_bitmask.valid_bits =
+			valid_std_mask;
+	}
+	cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDDETECT);
+	if (cptr) {
+		memcpy(&hdw->std_info_detect,cptr->info,
+		       sizeof(hdw->std_info_detect));
+		cptr->info = &hdw->std_info_detect;
+		hdw->std_info_detect.def.type_bitmask.bit_names =
+			hdw->std_mask_ptrs;
+		hdw->std_info_detect.def.type_bitmask.valid_bits =
 			valid_std_mask;
 	}
 
@@ -2711,8 +2664,6 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 		kfree(hdw->ctl_write_buffer);
 		kfree(hdw->controls);
 		kfree(hdw->mpeg_ctrl_info);
-		kfree(hdw->std_defs);
-		kfree(hdw->std_enum_names);
 		kfree(hdw);
 	}
 	return NULL;
@@ -2788,8 +2739,6 @@ void pvr2_hdw_destroy(struct pvr2_hdw *hdw)
 	} while (0); mutex_unlock(&pvr2_unit_mtx);
 	kfree(hdw->controls);
 	kfree(hdw->mpeg_ctrl_info);
-	kfree(hdw->std_defs);
-	kfree(hdw->std_enum_names);
 	kfree(hdw);
 }
 
@@ -2812,86 +2761,6 @@ void pvr2_hdw_disconnect(struct pvr2_hdw *hdw)
 }
 
 
-// Attempt to autoselect an appropriate value for std_enum_cur given
-// whatever is currently in std_mask_cur
-static void pvr2_hdw_internal_find_stdenum(struct pvr2_hdw *hdw)
-{
-	unsigned int idx;
-	for (idx = 1; idx < hdw->std_enum_cnt; idx++) {
-		if (hdw->std_defs[idx-1].id == hdw->std_mask_cur) {
-			hdw->std_enum_cur = idx;
-			return;
-		}
-	}
-	hdw->std_enum_cur = 0;
-}
-
-
-// Calculate correct set of enumerated standards based on currently known
-// set of available standards bits.
-static void pvr2_hdw_internal_set_std_avail(struct pvr2_hdw *hdw)
-{
-	struct v4l2_standard *newstd;
-	unsigned int std_cnt;
-	unsigned int idx;
-
-	newstd = pvr2_std_create_enum(&std_cnt,hdw->std_mask_avail);
-
-	if (hdw->std_defs) {
-		kfree(hdw->std_defs);
-		hdw->std_defs = NULL;
-	}
-	hdw->std_enum_cnt = 0;
-	if (hdw->std_enum_names) {
-		kfree(hdw->std_enum_names);
-		hdw->std_enum_names = NULL;
-	}
-
-	if (!std_cnt) {
-		pvr2_trace(
-			PVR2_TRACE_ERROR_LEGS,
-			"WARNING: Failed to identify any viable standards");
-	}
-
-	/* Set up the dynamic control for this standard */
-	hdw->std_enum_names = kmalloc(sizeof(char *)*(std_cnt+1),GFP_KERNEL);
-	if (hdw->std_enum_names) {
-		hdw->std_enum_names[0] = "none";
-		for (idx = 0; idx < std_cnt; idx++)
-			hdw->std_enum_names[idx+1] = newstd[idx].name;
-		hdw->std_info_enum.def.type_enum.value_names =
-						hdw->std_enum_names;
-		hdw->std_info_enum.def.type_enum.count = std_cnt+1;
-	} else {
-		pvr2_trace(
-			PVR2_TRACE_ERROR_LEGS,
-			"WARNING: Failed to alloc memory for names");
-		hdw->std_info_enum.def.type_enum.value_names = NULL;
-		hdw->std_info_enum.def.type_enum.count = 0;
-	}
-	hdw->std_defs = newstd;
-	hdw->std_enum_cnt = std_cnt+1;
-	hdw->std_enum_cur = 0;
-	hdw->std_info_cur.def.type_bitmask.valid_bits = hdw->std_mask_avail;
-}
-
-
-int pvr2_hdw_get_stdenum_value(struct pvr2_hdw *hdw,
-			       struct v4l2_standard *std,
-			       unsigned int idx)
-{
-	int ret = -EINVAL;
-	if (!idx) return ret;
-	LOCK_TAKE(hdw->big_lock); do {
-		if (idx >= hdw->std_enum_cnt) break;
-		idx--;
-		memcpy(std,hdw->std_defs+idx,sizeof(*std));
-		ret = 0;
-	} while (0); LOCK_GIVE(hdw->big_lock);
-	return ret;
-}
-
-
 /* Get the number of defined controls */
 unsigned int pvr2_hdw_get_ctrl_count(struct pvr2_hdw *hdw)
 {
@@ -2995,11 +2864,13 @@ static void pvr2_subdev_set_control(struct pvr2_hdw *hdw, int id,
 		pvr2_subdev_set_control(hdw, id, #lab, (hdw)->lab##_val); \
 	}
 
-int pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw, v4l2_std_id *std)
+v4l2_std_id pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw)
 {
+	v4l2_std_id std;
+	std = (v4l2_std_id)hdw->std_mask_avail;
 	v4l2_device_call_all(&hdw->v4l2_dev, 0,
-			     video, querystd, std);
-	return 0;
+			     video, querystd, &std);
+	return std;
 }
 
 /* Execute whatever commands are required to update the state of all the
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.h b/drivers/media/video/pvrusb2/pvrusb2-hdw.h
index 66546580b17d..8060fc666eeb 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.h
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.h
@@ -28,7 +28,6 @@
 
 /* Private internal control ids, look these up with
    pvr2_hdw_get_ctrl_by_id() - these are NOT visible in V4L */
-#define PVR2_CID_STDENUM 1
 #define PVR2_CID_STDCUR 2
 #define PVR2_CID_STDAVAIL 3
 #define PVR2_CID_INPUT 4
@@ -46,6 +45,7 @@
 #define PVR2_CID_CROPCAPBT 16
 #define PVR2_CID_CROPCAPBW 17
 #define PVR2_CID_CROPCAPBH 18
+#define PVR2_CID_STDDETECT 19
 
 /* Legal values for the INPUT state variable */
 #define PVR2_CVAL_INPUT_TV 0
@@ -210,13 +210,6 @@ int pvr2_hdw_set_stream_type(struct pvr2_hdw *, enum pvr2_config);
 /* Get handle to video output stream */
 struct pvr2_stream *pvr2_hdw_get_video_stream(struct pvr2_hdw *);
 
-/* Emit a video standard struct */
-int pvr2_hdw_get_stdenum_value(struct pvr2_hdw *hdw,struct v4l2_standard *std,
-			       unsigned int idx);
-
-/* Get the detected video standard */
-int pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw, v4l2_std_id *std);
-
 /* Enable / disable retrieval of CPU firmware or prom contents.  This must
    be enabled before pvr2_hdw_cpufw_get() will function.  Note that doing
    this may prevent the device from running (and leaving this mode may
diff --git a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
index e1111d968a3d..7bddfaeeafc3 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
@@ -107,7 +107,6 @@ static struct v4l2_fmtdesc pvr_fmtdesc [] = {
 		// This should really be V4L2_PIX_FMT_MPEG, but xawtv
 		// breaks when I do that.
 		.pixelformat    = 0, // V4L2_PIX_FMT_MPEG,
-		.reserved       = { 0, 0, 0, 0 }
 	}
 };
 
@@ -145,740 +144,739 @@ static struct v4l2_format pvr_format [] = {
 				.start = { 0, 0 },
 				.count = { 0, 0 },
 				.flags = 0,
-				.reserved = { 0, 0 }
 			}
 		}
 	}
 };
 
 
+
 /*
- * pvr_ioctl()
- *
- * This is part of Video 4 Linux API. The procedure handles ioctl() calls.
- *
+ * This is part of Video 4 Linux API. These procedures handle ioctl() calls.
  */
-static long pvr2_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
+static int pvr2_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
-	struct pvr2_v4l2 *vp = fh->vhead;
-	struct pvr2_v4l2_dev *pdi = fh->pdi;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
-	long ret = -EINVAL;
 
-	if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
-		v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw),cmd);
-	}
+	memcpy(cap, &pvr_capability, sizeof(struct v4l2_capability));
+	strlcpy(cap->bus_info, pvr2_hdw_get_bus_info(hdw),
+			sizeof(cap->bus_info));
+	strlcpy(cap->card, pvr2_hdw_get_desc(hdw), sizeof(cap->card));
+	return 0;
+}
 
-	if (!pvr2_hdw_dev_ok(hdw)) {
-		pvr2_trace(PVR2_TRACE_ERROR_LEGS,
-			   "ioctl failed - bad or no context");
-		return -EFAULT;
-	}
+static int pvr2_g_priority(struct file *file, void *priv, enum v4l2_priority *p)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_v4l2 *vp = fh->vhead;
 
-	/* check priority */
-	switch (cmd) {
-	case VIDIOC_S_CTRL:
-	case VIDIOC_S_STD:
-	case VIDIOC_S_INPUT:
-	case VIDIOC_S_TUNER:
-	case VIDIOC_S_FREQUENCY:
-		ret = v4l2_prio_check(&vp->prio, fh->prio);
-		if (ret)
-			return ret;
-	}
+	*p = v4l2_prio_max(&vp->prio);
+	return 0;
+}
 
-	switch (cmd) {
-	case VIDIOC_QUERYCAP:
-	{
-		struct v4l2_capability *cap = arg;
+static int pvr2_s_priority(struct file *file, void *priv, enum v4l2_priority prio)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_v4l2 *vp = fh->vhead;
 
-		memcpy(cap, &pvr_capability, sizeof(struct v4l2_capability));
-		strlcpy(cap->bus_info,pvr2_hdw_get_bus_info(hdw),
-			sizeof(cap->bus_info));
-		strlcpy(cap->card,pvr2_hdw_get_desc(hdw),sizeof(cap->card));
+	return v4l2_prio_change(&vp->prio, &fh->prio, prio);
+}
 
-		ret = 0;
-		break;
-	}
+static int pvr2_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int ret;
 
-	case VIDIOC_G_PRIORITY:
-	{
-		enum v4l2_priority *p = arg;
+	ret = pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), &val);
+	*std = val;
+	return ret;
+}
 
-		*p = v4l2_prio_max(&vp->prio);
-		ret = 0;
-		break;
-	}
+int pvr2_s_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 
-	case VIDIOC_S_PRIORITY:
-	{
-		enum v4l2_priority *prio = arg;
+	return pvr2_ctrl_set_value(
+		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), *std);
+}
 
-		ret = v4l2_prio_change(&vp->prio, &fh->prio, *prio);
-		break;
-	}
+static int pvr2_querystd(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int ret;
 
-	case VIDIOC_ENUMSTD:
-	{
-		struct v4l2_standard *vs = (struct v4l2_standard *)arg;
-		int idx = vs->index;
-		ret = pvr2_hdw_get_stdenum_value(hdw,vs,idx+1);
-		break;
-	}
+	ret = pvr2_ctrl_get_value(
+		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDDETECT), &val);
+	*std = val;
+	return ret;
+}
 
-	case VIDIOC_QUERYSTD:
-	{
-		v4l2_std_id *std = arg;
-		*std = V4L2_STD_ALL;
-		ret = pvr2_hdw_get_detected_std(hdw, std);
-		break;
-	}
+static int pvr2_enum_input(struct file *file, void *priv, struct v4l2_input *vi)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_ctrl *cptr;
+	struct v4l2_input tmp;
+	unsigned int cnt;
+	int val;
+	int ret;
 
-	case VIDIOC_G_STD:
-	{
-		int val = 0;
-		ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDCUR),&val);
-		*(v4l2_std_id *)arg = val;
+	cptr = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT);
+
+	memset(&tmp, 0, sizeof(tmp));
+	tmp.index = vi->index;
+	ret = 0;
+	if (vi->index >= fh->input_cnt)
+		return -EINVAL;
+	val = fh->input_map[vi->index];
+	switch (val) {
+	case PVR2_CVAL_INPUT_TV:
+	case PVR2_CVAL_INPUT_DTV:
+	case PVR2_CVAL_INPUT_RADIO:
+		tmp.type = V4L2_INPUT_TYPE_TUNER;
 		break;
-	}
-
-	case VIDIOC_S_STD:
-	{
-		ret = pvr2_ctrl_set_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDCUR),
-			*(v4l2_std_id *)arg);
+	case PVR2_CVAL_INPUT_SVIDEO:
+	case PVR2_CVAL_INPUT_COMPOSITE:
+		tmp.type = V4L2_INPUT_TYPE_CAMERA;
 		break;
+	default:
+		return -EINVAL;
 	}
 
-	case VIDIOC_ENUMINPUT:
-	{
-		struct pvr2_ctrl *cptr;
-		struct v4l2_input *vi = (struct v4l2_input *)arg;
-		struct v4l2_input tmp;
-		unsigned int cnt;
-		int val;
+	cnt = 0;
+	pvr2_ctrl_get_valname(cptr, val,
+			tmp.name, sizeof(tmp.name) - 1, &cnt);
+	tmp.name[cnt] = 0;
 
-		cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT);
+	/* Don't bother with audioset, since this driver currently
+	   always switches the audio whenever the video is
+	   switched. */
 
-		memset(&tmp,0,sizeof(tmp));
-		tmp.index = vi->index;
-		ret = 0;
-		if (vi->index >= fh->input_cnt) {
-			ret = -EINVAL;
-			break;
-		}
-		val = fh->input_map[vi->index];
-		switch (val) {
-		case PVR2_CVAL_INPUT_TV:
-		case PVR2_CVAL_INPUT_DTV:
-		case PVR2_CVAL_INPUT_RADIO:
-			tmp.type = V4L2_INPUT_TYPE_TUNER;
-			break;
-		case PVR2_CVAL_INPUT_SVIDEO:
-		case PVR2_CVAL_INPUT_COMPOSITE:
-			tmp.type = V4L2_INPUT_TYPE_CAMERA;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-		if (ret < 0) break;
-
-		cnt = 0;
-		pvr2_ctrl_get_valname(cptr,val,
-				      tmp.name,sizeof(tmp.name)-1,&cnt);
-		tmp.name[cnt] = 0;
-
-		/* Don't bother with audioset, since this driver currently
-		   always switches the audio whenever the video is
-		   switched. */
-
-		/* Handling std is a tougher problem.  It doesn't make
-		   sense in cases where a device might be multi-standard.
-		   We could just copy out the current value for the
-		   standard, but it can change over time.  For now just
-		   leave it zero. */
-
-		memcpy(vi, &tmp, sizeof(tmp));
-
-		ret = 0;
-		break;
-	}
+	/* Handling std is a tougher problem.  It doesn't make
+	   sense in cases where a device might be multi-standard.
+	   We could just copy out the current value for the
+	   standard, but it can change over time.  For now just
+	   leave it zero. */
+	*vi = tmp;
+	return 0;
+}
 
-	case VIDIOC_G_INPUT:
-	{
-		unsigned int idx;
-		struct pvr2_ctrl *cptr;
-		struct v4l2_input *vi = (struct v4l2_input *)arg;
-		int val;
-		cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT);
-		val = 0;
-		ret = pvr2_ctrl_get_value(cptr,&val);
-		vi->index = 0;
-		for (idx = 0; idx < fh->input_cnt; idx++) {
-			if (fh->input_map[idx] == val) {
-				vi->index = idx;
-				break;
-			}
-		}
-		break;
-	}
+static int pvr2_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	unsigned int idx;
+	struct pvr2_ctrl *cptr;
+	int val;
+	int ret;
 
-	case VIDIOC_S_INPUT:
-	{
-		struct v4l2_input *vi = (struct v4l2_input *)arg;
-		if (vi->index >= fh->input_cnt) {
-			ret = -ERANGE;
+	cptr = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT);
+	val = 0;
+	ret = pvr2_ctrl_get_value(cptr, &val);
+	*i = 0;
+	for (idx = 0; idx < fh->input_cnt; idx++) {
+		if (fh->input_map[idx] == val) {
+			*i = idx;
 			break;
 		}
-		ret = pvr2_ctrl_set_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT),
-			fh->input_map[vi->index]);
-		break;
 	}
+	return ret;
+}
 
-	case VIDIOC_ENUMAUDIO:
-	{
-		/* pkt: FIXME: We are returning one "fake" input here
-		   which could very well be called "whatever_we_like".
-		   This is for apps that want to see an audio input
-		   just to feel comfortable, as well as to test if
-		   it can do stereo or sth. There is actually no guarantee
-		   that the actual audio input cannot change behind the app's
-		   back, but most applications should not mind that either.
-
-		   Hopefully, mplayer people will work with us on this (this
-		   whole mess is to support mplayer pvr://), or Hans will come
-		   up with a more standard way to say "we have inputs but we
-		   don 't want you to change them independent of video" which
-		   will sort this mess.
-		 */
-		struct v4l2_audio *vin = arg;
-		ret = -EINVAL;
-		if (vin->index > 0) break;
-		strncpy(vin->name, "PVRUSB2 Audio",14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-		ret = 0;
-		break;
-		break;
-	}
+static int pvr2_s_input(struct file *file, void *priv, unsigned int inp)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 
-	case VIDIOC_G_AUDIO:
-	{
-		/* pkt: FIXME: see above comment (VIDIOC_ENUMAUDIO) */
-		struct v4l2_audio *vin = arg;
-		memset(vin,0,sizeof(*vin));
-		vin->index = 0;
-		strncpy(vin->name, "PVRUSB2 Audio",14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-		ret = 0;
-		break;
-	}
+	if (inp >= fh->input_cnt)
+		return -EINVAL;
+	return pvr2_ctrl_set_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT),
+			fh->input_map[inp]);
+}
 
-	case VIDIOC_G_TUNER:
-	{
-		struct v4l2_tuner *vt = (struct v4l2_tuner *)arg;
+static int pvr2_enumaudio(struct file *file, void *priv, struct v4l2_audio *vin)
+{
+	/* pkt: FIXME: We are returning one "fake" input here
+	   which could very well be called "whatever_we_like".
+	   This is for apps that want to see an audio input
+	   just to feel comfortable, as well as to test if
+	   it can do stereo or sth. There is actually no guarantee
+	   that the actual audio input cannot change behind the app's
+	   back, but most applications should not mind that either.
+
+	   Hopefully, mplayer people will work with us on this (this
+	   whole mess is to support mplayer pvr://), or Hans will come
+	   up with a more standard way to say "we have inputs but we
+	   don 't want you to change them independent of video" which
+	   will sort this mess.
+	 */
+
+	if (vin->index > 0)
+		return -EINVAL;
+	strncpy(vin->name, "PVRUSB2 Audio", 14);
+	vin->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
 
-		if (vt->index != 0) break; /* Only answer for the 1st tuner */
+static int pvr2_g_audio(struct file *file, void *priv, struct v4l2_audio *vin)
+{
+	/* pkt: FIXME: see above comment (VIDIOC_ENUMAUDIO) */
+	vin->index = 0;
+	strncpy(vin->name, "PVRUSB2 Audio", 14);
+	vin->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
 
-		pvr2_hdw_execute_tuner_poll(hdw);
-		ret = pvr2_hdw_get_tuner_status(hdw,vt);
-		break;
-	}
+static int pvr2_s_audio(struct file *file, void *priv, struct v4l2_audio *vout)
+{
+	if (vout->index)
+		return -EINVAL;
+	return 0;
+}
 
-	case VIDIOC_S_TUNER:
-	{
-		struct v4l2_tuner *vt=(struct v4l2_tuner *)arg;
+static int pvr2_g_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 
-		if (vt->index != 0)
-			break;
+	if (vt->index != 0)
+		return -EINVAL; /* Only answer for the 1st tuner */
 
-		ret = pvr2_ctrl_set_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_AUDIOMODE),
+	pvr2_hdw_execute_tuner_poll(hdw);
+	return pvr2_hdw_get_tuner_status(hdw, vt);
+}
+
+static int pvr2_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+
+	if (vt->index != 0)
+		return -EINVAL;
+
+	return pvr2_ctrl_set_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_AUDIOMODE),
 			vt->audmode);
-		break;
-	}
+}
 
-	case VIDIOC_S_FREQUENCY:
-	{
-		const struct v4l2_frequency *vf = (struct v4l2_frequency *)arg;
-		unsigned long fv;
-		struct v4l2_tuner vt;
-		int cur_input;
-		struct pvr2_ctrl *ctrlp;
-		ret = pvr2_hdw_get_tuner_status(hdw,&vt);
-		if (ret != 0) break;
-		ctrlp = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT);
-		ret = pvr2_ctrl_get_value(ctrlp,&cur_input);
-		if (ret != 0) break;
-		if (vf->type == V4L2_TUNER_RADIO) {
-			if (cur_input != PVR2_CVAL_INPUT_RADIO) {
-				pvr2_ctrl_set_value(ctrlp,
-						    PVR2_CVAL_INPUT_RADIO);
-			}
-		} else {
-			if (cur_input == PVR2_CVAL_INPUT_RADIO) {
-				pvr2_ctrl_set_value(ctrlp,
-						    PVR2_CVAL_INPUT_TV);
-			}
-		}
-		fv = vf->frequency;
-		if (vt.capability & V4L2_TUNER_CAP_LOW) {
-			fv = (fv * 125) / 2;
-		} else {
-			fv = fv * 62500;
-		}
-		ret = pvr2_ctrl_set_value(
+int pvr2_s_frequency(struct file *file, void *priv, struct v4l2_frequency *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	unsigned long fv;
+	struct v4l2_tuner vt;
+	int cur_input;
+	struct pvr2_ctrl *ctrlp;
+	int ret;
+
+	ret = pvr2_hdw_get_tuner_status(hdw, &vt);
+	if (ret != 0)
+		return ret;
+	ctrlp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT);
+	ret = pvr2_ctrl_get_value(ctrlp, &cur_input);
+	if (ret != 0)
+		return ret;
+	if (vf->type == V4L2_TUNER_RADIO) {
+		if (cur_input != PVR2_CVAL_INPUT_RADIO)
+			pvr2_ctrl_set_value(ctrlp, PVR2_CVAL_INPUT_RADIO);
+	} else {
+		if (cur_input == PVR2_CVAL_INPUT_RADIO)
+			pvr2_ctrl_set_value(ctrlp, PVR2_CVAL_INPUT_TV);
+	}
+	fv = vf->frequency;
+	if (vt.capability & V4L2_TUNER_CAP_LOW)
+		fv = (fv * 125) / 2;
+	else
+		fv = fv * 62500;
+	return pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_FREQUENCY),fv);
-		break;
-	}
+}
 
-	case VIDIOC_G_FREQUENCY:
-	{
-		struct v4l2_frequency *vf = (struct v4l2_frequency *)arg;
-		int val = 0;
-		int cur_input;
-		struct v4l2_tuner vt;
-		ret = pvr2_hdw_get_tuner_status(hdw,&vt);
-		if (ret != 0) break;
-		ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_FREQUENCY),
+static int pvr2_g_frequency(struct file *file, void *priv, struct v4l2_frequency *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int cur_input;
+	struct v4l2_tuner vt;
+	int ret;
+
+	ret = pvr2_hdw_get_tuner_status(hdw, &vt);
+	if (ret != 0)
+		return ret;
+	ret = pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_FREQUENCY),
 			&val);
-		if (ret != 0) break;
-		pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT),
+	if (ret != 0)
+		return ret;
+	pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT),
 			&cur_input);
-		if (cur_input == PVR2_CVAL_INPUT_RADIO) {
-			vf->type = V4L2_TUNER_RADIO;
-		} else {
-			vf->type = V4L2_TUNER_ANALOG_TV;
-		}
-		if (vt.capability & V4L2_TUNER_CAP_LOW) {
-			val = (val * 2) / 125;
-		} else {
-			val /= 62500;
-		}
-		vf->frequency = val;
-		break;
-	}
+	if (cur_input == PVR2_CVAL_INPUT_RADIO)
+		vf->type = V4L2_TUNER_RADIO;
+	else
+		vf->type = V4L2_TUNER_ANALOG_TV;
+	if (vt.capability & V4L2_TUNER_CAP_LOW)
+		val = (val * 2) / 125;
+	else
+		val /= 62500;
+	vf->frequency = val;
+	return 0;
+}
 
-	case VIDIOC_ENUM_FMT:
-	{
-		struct v4l2_fmtdesc *fd = (struct v4l2_fmtdesc *)arg;
+static int pvr2_enum_fmt_vid_cap(struct file *file, void *priv, struct v4l2_fmtdesc *fd)
+{
+	/* Only one format is supported : mpeg.*/
+	if (fd->index != 0)
+		return -EINVAL;
 
-		/* Only one format is supported : mpeg.*/
-		if (fd->index != 0)
-			break;
+	memcpy(fd, pvr_fmtdesc, sizeof(struct v4l2_fmtdesc));
+	return 0;
+}
 
-		memcpy(fd, pvr_fmtdesc, sizeof(struct v4l2_fmtdesc));
-		ret = 0;
-		break;
-	}
+static int pvr2_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val;
 
-	case VIDIOC_G_FMT:
-	{
-		struct v4l2_format *vf = (struct v4l2_format *)arg;
-		int val;
-		switch(vf->type) {
-		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-			memcpy(vf, &pvr_format[PVR_FORMAT_PIX],
-			       sizeof(struct v4l2_format));
-			val = 0;
-			pvr2_ctrl_get_value(
-				pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_HRES),
-				&val);
-			vf->fmt.pix.width = val;
-			val = 0;
-			pvr2_ctrl_get_value(
-				pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_VRES),
-				&val);
-			vf->fmt.pix.height = val;
-			ret = 0;
-			break;
-		case V4L2_BUF_TYPE_VBI_CAPTURE:
-			// ????? Still need to figure out to do VBI correctly
-			ret = -EINVAL;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-		break;
-	}
+	memcpy(vf, &pvr_format[PVR_FORMAT_PIX], sizeof(struct v4l2_format));
+	val = 0;
+	pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_HRES),
+			&val);
+	vf->fmt.pix.width = val;
+	val = 0;
+	pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_VRES),
+			&val);
+	vf->fmt.pix.height = val;
+	return 0;
+}
 
-	case VIDIOC_TRY_FMT:
-	case VIDIOC_S_FMT:
-	{
-		struct v4l2_format *vf = (struct v4l2_format *)arg;
-
-		ret = 0;
-		switch(vf->type) {
-		case V4L2_BUF_TYPE_VIDEO_CAPTURE: {
-			int lmin,lmax,ldef;
-			struct pvr2_ctrl *hcp,*vcp;
-			int h = vf->fmt.pix.height;
-			int w = vf->fmt.pix.width;
-			hcp = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_HRES);
-			vcp = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_VRES);
-
-			lmin = pvr2_ctrl_get_min(hcp);
-			lmax = pvr2_ctrl_get_max(hcp);
-			pvr2_ctrl_get_def(hcp, &ldef);
-			if (w == -1) {
-				w = ldef;
-			} else if (w < lmin) {
-				w = lmin;
-			} else if (w > lmax) {
-				w = lmax;
-			}
-			lmin = pvr2_ctrl_get_min(vcp);
-			lmax = pvr2_ctrl_get_max(vcp);
-			pvr2_ctrl_get_def(vcp, &ldef);
-			if (h == -1) {
-				h = ldef;
-			} else if (h < lmin) {
-				h = lmin;
-			} else if (h > lmax) {
-				h = lmax;
-			}
+static int pvr2_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int lmin, lmax, ldef;
+	struct pvr2_ctrl *hcp, *vcp;
+	int h = vf->fmt.pix.height;
+	int w = vf->fmt.pix.width;
+
+	hcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_HRES);
+	vcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_VRES);
+
+	lmin = pvr2_ctrl_get_min(hcp);
+	lmax = pvr2_ctrl_get_max(hcp);
+	pvr2_ctrl_get_def(hcp, &ldef);
+	if (w == -1)
+		w = ldef;
+	else if (w < lmin)
+		w = lmin;
+	else if (w > lmax)
+		w = lmax;
+	lmin = pvr2_ctrl_get_min(vcp);
+	lmax = pvr2_ctrl_get_max(vcp);
+	pvr2_ctrl_get_def(vcp, &ldef);
+	if (h == -1)
+		h = ldef;
+	else if (h < lmin)
+		h = lmin;
+	else if (h > lmax)
+		h = lmax;
+
+	memcpy(vf, &pvr_format[PVR_FORMAT_PIX],
+			sizeof(struct v4l2_format));
+	vf->fmt.pix.width = w;
+	vf->fmt.pix.height = h;
+	return 0;
+}
 
-			memcpy(vf, &pvr_format[PVR_FORMAT_PIX],
-			       sizeof(struct v4l2_format));
-			vf->fmt.pix.width = w;
-			vf->fmt.pix.height = h;
+static int pvr2_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_ctrl *hcp, *vcp;
+	int ret = pvr2_try_fmt_vid_cap(file, fh, vf);
 
-			if (cmd == VIDIOC_S_FMT) {
-				pvr2_ctrl_set_value(hcp,vf->fmt.pix.width);
-				pvr2_ctrl_set_value(vcp,vf->fmt.pix.height);
-			}
-		} break;
-		case V4L2_BUF_TYPE_VBI_CAPTURE:
-			// ????? Still need to figure out to do VBI correctly
-			ret = -EINVAL;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-		break;
-	}
+	if (ret)
+		return ret;
+	hcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_HRES);
+	vcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_VRES);
+	pvr2_ctrl_set_value(hcp, vf->fmt.pix.width);
+	pvr2_ctrl_set_value(vcp, vf->fmt.pix.height);
+	return 0;
+}
 
-	case VIDIOC_STREAMON:
-	{
-		if (!fh->pdi->stream) {
-			/* No stream defined for this node.  This means
-			   that we're not currently allowed to stream from
-			   this node. */
-			ret = -EPERM;
-			break;
-		}
-		ret = pvr2_hdw_set_stream_type(hdw,pdi->config);
-		if (ret < 0) return ret;
-		ret = pvr2_hdw_set_streaming(hdw,!0);
-		break;
+static int pvr2_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_v4l2_dev *pdi = fh->pdi;
+	int ret;
+
+	if (!fh->pdi->stream) {
+		/* No stream defined for this node.  This means
+		   that we're not currently allowed to stream from
+		   this node. */
+		return -EPERM;
 	}
+	ret = pvr2_hdw_set_stream_type(hdw, pdi->config);
+	if (ret < 0)
+		return ret;
+	return pvr2_hdw_set_streaming(hdw, !0);
+}
 
-	case VIDIOC_STREAMOFF:
-	{
-		if (!fh->pdi->stream) {
-			/* No stream defined for this node.  This means
-			   that we're not currently allowed to stream from
-			   this node. */
-			ret = -EPERM;
-			break;
-		}
-		ret = pvr2_hdw_set_streaming(hdw,0);
-		break;
+static int pvr2_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+
+	if (!fh->pdi->stream) {
+		/* No stream defined for this node.  This means
+		   that we're not currently allowed to stream from
+		   this node. */
+		return -EPERM;
 	}
+	return pvr2_hdw_set_streaming(hdw, 0);
+}
 
-	case VIDIOC_QUERYCTRL:
-	{
-		struct pvr2_ctrl *cptr;
-		int val;
-		struct v4l2_queryctrl *vc = (struct v4l2_queryctrl *)arg;
-		ret = 0;
-		if (vc->id & V4L2_CTRL_FLAG_NEXT_CTRL) {
-			cptr = pvr2_hdw_get_ctrl_nextv4l(
-				hdw,(vc->id & ~V4L2_CTRL_FLAG_NEXT_CTRL));
-			if (cptr) vc->id = pvr2_ctrl_get_v4lid(cptr);
-		} else {
-			cptr = pvr2_hdw_get_ctrl_v4l(hdw,vc->id);
-		}
-		if (!cptr) {
-			pvr2_trace(PVR2_TRACE_V4LIOCTL,
-				   "QUERYCTRL id=0x%x not implemented here",
-				   vc->id);
-			ret = -EINVAL;
-			break;
-		}
+static int pvr2_queryctrl(struct file *file, void *priv,
+		struct v4l2_queryctrl *vc)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_ctrl *cptr;
+	int val;
+	int ret;
 
+	ret = 0;
+	if (vc->id & V4L2_CTRL_FLAG_NEXT_CTRL) {
+		cptr = pvr2_hdw_get_ctrl_nextv4l(
+				hdw, (vc->id & ~V4L2_CTRL_FLAG_NEXT_CTRL));
+		if (cptr)
+			vc->id = pvr2_ctrl_get_v4lid(cptr);
+	} else {
+		cptr = pvr2_hdw_get_ctrl_v4l(hdw, vc->id);
+	}
+	if (!cptr) {
 		pvr2_trace(PVR2_TRACE_V4LIOCTL,
-			   "QUERYCTRL id=0x%x mapping name=%s (%s)",
-			   vc->id,pvr2_ctrl_get_name(cptr),
-			   pvr2_ctrl_get_desc(cptr));
-		strlcpy(vc->name,pvr2_ctrl_get_desc(cptr),sizeof(vc->name));
-		vc->flags = pvr2_ctrl_get_v4lflags(cptr);
-		pvr2_ctrl_get_def(cptr, &val);
-		vc->default_value = val;
-		switch (pvr2_ctrl_get_type(cptr)) {
-		case pvr2_ctl_enum:
-			vc->type = V4L2_CTRL_TYPE_MENU;
-			vc->minimum = 0;
-			vc->maximum = pvr2_ctrl_get_cnt(cptr) - 1;
-			vc->step = 1;
-			break;
-		case pvr2_ctl_bool:
-			vc->type = V4L2_CTRL_TYPE_BOOLEAN;
-			vc->minimum = 0;
-			vc->maximum = 1;
-			vc->step = 1;
-			break;
-		case pvr2_ctl_int:
-			vc->type = V4L2_CTRL_TYPE_INTEGER;
-			vc->minimum = pvr2_ctrl_get_min(cptr);
-			vc->maximum = pvr2_ctrl_get_max(cptr);
-			vc->step = 1;
-			break;
-		default:
-			pvr2_trace(PVR2_TRACE_V4LIOCTL,
-				   "QUERYCTRL id=0x%x name=%s not mappable",
-				   vc->id,pvr2_ctrl_get_name(cptr));
-			ret = -EINVAL;
-			break;
-		}
+				"QUERYCTRL id=0x%x not implemented here",
+				vc->id);
+		return -EINVAL;
+	}
+
+	pvr2_trace(PVR2_TRACE_V4LIOCTL,
+			"QUERYCTRL id=0x%x mapping name=%s (%s)",
+			vc->id, pvr2_ctrl_get_name(cptr),
+			pvr2_ctrl_get_desc(cptr));
+	strlcpy(vc->name, pvr2_ctrl_get_desc(cptr), sizeof(vc->name));
+	vc->flags = pvr2_ctrl_get_v4lflags(cptr);
+	pvr2_ctrl_get_def(cptr, &val);
+	vc->default_value = val;
+	switch (pvr2_ctrl_get_type(cptr)) {
+	case pvr2_ctl_enum:
+		vc->type = V4L2_CTRL_TYPE_MENU;
+		vc->minimum = 0;
+		vc->maximum = pvr2_ctrl_get_cnt(cptr) - 1;
+		vc->step = 1;
 		break;
-	}
-
-	case VIDIOC_QUERYMENU:
-	{
-		struct v4l2_querymenu *vm = (struct v4l2_querymenu *)arg;
-		unsigned int cnt = 0;
-		ret = pvr2_ctrl_get_valname(pvr2_hdw_get_ctrl_v4l(hdw,vm->id),
-					    vm->index,
-					    vm->name,sizeof(vm->name)-1,
-					    &cnt);
-		vm->name[cnt] = 0;
+	case pvr2_ctl_bool:
+		vc->type = V4L2_CTRL_TYPE_BOOLEAN;
+		vc->minimum = 0;
+		vc->maximum = 1;
+		vc->step = 1;
 		break;
-	}
-
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *vc = (struct v4l2_control *)arg;
-		int val = 0;
-		ret = pvr2_ctrl_get_value(pvr2_hdw_get_ctrl_v4l(hdw,vc->id),
-					  &val);
-		vc->value = val;
+	case pvr2_ctl_int:
+		vc->type = V4L2_CTRL_TYPE_INTEGER;
+		vc->minimum = pvr2_ctrl_get_min(cptr);
+		vc->maximum = pvr2_ctrl_get_max(cptr);
+		vc->step = 1;
 		break;
+	default:
+		pvr2_trace(PVR2_TRACE_V4LIOCTL,
+				"QUERYCTRL id=0x%x name=%s not mappable",
+				vc->id, pvr2_ctrl_get_name(cptr));
+		return -EINVAL;
 	}
+	return 0;
+}
 
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *vc = (struct v4l2_control *)arg;
-		ret = pvr2_ctrl_set_value(pvr2_hdw_get_ctrl_v4l(hdw,vc->id),
-					  vc->value);
-		break;
-	}
+static int pvr2_querymenu(struct file *file, void *priv, struct v4l2_querymenu *vm)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	unsigned int cnt = 0;
+	int ret;
 
-	case VIDIOC_G_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *ctls =
-			(struct v4l2_ext_controls *)arg;
-		struct v4l2_ext_control *ctrl;
-		unsigned int idx;
-		int val;
-		ret = 0;
-		for (idx = 0; idx < ctls->count; idx++) {
-			ctrl = ctls->controls + idx;
-			ret = pvr2_ctrl_get_value(
-				pvr2_hdw_get_ctrl_v4l(hdw,ctrl->id),&val);
-			if (ret) {
-				ctls->error_idx = idx;
-				break;
-			}
-			/* Ensure that if read as a 64 bit value, the user
-			   will still get a hopefully sane value */
-			ctrl->value64 = 0;
-			ctrl->value = val;
+	ret = pvr2_ctrl_get_valname(pvr2_hdw_get_ctrl_v4l(hdw, vm->id),
+			vm->index,
+			vm->name, sizeof(vm->name) - 1,
+			&cnt);
+	vm->name[cnt] = 0;
+	return ret;
+}
+
+static int pvr2_g_ctrl(struct file *file, void *priv, struct v4l2_control *vc)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int ret;
+
+	ret = pvr2_ctrl_get_value(pvr2_hdw_get_ctrl_v4l(hdw, vc->id),
+			&val);
+	vc->value = val;
+	return ret;
+}
+
+static int pvr2_s_ctrl(struct file *file, void *priv, struct v4l2_control *vc)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+
+	return pvr2_ctrl_set_value(pvr2_hdw_get_ctrl_v4l(hdw, vc->id),
+			vc->value);
+}
+
+static int pvr2_g_ext_ctrls(struct file *file, void *priv,
+					struct v4l2_ext_controls *ctls)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_ext_control *ctrl;
+	unsigned int idx;
+	int val;
+	int ret;
+
+	ret = 0;
+	for (idx = 0; idx < ctls->count; idx++) {
+		ctrl = ctls->controls + idx;
+		ret = pvr2_ctrl_get_value(
+				pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id), &val);
+		if (ret) {
+			ctls->error_idx = idx;
+			return ret;
 		}
-		break;
+		/* Ensure that if read as a 64 bit value, the user
+		   will still get a hopefully sane value */
+		ctrl->value64 = 0;
+		ctrl->value = val;
 	}
+	return 0;
+}
 
-	case VIDIOC_S_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *ctls =
-			(struct v4l2_ext_controls *)arg;
-		struct v4l2_ext_control *ctrl;
-		unsigned int idx;
-		ret = 0;
-		for (idx = 0; idx < ctls->count; idx++) {
-			ctrl = ctls->controls + idx;
-			ret = pvr2_ctrl_set_value(
-				pvr2_hdw_get_ctrl_v4l(hdw,ctrl->id),
+static int pvr2_s_ext_ctrls(struct file *file, void *priv,
+		struct v4l2_ext_controls *ctls)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_ext_control *ctrl;
+	unsigned int idx;
+	int ret;
+
+	ret = 0;
+	for (idx = 0; idx < ctls->count; idx++) {
+		ctrl = ctls->controls + idx;
+		ret = pvr2_ctrl_set_value(
+				pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id),
 				ctrl->value);
-			if (ret) {
-				ctls->error_idx = idx;
-				break;
-			}
+		if (ret) {
+			ctls->error_idx = idx;
+			return ret;
 		}
-		break;
 	}
+	return 0;
+}
 
-	case VIDIOC_TRY_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *ctls =
-			(struct v4l2_ext_controls *)arg;
-		struct v4l2_ext_control *ctrl;
-		struct pvr2_ctrl *pctl;
-		unsigned int idx;
-		/* For the moment just validate that the requested control
-		   actually exists. */
-		ret = 0;
-		for (idx = 0; idx < ctls->count; idx++) {
-			ctrl = ctls->controls + idx;
-			pctl = pvr2_hdw_get_ctrl_v4l(hdw,ctrl->id);
-			if (!pctl) {
-				ret = -EINVAL;
-				ctls->error_idx = idx;
-				break;
-			}
-		}
-		break;
-	}
+static int pvr2_try_ext_ctrls(struct file *file, void *priv,
+		struct v4l2_ext_controls *ctls)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_ext_control *ctrl;
+	struct pvr2_ctrl *pctl;
+	unsigned int idx;
+	int ret;
 
-	case VIDIOC_CROPCAP:
-	{
-		struct v4l2_cropcap *cap = (struct v4l2_cropcap *)arg;
-		if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-			ret = -EINVAL;
-			break;
+	/* For the moment just validate that the requested control
+	   actually exists. */
+	ret = 0;
+	for (idx = 0; idx < ctls->count; idx++) {
+		ctrl = ctls->controls + idx;
+		pctl = pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id);
+		if (!pctl) {
+			ctls->error_idx = idx;
+			return -EINVAL;
 		}
-		ret = pvr2_hdw_get_cropcap(hdw, cap);
-		cap->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; /* paranoia */
-		break;
 	}
-	case VIDIOC_G_CROP:
-	{
-		struct v4l2_crop *crop = (struct v4l2_crop *)arg;
-		int val = 0;
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_get_value(
+	return 0;
+}
+
+static int pvr2_cropcap(struct file *file, void *priv, struct v4l2_cropcap *cap)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int ret;
+
+	if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	ret = pvr2_hdw_get_cropcap(hdw, cap);
+	cap->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; /* paranoia */
+	return ret;
+}
+
+static int pvr2_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int ret;
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.left = val;
-		ret = pvr2_ctrl_get_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.left = val;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPT), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.top = val;
-		ret = pvr2_ctrl_get_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.top = val;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPW), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.width = val;
-		ret = pvr2_ctrl_get_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.width = val;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPH), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.height = val;
-	}
-	case VIDIOC_S_CROP:
-	{
-		struct v4l2_crop *crop = (struct v4l2_crop *)arg;
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.height = val;
+	return 0;
+}
+
+static int pvr2_s_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_cropcap cap;
+	int ret;
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL),
 			crop->c.left);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPT),
 			crop->c.top);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPW),
 			crop->c.width);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPH),
 			crop->c.height);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-	}
-	case VIDIOC_LOG_STATUS:
-	{
-		pvr2_hdw_trigger_module_log(hdw);
-		ret = 0;
-		break;
-	}
+	if (ret != 0)
+		return -EINVAL;
+	return 0;
+}
+
+static int pvr2_log_status(struct file *file, void *priv)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+
+	pvr2_hdw_trigger_module_log(hdw);
+	return 0;
+}
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	case VIDIOC_DBG_S_REGISTER:
-	case VIDIOC_DBG_G_REGISTER:
-	{
-		u64 val;
-		struct v4l2_dbg_register *req = (struct v4l2_dbg_register *)arg;
-		if (cmd == VIDIOC_DBG_S_REGISTER) val = req->val;
-		ret = pvr2_hdw_register_access(
-			hdw, &req->match, req->reg,
-			cmd == VIDIOC_DBG_S_REGISTER, &val);
-		if (cmd == VIDIOC_DBG_G_REGISTER) req->val = val;
-		break;
-	}
-#endif
+static int pvr2_g_register(struct file *file, void *priv, struct v4l2_dbg_register *req)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	u64 val;
+	int ret;
 
-	default :
-		ret = -ENOTTY;
-		break;
-	}
+	ret = pvr2_hdw_register_access(
+			hdw, &req->match, req->reg,
+			0, &val);
+	req->val = val;
+	return ret;
+}
 
-	pvr2_hdw_commit_ctl(hdw);
+static int pvr2_s_register(struct file *file, void *priv, struct v4l2_dbg_register *req)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	u64 val;
+	int ret;
 
-	if (ret < 0) {
-		if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
-			pvr2_trace(PVR2_TRACE_V4LIOCTL,
-				   "pvr2_v4l2_do_ioctl failure, ret=%ld", ret);
-		} else {
-			if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
-				pvr2_trace(PVR2_TRACE_V4LIOCTL,
-					   "pvr2_v4l2_do_ioctl failure, ret=%ld"
-					   " command was:", ret);
-				v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw),
-						cmd);
-			}
-		}
-	} else {
-		pvr2_trace(PVR2_TRACE_V4LIOCTL,
-			   "pvr2_v4l2_do_ioctl complete, ret=%ld (0x%lx)",
-			   ret, ret);
-	}
+	val = req->val;
+	ret = pvr2_hdw_register_access(
+			hdw, &req->match, req->reg,
+			1, &val);
 	return ret;
 }
+#endif
+
+static const struct v4l2_ioctl_ops pvr2_ioctl_ops = {
+	.vidioc_querycap		    = pvr2_querycap,
+	.vidioc_g_priority		    = pvr2_g_priority,
+	.vidioc_s_priority		    = pvr2_s_priority,
+	.vidioc_s_audio			    = pvr2_s_audio,
+	.vidioc_g_audio			    = pvr2_g_audio,
+	.vidioc_enumaudio		    = pvr2_enumaudio,
+	.vidioc_enum_input		    = pvr2_enum_input,
+	.vidioc_cropcap			    = pvr2_cropcap,
+	.vidioc_s_crop			    = pvr2_s_crop,
+	.vidioc_g_crop			    = pvr2_g_crop,
+	.vidioc_g_input			    = pvr2_g_input,
+	.vidioc_s_input			    = pvr2_s_input,
+	.vidioc_g_frequency		    = pvr2_g_frequency,
+	.vidioc_s_frequency		    = pvr2_s_frequency,
+	.vidioc_s_tuner			    = pvr2_s_tuner,
+	.vidioc_g_tuner			    = pvr2_g_tuner,
+	.vidioc_g_std			    = pvr2_g_std,
+	.vidioc_s_std			    = pvr2_s_std,
+	.vidioc_querystd		    = pvr2_querystd,
+	.vidioc_log_status		    = pvr2_log_status,
+	.vidioc_enum_fmt_vid_cap	    = pvr2_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		    = pvr2_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		    = pvr2_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		    = pvr2_try_fmt_vid_cap,
+	.vidioc_streamon		    = pvr2_streamon,
+	.vidioc_streamoff		    = pvr2_streamoff,
+	.vidioc_queryctrl		    = pvr2_queryctrl,
+	.vidioc_querymenu		    = pvr2_querymenu,
+	.vidioc_g_ctrl			    = pvr2_g_ctrl,
+	.vidioc_s_ctrl			    = pvr2_s_ctrl,
+	.vidioc_g_ext_ctrls		    = pvr2_g_ext_ctrls,
+	.vidioc_s_ext_ctrls		    = pvr2_s_ext_ctrls,
+	.vidioc_try_ext_ctrls		    = pvr2_try_ext_ctrls,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register		    = pvr2_g_register,
+	.vidioc_s_register		    = pvr2_s_register,
+#endif
+};
 
 static void pvr2_v4l2_dev_destroy(struct pvr2_v4l2_dev *dip)
 {
@@ -961,7 +959,56 @@ static long pvr2_v4l2_ioctl(struct file *file,
 			   unsigned int cmd, unsigned long arg)
 {
 
-	return video_usercopy(file, cmd, arg, pvr2_v4l2_do_ioctl);
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_v4l2 *vp = fh->vhead;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	long ret = -EINVAL;
+
+	if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL)
+		v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw), cmd);
+
+	if (!pvr2_hdw_dev_ok(hdw)) {
+		pvr2_trace(PVR2_TRACE_ERROR_LEGS,
+			   "ioctl failed - bad or no context");
+		return -EFAULT;
+	}
+
+	/* check priority */
+	switch (cmd) {
+	case VIDIOC_S_CTRL:
+	case VIDIOC_S_STD:
+	case VIDIOC_S_INPUT:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_S_FREQUENCY:
+		ret = v4l2_prio_check(&vp->prio, fh->prio);
+		if (ret)
+			return ret;
+	}
+
+	ret = video_ioctl2(file, cmd, arg);
+
+	pvr2_hdw_commit_ctl(hdw);
+
+	if (ret < 0) {
+		if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
+			pvr2_trace(PVR2_TRACE_V4LIOCTL,
+				   "pvr2_v4l2_do_ioctl failure, ret=%ld", ret);
+		} else {
+			if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
+				pvr2_trace(PVR2_TRACE_V4LIOCTL,
+					   "pvr2_v4l2_do_ioctl failure, ret=%ld"
+					   " command was:", ret);
+				v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw),
+						cmd);
+			}
+		}
+	} else {
+		pvr2_trace(PVR2_TRACE_V4LIOCTL,
+			   "pvr2_v4l2_do_ioctl complete, ret=%ld (0x%lx)",
+			   ret, ret);
+	}
+	return ret;
+
 }
 
 
@@ -1262,10 +1309,12 @@ static void pvr2_v4l2_dev_init(struct pvr2_v4l2_dev *dip,
 	struct usb_device *usbdev;
 	int mindevnum;
 	int unit_number;
+	struct pvr2_hdw *hdw;
 	int *nr_ptr = NULL;
 	dip->v4lp = vp;
 
-	usbdev = pvr2_hdw_get_dev(vp->channel.mc_head->hdw);
+	hdw = vp->channel.mc_head->hdw;
+	usbdev = pvr2_hdw_get_dev(hdw);
 	dip->v4l_type = v4l_type;
 	switch (v4l_type) {
 	case VFL_TYPE_GRABBER:
@@ -1300,9 +1349,17 @@ static void pvr2_v4l2_dev_init(struct pvr2_v4l2_dev *dip,
 
 	memcpy(&dip->devbase,&vdev_template,sizeof(vdev_template));
 	dip->devbase.release = pvr2_video_device_release;
+	dip->devbase.ioctl_ops = &pvr2_ioctl_ops;
+	{
+		int val;
+		pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw,
+						PVR2_CID_STDAVAIL), &val);
+		dip->devbase.tvnorms = (v4l2_std_id)val;
+	}
 
 	mindevnum = -1;
-	unit_number = pvr2_hdw_get_unit_number(vp->channel.mc_head->hdw);
+	unit_number = pvr2_hdw_get_unit_number(hdw);
 	if (nr_ptr && (unit_number >= 0) && (unit_number < PVR_NUM)) {
 		mindevnum = nr_ptr[unit_number];
 	}
@@ -1319,7 +1376,7 @@ static void pvr2_v4l2_dev_init(struct pvr2_v4l2_dev *dip,
 	       video_device_node_name(&dip->devbase),
 	       pvr2_config_get_name(dip->config));
 
-	pvr2_hdw_v4l_store_minor_number(vp->channel.mc_head->hdw,
+	pvr2_hdw_v4l_store_minor_number(hdw,
 					dip->minor_type,dip->devbase.minor);
 }
 
diff --git a/drivers/media/video/pwc/pwc-if.c b/drivers/media/video/pwc/pwc-if.c
index 122fbd0081eb..ec4e2ef54e65 100644
--- a/drivers/media/video/pwc/pwc-if.c
+++ b/drivers/media/video/pwc/pwc-if.c
@@ -357,6 +357,7 @@ handler_end:
 		PWC_ERROR("Error (%d) re-submitting urb in pwc_isoc_handler.\n", i);
 }
 
+/* Both v4l2_lock and vb_queue_lock should be locked when calling this */
 static int pwc_isoc_init(struct pwc_device *pdev)
 {
 	struct usb_device *udev;
@@ -366,9 +367,6 @@ static int pwc_isoc_init(struct pwc_device *pdev)
 	struct usb_host_interface *idesc = NULL;
 	int compression = 0; /* 0..3 = uncompressed..high */
 
-	if (pdev->iso_init)
-		return 0;
-
 	pdev->vsync = 0;
 	pdev->vlast_packet_size = 0;
 	pdev->fill_buf = NULL;
@@ -418,7 +416,6 @@ retry:
 		urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
 		if (urb == NULL) {
 			PWC_ERROR("Failed to allocate urb %d\n", i);
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			return -ENOMEM;
 		}
@@ -435,7 +432,6 @@ retry:
 							  &urb->transfer_dma);
 		if (urb->transfer_buffer == NULL) {
 			PWC_ERROR("Failed to allocate urb buffer %d\n", i);
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			return -ENOMEM;
 		}
@@ -455,13 +451,11 @@ retry:
 		ret = usb_submit_urb(pdev->urbs[i], GFP_KERNEL);
 		if (ret == -ENOSPC && compression < 3) {
 			compression++;
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			goto retry;
 		}
 		if (ret) {
 			PWC_ERROR("isoc_init() submit_urb %d failed with error %d\n", i, ret);
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			return ret;
 		}
@@ -469,7 +463,6 @@ retry:
 	}
 
 	/* All is done... */
-	pdev->iso_init = 1;
 	PWC_DEBUG_OPEN("<< pwc_isoc_init()\n");
 	return 0;
 }
@@ -507,21 +500,19 @@ static void pwc_iso_free(struct pwc_device *pdev)
 	}
 }
 
+/* Both v4l2_lock and vb_queue_lock should be locked when calling this */
 static void pwc_isoc_cleanup(struct pwc_device *pdev)
 {
 	PWC_DEBUG_OPEN(">> pwc_isoc_cleanup()\n");
 
-	if (pdev->iso_init == 0)
-		return;
-
 	pwc_iso_stop(pdev);
 	pwc_iso_free(pdev);
 	usb_set_interface(pdev->udev, 0, 0);
 
-	pdev->iso_init = 0;
 	PWC_DEBUG_OPEN("<< pwc_isoc_cleanup()\n");
 }
 
+/* Must be called with vb_queue_lock hold */
 static void pwc_cleanup_queued_bufs(struct pwc_device *pdev)
 {
 	unsigned long flags = 0;
@@ -573,18 +564,13 @@ static const char *pwc_sensor_type_to_string(unsigned int sensor_type)
 
 int pwc_test_n_set_capt_file(struct pwc_device *pdev, struct file *file)
 {
-	int r = 0;
-
-	mutex_lock(&pdev->capt_file_lock);
 	if (pdev->capt_file != NULL &&
-	    pdev->capt_file != file) {
-		r = -EBUSY;
-		goto leave;
-	}
+	    pdev->capt_file != file)
+		return -EBUSY;
+
 	pdev->capt_file = file;
-leave:
-	mutex_unlock(&pdev->capt_file_lock);
-	return r;
+
+	return 0;
 }
 
 static void pwc_video_release(struct v4l2_device *v)
@@ -592,6 +578,7 @@ static void pwc_video_release(struct v4l2_device *v)
 	struct pwc_device *pdev = container_of(v, struct pwc_device, v4l2_dev);
 
 	v4l2_ctrl_handler_free(&pdev->ctrl_handler);
+	v4l2_device_unregister(&pdev->v4l2_dev);
 	kfree(pdev->ctrl_buf);
 	kfree(pdev);
 }
@@ -600,10 +587,25 @@ static int pwc_video_close(struct file *file)
 {
 	struct pwc_device *pdev = video_drvdata(file);
 
+	/*
+	 * If we're still streaming vb2_queue_release will call stream_stop
+	 * so we must take both the v4l2_lock and the vb_queue_lock.
+	 */
+	if (mutex_lock_interruptible(&pdev->v4l2_lock))
+		return -ERESTARTSYS;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock)) {
+		mutex_unlock(&pdev->v4l2_lock);
+		return -ERESTARTSYS;
+	}
+
 	if (pdev->capt_file == file) {
 		vb2_queue_release(&pdev->vb_queue);
 		pdev->capt_file = NULL;
 	}
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	mutex_unlock(&pdev->v4l2_lock);
+
 	return v4l2_fh_release(file);
 }
 
@@ -611,35 +613,81 @@ static ssize_t pwc_video_read(struct file *file, char __user *buf,
 			      size_t count, loff_t *ppos)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int lock_v4l2 = 0;
+	ssize_t ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret)
+		goto out;
 
-	return vb2_read(&pdev->vb_queue, buf, count, ppos,
-			file->f_flags & O_NONBLOCK);
+	/* stream_start will get called so we must take the v4l2_lock */
+	if (pdev->vb_queue.fileio == NULL)
+		lock_v4l2 = 1;
+
+	/* Use try_lock, since we're taking the locks in the *wrong* order! */
+	if (lock_v4l2 && !mutex_trylock(&pdev->v4l2_lock)) {
+		ret = -ERESTARTSYS;
+		goto out;
+	}
+	ret = vb2_read(&pdev->vb_queue, buf, count, ppos,
+		       file->f_flags & O_NONBLOCK);
+	if (lock_v4l2)
+		mutex_unlock(&pdev->v4l2_lock);
+out:
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static unsigned int pwc_video_poll(struct file *file, poll_table *wait)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	struct vb2_queue *q = &pdev->vb_queue;
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int ret = POLL_ERR;
+	int lock_v4l2 = 0;
 
-	if (!pdev->udev)
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
 		return POLL_ERR;
 
-	return vb2_poll(&pdev->vb_queue, file, wait);
+	/* Will this start fileio and thus call start_stream? */
+	if ((req_events & (POLLIN | POLLRDNORM)) &&
+	    q->num_buffers == 0 && !q->streaming && q->fileio == NULL) {
+		if (pwc_test_n_set_capt_file(pdev, file))
+			goto out;
+		lock_v4l2 = 1;
+	}
+
+	/* Use try_lock, since we're taking the locks in the *wrong* order! */
+	if (lock_v4l2 && !mutex_trylock(&pdev->v4l2_lock))
+		goto out;
+	ret = vb2_poll(&pdev->vb_queue, file, wait);
+	if (lock_v4l2)
+		mutex_unlock(&pdev->v4l2_lock);
+
+out:
+	if (!pdev->udev)
+		ret |= POLLHUP;
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_video_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	return vb2_mmap(&pdev->vb_queue, vma);
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_mmap(&pdev->vb_queue, vma);
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 /***************************************************************************/
@@ -715,12 +763,14 @@ static void buffer_queue(struct vb2_buffer *vb)
 	struct pwc_frame_buf *buf = container_of(vb, struct pwc_frame_buf, vb);
 	unsigned long flags = 0;
 
-	spin_lock_irqsave(&pdev->queued_bufs_lock, flags);
 	/* Check the device has not disconnected between prep and queuing */
-	if (pdev->udev)
-		list_add_tail(&buf->list, &pdev->queued_bufs);
-	else
+	if (!pdev->udev) {
 		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+		return;
+	}
+
+	spin_lock_irqsave(&pdev->queued_bufs_lock, flags);
+	list_add_tail(&buf->list, &pdev->queued_bufs);
 	spin_unlock_irqrestore(&pdev->queued_bufs_lock, flags);
 }
 
@@ -729,11 +779,8 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	struct pwc_device *pdev = vb2_get_drv_priv(vq);
 	int r;
 
-	mutex_lock(&pdev->udevlock);
-	if (!pdev->udev) {
-		r = -ENODEV;
-		goto leave;
-	}
+	if (!pdev->udev)
+		return -ENODEV;
 
 	/* Turn on camera and set LEDS on */
 	pwc_camera_power(pdev, 1);
@@ -747,8 +794,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 		/* And cleanup any queued bufs!! */
 		pwc_cleanup_queued_bufs(pdev);
 	}
-leave:
-	mutex_unlock(&pdev->udevlock);
+
 	return r;
 }
 
@@ -756,19 +802,29 @@ static int stop_streaming(struct vb2_queue *vq)
 {
 	struct pwc_device *pdev = vb2_get_drv_priv(vq);
 
-	mutex_lock(&pdev->udevlock);
 	if (pdev->udev) {
 		pwc_set_leds(pdev, 0, 0);
 		pwc_camera_power(pdev, 0);
 		pwc_isoc_cleanup(pdev);
 	}
-	mutex_unlock(&pdev->udevlock);
 
 	pwc_cleanup_queued_bufs(pdev);
 
 	return 0;
 }
 
+static void wait_prepare(struct vb2_queue *vq)
+{
+	struct pwc_device *pdev = vb2_get_drv_priv(vq);
+	mutex_unlock(&pdev->vb_queue_lock);
+}
+
+static void wait_finish(struct vb2_queue *vq)
+{
+	struct pwc_device *pdev = vb2_get_drv_priv(vq);
+	mutex_lock(&pdev->vb_queue_lock);
+}
+
 static struct vb2_ops pwc_vb_queue_ops = {
 	.queue_setup		= queue_setup,
 	.buf_init		= buffer_init,
@@ -778,6 +834,8 @@ static struct vb2_ops pwc_vb_queue_ops = {
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
+	.wait_prepare		= wait_prepare,
+	.wait_finish		= wait_finish,
 };
 
 /***************************************************************************/
@@ -1057,8 +1115,8 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	pdev->features = features;
 	pwc_construct(pdev); /* set min/max sizes correct */
 
-	mutex_init(&pdev->capt_file_lock);
-	mutex_init(&pdev->udevlock);
+	mutex_init(&pdev->v4l2_lock);
+	mutex_init(&pdev->vb_queue_lock);
 	spin_lock_init(&pdev->queued_bufs_lock);
 	INIT_LIST_HEAD(&pdev->queued_bufs);
 
@@ -1130,6 +1188,16 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 
 	pdev->v4l2_dev.ctrl_handler = &pdev->ctrl_handler;
 	pdev->vdev.v4l2_dev = &pdev->v4l2_dev;
+	pdev->vdev.lock = &pdev->v4l2_lock;
+
+	/*
+	 * Don't take v4l2_lock for these ioctls. This improves latency if
+	 * v4l2_lock is taken for a long time, e.g. when changing a control
+	 * value, and a new frame is ready to be dequeued.
+	 */
+	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_DQBUF);
+	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_QBUF);
+	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_QUERYBUF);
 
 	rc = video_register_device(&pdev->vdev, VFL_TYPE_GRABBER, -1);
 	if (rc < 0) {
@@ -1185,16 +1253,20 @@ static void usb_pwc_disconnect(struct usb_interface *intf)
 	struct v4l2_device *v = usb_get_intfdata(intf);
 	struct pwc_device *pdev = container_of(v, struct pwc_device, v4l2_dev);
 
-	mutex_lock(&pdev->udevlock);
+	mutex_lock(&pdev->v4l2_lock);
+
+	mutex_lock(&pdev->vb_queue_lock);
 	/* No need to keep the urbs around after disconnection */
-	pwc_isoc_cleanup(pdev);
+	if (pdev->vb_queue.streaming)
+		pwc_isoc_cleanup(pdev);
 	pdev->udev = NULL;
-	mutex_unlock(&pdev->udevlock);
-
 	pwc_cleanup_queued_bufs(pdev);
+	mutex_unlock(&pdev->vb_queue_lock);
 
+	v4l2_device_disconnect(&pdev->v4l2_dev);
 	video_unregister_device(&pdev->vdev);
-	v4l2_device_unregister(&pdev->v4l2_dev);
+
+	mutex_unlock(&pdev->v4l2_lock);
 
 #ifdef CONFIG_USB_PWC_INPUT_EVDEV
 	if (pdev->button_dev)
@@ -1229,15 +1301,4 @@ MODULE_LICENSE("GPL");
 MODULE_ALIAS("pwcx");
 MODULE_VERSION( PWC_VERSION );
 
-static int __init usb_pwc_init(void)
-{
-	return usb_register(&pwc_driver);
-}
-
-static void __exit usb_pwc_exit(void)
-{
-	usb_deregister(&pwc_driver);
-}
-
-module_init(usb_pwc_init);
-module_exit(usb_pwc_exit);
+module_usb_driver(pwc_driver);
diff --git a/drivers/media/video/pwc/pwc-v4l.c b/drivers/media/video/pwc/pwc-v4l.c
index 2834e3e65b39..c691e29cc36e 100644
--- a/drivers/media/video/pwc/pwc-v4l.c
+++ b/drivers/media/video/pwc/pwc-v4l.c
@@ -464,26 +464,24 @@ static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 	struct pwc_device *pdev = video_drvdata(file);
 	int ret, pixelformat, compression = 0;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
-
 	ret = pwc_vidioc_try_fmt(pdev, f);
 	if (ret < 0)
 		return ret;
 
-	pixelformat = f->fmt.pix.pixelformat;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	mutex_lock(&pdev->udevlock);
-	if (!pdev->udev) {
-		ret = -ENODEV;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret)
 		goto leave;
-	}
 
-	if (pdev->iso_init) {
+	if (pdev->vb_queue.streaming) {
 		ret = -EBUSY;
 		goto leave;
 	}
 
+	pixelformat = f->fmt.pix.pixelformat;
+
 	PWC_DEBUG_IOCTL("Trying to set format to: width=%d height=%d fps=%d "
 			"format=%c%c%c%c\n",
 			f->fmt.pix.width, f->fmt.pix.height, pdev->vframes,
@@ -499,7 +497,7 @@ static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 
 	pwc_vidioc_fill_fmt(f, pdev->width, pdev->height, pdev->pixfmt);
 leave:
-	mutex_unlock(&pdev->udevlock);
+	mutex_unlock(&pdev->vb_queue_lock);
 	return ret;
 }
 
@@ -507,9 +505,6 @@ static int pwc_querycap(struct file *file, void *fh, struct v4l2_capability *cap
 {
 	struct pwc_device *pdev = video_drvdata(file);
 
-	if (!pdev->udev)
-		return -ENODEV;
-
 	strcpy(cap->driver, PWC_NAME);
 	strlcpy(cap->card, pdev->vdev.name, sizeof(cap->card));
 	usb_make_path(pdev->udev, cap->bus_info, sizeof(cap->bus_info));
@@ -540,15 +535,12 @@ static int pwc_s_input(struct file *file, void *fh, unsigned int i)
 	return i ? -EINVAL : 0;
 }
 
-static int pwc_g_volatile_ctrl_unlocked(struct v4l2_ctrl *ctrl)
+static int pwc_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct pwc_device *pdev =
 		container_of(ctrl->handler, struct pwc_device, ctrl_handler);
 	int ret = 0;
 
-	if (!pdev->udev)
-		return -ENODEV;
-
 	switch (ctrl->id) {
 	case V4L2_CID_AUTO_WHITE_BALANCE:
 		if (pdev->color_bal_valid &&
@@ -615,18 +607,6 @@ static int pwc_g_volatile_ctrl_unlocked(struct v4l2_ctrl *ctrl)
 	return ret;
 }
 
-static int pwc_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct pwc_device *pdev =
-		container_of(ctrl->handler, struct pwc_device, ctrl_handler);
-	int ret;
-
-	mutex_lock(&pdev->udevlock);
-	ret = pwc_g_volatile_ctrl_unlocked(ctrl);
-	mutex_unlock(&pdev->udevlock);
-	return ret;
-}
-
 static int pwc_set_awb(struct pwc_device *pdev)
 {
 	int ret;
@@ -648,7 +628,7 @@ static int pwc_set_awb(struct pwc_device *pdev)
 		if (pdev->auto_white_balance->val == awb_indoor ||
 		    pdev->auto_white_balance->val == awb_outdoor ||
 		    pdev->auto_white_balance->val == awb_fl)
-			pwc_g_volatile_ctrl_unlocked(pdev->auto_white_balance);
+			pwc_g_volatile_ctrl(pdev->auto_white_balance);
 	}
 	if (pdev->auto_white_balance->val != awb_manual)
 		return 0;
@@ -812,13 +792,6 @@ static int pwc_s_ctrl(struct v4l2_ctrl *ctrl)
 		container_of(ctrl->handler, struct pwc_device, ctrl_handler);
 	int ret = 0;
 
-	mutex_lock(&pdev->udevlock);
-
-	if (!pdev->udev) {
-		ret = -ENODEV;
-		goto leave;
-	}
-
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
 		ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
@@ -915,8 +888,6 @@ static int pwc_s_ctrl(struct v4l2_ctrl *ctrl)
 	if (ret)
 		PWC_ERROR("s_ctrl %s error %d\n", ctrl->name, ret);
 
-leave:
-	mutex_unlock(&pdev->udevlock);
 	return ret;
 }
 
@@ -949,11 +920,9 @@ static int pwc_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	mutex_lock(&pdev->udevlock); /* To avoid race with s_fmt */
 	PWC_DEBUG_IOCTL("ioctl(VIDIOC_G_FMT) return size %dx%d\n",
 			pdev->width, pdev->height);
 	pwc_vidioc_fill_fmt(f, pdev->width, pdev->height, pdev->pixfmt);
-	mutex_unlock(&pdev->udevlock);
 	return 0;
 }
 
@@ -968,70 +937,98 @@ static int pwc_reqbufs(struct file *file, void *fh,
 		       struct v4l2_requestbuffers *rb)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	return vb2_reqbufs(&pdev->vb_queue, rb);
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_reqbufs(&pdev->vb_queue, rb);
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	return vb2_querybuf(&pdev->vb_queue, buf);
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
+
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_querybuf(&pdev->vb_queue, buf);
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_qbuf(&pdev->vb_queue, buf);
 
-	return vb2_qbuf(&pdev->vb_queue, buf);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_dqbuf(&pdev->vb_queue, buf,
+				file->f_flags & O_NONBLOCK);
 
-	return vb2_dqbuf(&pdev->vb_queue, buf, file->f_flags & O_NONBLOCK);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_streamon(&pdev->vb_queue, i);
 
-	return vb2_streamon(&pdev->vb_queue, i);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_streamoff(&pdev->vb_queue, i);
 
-	return vb2_streamoff(&pdev->vb_queue, i);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_enum_framesizes(struct file *file, void *fh,
@@ -1119,19 +1116,17 @@ static int pwc_s_parm(struct file *file, void *fh,
 	    parm->parm.capture.timeperframe.numerator == 0)
 		return -EINVAL;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
-
 	fps = parm->parm.capture.timeperframe.denominator /
 	      parm->parm.capture.timeperframe.numerator;
 
-	mutex_lock(&pdev->udevlock);
-	if (!pdev->udev) {
-		ret = -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
+
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret)
 		goto leave;
-	}
 
-	if (pdev->iso_init) {
+	if (pdev->vb_queue.streaming) {
 		ret = -EBUSY;
 		goto leave;
 	}
@@ -1142,7 +1137,7 @@ static int pwc_s_parm(struct file *file, void *fh,
 	pwc_g_parm(file, fh, parm);
 
 leave:
-	mutex_unlock(&pdev->udevlock);
+	mutex_unlock(&pdev->vb_queue_lock);
 	return ret;
 }
 
@@ -1166,4 +1161,6 @@ const struct v4l2_ioctl_ops pwc_ioctl_ops = {
 	.vidioc_enum_frameintervals	    = pwc_enum_frameintervals,
 	.vidioc_g_parm			    = pwc_g_parm,
 	.vidioc_s_parm			    = pwc_s_parm,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
 };
diff --git a/drivers/media/video/pwc/pwc.h b/drivers/media/video/pwc/pwc.h
index e4d4d711dd1f..d6b5b216b9d6 100644
--- a/drivers/media/video/pwc/pwc.h
+++ b/drivers/media/video/pwc/pwc.h
@@ -221,9 +221,17 @@ struct pwc_device
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
 
-	/* Pointer to our usb_device, may be NULL after unplug */
-	struct usb_device *udev;
-	struct mutex udevlock;
+	/* videobuf2 queue and queued buffers list */
+	struct vb2_queue vb_queue;
+	struct list_head queued_bufs;
+	spinlock_t queued_bufs_lock; /* Protects queued_bufs */
+
+	/* Note if taking both locks v4l2_lock must always be locked first! */
+	struct mutex v4l2_lock;      /* Protects everything else */
+	struct mutex vb_queue_lock;  /* Protects vb_queue and capt_file */
+
+	/* Pointer to our usb_device, will be NULL after unplug */
+	struct usb_device *udev; /* Both mutexes most be hold when setting! */
 
 	/* type of cam (645, 646, 675, 680, 690, 720, 730, 740, 750) */
 	int type;
@@ -232,7 +240,6 @@ struct pwc_device
 
 	/*** Video data ***/
 	struct file *capt_file;	/* file doing video capture */
-	struct mutex capt_file_lock;
 	int vendpoint;		/* video isoc endpoint */
 	int vcinterface;	/* video control interface */
 	int valternate;		/* alternate interface needed */
@@ -251,12 +258,6 @@ struct pwc_device
 	unsigned char *ctrl_buf;
 
 	struct urb *urbs[MAX_ISO_BUFS];
-	char iso_init;
-
-	/* videobuf2 queue and queued buffers list */
-	struct vb2_queue vb_queue;
-	struct list_head queued_bufs;
-	spinlock_t queued_bufs_lock;
 
 	/*
 	 * Frame currently being filled, this only gets touched by the
diff --git a/drivers/media/video/pxa_camera.c b/drivers/media/video/pxa_camera.c
index 5a413f4427e0..9c21e01f2c24 100644
--- a/drivers/media/video/pxa_camera.c
+++ b/drivers/media/video/pxa_camera.c
@@ -241,15 +241,10 @@ static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 			      unsigned int *size)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
 	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 
-	*size = bytes_per_line * icd->user_height;
+	*size = icd->sizeimage;
 
 	if (0 == *count)
 		*count = 32;
@@ -435,11 +430,6 @@ static int pxa_videobuf_prepare(struct videobuf_queue *vq,
 	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 	int ret;
 	int size_y, size_u = 0, size_v = 0;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
 	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 		vb, vb->baddr, vb->bsize);
@@ -474,7 +464,7 @@ static int pxa_videobuf_prepare(struct videobuf_queue *vq,
 		vb->state	= VIDEOBUF_NEEDS_INIT;
 	}
 
-	vb->size = bytes_per_line * vb->height;
+	vb->size = icd->sizeimage;
 	if (0 != vb->baddr && vb->bsize < vb->size) {
 		ret = -EINVAL;
 		goto out;
@@ -1244,6 +1234,7 @@ static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
 	},
 };
 
diff --git a/drivers/media/video/s2255drv.c b/drivers/media/video/s2255drv.c
index 4894cbb1c547..01c2179f0520 100644
--- a/drivers/media/video/s2255drv.c
+++ b/drivers/media/video/s2255drv.c
@@ -634,13 +634,11 @@ static void s2255_fillbuff(struct s2255_channel *channel,
 	const char *tmpbuf;
 	char *vbuf = videobuf_to_vmalloc(&buf->vb);
 	unsigned long last_frame;
-	struct s2255_framei *frm;
 
 	if (!vbuf)
 		return;
 	last_frame = channel->last_frame;
 	if (last_frame != -1) {
-		frm = &channel->buffer.frame[last_frame];
 		tmpbuf =
 		    (const char *)channel->buffer.frame[last_frame].lpvbits;
 		switch (buf->fmt->fourcc) {
@@ -987,7 +985,6 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	struct videobuf_queue *q = &fh->vb_vidq;
 	struct s2255_mode mode;
 	int ret;
-	int norm;
 
 	ret = vidioc_try_fmt_vid_cap(file, fh, f);
 
@@ -1018,7 +1015,6 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	channel->height = f->fmt.pix.height;
 	fh->vb_vidq.field = f->fmt.pix.field;
 	fh->type = f->type;
-	norm = norm_minw(&channel->vdev);
 	if (channel->width > norm_minw(&channel->vdev)) {
 		if (channel->height > norm_minh(&channel->vdev)) {
 			if (channel->cap_parm.capturemode &
@@ -1826,8 +1822,7 @@ static void s2255_destroy(struct s2255_dev *dev)
 		usb_free_urb(dev->fw_data->fw_urb);
 		dev->fw_data->fw_urb = NULL;
 	}
-	if (dev->fw_data->fw)
-		release_firmware(dev->fw_data->fw);
+	release_firmware(dev->fw_data->fw);
 	kfree(dev->fw_data->pfw_data);
 	kfree(dev->fw_data);
 	/* reset the DSP so firmware can be reloaded next time */
@@ -1949,6 +1944,10 @@ static int s2255_probe_v4l(struct s2255_dev *dev)
 		/* register 4 video devices */
 		channel->vdev = template;
 		channel->vdev.lock = &dev->lock;
+		/* Locking in file operations other than ioctl should be done
+		   by the driver, not the V4L2 core.
+		   This driver needs auditing so that this flag can be removed. */
+		set_bit(V4L2_FL_LOCK_ALL_FOPS, &channel->vdev.flags);
 		channel->vdev.v4l2_dev = &dev->v4l2_dev;
 		video_set_drvdata(&channel->vdev, channel);
 		if (video_nr == -1)
diff --git a/drivers/media/video/s5p-fimc/Kconfig b/drivers/media/video/s5p-fimc/Kconfig
new file mode 100644
index 000000000000..a564f7eeb064
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/Kconfig
@@ -0,0 +1,48 @@
+
+config VIDEO_SAMSUNG_S5P_FIMC
+	bool "Samsung S5P/EXYNOS SoC camera interface driver (experimental)"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && PLAT_S5P && PM_RUNTIME
+	depends on EXPERIMENTAL
+	help
+	  Say Y here to enable camera host interface devices for
+	  Samsung S5P and EXYNOS SoC series.
+
+if VIDEO_SAMSUNG_S5P_FIMC
+
+config VIDEO_S5P_FIMC
+	tristate "S5P/EXYNOS4 FIMC/CAMIF camera interface driver"
+	depends on I2C
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC camera host
+	  interface and video postprocessor (FIMC and FIMC-LITE) devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s5p-fimc.
+
+config VIDEO_S5P_MIPI_CSIS
+	tristate "S5P/EXYNOS MIPI-CSI2 receiver (MIPI-CSIS) driver"
+	depends on REGULATOR
+	help
+	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC MIPI-CSI2
+	  receiver (MIPI-CSIS) devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s5p-csis.
+
+if ARCH_EXYNOS
+
+config VIDEO_EXYNOS_FIMC_LITE
+	tristate "EXYNOS FIMC-LITE camera interface driver"
+	depends on I2C
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  This is a V4L2 driver for Samsung EXYNOS4/5 SoC FIMC-LITE camera
+	  host interface.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called exynos-fimc-lite.
+endif
+
+endif # VIDEO_SAMSUNG_S5P_FIMC
diff --git a/drivers/media/video/s5p-fimc/Makefile b/drivers/media/video/s5p-fimc/Makefile
index 33dec7f890e7..46485143e1ca 100644
--- a/drivers/media/video/s5p-fimc/Makefile
+++ b/drivers/media/video/s5p-fimc/Makefile
@@ -1,5 +1,7 @@
-s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-capture.o fimc-mdevice.o
+s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-m2m.o fimc-capture.o fimc-mdevice.o
+exynos-fimc-lite-objs += fimc-lite-reg.o fimc-lite.o
 s5p-csis-objs := mipi-csis.o
 
 obj-$(CONFIG_VIDEO_S5P_MIPI_CSIS)	+= s5p-csis.o
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_FIMC)	+= s5p-fimc.o
+obj-$(CONFIG_VIDEO_EXYNOS_FIMC_LITE)	+= exynos-fimc-lite.o
+obj-$(CONFIG_VIDEO_S5P_FIMC)		+= s5p-fimc.o
diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index 7e9b2c612b03..354574591908 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -1,8 +1,8 @@
 /*
  * Samsung S5P/EXYNOS4 SoC series camera interface (camera capture) driver
  *
- * Copyright (C) 2010 - 2011 Samsung Electronics Co., Ltd.
- * Author: Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ * 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
@@ -29,20 +29,22 @@
 
 #include "fimc-mdevice.h"
 #include "fimc-core.h"
+#include "fimc-reg.h"
 
-static int fimc_init_capture(struct fimc_dev *fimc)
+static int fimc_capture_hw_init(struct fimc_dev *fimc)
 {
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_pipeline *p = &fimc->pipeline;
 	struct fimc_sensor_info *sensor;
 	unsigned long flags;
 	int ret = 0;
 
-	if (fimc->pipeline.sensor == NULL || ctx == NULL)
+	if (p->subdevs[IDX_SENSOR] == NULL || ctx == NULL)
 		return -ENXIO;
 	if (ctx->s_frame.fmt == NULL)
 		return -EINVAL;
 
-	sensor = v4l2_get_subdev_hostdata(fimc->pipeline.sensor);
+	sensor = v4l2_get_subdev_hostdata(p->subdevs[IDX_SENSOR]);
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
@@ -60,7 +62,7 @@ static int fimc_init_capture(struct fimc_dev *fimc)
 		fimc_hw_set_mainscaler(ctx);
 		fimc_hw_set_target_format(ctx);
 		fimc_hw_set_rotation(ctx);
-		fimc_hw_set_effect(ctx, false);
+		fimc_hw_set_effect(ctx);
 		fimc_hw_set_output_path(ctx);
 		fimc_hw_set_out_dma(ctx);
 		if (fimc->variant->has_alpha)
@@ -71,6 +73,14 @@ static int fimc_init_capture(struct fimc_dev *fimc)
 	return ret;
 }
 
+/*
+ * Reinitialize the driver so it is ready to start the streaming again.
+ * Set fimc->state to indicate stream off and the hardware shut down state.
+ * If not suspending (@suspend is false), return any buffers to videobuf2.
+ * Otherwise put any owned buffers onto the pending buffers queue, so they
+ * can be re-spun when the device is being resumed. Also perform FIMC
+ * software reset and disable streaming on the whole pipeline if required.
+ */
 static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 {
 	struct fimc_vid_cap *cap = &fimc->vid_cap;
@@ -83,7 +93,9 @@ static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 
 	fimc->state &= ~(1 << ST_CAPT_RUN | 1 << ST_CAPT_SHUT |
 			 1 << ST_CAPT_STREAM | 1 << ST_CAPT_ISP_STREAM);
-	if (!suspend)
+	if (suspend)
+		fimc->state |= (1 << ST_CAPT_SUSPENDED);
+	else
 		fimc->state &= ~(1 << ST_CAPT_PEND | 1 << ST_CAPT_SUSPENDED);
 
 	/* Release unused buffers */
@@ -99,7 +111,6 @@ static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 		else
 			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 	}
-	set_bit(ST_CAPT_SUSPENDED, &fimc->state);
 
 	fimc_hw_reset(fimc);
 	cap->buf_index = 0;
@@ -107,7 +118,7 @@ static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
 	if (streaming)
-		return fimc_pipeline_s_stream(fimc, 0);
+		return fimc_pipeline_s_stream(&fimc->pipeline, 0);
 	else
 		return 0;
 }
@@ -138,32 +149,96 @@ static int fimc_stop_capture(struct fimc_dev *fimc, bool suspend)
  * spinlock held. It updates the camera pixel crop, rotation and
  * image flip in H/W.
  */
-int fimc_capture_config_update(struct fimc_ctx *ctx)
+static int fimc_capture_config_update(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	int ret;
 
-	if (!test_bit(ST_CAPT_APPLY_CFG, &fimc->state))
-		return 0;
-
-	spin_lock(&ctx->slock);
 	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
+
 	ret = fimc_set_scaler_info(ctx);
-	if (ret == 0) {
-		fimc_hw_set_prescaler(ctx);
-		fimc_hw_set_mainscaler(ctx);
-		fimc_hw_set_target_format(ctx);
-		fimc_hw_set_rotation(ctx);
-		fimc_prepare_dma_offset(ctx, &ctx->d_frame);
-		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
-			fimc_hw_set_rgb_alpha(ctx);
-		clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
-	}
-	spin_unlock(&ctx->slock);
+	if (ret)
+		return ret;
+
+	fimc_hw_set_prescaler(ctx);
+	fimc_hw_set_mainscaler(ctx);
+	fimc_hw_set_target_format(ctx);
+	fimc_hw_set_rotation(ctx);
+	fimc_hw_set_effect(ctx);
+	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
+	fimc_hw_set_out_dma(ctx);
+	if (fimc->variant->has_alpha)
+		fimc_hw_set_rgb_alpha(ctx);
+
+	clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
 	return ret;
 }
 
+void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
+{
+	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_vid_buffer *v_buf;
+	struct timeval *tv;
+	struct timespec ts;
+
+	if (test_and_clear_bit(ST_CAPT_SHUT, &fimc->state)) {
+		wake_up(&fimc->irq_queue);
+		goto done;
+	}
+
+	if (!list_empty(&cap->active_buf_q) &&
+	    test_bit(ST_CAPT_RUN, &fimc->state) && deq_buf) {
+		ktime_get_real_ts(&ts);
+
+		v_buf = fimc_active_queue_pop(cap);
+
+		tv = &v_buf->vb.v4l2_buf.timestamp;
+		tv->tv_sec = ts.tv_sec;
+		tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+		v_buf->vb.v4l2_buf.sequence = cap->frame_count++;
+
+		vb2_buffer_done(&v_buf->vb, VB2_BUF_STATE_DONE);
+	}
+
+	if (!list_empty(&cap->pending_buf_q)) {
+
+		v_buf = fimc_pending_queue_pop(cap);
+		fimc_hw_set_output_addr(fimc, &v_buf->paddr, cap->buf_index);
+		v_buf->index = cap->buf_index;
+
+		/* Move the buffer to the capture active queue */
+		fimc_active_queue_add(cap, v_buf);
+
+		dbg("next frame: %d, done frame: %d",
+		    fimc_hw_get_frame_index(fimc), v_buf->index);
+
+		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			cap->buf_index = 0;
+	}
+
+	if (cap->active_buf_cnt == 0) {
+		if (deq_buf)
+			clear_bit(ST_CAPT_RUN, &fimc->state);
+
+		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			cap->buf_index = 0;
+	} else {
+		set_bit(ST_CAPT_RUN, &fimc->state);
+	}
+
+	if (test_bit(ST_CAPT_APPLY_CFG, &fimc->state))
+		fimc_capture_config_update(cap->ctx);
+done:
+	if (cap->active_buf_cnt == 1) {
+		fimc_deactivate_capture(fimc);
+		clear_bit(ST_CAPT_STREAM, &fimc->state);
+	}
+
+	dbg("frame: %d, active_buf_cnt: %d",
+	    fimc_hw_get_frame_index(fimc), cap->active_buf_cnt);
+}
+
+
 static int start_streaming(struct vb2_queue *q, unsigned int count)
 {
 	struct fimc_ctx *ctx = q->drv_priv;
@@ -174,9 +249,11 @@ static int start_streaming(struct vb2_queue *q, unsigned int count)
 
 	vid_cap->frame_count = 0;
 
-	ret = fimc_init_capture(fimc);
-	if (ret)
-		goto error;
+	ret = fimc_capture_hw_init(fimc);
+	if (ret) {
+		fimc_capture_state_cleanup(fimc, false);
+		return ret;
+	}
 
 	set_bit(ST_CAPT_PEND, &fimc->state);
 
@@ -187,13 +264,10 @@ static int start_streaming(struct vb2_queue *q, unsigned int count)
 		fimc_activate_capture(ctx);
 
 		if (!test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
-			fimc_pipeline_s_stream(fimc, 1);
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
 	}
 
 	return 0;
-error:
-	fimc_capture_state_cleanup(fimc, false);
-	return ret;
 }
 
 static int stop_streaming(struct vb2_queue *q)
@@ -214,7 +288,7 @@ int fimc_capture_suspend(struct fimc_dev *fimc)
 	int ret = fimc_stop_capture(fimc, suspend);
 	if (ret)
 		return ret;
-	return fimc_pipeline_shutdown(fimc);
+	return fimc_pipeline_shutdown(&fimc->pipeline);
 }
 
 static void buffer_queue(struct vb2_buffer *vb);
@@ -230,9 +304,9 @@ int fimc_capture_resume(struct fimc_dev *fimc)
 
 	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
 	vid_cap->buf_index = 0;
-	fimc_pipeline_initialize(fimc, &fimc->vid_cap.vfd->entity,
+	fimc_pipeline_initialize(&fimc->pipeline, &vid_cap->vfd->entity,
 				 false);
-	fimc_init_capture(fimc);
+	fimc_capture_hw_init(fimc);
 
 	clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
 
@@ -347,7 +421,7 @@ static void buffer_queue(struct vb2_buffer *vb)
 		spin_unlock_irqrestore(&fimc->slock, flags);
 
 		if (!test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
-			fimc_pipeline_s_stream(fimc, 1);
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
 		return;
 	}
 	spin_unlock_irqrestore(&fimc->slock, flags);
@@ -389,15 +463,15 @@ int fimc_capture_ctrls_create(struct fimc_dev *fimc)
 
 	if (WARN_ON(vid_cap->ctx == NULL))
 		return -ENXIO;
-	if (vid_cap->ctx->ctrls_rdy)
+	if (vid_cap->ctx->ctrls.ready)
 		return 0;
 
 	ret = fimc_ctrls_create(vid_cap->ctx);
-	if (ret || vid_cap->user_subdev_api)
+	if (ret || vid_cap->user_subdev_api || !vid_cap->ctx->ctrls.ready)
 		return ret;
 
-	return v4l2_ctrl_add_handler(&vid_cap->ctx->ctrl_handler,
-				    fimc->pipeline.sensor->ctrl_handler);
+	return v4l2_ctrl_add_handler(&vid_cap->ctx->ctrls.handler,
+		    fimc->pipeline.subdevs[IDX_SENSOR]->ctrl_handler);
 }
 
 static int fimc_capture_set_default_format(struct fimc_dev *fimc);
@@ -420,7 +494,7 @@ static int fimc_capture_open(struct file *file)
 	pm_runtime_get_sync(&fimc->pdev->dev);
 
 	if (++fimc->vid_cap.refcnt == 1) {
-		ret = fimc_pipeline_initialize(fimc,
+		ret = fimc_pipeline_initialize(&fimc->pipeline,
 			       &fimc->vid_cap.vfd->entity, true);
 		if (ret < 0) {
 			dev_err(&fimc->pdev->dev,
@@ -448,7 +522,7 @@ static int fimc_capture_close(struct file *file)
 	if (--fimc->vid_cap.refcnt == 0) {
 		clear_bit(ST_CAPT_BUSY, &fimc->state);
 		fimc_stop_capture(fimc, false);
-		fimc_pipeline_shutdown(fimc);
+		fimc_pipeline_shutdown(&fimc->pipeline);
 		clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
 	}
 
@@ -495,7 +569,7 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 {
 	bool rotation = ctx->rotation == 90 || ctx->rotation == 270;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *var = fimc->variant;
+	struct fimc_variant *var = fimc->variant;
 	struct fimc_pix_limit *pl = var->pix_limit;
 	struct fimc_frame *dst = &ctx->d_frame;
 	u32 depth, min_w, max_w, min_h, align_h = 3;
@@ -537,8 +611,13 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 	}
 	/* Apply the scaler and the output DMA constraints */
 	max_w = rotation ? pl->out_rot_en_w : pl->out_rot_dis_w;
-	min_w = ctx->state & FIMC_DST_CROP ? dst->width : var->min_out_pixsize;
-	min_h = ctx->state & FIMC_DST_CROP ? dst->height : var->min_out_pixsize;
+	if (ctx->state & FIMC_COMPOSE) {
+		min_w = dst->offs_h + dst->width;
+		min_h = dst->offs_v + dst->height;
+	} else {
+		min_w = var->min_out_pixsize;
+		min_h = var->min_out_pixsize;
+	}
 	if (var->min_vsize_align == 1 && !rotation)
 		align_h = fimc_fmt_is_rgb(ffmt->color) ? 0 : 1;
 
@@ -556,12 +635,13 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 	return ffmt;
 }
 
-static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
-				  int pad)
+static void fimc_capture_try_selection(struct fimc_ctx *ctx,
+				       struct v4l2_rect *r,
+				       int target)
 {
 	bool rotate = ctx->rotation == 90 || ctx->rotation == 270;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *var = fimc->variant;
+	struct fimc_variant *var = fimc->variant;
 	struct fimc_pix_limit *pl = var->pix_limit;
 	struct fimc_frame *sink = &ctx->s_frame;
 	u32 max_w, max_h, min_w = 0, min_h = 0, min_sz;
@@ -575,7 +655,7 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 		r->left   = r->top = 0;
 		return;
 	}
-	if (pad == FIMC_SD_PAD_SOURCE) {
+	if (target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
 		if (ctx->rotation != 90 && ctx->rotation != 270)
 			align_h = 1;
 		max_sc_h = min(SCALER_MAX_HRATIO, 1 << (ffs(sink->width) - 3));
@@ -589,8 +669,7 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 		max_sc_h = max_sc_v = 1;
 	}
 	/*
-	 * For the crop rectangle at source pad the following constraints
-	 * must be met:
+	 * For the compose rectangle the following constraints must be met:
 	 * - it must fit in the sink pad format rectangle (f_width/f_height);
 	 * - maximum downscaling ratio is 64;
 	 * - maximum crop size depends if the rotator is used or not;
@@ -602,7 +681,8 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 		      rotate ? pl->out_rot_en_w : pl->out_rot_dis_w,
 		      rotate ? sink->f_height : sink->f_width);
 	max_h = min_t(u32, FIMC_CAMIF_MAX_HEIGHT, sink->f_height);
-	if (pad == FIMC_SD_PAD_SOURCE) {
+
+	if (target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
 		min_w = min_t(u32, max_w, sink->f_width / max_sc_h);
 		min_h = min_t(u32, max_h, sink->f_height / max_sc_v);
 		if (rotate) {
@@ -613,13 +693,13 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 	v4l_bound_align_image(&r->width, min_w, max_w, ffs(min_sz) - 1,
 			      &r->height, min_h, max_h, align_h,
 			      align_sz);
-	/* Adjust left/top if cropping rectangle is out of bounds */
+	/* Adjust left/top if crop/compose rectangle is out of bounds */
 	r->left = clamp_t(u32, r->left, 0, sink->f_width - r->width);
 	r->top  = clamp_t(u32, r->top, 0, sink->f_height - r->height);
 	r->left = round_down(r->left, var->hor_offs_align);
 
-	dbg("pad%d: (%d,%d)/%dx%d, sink fmt: %dx%d",
-	    pad, r->left, r->top, r->width, r->height,
+	dbg("target %#x: (%d,%d)/%dx%d, sink fmt: %dx%d",
+	    target, r->left, r->top, r->width, r->height,
 	    sink->f_width, sink->f_height);
 }
 
@@ -669,8 +749,8 @@ static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
 				    bool set)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct v4l2_subdev *sd = fimc->pipeline.sensor;
-	struct v4l2_subdev *csis = fimc->pipeline.csis;
+	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
+	struct v4l2_subdev *csis = fimc->pipeline.subdevs[IDX_CSIS];
 	struct v4l2_subdev_format sfmt;
 	struct v4l2_mbus_framefmt *mf = &sfmt.format;
 	struct fimc_fmt *ffmt = NULL;
@@ -851,7 +931,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
 
 	set_frame_bounds(ff, pix->width, pix->height);
 	/* Reset the composition rectangle if not yet configured */
-	if (!(ctx->state & FIMC_DST_CROP))
+	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));
@@ -878,7 +958,7 @@ static int fimc_cap_enum_input(struct file *file, void *priv,
 			       struct v4l2_input *i)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
-	struct v4l2_subdev *sd = fimc->pipeline.sensor;
+	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
 
 	if (i->index != 0)
 		return -EINVAL;
@@ -927,7 +1007,7 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
 			break;
 		/* Don't call FIMC subdev operation to avoid nested locking */
-		if (sd == fimc->vid_cap.subdev) {
+		if (sd == &fimc->vid_cap.subdev) {
 			struct fimc_frame *ff = &vid_cap->ctx->s_frame;
 			sink_fmt.format.width = ff->f_width;
 			sink_fmt.format.height = ff->f_height;
@@ -970,7 +1050,8 @@ static int fimc_cap_streamon(struct file *file, void *priv,
 	if (fimc_capture_active(fimc))
 		return -EBUSY;
 
-	media_entity_pipeline_start(&p->sensor->entity, p->pipe);
+	media_entity_pipeline_start(&p->subdevs[IDX_SENSOR]->entity,
+				    p->m_pipeline);
 
 	if (fimc->vid_cap.user_subdev_api) {
 		ret = fimc_pipeline_validate(fimc);
@@ -984,7 +1065,7 @@ static int fimc_cap_streamoff(struct file *file, void *priv,
 			    enum v4l2_buf_type type)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
-	struct v4l2_subdev *sd = fimc->pipeline.sensor;
+	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
 	int ret;
 
 	ret = vb2_streamoff(&fimc->vid_cap.vbq, type);
@@ -1100,29 +1181,18 @@ static int fimc_cap_s_selection(struct file *file, void *fh,
 	struct v4l2_rect rect = s->r;
 	struct fimc_frame *f;
 	unsigned long flags;
-	unsigned int pad;
 
 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 
-	switch (s->target) {
-	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
-	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
-	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+	if (s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE)
 		f = &ctx->d_frame;
-		pad = FIMC_SD_PAD_SOURCE;
-		break;
-	case V4L2_SEL_TGT_CROP_BOUNDS:
-	case V4L2_SEL_TGT_CROP_DEFAULT:
-	case V4L2_SEL_TGT_CROP_ACTIVE:
+	else if (s->target == V4L2_SEL_TGT_CROP_ACTIVE)
 		f = &ctx->s_frame;
-		pad = FIMC_SD_PAD_SINK;
-		break;
-	default:
+	else
 		return -EINVAL;
-	}
 
-	fimc_capture_try_crop(ctx, &rect, pad);
+	fimc_capture_try_selection(ctx, &rect, s->target);
 
 	if (s->flags & V4L2_SEL_FLAG_LE &&
 	    !enclosed_rectangle(&rect, &s->r))
@@ -1243,7 +1313,7 @@ void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 					 struct fimc_vid_buffer, list);
 			vb2_set_plane_payload(&buf->vb, 0, *((u32 *)arg));
 		}
-		fimc_capture_irq_handler(fimc, true);
+		fimc_capture_irq_handler(fimc, 1);
 		fimc_deactivate_capture(fimc);
 		spin_unlock_irqrestore(&fimc->slock, irq_flags);
 	}
@@ -1334,77 +1404,122 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
 	ff->fmt = ffmt;
 
 	/* Reset the crop rectangle if required. */
-	if (!(fmt->pad == FIMC_SD_PAD_SOURCE && (ctx->state & FIMC_DST_CROP)))
+	if (!(fmt->pad == FIMC_SD_PAD_SOURCE && (ctx->state & FIMC_COMPOSE)))
 		set_frame_crop(ff, 0, 0, mf->width, mf->height);
 
 	if (fmt->pad == FIMC_SD_PAD_SINK)
-		ctx->state &= ~FIMC_DST_CROP;
+		ctx->state &= ~FIMC_COMPOSE;
 	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
-static int fimc_subdev_get_crop(struct v4l2_subdev *sd,
-				struct v4l2_subdev_fh *fh,
-				struct v4l2_subdev_crop *crop)
+static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_fh *fh,
+				     struct v4l2_subdev_selection *sel)
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
-	struct v4l2_rect *r = &crop->rect;
-	struct fimc_frame *ff;
+	struct fimc_frame *f = &ctx->s_frame;
+	struct v4l2_rect *r = &sel->r;
+	struct v4l2_rect *try_sel;
 
-	if (crop->which == V4L2_SUBDEV_FORMAT_TRY) {
-		crop->rect = *v4l2_subdev_get_try_crop(fh, crop->pad);
+	if (sel->pad != FIMC_SD_PAD_SINK)
+		return -EINVAL;
+
+	mutex_lock(&fimc->lock);
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		f = &ctx->d_frame;
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		r->width = f->o_width;
+		r->height = f->o_height;
+		r->left = 0;
+		r->top = 0;
+		mutex_unlock(&fimc->lock);
 		return 0;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		try_sel = v4l2_subdev_get_try_crop(fh, sel->pad);
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		try_sel = v4l2_subdev_get_try_compose(fh, sel->pad);
+		f = &ctx->d_frame;
+		break;
+	default:
+		mutex_unlock(&fimc->lock);
+		return -EINVAL;
 	}
-	ff = crop->pad == FIMC_SD_PAD_SINK ?
-		&ctx->s_frame : &ctx->d_frame;
 
-	mutex_lock(&fimc->lock);
-	r->left	  = ff->offs_h;
-	r->top	  = ff->offs_v;
-	r->width  = ff->width;
-	r->height = ff->height;
-	mutex_unlock(&fimc->lock);
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sel->r = *try_sel;
+	} else {
+		r->left = f->offs_h;
+		r->top = f->offs_v;
+		r->width = f->width;
+		r->height = f->height;
+	}
 
-	dbg("ff:%p, pad%d: l:%d, t:%d, %dx%d, f_w: %d, f_h: %d",
-	    ff, crop->pad, r->left, r->top, r->width, r->height,
-	    ff->f_width, ff->f_height);
+	dbg("target %#x: l:%d, t:%d, %dx%d, f_w: %d, f_h: %d",
+	    sel->pad, r->left, r->top, r->width, r->height,
+	    f->f_width, f->f_height);
 
+	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
-static int fimc_subdev_set_crop(struct v4l2_subdev *sd,
-				struct v4l2_subdev_fh *fh,
-				struct v4l2_subdev_crop *crop)
+static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_fh *fh,
+				     struct v4l2_subdev_selection *sel)
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
-	struct v4l2_rect *r = &crop->rect;
-	struct fimc_frame *ff;
+	struct fimc_frame *f = &ctx->s_frame;
+	struct v4l2_rect *r = &sel->r;
+	struct v4l2_rect *try_sel;
 	unsigned long flags;
 
-	dbg("(%d,%d)/%dx%d", r->left, r->top, r->width, r->height);
-
-	ff = crop->pad == FIMC_SD_PAD_SOURCE ?
-		&ctx->d_frame : &ctx->s_frame;
+	if (sel->pad != FIMC_SD_PAD_SINK)
+		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
-	fimc_capture_try_crop(ctx, r, crop->pad);
+	fimc_capture_try_selection(ctx, r, V4L2_SEL_TGT_CROP_ACTIVE);
 
-	if (crop->which == V4L2_SUBDEV_FORMAT_TRY) {
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		f = &ctx->d_frame;
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		r->width = f->o_width;
+		r->height = f->o_height;
+		r->left = 0;
+		r->top = 0;
 		mutex_unlock(&fimc->lock);
-		*v4l2_subdev_get_try_crop(fh, crop->pad) = *r;
 		return 0;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		try_sel = v4l2_subdev_get_try_crop(fh, sel->pad);
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		try_sel = v4l2_subdev_get_try_compose(fh, sel->pad);
+		f = &ctx->d_frame;
+		break;
+	default:
+		mutex_unlock(&fimc->lock);
+		return -EINVAL;
 	}
-	spin_lock_irqsave(&fimc->slock, flags);
-	set_frame_crop(ff, r->left, r->top, r->width, r->height);
-	if (crop->pad == FIMC_SD_PAD_SOURCE)
-		ctx->state |= FIMC_DST_CROP;
 
-	set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
-	spin_unlock_irqrestore(&fimc->slock, flags);
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*try_sel = sel->r;
+	} else {
+		spin_lock_irqsave(&fimc->slock, flags);
+		set_frame_crop(f, r->left, r->top, r->width, r->height);
+		set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		if (sel->target == V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL)
+			ctx->state |= FIMC_COMPOSE;
+	}
 
-	dbg("pad%d: (%d,%d)/%dx%d", crop->pad, r->left, r->top,
+	dbg("target %#x: (%d,%d)/%dx%d", sel->target, r->left, r->top,
 	    r->width, r->height);
 
 	mutex_unlock(&fimc->lock);
@@ -1413,63 +1528,16 @@ static int fimc_subdev_set_crop(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_pad_ops fimc_subdev_pad_ops = {
 	.enum_mbus_code = fimc_subdev_enum_mbus_code,
+	.get_selection = fimc_subdev_get_selection,
+	.set_selection = fimc_subdev_set_selection,
 	.get_fmt = fimc_subdev_get_fmt,
 	.set_fmt = fimc_subdev_set_fmt,
-	.get_crop = fimc_subdev_get_crop,
-	.set_crop = fimc_subdev_set_crop,
 };
 
 static struct v4l2_subdev_ops fimc_subdev_ops = {
 	.pad = &fimc_subdev_pad_ops,
 };
 
-static int fimc_create_capture_subdev(struct fimc_dev *fimc,
-				      struct v4l2_device *v4l2_dev)
-{
-	struct v4l2_subdev *sd;
-	int ret;
-
-	sd = kzalloc(sizeof(*sd), GFP_KERNEL);
-	if (!sd)
-		return -ENOMEM;
-
-	v4l2_subdev_init(sd, &fimc_subdev_ops);
-	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
-	snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->pdev->id);
-
-	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
-	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
-				fimc->vid_cap.sd_pads, 0);
-	if (ret)
-		goto me_err;
-	ret = v4l2_device_register_subdev(v4l2_dev, sd);
-	if (ret)
-		goto sd_err;
-
-	fimc->vid_cap.subdev = sd;
-	v4l2_set_subdevdata(sd, fimc);
-	sd->entity.ops = &fimc_sd_media_ops;
-	return 0;
-sd_err:
-	media_entity_cleanup(&sd->entity);
-me_err:
-	kfree(sd);
-	return ret;
-}
-
-static void fimc_destroy_capture_subdev(struct fimc_dev *fimc)
-{
-	struct v4l2_subdev *sd = fimc->vid_cap.subdev;
-
-	if (!sd)
-		return;
-	media_entity_cleanup(&sd->entity);
-	v4l2_device_unregister_subdev(sd);
-	kfree(sd);
-	fimc->vid_cap.subdev = NULL;
-}
-
 /* Set default format at the sensor and host interface */
 static int fimc_capture_set_default_format(struct fimc_dev *fimc)
 {
@@ -1488,7 +1556,7 @@ static int fimc_capture_set_default_format(struct fimc_dev *fimc)
 }
 
 /* fimc->lock must be already initialized */
-int fimc_register_capture_device(struct fimc_dev *fimc,
+static int fimc_register_capture_device(struct fimc_dev *fimc,
 				 struct v4l2_device *v4l2_dev)
 {
 	struct video_device *vfd;
@@ -1502,11 +1570,11 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 		return -ENOMEM;
 
 	ctx->fimc_dev	 = fimc;
-	ctx->in_path	 = FIMC_CAMERA;
-	ctx->out_path	 = FIMC_DMA;
+	ctx->in_path	 = FIMC_IO_CAMERA;
+	ctx->out_path	 = FIMC_IO_DMA;
 	ctx->state	 = FIMC_CTX_CAP;
 	ctx->s_frame.fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
-	ctx->d_frame.fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
+	ctx->d_frame.fmt = ctx->s_frame.fmt;
 
 	vfd = video_device_alloc();
 	if (!vfd) {
@@ -1514,8 +1582,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 		goto err_vd_alloc;
 	}
 
-	snprintf(vfd->name, sizeof(vfd->name), "%s.capture",
-		 dev_name(&fimc->pdev->dev));
+	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.capture", fimc->id);
 
 	vfd->fops	= &fimc_capture_fops;
 	vfd->ioctl_ops	= &fimc_capture_ioctl_ops;
@@ -1523,6 +1590,10 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 	vfd->minor	= -1;
 	vfd->release	= video_device_release;
 	vfd->lock	= &fimc->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	video_set_drvdata(vfd, fimc);
 
 	vid_cap = &fimc->vid_cap;
@@ -1533,7 +1604,6 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 
 	INIT_LIST_HEAD(&vid_cap->pending_buf_q);
 	INIT_LIST_HEAD(&vid_cap->active_buf_q);
-	spin_lock_init(&ctx->slock);
 	vid_cap->ctx = ctx;
 
 	q = &fimc->vid_cap.vbq;
@@ -1547,18 +1617,22 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 
 	vb2_queue_init(q);
 
-	fimc->vid_cap.vd_pad.flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&vfd->entity, 1, &fimc->vid_cap.vd_pad, 0);
+	vid_cap->vd_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vfd->entity, 1, &vid_cap->vd_pad, 0);
 	if (ret)
 		goto err_ent;
-	ret = fimc_create_capture_subdev(fimc, v4l2_dev);
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 	if (ret)
-		goto err_sd_reg;
+		goto err_vd;
 
-	vfd->ctrl_handler = &ctx->ctrl_handler;
+	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+
+	vfd->ctrl_handler = &ctx->ctrls.handler;
 	return 0;
 
-err_sd_reg:
+err_vd:
 	media_entity_cleanup(&vfd->entity);
 err_ent:
 	video_device_release(vfd);
@@ -1567,17 +1641,73 @@ err_vd_alloc:
 	return ret;
 }
 
-void fimc_unregister_capture_device(struct fimc_dev *fimc)
+static int fimc_capture_subdev_registered(struct v4l2_subdev *sd)
 {
-	struct video_device *vfd = fimc->vid_cap.vfd;
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = fimc_register_m2m_device(fimc, sd->v4l2_dev);
+	if (ret)
+		return ret;
 
-	if (vfd) {
-		media_entity_cleanup(&vfd->entity);
-		/* Can also be called if video device was
-		   not registered */
-		video_unregister_device(vfd);
+	ret = fimc_register_capture_device(fimc, sd->v4l2_dev);
+	if (ret)
+		fimc_unregister_m2m_device(fimc);
+
+	return ret;
+}
+
+static void fimc_capture_subdev_unregistered(struct v4l2_subdev *sd)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc == NULL)
+		return;
+
+	fimc_unregister_m2m_device(fimc);
+
+	if (fimc->vid_cap.vfd) {
+		media_entity_cleanup(&fimc->vid_cap.vfd->entity);
+		video_unregister_device(fimc->vid_cap.vfd);
+		fimc->vid_cap.vfd = NULL;
 	}
-	fimc_destroy_capture_subdev(fimc);
+
 	kfree(fimc->vid_cap.ctx);
 	fimc->vid_cap.ctx = NULL;
 }
+
+static const struct v4l2_subdev_internal_ops fimc_capture_sd_internal_ops = {
+	.registered = fimc_capture_subdev_registered,
+	.unregistered = fimc_capture_subdev_unregistered,
+};
+
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+	int ret;
+
+	v4l2_subdev_init(sd, &fimc_subdev_ops);
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->pdev->id);
+
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
+				fimc->vid_cap.sd_pads, 0);
+	if (ret)
+		return ret;
+
+	sd->entity.ops = &fimc_sd_media_ops;
+	sd->internal_ops = &fimc_capture_sd_internal_ops;
+	v4l2_set_subdevdata(sd, fimc);
+	return 0;
+}
+
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_set_subdevdata(sd, NULL);
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index e09ba7b0076e..fedcd561ba27 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -1,8 +1,8 @@
 /*
- * Samsung S5P/EXYNOS4 SoC series camera interface (video postprocessor) driver
+ * Samsung S5P/EXYNOS4 SoC series FIMC (CAMIF) driver
  *
- * Copyright (C) 2010-2011 Samsung Electronics Co., Ltd.
- * Contact: Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Copyright (C) 2010-2012 Samsung Electronics Co., Ltd.
+ * 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 as published
@@ -28,6 +28,7 @@
 #include <media/videobuf2-dma-contig.h>
 
 #include "fimc-core.h"
+#include "fimc-reg.h"
 #include "fimc-mdevice.h"
 
 static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
@@ -39,7 +40,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "RGB565",
 		.fourcc		= V4L2_PIX_FMT_RGB565,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_RGB565,
+		.color		= FIMC_FMT_RGB565,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M,
@@ -47,7 +48,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "BGR666",
 		.fourcc		= V4L2_PIX_FMT_BGR666,
 		.depth		= { 32 },
-		.color		= S5P_FIMC_RGB666,
+		.color		= FIMC_FMT_RGB666,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M,
@@ -55,7 +56,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "ARGB8888, 32 bpp",
 		.fourcc		= V4L2_PIX_FMT_RGB32,
 		.depth		= { 32 },
-		.color		= S5P_FIMC_RGB888,
+		.color		= FIMC_FMT_RGB888,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M | FMT_HAS_ALPHA,
@@ -63,7 +64,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "ARGB1555",
 		.fourcc		= V4L2_PIX_FMT_RGB555,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_RGB555,
+		.color		= FIMC_FMT_RGB555,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
@@ -71,7 +72,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "ARGB4444",
 		.fourcc		= V4L2_PIX_FMT_RGB444,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_RGB444,
+		.color		= FIMC_FMT_RGB444,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
@@ -79,7 +80,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, YCbYCr",
 		.fourcc		= V4L2_PIX_FMT_YUYV,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCBYCR422,
+		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
@@ -88,7 +89,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, CbYCrY",
 		.fourcc		= V4L2_PIX_FMT_UYVY,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_CBYCRY422,
+		.color		= FIMC_FMT_CBYCRY422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
@@ -97,7 +98,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, CrYCbY",
 		.fourcc		= V4L2_PIX_FMT_VYUY,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_CRYCBY422,
+		.color		= FIMC_FMT_CRYCBY422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
@@ -106,7 +107,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, YCrYCb",
 		.fourcc		= V4L2_PIX_FMT_YVYU,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCRYCB422,
+		.color		= FIMC_FMT_YCRYCB422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
@@ -115,7 +116,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
 		.fourcc		= V4L2_PIX_FMT_YUV422P,
 		.depth		= { 12 },
-		.color		= S5P_FIMC_YCBYCR422,
+		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 3,
 		.flags		= FMT_FLAGS_M2M,
@@ -123,7 +124,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 planar, Y/CbCr",
 		.fourcc		= V4L2_PIX_FMT_NV16,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCBYCR422,
+		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 2,
 		.flags		= FMT_FLAGS_M2M,
@@ -131,7 +132,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 planar, Y/CrCb",
 		.fourcc		= V4L2_PIX_FMT_NV61,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCRYCB422,
+		.color		= FIMC_FMT_YCRYCB422,
 		.memplanes	= 1,
 		.colplanes	= 2,
 		.flags		= FMT_FLAGS_M2M,
@@ -139,7 +140,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:0 planar, YCbCr",
 		.fourcc		= V4L2_PIX_FMT_YUV420,
 		.depth		= { 12 },
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.memplanes	= 1,
 		.colplanes	= 3,
 		.flags		= FMT_FLAGS_M2M,
@@ -147,14 +148,14 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:0 planar, Y/CbCr",
 		.fourcc		= V4L2_PIX_FMT_NV12,
 		.depth		= { 12 },
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.memplanes	= 1,
 		.colplanes	= 2,
 		.flags		= FMT_FLAGS_M2M,
 	}, {
 		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr",
 		.fourcc		= V4L2_PIX_FMT_NV12M,
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.depth		= { 8, 4 },
 		.memplanes	= 2,
 		.colplanes	= 2,
@@ -162,7 +163,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "YUV 4:2:0 non-contiguous 3-planar, Y/Cb/Cr",
 		.fourcc		= V4L2_PIX_FMT_YUV420M,
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.depth		= { 8, 2, 2 },
 		.memplanes	= 3,
 		.colplanes	= 3,
@@ -170,7 +171,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr, tiled",
 		.fourcc		= V4L2_PIX_FMT_NV12MT,
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.depth		= { 8, 4 },
 		.memplanes	= 2,
 		.colplanes	= 2,
@@ -178,7 +179,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "JPEG encoded data",
 		.fourcc		= V4L2_PIX_FMT_JPEG,
-		.color		= S5P_FIMC_JPEG,
+		.color		= FIMC_FMT_JPEG,
 		.depth		= { 8 },
 		.memplanes	= 1,
 		.colplanes	= 1,
@@ -187,12 +188,12 @@ static struct fimc_fmt fimc_formats[] = {
 	},
 };
 
-static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
+struct fimc_fmt *fimc_get_format(unsigned int index)
 {
-	if (stream_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		return FMT_FLAGS_M2M_IN;
-	else
-		return FMT_FLAGS_M2M_OUT;
+	if (index >= ARRAY_SIZE(fimc_formats))
+		return NULL;
+
+	return &fimc_formats[index];
 }
 
 int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
@@ -230,7 +231,7 @@ static int fimc_get_scaler_factor(u32 src, u32 tar, u32 *ratio, u32 *shift)
 
 int fimc_set_scaler_info(struct fimc_ctx *ctx)
 {
-	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	struct fimc_variant *variant = ctx->fimc_dev->variant;
 	struct device *dev = &ctx->fimc_dev->pdev->dev;
 	struct fimc_scaler *sc = &ctx->scaler;
 	struct fimc_frame *s_frame = &ctx->s_frame;
@@ -293,126 +294,9 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	return 0;
 }
 
-static void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
-{
-	struct vb2_buffer *src_vb, *dst_vb;
-
-	if (!ctx || !ctx->m2m_ctx)
-		return;
-
-	src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
-	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
-
-	if (src_vb && dst_vb) {
-		v4l2_m2m_buf_done(src_vb, vb_state);
-		v4l2_m2m_buf_done(dst_vb, vb_state);
-		v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
-				    ctx->m2m_ctx);
-	}
-}
-
-/* Complete the transaction which has been scheduled for execution. */
-static int fimc_m2m_shutdown(struct fimc_ctx *ctx)
-{
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret;
-
-	if (!fimc_m2m_pending(fimc))
-		return 0;
-
-	fimc_ctx_state_lock_set(FIMC_CTX_SHUT, ctx);
-
-	ret = wait_event_timeout(fimc->irq_queue,
-			   !fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
-			   FIMC_SHUTDOWN_TIMEOUT);
-
-	return ret == 0 ? -ETIMEDOUT : ret;
-}
-
-static int start_streaming(struct vb2_queue *q, unsigned int count)
-{
-	struct fimc_ctx *ctx = q->drv_priv;
-	int ret;
-
-	ret = pm_runtime_get_sync(&ctx->fimc_dev->pdev->dev);
-	return ret > 0 ? 0 : ret;
-}
-
-static int stop_streaming(struct vb2_queue *q)
-{
-	struct fimc_ctx *ctx = q->drv_priv;
-	int ret;
-
-	ret = fimc_m2m_shutdown(ctx);
-	if (ret == -ETIMEDOUT)
-		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
-
-	pm_runtime_put(&ctx->fimc_dev->pdev->dev);
-	return 0;
-}
-
-void fimc_capture_irq_handler(struct fimc_dev *fimc, bool final)
-{
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	struct fimc_vid_buffer *v_buf;
-	struct timeval *tv;
-	struct timespec ts;
-
-	if (test_and_clear_bit(ST_CAPT_SHUT, &fimc->state)) {
-		wake_up(&fimc->irq_queue);
-		return;
-	}
-
-	if (!list_empty(&cap->active_buf_q) &&
-	    test_bit(ST_CAPT_RUN, &fimc->state) && final) {
-		ktime_get_real_ts(&ts);
-
-		v_buf = fimc_active_queue_pop(cap);
-
-		tv = &v_buf->vb.v4l2_buf.timestamp;
-		tv->tv_sec = ts.tv_sec;
-		tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
-		v_buf->vb.v4l2_buf.sequence = cap->frame_count++;
-
-		vb2_buffer_done(&v_buf->vb, VB2_BUF_STATE_DONE);
-	}
-
-	if (!list_empty(&cap->pending_buf_q)) {
-
-		v_buf = fimc_pending_queue_pop(cap);
-		fimc_hw_set_output_addr(fimc, &v_buf->paddr, cap->buf_index);
-		v_buf->index = cap->buf_index;
-
-		/* Move the buffer to the capture active queue */
-		fimc_active_queue_add(cap, v_buf);
-
-		dbg("next frame: %d, done frame: %d",
-		    fimc_hw_get_frame_index(fimc), v_buf->index);
-
-		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
-			cap->buf_index = 0;
-	}
-
-	if (cap->active_buf_cnt == 0) {
-		if (final)
-			clear_bit(ST_CAPT_RUN, &fimc->state);
-
-		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
-			cap->buf_index = 0;
-	} else {
-		set_bit(ST_CAPT_RUN, &fimc->state);
-	}
-
-	fimc_capture_config_update(cap->ctx);
-
-	dbg("frame: %d, active_buf_cnt: %d",
-	    fimc_hw_get_frame_index(fimc), cap->active_buf_cnt);
-}
-
 static irqreturn_t fimc_irq_handler(int irq, void *priv)
 {
 	struct fimc_dev *fimc = priv;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
 	struct fimc_ctx *ctx;
 
 	fimc_hw_clear_irq(fimc);
@@ -430,21 +314,16 @@ static irqreturn_t fimc_irq_handler(int irq, void *priv)
 			spin_unlock(&fimc->slock);
 			fimc_m2m_job_finish(ctx, VB2_BUF_STATE_DONE);
 
-			spin_lock(&ctx->slock);
 			if (ctx->state & FIMC_CTX_SHUT) {
 				ctx->state &= ~FIMC_CTX_SHUT;
 				wake_up(&fimc->irq_queue);
 			}
-			spin_unlock(&ctx->slock);
+			return IRQ_HANDLED;
 		}
-		return IRQ_HANDLED;
 	} else if (test_bit(ST_CAPT_PEND, &fimc->state)) {
-		fimc_capture_irq_handler(fimc,
-				 !test_bit(ST_CAPT_JPEG, &fimc->state));
-		if (cap->active_buf_cnt == 1) {
-			fimc_deactivate_capture(fimc);
-			clear_bit(ST_CAPT_STREAM, &fimc->state);
-		}
+		int last_buf = test_bit(ST_CAPT_JPEG, &fimc->state) &&
+				fimc->vid_cap.reqbufs_count == 1;
+		fimc_capture_irq_handler(fimc, !last_buf);
 	}
 out:
 	spin_unlock(&fimc->slock);
@@ -482,7 +361,7 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 		case 3:
 			paddr->cb = (u32)(paddr->y + pix_size);
 			/* decompose Y into Y/Cb/Cr */
-			if (S5P_FIMC_YCBCR420 == frame->fmt->color)
+			if (FIMC_FMT_YCBCR420 == frame->fmt->color)
 				paddr->cr = (u32)(paddr->cb
 						+ (pix_size >> 2));
 			else /* 422 */
@@ -510,40 +389,40 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 void fimc_set_yuv_order(struct fimc_ctx *ctx)
 {
 	/* The one only mode supported in SoC. */
-	ctx->in_order_2p = S5P_FIMC_LSB_CRCB;
-	ctx->out_order_2p = S5P_FIMC_LSB_CRCB;
+	ctx->in_order_2p = FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB;
+	ctx->out_order_2p = FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB;
 
 	/* Set order for 1 plane input formats. */
 	switch (ctx->s_frame.fmt->color) {
-	case S5P_FIMC_YCRYCB422:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CBYCRY;
+	case FIMC_FMT_YCRYCB422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CBYCRY;
 		break;
-	case S5P_FIMC_CBYCRY422:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCRYCB;
+	case FIMC_FMT_CBYCRY422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCRYCB;
 		break;
-	case S5P_FIMC_CRYCBY422:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCBYCR;
+	case FIMC_FMT_CRYCBY422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCBYCR;
 		break;
-	case S5P_FIMC_YCBYCR422:
+	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CRYCBY;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->in_order_1p= %d", ctx->in_order_1p);
 
 	switch (ctx->d_frame.fmt->color) {
-	case S5P_FIMC_YCRYCB422:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CBYCRY;
+	case FIMC_FMT_YCRYCB422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CBYCRY;
 		break;
-	case S5P_FIMC_CBYCRY422:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCRYCB;
+	case FIMC_FMT_CBYCRY422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCRYCB;
 		break;
-	case S5P_FIMC_CRYCBY422:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCBYCR;
+	case FIMC_FMT_CRYCBY422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCBYCR;
 		break;
-	case S5P_FIMC_YCBYCR422:
+	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CRYCBY;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->out_order_1p= %d", ctx->out_order_1p);
@@ -551,7 +430,7 @@ void fimc_set_yuv_order(struct fimc_ctx *ctx)
 
 void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 {
-	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	struct fimc_variant *variant = ctx->fimc_dev->variant;
 	u32 i, depth = 0;
 
 	for (i = 0; i < f->fmt->colplanes; i++)
@@ -574,7 +453,7 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 			f->dma_offset.cb_h >>= 1;
 			f->dma_offset.cr_h >>= 1;
 		}
-		if (f->fmt->color == S5P_FIMC_YCBCR420) {
+		if (f->fmt->color == FIMC_FMT_YCBCR420) {
 			f->dma_offset.cb_v >>= 1;
 			f->dma_offset.cr_v >>= 1;
 		}
@@ -584,203 +463,58 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	    f->fmt->color, f->dma_offset.y_h, f->dma_offset.y_v);
 }
 
-/**
- * fimc_prepare_config - check dimensions, operation and color mode
- *			 and pre-calculate offset and the scaling coefficients.
- *
- * @ctx: hardware context information
- * @flags: flags indicating which parameters to check/update
- *
- * Return: 0 if dimensions are valid or non zero otherwise.
- */
-int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags)
-{
-	struct fimc_frame *s_frame, *d_frame;
-	struct vb2_buffer *vb = NULL;
-	int ret = 0;
-
-	s_frame = &ctx->s_frame;
-	d_frame = &ctx->d_frame;
-
-	if (flags & FIMC_PARAMS) {
-		/* Prepare the DMA offset ratios for scaler. */
-		fimc_prepare_dma_offset(ctx, &ctx->s_frame);
-		fimc_prepare_dma_offset(ctx, &ctx->d_frame);
-
-		if (s_frame->height > (SCALER_MAX_VRATIO * d_frame->height) ||
-		    s_frame->width > (SCALER_MAX_HRATIO * d_frame->width)) {
-			err("out of scaler range");
-			return -EINVAL;
-		}
-		fimc_set_yuv_order(ctx);
-	}
-
-	if (flags & FIMC_SRC_ADDR) {
-		vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, vb, s_frame, &s_frame->paddr);
-		if (ret)
-			return ret;
-	}
-
-	if (flags & FIMC_DST_ADDR) {
-		vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, vb, d_frame, &d_frame->paddr);
-	}
-
-	return ret;
-}
-
-static void fimc_dma_run(void *priv)
-{
-	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc;
-	unsigned long flags;
-	u32 ret;
-
-	if (WARN(!ctx, "null hardware context\n"))
-		return;
-
-	fimc = ctx->fimc_dev;
-	spin_lock_irqsave(&fimc->slock, flags);
-	set_bit(ST_M2M_PEND, &fimc->state);
-
-	spin_lock(&ctx->slock);
-	ctx->state |= (FIMC_SRC_ADDR | FIMC_DST_ADDR);
-	ret = fimc_prepare_config(ctx, ctx->state);
-	if (ret)
-		goto dma_unlock;
-
-	/* Reconfigure hardware if the context has changed. */
-	if (fimc->m2m.ctx != ctx) {
-		ctx->state |= FIMC_PARAMS;
-		fimc->m2m.ctx = ctx;
-	}
-	fimc_hw_set_input_addr(fimc, &ctx->s_frame.paddr);
-
-	if (ctx->state & FIMC_PARAMS) {
-		fimc_hw_set_input_path(ctx);
-		fimc_hw_set_in_dma(ctx);
-		ret = fimc_set_scaler_info(ctx);
-		if (ret) {
-			spin_unlock(&fimc->slock);
-			goto dma_unlock;
-		}
-		fimc_hw_set_prescaler(ctx);
-		fimc_hw_set_mainscaler(ctx);
-		fimc_hw_set_target_format(ctx);
-		fimc_hw_set_rotation(ctx);
-		fimc_hw_set_effect(ctx, false);
-	}
-
-	fimc_hw_set_output_path(ctx);
-	if (ctx->state & (FIMC_DST_ADDR | FIMC_PARAMS))
-		fimc_hw_set_output_addr(fimc, &ctx->d_frame.paddr, -1);
-
-	if (ctx->state & FIMC_PARAMS) {
-		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
-			fimc_hw_set_rgb_alpha(ctx);
-	}
-
-	fimc_activate_capture(ctx);
-
-	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP |
-		       FIMC_SRC_FMT | FIMC_DST_FMT);
-	fimc_hw_activate_input_dma(fimc, true);
-dma_unlock:
-	spin_unlock(&ctx->slock);
-	spin_unlock_irqrestore(&fimc->slock, flags);
-}
-
-static void fimc_job_abort(void *priv)
+int fimc_set_color_effect(struct fimc_ctx *ctx, enum v4l2_colorfx colorfx)
 {
-	fimc_m2m_shutdown(priv);
-}
-
-static int fimc_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
-			    unsigned int *num_buffers, unsigned int *num_planes,
-			    unsigned int sizes[], void *allocators[])
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	struct fimc_frame *f;
-	int i;
+	struct fimc_effect *effect = &ctx->effect;
 
-	f = ctx_get_frame(ctx, vq->type);
-	if (IS_ERR(f))
-		return PTR_ERR(f);
-	/*
-	 * Return number of non-contigous planes (plane buffers)
-	 * depending on the configured color format.
-	 */
-	if (!f->fmt)
+	switch (colorfx) {
+	case V4L2_COLORFX_NONE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
+		break;
+	case V4L2_COLORFX_BW:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = 128;
+		effect->pat_cr = 128;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = 115;
+		effect->pat_cr = 145;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_NEGATIVE;
+		break;
+	case V4L2_COLORFX_EMBOSS:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_EMBOSSING;
+		break;
+	case V4L2_COLORFX_ART_FREEZE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARTFREEZE;
+		break;
+	case V4L2_COLORFX_SILHOUETTE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_SILHOUETTE;
+		break;
+	case V4L2_COLORFX_SET_CBCR:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = ctx->ctrls.colorfx_cbcr->val >> 8;
+		effect->pat_cr = ctx->ctrls.colorfx_cbcr->val & 0xff;
+		break;
+	default:
 		return -EINVAL;
-
-	*num_planes = f->fmt->memplanes;
-	for (i = 0; i < f->fmt->memplanes; i++) {
-		sizes[i] = (f->f_width * f->f_height * f->fmt->depth[i]) / 8;
-		allocators[i] = ctx->fimc_dev->alloc_ctx;
 	}
-	return 0;
-}
-
-static int fimc_buf_prepare(struct vb2_buffer *vb)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-	struct fimc_frame *frame;
-	int i;
-
-	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	for (i = 0; i < frame->fmt->memplanes; i++)
-		vb2_set_plane_payload(vb, i, frame->payload[i]);
 
 	return 0;
 }
 
-static void fimc_buf_queue(struct vb2_buffer *vb)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-
-	dbg("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
-
-	if (ctx->m2m_ctx)
-		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
-}
-
-static void fimc_lock(struct vb2_queue *vq)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	mutex_lock(&ctx->fimc_dev->lock);
-}
-
-static void fimc_unlock(struct vb2_queue *vq)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	mutex_unlock(&ctx->fimc_dev->lock);
-}
-
-static struct vb2_ops fimc_qops = {
-	.queue_setup	 = fimc_queue_setup,
-	.buf_prepare	 = fimc_buf_prepare,
-	.buf_queue	 = fimc_buf_queue,
-	.wait_prepare	 = fimc_unlock,
-	.wait_finish	 = fimc_lock,
-	.stop_streaming	 = stop_streaming,
-	.start_streaming = start_streaming,
-};
-
 /*
  * V4L2 controls handling
  */
 #define ctrl_to_ctx(__ctrl) \
-	container_of((__ctrl)->handler, struct fimc_ctx, ctrl_handler)
+	container_of((__ctrl)->handler, struct fimc_ctx, ctrls.handler)
 
 static int __fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_ctrl *ctrl)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *variant = fimc->variant;
+	struct fimc_variant *variant = fimc->variant;
 	unsigned int flags = FIMC_DST_FMT | FIMC_SRC_FMT;
 	int ret = 0;
 
@@ -815,7 +549,14 @@ static int __fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_ctrl *ctrl)
 	case V4L2_CID_ALPHA_COMPONENT:
 		ctx->d_frame.alpha = ctrl->val;
 		break;
+
+	case V4L2_CID_COLORFX:
+		ret = fimc_set_color_effect(ctx, ctrl->val);
+		if (ret)
+			return ret;
+		break;
 	}
+
 	ctx->state |= FIMC_PARAMS;
 	set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
 	return 0;
@@ -827,9 +568,9 @@ static int fimc_s_ctrl(struct v4l2_ctrl *ctrl)
 	unsigned long flags;
 	int ret;
 
-	spin_lock_irqsave(&ctx->slock, flags);
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
 	ret = __fimc_s_ctrl(ctx, ctrl);
-	spin_unlock_irqrestore(&ctx->slock, flags);
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
 
 	return ret;
 }
@@ -840,71 +581,93 @@ static const struct v4l2_ctrl_ops fimc_ctrl_ops = {
 
 int fimc_ctrls_create(struct fimc_ctx *ctx)
 {
-	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	struct fimc_variant *variant = ctx->fimc_dev->variant;
 	unsigned int max_alpha = fimc_get_alpha_mask(ctx->d_frame.fmt);
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
+	struct v4l2_ctrl_handler *handler = &ctrls->handler;
 
-	if (ctx->ctrls_rdy)
+	if (ctx->ctrls.ready)
 		return 0;
-	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 4);
 
-	ctx->ctrl_rotate = v4l2_ctrl_new_std(&ctx->ctrl_handler, &fimc_ctrl_ops,
+	v4l2_ctrl_handler_init(handler, 6);
+
+	ctrls->rotate = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_ROTATE, 0, 270, 90, 0);
-	ctx->ctrl_hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler, &fimc_ctrl_ops,
+	ctrls->hflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_HFLIP, 0, 1, 1, 0);
-	ctx->ctrl_vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler, &fimc_ctrl_ops,
+	ctrls->vflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_VFLIP, 0, 1, 1, 0);
+
 	if (variant->has_alpha)
-		ctx->ctrl_alpha = v4l2_ctrl_new_std(&ctx->ctrl_handler,
-				    &fimc_ctrl_ops, V4L2_CID_ALPHA_COMPONENT,
-				    0, max_alpha, 1, 0);
+		ctrls->alpha = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+					V4L2_CID_ALPHA_COMPONENT,
+					0, max_alpha, 1, 0);
 	else
-		ctx->ctrl_alpha = NULL;
+		ctrls->alpha = NULL;
 
-	ctx->ctrls_rdy = ctx->ctrl_handler.error == 0;
+	ctrls->colorfx = v4l2_ctrl_new_std_menu(handler, &fimc_ctrl_ops,
+				V4L2_CID_COLORFX, V4L2_COLORFX_SET_CBCR,
+				~0x983f, V4L2_COLORFX_NONE);
 
-	return ctx->ctrl_handler.error;
+	ctrls->colorfx_cbcr = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+				V4L2_CID_COLORFX_CBCR, 0, 0xffff, 1, 0);
+
+	ctx->effect.type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
+
+	if (!handler->error) {
+		v4l2_ctrl_cluster(3, &ctrls->colorfx);
+		ctrls->ready = true;
+	}
+
+	return handler->error;
 }
 
 void fimc_ctrls_delete(struct fimc_ctx *ctx)
 {
-	if (ctx->ctrls_rdy) {
-		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
-		ctx->ctrls_rdy = false;
-		ctx->ctrl_alpha = NULL;
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
+
+	if (ctrls->ready) {
+		v4l2_ctrl_handler_free(&ctrls->handler);
+		ctrls->ready = false;
+		ctrls->alpha = NULL;
 	}
 }
 
 void fimc_ctrls_activate(struct fimc_ctx *ctx, bool active)
 {
 	unsigned int has_alpha = ctx->d_frame.fmt->flags & FMT_HAS_ALPHA;
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
 
-	if (!ctx->ctrls_rdy)
+	if (!ctrls->ready)
 		return;
 
-	mutex_lock(&ctx->ctrl_handler.lock);
-	v4l2_ctrl_activate(ctx->ctrl_rotate, active);
-	v4l2_ctrl_activate(ctx->ctrl_hflip, active);
-	v4l2_ctrl_activate(ctx->ctrl_vflip, active);
-	if (ctx->ctrl_alpha)
-		v4l2_ctrl_activate(ctx->ctrl_alpha, active && has_alpha);
+	mutex_lock(&ctrls->handler.lock);
+	v4l2_ctrl_activate(ctrls->rotate, active);
+	v4l2_ctrl_activate(ctrls->hflip, active);
+	v4l2_ctrl_activate(ctrls->vflip, active);
+	v4l2_ctrl_activate(ctrls->colorfx, active);
+	if (ctrls->alpha)
+		v4l2_ctrl_activate(ctrls->alpha, active && has_alpha);
 
 	if (active) {
-		ctx->rotation = ctx->ctrl_rotate->val;
-		ctx->hflip    = ctx->ctrl_hflip->val;
-		ctx->vflip    = ctx->ctrl_vflip->val;
+		fimc_set_color_effect(ctx, ctrls->colorfx->cur.val);
+		ctx->rotation = ctrls->rotate->val;
+		ctx->hflip    = ctrls->hflip->val;
+		ctx->vflip    = ctrls->vflip->val;
 	} else {
+		ctx->effect.type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
 		ctx->rotation = 0;
 		ctx->hflip    = 0;
 		ctx->vflip    = 0;
 	}
-	mutex_unlock(&ctx->ctrl_handler.lock);
+	mutex_unlock(&ctrls->handler.lock);
 }
 
 /* Update maximum value of the alpha color control */
 void fimc_alpha_ctrl_update(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct v4l2_ctrl *ctrl = ctx->ctrl_alpha;
+	struct v4l2_ctrl *ctrl = ctx->ctrls.alpha;
 
 	if (ctrl == NULL || !fimc->variant->has_alpha)
 		return;
@@ -918,39 +681,6 @@ void fimc_alpha_ctrl_update(struct fimc_ctx *ctx)
 	v4l2_ctrl_unlock(ctrl);
 }
 
-/*
- * V4L2 ioctl handlers
- */
-static int fimc_m2m_querycap(struct file *file, void *fh,
-			     struct v4l2_capability *cap)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-
-	strncpy(cap->driver, fimc->pdev->name, sizeof(cap->driver) - 1);
-	strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
-	cap->bus_info[0] = 0;
-	cap->capabilities = V4L2_CAP_STREAMING |
-		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
-
-	return 0;
-}
-
-static int fimc_m2m_enum_fmt_mplane(struct file *file, void *priv,
-				    struct v4l2_fmtdesc *f)
-{
-	struct fimc_fmt *fmt;
-
-	fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
-			       f->index);
-	if (!fmt)
-		return -EINVAL;
-
-	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
-	f->pixelformat = fmt->fourcc;
-	return 0;
-}
-
 int fimc_fill_format(struct fimc_frame *frame, struct v4l2_format *f)
 {
 	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
@@ -1029,18 +759,6 @@ void fimc_adjust_mplane_format(struct fimc_fmt *fmt, u32 width, u32 height,
 	}
 }
 
-static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
-				 struct v4l2_format *f)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
-
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	return fimc_fill_format(frame, f);
-}
-
 /**
  * fimc_find_format - lookup fimc color format by fourcc or media bus format
  * @pixelformat: fourcc to match, ignored if null
@@ -1073,535 +791,10 @@ struct fimc_fmt *fimc_find_format(const u32 *pixelformat, const u32 *mbus_code,
 	return def_fmt;
 }
 
-static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
-{
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *variant = fimc->variant;
-	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
-	struct fimc_fmt *fmt;
-	u32 max_w, mod_x, mod_y;
-
-	if (!IS_M2M(f->type))
-		return -EINVAL;
-
-	dbg("w: %d, h: %d", pix->width, pix->height);
-
-	fmt = fimc_find_format(&pix->pixelformat, NULL,
-			       get_m2m_fmt_flags(f->type), 0);
-	if (WARN(fmt == NULL, "Pixel format lookup failed"))
-		return -EINVAL;
-
-	if (pix->field == V4L2_FIELD_ANY)
-		pix->field = V4L2_FIELD_NONE;
-	else if (pix->field != V4L2_FIELD_NONE)
-		return -EINVAL;
-
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-		max_w = variant->pix_limit->scaler_dis_w;
-		mod_x = ffs(variant->min_inp_pixsize) - 1;
-	} else {
-		max_w = variant->pix_limit->out_rot_dis_w;
-		mod_x = ffs(variant->min_out_pixsize) - 1;
-	}
-
-	if (tiled_fmt(fmt)) {
-		mod_x = 6; /* 64 x 32 pixels tile */
-		mod_y = 5;
-	} else {
-		if (variant->min_vsize_align == 1)
-			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
-		else
-			mod_y = ffs(variant->min_vsize_align) - 1;
-	}
-
-	v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
-		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
-
-	fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
-	return 0;
-}
-
-static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
-				   struct v4l2_format *f)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return fimc_try_fmt_mplane(ctx, f);
-}
-
-static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
-				 struct v4l2_format *f)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct vb2_queue *vq;
-	struct fimc_frame *frame;
-	struct v4l2_pix_format_mplane *pix;
-	int i, ret = 0;
-
-	ret = fimc_try_fmt_mplane(ctx, f);
-	if (ret)
-		return ret;
-
-	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
-
-	if (vb2_is_busy(vq)) {
-		v4l2_err(fimc->m2m.vfd, "queue (%d) busy\n", f->type);
-		return -EBUSY;
-	}
-
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		frame = &ctx->s_frame;
-	else
-		frame = &ctx->d_frame;
-
-	pix = &f->fmt.pix_mp;
-	frame->fmt = fimc_find_format(&pix->pixelformat, NULL,
-				      get_m2m_fmt_flags(f->type), 0);
-	if (!frame->fmt)
-		return -EINVAL;
-
-	/* Update RGB Alpha control state and value range */
-	fimc_alpha_ctrl_update(ctx);
-
-	for (i = 0; i < frame->fmt->colplanes; i++) {
-		frame->payload[i] =
-			(pix->width * pix->height * frame->fmt->depth[i]) / 8;
-	}
-
-	fimc_fill_frame(frame, f);
-
-	ctx->scaler.enabled = 1;
-
-	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
-		fimc_ctx_state_lock_set(FIMC_PARAMS | FIMC_DST_FMT, ctx);
-	else
-		fimc_ctx_state_lock_set(FIMC_PARAMS | FIMC_SRC_FMT, ctx);
-
-	dbg("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
-
-	return 0;
-}
-
-static int fimc_m2m_reqbufs(struct file *file, void *fh,
-			    struct v4l2_requestbuffers *reqbufs)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
-}
-
-static int fimc_m2m_querybuf(struct file *file, void *fh,
-			     struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_qbuf(struct file *file, void *fh,
-			 struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_dqbuf(struct file *file, void *fh,
-			  struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_streamon(struct file *file, void *fh,
-			     enum v4l2_buf_type type)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	/* The source and target color format need to be set */
-	if (V4L2_TYPE_IS_OUTPUT(type)) {
-		if (!fimc_ctx_state_is_set(FIMC_SRC_FMT, ctx))
-			return -EINVAL;
-	} else if (!fimc_ctx_state_is_set(FIMC_DST_FMT, ctx)) {
-		return -EINVAL;
-	}
-
-	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
-}
-
-static int fimc_m2m_streamoff(struct file *file, void *fh,
-			    enum v4l2_buf_type type)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
-}
-
-static int fimc_m2m_cropcap(struct file *file, void *fh,
-			    struct v4l2_cropcap *cr)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_frame *frame;
-
-	frame = ctx_get_frame(ctx, cr->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	cr->bounds.left		= 0;
-	cr->bounds.top		= 0;
-	cr->bounds.width	= frame->o_width;
-	cr->bounds.height	= frame->o_height;
-	cr->defrect		= cr->bounds;
-
-	return 0;
-}
-
-static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_frame *frame;
-
-	frame = ctx_get_frame(ctx, cr->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	cr->c.left = frame->offs_h;
-	cr->c.top = frame->offs_v;
-	cr->c.width = frame->width;
-	cr->c.height = frame->height;
-
-	return 0;
-}
-
-static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
-{
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_frame *f;
-	u32 min_size, halign, depth = 0;
-	int i;
-
-	if (cr->c.top < 0 || cr->c.left < 0) {
-		v4l2_err(fimc->m2m.vfd,
-			"doesn't support negative values for top & left\n");
-		return -EINVAL;
-	}
-	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
-		f = &ctx->d_frame;
-	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		f = &ctx->s_frame;
-	else
-		return -EINVAL;
-
-	min_size = (f == &ctx->s_frame) ?
-		fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
-
-	/* Get pixel alignment constraints. */
-	if (fimc->variant->min_vsize_align == 1)
-		halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
-	else
-		halign = ffs(fimc->variant->min_vsize_align) - 1;
-
-	for (i = 0; i < f->fmt->colplanes; i++)
-		depth += f->fmt->depth[i];
-
-	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
-			      ffs(min_size) - 1,
-			      &cr->c.height, min_size, f->o_height,
-			      halign, 64/(ALIGN(depth, 8)));
-
-	/* adjust left/top if cropping rectangle is out of bounds */
-	if (cr->c.left + cr->c.width > f->o_width)
-		cr->c.left = f->o_width - cr->c.width;
-	if (cr->c.top + cr->c.height > f->o_height)
-		cr->c.top = f->o_height - cr->c.height;
-
-	cr->c.left = round_down(cr->c.left, min_size);
-	cr->c.top  = round_down(cr->c.top, fimc->variant->hor_offs_align);
-
-	dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
-	    cr->c.left, cr->c.top, cr->c.width, cr->c.height,
-	    f->f_width, f->f_height);
-
-	return 0;
-}
-
-static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_frame *f;
-	int ret;
-
-	ret = fimc_m2m_try_crop(ctx, cr);
-	if (ret)
-		return ret;
-
-	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,
-					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);
-		}
-		if (ret) {
-			v4l2_err(fimc->m2m.vfd, "Out of scaler range\n");
-			return -EINVAL;
-		}
-	}
-
-	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_lock_set(FIMC_PARAMS, ctx);
-
-	return 0;
-}
-
-static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
-	.vidioc_querycap		= fimc_m2m_querycap,
-
-	.vidioc_enum_fmt_vid_cap_mplane	= fimc_m2m_enum_fmt_mplane,
-	.vidioc_enum_fmt_vid_out_mplane	= fimc_m2m_enum_fmt_mplane,
-
-	.vidioc_g_fmt_vid_cap_mplane	= fimc_m2m_g_fmt_mplane,
-	.vidioc_g_fmt_vid_out_mplane	= fimc_m2m_g_fmt_mplane,
-
-	.vidioc_try_fmt_vid_cap_mplane	= fimc_m2m_try_fmt_mplane,
-	.vidioc_try_fmt_vid_out_mplane	= fimc_m2m_try_fmt_mplane,
-
-	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
-	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
-
-	.vidioc_reqbufs			= fimc_m2m_reqbufs,
-	.vidioc_querybuf		= fimc_m2m_querybuf,
-
-	.vidioc_qbuf			= fimc_m2m_qbuf,
-	.vidioc_dqbuf			= fimc_m2m_dqbuf,
-
-	.vidioc_streamon		= fimc_m2m_streamon,
-	.vidioc_streamoff		= fimc_m2m_streamoff,
-
-	.vidioc_g_crop			= fimc_m2m_g_crop,
-	.vidioc_s_crop			= fimc_m2m_s_crop,
-	.vidioc_cropcap			= fimc_m2m_cropcap
-
-};
-
-static int queue_init(void *priv, struct vb2_queue *src_vq,
-		      struct vb2_queue *dst_vq)
-{
-	struct fimc_ctx *ctx = priv;
-	int ret;
-
-	memset(src_vq, 0, sizeof(*src_vq));
-	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
-	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
-	src_vq->drv_priv = ctx;
-	src_vq->ops = &fimc_qops;
-	src_vq->mem_ops = &vb2_dma_contig_memops;
-	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
-
-	ret = vb2_queue_init(src_vq);
-	if (ret)
-		return ret;
-
-	memset(dst_vq, 0, sizeof(*dst_vq));
-	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
-	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
-	dst_vq->drv_priv = ctx;
-	dst_vq->ops = &fimc_qops;
-	dst_vq->mem_ops = &vb2_dma_contig_memops;
-	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
-
-	return vb2_queue_init(dst_vq);
-}
-
-static int fimc_m2m_open(struct file *file)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-	struct fimc_ctx *ctx;
-	int ret;
-
-	dbg("pid: %d, state: 0x%lx, refcnt: %d",
-		task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
-
-	/*
-	 * Return if the corresponding video capture node
-	 * is already opened.
-	 */
-	if (fimc->vid_cap.refcnt > 0)
-		return -EBUSY;
-
-	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
-	if (!ctx)
-		return -ENOMEM;
-	v4l2_fh_init(&ctx->fh, fimc->m2m.vfd);
-	ctx->fimc_dev = fimc;
-
-	/* Default color format */
-	ctx->s_frame.fmt = &fimc_formats[0];
-	ctx->d_frame.fmt = &fimc_formats[0];
-
-	ret = fimc_ctrls_create(ctx);
-	if (ret)
-		goto error_fh;
-
-	/* Use separate control handler per file handle */
-	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
-	file->private_data = &ctx->fh;
-	v4l2_fh_add(&ctx->fh);
-
-	/* Setup the device context for memory-to-memory mode */
-	ctx->state = FIMC_CTX_M2M;
-	ctx->flags = 0;
-	ctx->in_path = FIMC_DMA;
-	ctx->out_path = FIMC_DMA;
-	spin_lock_init(&ctx->slock);
-
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
-	if (IS_ERR(ctx->m2m_ctx)) {
-		ret = PTR_ERR(ctx->m2m_ctx);
-		goto error_c;
-	}
-
-	if (fimc->m2m.refcnt++ == 0)
-		set_bit(ST_M2M_RUN, &fimc->state);
-	return 0;
-
-error_c:
-	fimc_ctrls_delete(ctx);
-error_fh:
-	v4l2_fh_del(&ctx->fh);
-	v4l2_fh_exit(&ctx->fh);
-	kfree(ctx);
-	return ret;
-}
-
-static int fimc_m2m_release(struct file *file)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-
-	dbg("pid: %d, state: 0x%lx, refcnt= %d",
-		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
-
-	v4l2_m2m_ctx_release(ctx->m2m_ctx);
-	fimc_ctrls_delete(ctx);
-	v4l2_fh_del(&ctx->fh);
-	v4l2_fh_exit(&ctx->fh);
-
-	if (--fimc->m2m.refcnt <= 0)
-		clear_bit(ST_M2M_RUN, &fimc->state);
-	kfree(ctx);
-	return 0;
-}
-
-static unsigned int fimc_m2m_poll(struct file *file,
-				  struct poll_table_struct *wait)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-
-	return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
-}
-
-
-static int fimc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-
-	return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
-}
-
-static const struct v4l2_file_operations fimc_m2m_fops = {
-	.owner		= THIS_MODULE,
-	.open		= fimc_m2m_open,
-	.release	= fimc_m2m_release,
-	.poll		= fimc_m2m_poll,
-	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= fimc_m2m_mmap,
-};
-
-static struct v4l2_m2m_ops m2m_ops = {
-	.device_run	= fimc_dma_run,
-	.job_abort	= fimc_job_abort,
-};
-
-int fimc_register_m2m_device(struct fimc_dev *fimc,
-			     struct v4l2_device *v4l2_dev)
-{
-	struct video_device *vfd;
-	struct platform_device *pdev;
-	int ret = 0;
-
-	if (!fimc)
-		return -ENODEV;
-
-	pdev = fimc->pdev;
-	fimc->v4l2_dev = v4l2_dev;
-
-	vfd = video_device_alloc();
-	if (!vfd) {
-		v4l2_err(v4l2_dev, "Failed to allocate video device\n");
-		return -ENOMEM;
-	}
-
-	vfd->fops	= &fimc_m2m_fops;
-	vfd->ioctl_ops	= &fimc_m2m_ioctl_ops;
-	vfd->v4l2_dev	= v4l2_dev;
-	vfd->minor	= -1;
-	vfd->release	= video_device_release;
-	vfd->lock	= &fimc->lock;
-
-	snprintf(vfd->name, sizeof(vfd->name), "%s.m2m", dev_name(&pdev->dev));
-	video_set_drvdata(vfd, fimc);
-
-	fimc->m2m.vfd = vfd;
-	fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
-	if (IS_ERR(fimc->m2m.m2m_dev)) {
-		v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
-		ret = PTR_ERR(fimc->m2m.m2m_dev);
-		goto err_init;
-	}
-
-	ret = media_entity_init(&vfd->entity, 0, NULL, 0);
-	if (!ret)
-		return 0;
-
-	v4l2_m2m_release(fimc->m2m.m2m_dev);
-err_init:
-	video_device_release(fimc->m2m.vfd);
-	return ret;
-}
-
-void fimc_unregister_m2m_device(struct fimc_dev *fimc)
-{
-	if (!fimc)
-		return;
-
-	if (fimc->m2m.m2m_dev)
-		v4l2_m2m_release(fimc->m2m.m2m_dev);
-	if (fimc->m2m.vfd) {
-		media_entity_cleanup(&fimc->m2m.vfd->entity);
-		/* Can also be called if video device wasn't registered */
-		video_unregister_device(fimc->m2m.vfd);
-	}
-}
-
 static void fimc_clk_put(struct fimc_dev *fimc)
 {
 	int i;
-	for (i = 0; i < fimc->num_clocks; i++) {
+	for (i = 0; i < MAX_FIMC_CLOCKS; i++) {
 		if (IS_ERR_OR_NULL(fimc->clock[i]))
 			continue;
 		clk_unprepare(fimc->clock[i]);
@@ -1614,7 +807,7 @@ static int fimc_clk_get(struct fimc_dev *fimc)
 {
 	int i, ret;
 
-	for (i = 0; i < fimc->num_clocks; i++) {
+	for (i = 0; i < MAX_FIMC_CLOCKS; i++) {
 		fimc->clock[i] = clk_get(&fimc->pdev->dev, fimc_clocks[i]);
 		if (IS_ERR(fimc->clock[i]))
 			goto err;
@@ -1672,15 +865,12 @@ static int fimc_m2m_resume(struct fimc_dev *fimc)
 
 static int fimc_probe(struct platform_device *pdev)
 {
+	struct fimc_drvdata *drv_data = fimc_get_drvdata(pdev);
+	struct s5p_platform_fimc *pdata;
 	struct fimc_dev *fimc;
 	struct resource *res;
-	struct samsung_fimc_driverdata *drv_data;
-	struct s5p_platform_fimc *pdata;
 	int ret = 0;
 
-	drv_data = (struct samsung_fimc_driverdata *)
-		platform_get_device_id(pdev)->driver_data;
-
 	if (pdev->id >= drv_data->num_entities) {
 		dev_err(&pdev->dev, "Invalid platform device id: %d\n",
 			pdev->id);
@@ -1714,28 +904,29 @@ static int fimc_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
 		return -ENXIO;
 	}
-	fimc->irq = res->start;
 
-	fimc->num_clocks = MAX_FIMC_CLOCKS;
 	ret = fimc_clk_get(fimc);
 	if (ret)
 		return ret;
 	clk_set_rate(fimc->clock[CLK_BUS], drv_data->lclk_frequency);
 	clk_enable(fimc->clock[CLK_BUS]);
 
-	platform_set_drvdata(pdev, fimc);
-
-	ret = devm_request_irq(&pdev->dev, fimc->irq, fimc_irq_handler,
-			       0, pdev->name, fimc);
+	ret = devm_request_irq(&pdev->dev, res->start, fimc_irq_handler,
+			       0, dev_name(&pdev->dev), fimc);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to install irq (%d)\n", ret);
 		goto err_clk;
 	}
 
+	ret = fimc_initialize_capture_subdev(fimc);
+	if (ret)
+		goto err_clk;
+
+	platform_set_drvdata(pdev, fimc);
 	pm_runtime_enable(&pdev->dev);
 	ret = pm_runtime_get_sync(&pdev->dev);
 	if (ret < 0)
-		goto err_clk;
+		goto err_sd;
 	/* Initialize contiguous memory allocator */
 	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
@@ -1747,9 +938,10 @@ static int fimc_probe(struct platform_device *pdev)
 
 	pm_runtime_put(&pdev->dev);
 	return 0;
-
 err_pm:
 	pm_runtime_put(&pdev->dev);
+err_sd:
+	fimc_unregister_capture_subdev(fimc);
 err_clk:
 	fimc_clk_put(fimc);
 	return ret;
@@ -1834,6 +1026,7 @@ static int __devexit fimc_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 	pm_runtime_set_suspended(&pdev->dev);
 
+	fimc_unregister_capture_subdev(fimc);
 	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
 
 	clk_disable(fimc->clock[CLK_BUS]);
@@ -1879,7 +1072,7 @@ static struct fimc_pix_limit s5p_pix_limit[4] = {
 	},
 };
 
-static struct samsung_fimc_variant fimc0_variant_s5p = {
+static struct fimc_variant fimc0_variant_s5p = {
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cam_if	 = 1,
@@ -1891,17 +1084,17 @@ static struct samsung_fimc_variant fimc0_variant_s5p = {
 	.pix_limit	 = &s5p_pix_limit[0],
 };
 
-static struct samsung_fimc_variant fimc2_variant_s5p = {
+static struct fimc_variant fimc2_variant_s5p = {
 	.has_cam_if	 = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
 	.out_buf_count	 = 4,
-	.pix_limit = &s5p_pix_limit[1],
+	.pix_limit	 = &s5p_pix_limit[1],
 };
 
-static struct samsung_fimc_variant fimc0_variant_s5pv210 = {
+static struct fimc_variant fimc0_variant_s5pv210 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
@@ -1914,7 +1107,7 @@ static struct samsung_fimc_variant fimc0_variant_s5pv210 = {
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
-static struct samsung_fimc_variant fimc1_variant_s5pv210 = {
+static struct fimc_variant fimc1_variant_s5pv210 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
@@ -1928,7 +1121,7 @@ static struct samsung_fimc_variant fimc1_variant_s5pv210 = {
 	.pix_limit	 = &s5p_pix_limit[2],
 };
 
-static struct samsung_fimc_variant fimc2_variant_s5pv210 = {
+static struct fimc_variant fimc2_variant_s5pv210 = {
 	.has_cam_if	 = 1,
 	.pix_hoff	 = 1,
 	.min_inp_pixsize = 16,
@@ -1939,7 +1132,7 @@ static struct samsung_fimc_variant fimc2_variant_s5pv210 = {
 	.pix_limit	 = &s5p_pix_limit[2],
 };
 
-static struct samsung_fimc_variant fimc0_variant_exynos4 = {
+static struct fimc_variant fimc0_variant_exynos4 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
@@ -1955,7 +1148,7 @@ static struct samsung_fimc_variant fimc0_variant_exynos4 = {
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
-static struct samsung_fimc_variant fimc3_variant_exynos4 = {
+static struct fimc_variant fimc3_variant_exynos4 = {
 	.pix_hoff	 = 1,
 	.has_cam_if	 = 1,
 	.has_cistatus2	 = 1,
@@ -1970,7 +1163,7 @@ static struct samsung_fimc_variant fimc3_variant_exynos4 = {
 };
 
 /* S5PC100 */
-static struct samsung_fimc_driverdata fimc_drvdata_s5p = {
+static struct fimc_drvdata fimc_drvdata_s5p = {
 	.variant = {
 		[0] = &fimc0_variant_s5p,
 		[1] = &fimc0_variant_s5p,
@@ -1981,7 +1174,7 @@ static struct samsung_fimc_driverdata fimc_drvdata_s5p = {
 };
 
 /* S5PV210, S5PC110 */
-static struct samsung_fimc_driverdata fimc_drvdata_s5pv210 = {
+static struct fimc_drvdata fimc_drvdata_s5pv210 = {
 	.variant = {
 		[0] = &fimc0_variant_s5pv210,
 		[1] = &fimc1_variant_s5pv210,
@@ -1991,8 +1184,8 @@ static struct samsung_fimc_driverdata fimc_drvdata_s5pv210 = {
 	.lclk_frequency = 166000000UL,
 };
 
-/* S5PV310, S5PC210 */
-static struct samsung_fimc_driverdata fimc_drvdata_exynos4 = {
+/* EXYNOS4210, S5PV310, S5PC210 */
+static struct fimc_drvdata fimc_drvdata_exynos4 = {
 	.variant = {
 		[0] = &fimc0_variant_exynos4,
 		[1] = &fimc0_variant_exynos4,
@@ -2036,7 +1229,7 @@ static struct platform_driver fimc_driver = {
 
 int __init fimc_register_driver(void)
 {
-	return platform_driver_probe(&fimc_driver, fimc_probe);
+	return platform_driver_register(&fimc_driver);
 }
 
 void __exit fimc_unregister_driver(void)
diff --git a/drivers/media/video/s5p-fimc/fimc-core.h b/drivers/media/video/s5p-fimc/fimc-core.h
index 84fd83550bd7..95b27ae5cf27 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.h
+++ b/drivers/media/video/s5p-fimc/fimc-core.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 - 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
  *
  * 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
@@ -17,6 +17,7 @@
 #include <linux/types.h>
 #include <linux/videodev2.h>
 #include <linux/io.h>
+#include <asm/sizes.h>
 
 #include <media/media-entity.h>
 #include <media/videobuf2-core.h>
@@ -26,8 +27,6 @@
 #include <media/v4l2-mediabus.h>
 #include <media/s5p_fimc.h>
 
-#include "regs-fimc.h"
-
 #define err(fmt, args...) \
 	printk(KERN_ERR "%s:%d: " fmt "\n", __func__, __LINE__, ##args)
 
@@ -78,26 +77,31 @@ enum fimc_dev_flags {
 #define fimc_capture_busy(dev) test_bit(ST_CAPT_BUSY, &(dev)->state)
 
 enum fimc_datapath {
-	FIMC_CAMERA,
-	FIMC_DMA,
-	FIMC_LCDFIFO,
-	FIMC_WRITEBACK
+	FIMC_IO_NONE,
+	FIMC_IO_CAMERA,
+	FIMC_IO_DMA,
+	FIMC_IO_LCDFIFO,
+	FIMC_IO_WRITEBACK,
+	FIMC_IO_ISP,
 };
 
 enum fimc_color_fmt {
-	S5P_FIMC_RGB444 = 0x10,
-	S5P_FIMC_RGB555,
-	S5P_FIMC_RGB565,
-	S5P_FIMC_RGB666,
-	S5P_FIMC_RGB888,
-	S5P_FIMC_RGB30_LOCAL,
-	S5P_FIMC_YCBCR420 = 0x20,
-	S5P_FIMC_YCBYCR422,
-	S5P_FIMC_YCRYCB422,
-	S5P_FIMC_CBYCRY422,
-	S5P_FIMC_CRYCBY422,
-	S5P_FIMC_YCBCR444_LOCAL,
-	S5P_FIMC_JPEG = 0x40,
+	FIMC_FMT_RGB444 = 0x10,
+	FIMC_FMT_RGB555,
+	FIMC_FMT_RGB565,
+	FIMC_FMT_RGB666,
+	FIMC_FMT_RGB888,
+	FIMC_FMT_RGB30_LOCAL,
+	FIMC_FMT_YCBCR420 = 0x20,
+	FIMC_FMT_YCBYCR422,
+	FIMC_FMT_YCRYCB422,
+	FIMC_FMT_CBYCRY422,
+	FIMC_FMT_CRYCBY422,
+	FIMC_FMT_YCBCR444_LOCAL,
+	FIMC_FMT_JPEG = 0x40,
+	FIMC_FMT_RAW8 = 0x80,
+	FIMC_FMT_RAW10,
+	FIMC_FMT_RAW12,
 };
 
 #define fimc_fmt_is_rgb(x) (!!((x) & 0x10))
@@ -106,24 +110,11 @@ enum fimc_color_fmt {
 #define IS_M2M(__strt) ((__strt) == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE || \
 			__strt == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 
-/* Cb/Cr chrominance components order for 2 plane Y/CbCr 4:2:2 formats. */
-#define	S5P_FIMC_LSB_CRCB	S5P_CIOCTRL_ORDER422_2P_LSB_CRCB
-
-/* The embedded image effect selection */
-#define	S5P_FIMC_EFFECT_ORIGINAL	S5P_CIIMGEFF_FIN_BYPASS
-#define	S5P_FIMC_EFFECT_ARBITRARY	S5P_CIIMGEFF_FIN_ARBITRARY
-#define	S5P_FIMC_EFFECT_NEGATIVE	S5P_CIIMGEFF_FIN_NEGATIVE
-#define	S5P_FIMC_EFFECT_ARTFREEZE	S5P_CIIMGEFF_FIN_ARTFREEZE
-#define	S5P_FIMC_EFFECT_EMBOSSING	S5P_CIIMGEFF_FIN_EMBOSSING
-#define	S5P_FIMC_EFFECT_SIKHOUETTE	S5P_CIIMGEFF_FIN_SILHOUETTE
-
 /* The hardware context state. */
 #define	FIMC_PARAMS		(1 << 0)
-#define	FIMC_SRC_ADDR		(1 << 1)
-#define	FIMC_DST_ADDR		(1 << 2)
 #define	FIMC_SRC_FMT		(1 << 3)
 #define	FIMC_DST_FMT		(1 << 4)
-#define	FIMC_DST_CROP		(1 << 5)
+#define	FIMC_COMPOSE		(1 << 5)
 #define	FIMC_CTX_M2M		(1 << 16)
 #define	FIMC_CTX_CAP		(1 << 17)
 #define	FIMC_CTX_SHUT		(1 << 18)
@@ -333,7 +324,7 @@ struct fimc_vid_cap {
 	struct fimc_ctx			*ctx;
 	struct vb2_alloc_ctx		*alloc_ctx;
 	struct video_device		*vfd;
-	struct v4l2_subdev		*subdev;
+	struct v4l2_subdev		subdev;
 	struct media_pad		vd_pad;
 	struct v4l2_mbus_framefmt	mf;
 	struct media_pad		sd_pads[FIMC_SD_PADS_NUM];
@@ -370,8 +361,7 @@ struct fimc_pix_limit {
 };
 
 /**
- * struct samsung_fimc_variant - camera interface variant information
- *
+ * struct fimc_variant - FIMC device variant information
  * @pix_hoff: indicate whether horizontal offset is in pixels or in bytes
  * @has_inp_rot: set if has input rotator
  * @has_out_rot: set if has output rotator
@@ -386,7 +376,7 @@ struct fimc_pix_limit {
  * @min_vsize_align: minimum vertical pixel size alignment
  * @out_buf_count: the number of buffers in output DMA sequence
  */
-struct samsung_fimc_variant {
+struct fimc_variant {
 	unsigned int	pix_hoff:1;
 	unsigned int	has_inp_rot:1;
 	unsigned int	has_out_rot:1;
@@ -403,23 +393,19 @@ struct samsung_fimc_variant {
 };
 
 /**
- * struct samsung_fimc_driverdata - per device type driver data for init time.
- *
- * @variant: the variant information for this driver.
- * @dev_cnt: number of fimc sub-devices available in SoC
- * @lclk_frequency: fimc bus clock frequency
+ * struct fimc_drvdata - per device type driver data
+ * @variant: variant information for this device
+ * @num_entities: number of fimc instances available in a SoC
+ * @lclk_frequency: local bus clock frequency
  */
-struct samsung_fimc_driverdata {
-	struct samsung_fimc_variant *variant[FIMC_MAX_DEVS];
-	unsigned long	lclk_frequency;
-	int		num_entities;
+struct fimc_drvdata {
+	struct fimc_variant *variant[FIMC_MAX_DEVS];
+	int num_entities;
+	unsigned long lclk_frequency;
 };
 
-struct fimc_pipeline {
-	struct media_pipeline *pipe;
-	struct v4l2_subdev *sensor;
-	struct v4l2_subdev *csis;
-};
+#define fimc_get_drvdata(_pdev) \
+	((struct fimc_drvdata *) platform_get_device_id(_pdev)->driver_data)
 
 struct fimc_ctx;
 
@@ -431,10 +417,8 @@ struct fimc_ctx;
  * @pdata:	pointer to the device platform data
  * @variant:	the IP variant information
  * @id:		FIMC device index (0..FIMC_MAX_DEVS)
- * @num_clocks: the number of clocks managed by this device instance
  * @clock:	clocks required for FIMC operation
  * @regs:	the mapped hardware registers
- * @irq:	FIMC interrupt number
  * @irq_queue:	interrupt handler waitqueue
  * @v4l2_dev:	root v4l2_device
  * @m2m:	memory-to-memory V4L2 device information
@@ -448,12 +432,10 @@ struct fimc_dev {
 	struct mutex			lock;
 	struct platform_device		*pdev;
 	struct s5p_platform_fimc	*pdata;
-	struct samsung_fimc_variant	*variant;
+	struct fimc_variant		*variant;
 	u16				id;
-	u16				num_clocks;
 	struct clk			*clock[MAX_FIMC_CLOCKS];
 	void __iomem			*regs;
-	int				irq;
 	wait_queue_head_t		irq_queue;
 	struct v4l2_device		*v4l2_dev;
 	struct fimc_m2m_device		m2m;
@@ -464,8 +446,31 @@ struct fimc_dev {
 };
 
 /**
+ * struct fimc_ctrls - v4l2 controls structure
+ * @handler: the control handler
+ * @colorfx: image effect control
+ * @colorfx_cbcr: Cb/Cr coefficients control
+ * @rotate: image rotation control
+ * @hflip: horizontal flip control
+ * @vflip: vertical flip control
+ * @alpha: RGB alpha control
+ * @ready: true if @handler is initialized
+ */
+struct fimc_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct {
+		struct v4l2_ctrl *colorfx;
+		struct v4l2_ctrl *colorfx_cbcr;
+	};
+	struct v4l2_ctrl *rotate;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *alpha;
+	bool ready;
+};
+
+/**
  * fimc_ctx - the device context data
- * @slock:		spinlock protecting this data structure
  * @s_frame:		source frame properties
  * @d_frame:		destination frame properties
  * @out_order_1p:	output 1-plane YCBCR order
@@ -484,15 +489,9 @@ struct fimc_dev {
  * @fimc_dev:		the FIMC device this context applies to
  * @m2m_ctx:		memory-to-memory device context
  * @fh:			v4l2 file handle
- * @ctrl_handler:	v4l2 controls handler
- * @ctrl_rotate		image rotation control
- * @ctrl_hflip		horizontal flip control
- * @ctrl_vflip		vertical flip control
- * @ctrl_alpha		RGB alpha control
- * @ctrls_rdy:		true if the control handler is initialized
+ * @ctrls:		v4l2 controls structure
  */
 struct fimc_ctx {
-	spinlock_t		slock;
 	struct fimc_frame	s_frame;
 	struct fimc_frame	d_frame;
 	u32			out_order_1p;
@@ -511,12 +510,7 @@ struct fimc_ctx {
 	struct fimc_dev		*fimc_dev;
 	struct v4l2_m2m_ctx	*m2m_ctx;
 	struct v4l2_fh		fh;
-	struct v4l2_ctrl_handler ctrl_handler;
-	struct v4l2_ctrl	*ctrl_rotate;
-	struct v4l2_ctrl	*ctrl_hflip;
-	struct v4l2_ctrl	*ctrl_vflip;
-	struct v4l2_ctrl	*ctrl_alpha;
-	bool			ctrls_rdy;
+	struct fimc_ctrls	ctrls;
 };
 
 #define fh_to_ctx(__fh) container_of(__fh, struct fimc_ctx, fh)
@@ -560,13 +554,13 @@ static inline bool fimc_capture_active(struct fimc_dev *fimc)
 	return ret;
 }
 
-static inline void fimc_ctx_state_lock_set(u32 state, struct fimc_ctx *ctx)
+static inline void fimc_ctx_state_set(u32 state, struct fimc_ctx *ctx)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&ctx->slock, flags);
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
 	ctx->state |= state;
-	spin_unlock_irqrestore(&ctx->slock, flags);
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
 }
 
 static inline bool fimc_ctx_state_is_set(u32 mask, struct fimc_ctx *ctx)
@@ -574,9 +568,9 @@ static inline bool fimc_ctx_state_is_set(u32 mask, struct fimc_ctx *ctx)
 	unsigned long flags;
 	bool ret;
 
-	spin_lock_irqsave(&ctx->slock, flags);
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
 	ret = (ctx->state & mask) == mask;
-	spin_unlock_irqrestore(&ctx->slock, flags);
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
 	return ret;
 }
 
@@ -589,61 +583,13 @@ static inline int tiled_fmt(struct fimc_fmt *fmt)
 static inline int fimc_get_alpha_mask(struct fimc_fmt *fmt)
 {
 	switch (fmt->color) {
-	case S5P_FIMC_RGB444:	return 0x0f;
-	case S5P_FIMC_RGB555:	return 0x01;
-	case S5P_FIMC_RGB888:	return 0xff;
+	case FIMC_FMT_RGB444:	return 0x0f;
+	case FIMC_FMT_RGB555:	return 0x01;
+	case FIMC_FMT_RGB888:	return 0xff;
 	default:		return 0;
 	};
 }
 
-static inline void fimc_hw_clear_irq(struct fimc_dev *dev)
-{
-	u32 cfg = readl(dev->regs + S5P_CIGCTRL);
-	cfg |= S5P_CIGCTRL_IRQ_CLR;
-	writel(cfg, dev->regs + S5P_CIGCTRL);
-}
-
-static inline void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on)
-{
-	u32 cfg = readl(dev->regs + S5P_CISCCTRL);
-	if (on)
-		cfg |= S5P_CISCCTRL_SCALERSTART;
-	else
-		cfg &= ~S5P_CISCCTRL_SCALERSTART;
-	writel(cfg, dev->regs + S5P_CISCCTRL);
-}
-
-static inline void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on)
-{
-	u32 cfg = readl(dev->regs + S5P_MSCTRL);
-	if (on)
-		cfg |= S5P_MSCTRL_ENVID;
-	else
-		cfg &= ~S5P_MSCTRL_ENVID;
-	writel(cfg, dev->regs + S5P_MSCTRL);
-}
-
-static inline void fimc_hw_dis_capture(struct fimc_dev *dev)
-{
-	u32 cfg = readl(dev->regs + S5P_CIIMGCPT);
-	cfg &= ~(S5P_CIIMGCPT_IMGCPTEN | S5P_CIIMGCPT_IMGCPTEN_SC);
-	writel(cfg, dev->regs + S5P_CIIMGCPT);
-}
-
-/**
- * fimc_hw_set_dma_seq - configure output DMA buffer sequence
- * @mask: each bit corresponds to one of 32 output buffer registers set
- *	  1 to include buffer in the sequence, 0 to disable
- *
- * This function mask output DMA ring buffers, i.e. it allows to configure
- * which of the output buffer address registers will be used by the DMA
- * engine.
- */
-static inline void fimc_hw_set_dma_seq(struct fimc_dev *dev, u32 mask)
-{
-	writel(mask, dev->regs + S5P_CIFCNTSEQ);
-}
-
 static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 					       enum v4l2_buf_type type)
 {
@@ -665,48 +611,6 @@ static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 	return frame;
 }
 
-/* Return an index to the buffer actually being written. */
-static inline u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
-{
-	u32 reg;
-
-	if (dev->variant->has_cistatus2) {
-		reg = readl(dev->regs + S5P_CISTATUS2) & 0x3F;
-		return reg > 0 ? --reg : reg;
-	} else {
-		reg = readl(dev->regs + S5P_CISTATUS);
-		return (reg & S5P_CISTATUS_FRAMECNT_MASK) >>
-			S5P_CISTATUS_FRAMECNT_SHIFT;
-	}
-}
-
-/* -----------------------------------------------------*/
-/* fimc-reg.c						*/
-void fimc_hw_reset(struct fimc_dev *fimc);
-void fimc_hw_set_rotation(struct fimc_ctx *ctx);
-void fimc_hw_set_target_format(struct fimc_ctx *ctx);
-void fimc_hw_set_out_dma(struct fimc_ctx *ctx);
-void fimc_hw_en_lastirq(struct fimc_dev *fimc, int enable);
-void fimc_hw_en_irq(struct fimc_dev *fimc, int enable);
-void fimc_hw_set_prescaler(struct fimc_ctx *ctx);
-void fimc_hw_set_mainscaler(struct fimc_ctx *ctx);
-void fimc_hw_en_capture(struct fimc_ctx *ctx);
-void fimc_hw_set_effect(struct fimc_ctx *ctx, bool active);
-void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx);
-void fimc_hw_set_in_dma(struct fimc_ctx *ctx);
-void fimc_hw_set_input_path(struct fimc_ctx *ctx);
-void fimc_hw_set_output_path(struct fimc_ctx *ctx);
-void fimc_hw_set_input_addr(struct fimc_dev *fimc, struct fimc_addr *paddr);
-void fimc_hw_set_output_addr(struct fimc_dev *fimc, struct fimc_addr *paddr,
-			     int index);
-int fimc_hw_set_camera_source(struct fimc_dev *fimc,
-			      struct s5p_fimc_isp_info *cam);
-int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f);
-int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
-				struct s5p_fimc_isp_info *cam);
-int fimc_hw_set_camera_type(struct fimc_dev *fimc,
-			    struct s5p_fimc_isp_info *cam);
-
 /* -----------------------------------------------------*/
 /* fimc-core.c */
 int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
@@ -720,6 +624,7 @@ void fimc_adjust_mplane_format(struct fimc_fmt *fmt, u32 width, u32 height,
 			       struct v4l2_pix_format_mplane *pix);
 struct fimc_fmt *fimc_find_format(const u32 *pixelformat, const u32 *mbus_code,
 				  unsigned int mask, int index);
+struct fimc_fmt *fimc_get_format(unsigned int index);
 
 int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
 			    int dw, int dh, int rotation);
@@ -730,7 +635,7 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f);
 void fimc_set_yuv_order(struct fimc_ctx *ctx);
 void fimc_fill_frame(struct fimc_frame *frame, struct v4l2_format *f);
-void fimc_capture_irq_handler(struct fimc_dev *fimc, bool done);
+void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf);
 
 int fimc_register_m2m_device(struct fimc_dev *fimc,
 			     struct v4l2_device *v4l2_dev);
@@ -739,33 +644,18 @@ int fimc_register_driver(void);
 void fimc_unregister_driver(void);
 
 /* -----------------------------------------------------*/
+/* fimc-m2m.c */
+void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state);
+
+/* -----------------------------------------------------*/
 /* fimc-capture.c					*/
-int fimc_register_capture_device(struct fimc_dev *fimc,
-				 struct v4l2_device *v4l2_dev);
-void fimc_unregister_capture_device(struct fimc_dev *fimc);
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc);
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc);
 int fimc_capture_ctrls_create(struct fimc_dev *fimc);
-int fimc_vid_cap_buf_queue(struct fimc_dev *fimc,
-			     struct fimc_vid_buffer *fimc_vb);
 void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 			void *arg);
 int fimc_capture_suspend(struct fimc_dev *fimc);
 int fimc_capture_resume(struct fimc_dev *fimc);
-int fimc_capture_config_update(struct fimc_ctx *ctx);
-
-/* Locking: the caller holds fimc->slock */
-static inline void fimc_activate_capture(struct fimc_ctx *ctx)
-{
-	fimc_hw_enable_scaler(ctx->fimc_dev, ctx->scaler.enabled);
-	fimc_hw_en_capture(ctx);
-}
-
-static inline void fimc_deactivate_capture(struct fimc_dev *fimc)
-{
-	fimc_hw_en_lastirq(fimc, true);
-	fimc_hw_dis_capture(fimc);
-	fimc_hw_enable_scaler(fimc, false);
-	fimc_hw_en_lastirq(fimc, false);
-}
 
 /*
  * Buffer list manipulation functions. Must be called with fimc.slock held.
diff --git a/drivers/media/video/s5p-fimc/fimc-lite-reg.c b/drivers/media/video/s5p-fimc/fimc-lite-reg.c
new file mode 100644
index 000000000000..419adfb7cdf9
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite-reg.c
@@ -0,0 +1,300 @@
+/*
+ * Register interface file for EXYNOS FIMC-LITE (camera interface) driver
+ *
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ * 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
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <media/s5p_fimc.h>
+
+#include "fimc-lite-reg.h"
+#include "fimc-lite.h"
+#include "fimc-core.h"
+
+#define FLITE_RESET_TIMEOUT 50 /* in ms */
+
+void flite_hw_reset(struct fimc_lite *dev)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(FLITE_RESET_TIMEOUT);
+	u32 cfg;
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg |= FLITE_REG_CIGCTRL_SWRST_REQ;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	while (time_is_after_jiffies(end)) {
+		cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+		if (cfg & FLITE_REG_CIGCTRL_SWRST_RDY)
+			break;
+		usleep_range(1000, 5000);
+	}
+
+	cfg |= FLITE_REG_CIGCTRL_SWRST;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+void flite_hw_clear_pending_irq(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CISTATUS);
+	cfg &= ~FLITE_REG_CISTATUS_IRQ_CAM;
+	writel(cfg, dev->regs + FLITE_REG_CISTATUS);
+}
+
+u32 flite_hw_get_interrupt_source(struct fimc_lite *dev)
+{
+	u32 intsrc = readl(dev->regs + FLITE_REG_CISTATUS);
+	return intsrc & FLITE_REG_CISTATUS_IRQ_MASK;
+}
+
+void flite_hw_clear_last_capture_end(struct fimc_lite *dev)
+{
+
+	u32 cfg = readl(dev->regs + FLITE_REG_CISTATUS2);
+	cfg &= ~FLITE_REG_CISTATUS2_LASTCAPEND;
+	writel(cfg, dev->regs + FLITE_REG_CISTATUS2);
+}
+
+void flite_hw_set_interrupt_mask(struct fimc_lite *dev)
+{
+	u32 cfg, intsrc;
+
+	/* Select interrupts to be enabled for each output mode */
+	if (dev->out_path == FIMC_IO_DMA) {
+		intsrc = FLITE_REG_CIGCTRL_IRQ_OVFEN |
+			 FLITE_REG_CIGCTRL_IRQ_LASTEN |
+			 FLITE_REG_CIGCTRL_IRQ_STARTEN;
+	} else {
+		/* An output to the FIMC-IS */
+		intsrc = FLITE_REG_CIGCTRL_IRQ_OVFEN |
+			 FLITE_REG_CIGCTRL_IRQ_LASTEN;
+	}
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg |= FLITE_REG_CIGCTRL_IRQ_DISABLE_MASK;
+	cfg &= ~intsrc;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+void flite_hw_capture_start(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIIMGCPT);
+	cfg |= FLITE_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FLITE_REG_CIIMGCPT);
+}
+
+void flite_hw_capture_stop(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIIMGCPT);
+	cfg &= ~FLITE_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FLITE_REG_CIIMGCPT);
+}
+
+/*
+ * Test pattern (color bars) enable/disable. External sensor
+ * pixel clock must be active for the test pattern to work.
+ */
+void flite_hw_set_test_pattern(struct fimc_lite *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	if (on)
+		cfg |= FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR;
+	else
+		cfg &= ~FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+static const u32 src_pixfmt_map[8][3] = {
+	{ V4L2_MBUS_FMT_YUYV8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_YCBYCR,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_MBUS_FMT_YVYU8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_YCRYCB,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_MBUS_FMT_UYVY8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_CBYCRY,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_MBUS_FMT_VYUY8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_CRYCBY,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_PIX_FMT_SGRBG8, 0, FLITE_REG_CIGCTRL_RAW8 },
+	{ V4L2_PIX_FMT_SGRBG10, 0, FLITE_REG_CIGCTRL_RAW10 },
+	{ V4L2_PIX_FMT_SGRBG12, 0, FLITE_REG_CIGCTRL_RAW12 },
+	{ V4L2_MBUS_FMT_JPEG_1X8, 0, FLITE_REG_CIGCTRL_USER(1) },
+};
+
+/* Set camera input pixel format and resolution */
+void flite_hw_set_source_format(struct fimc_lite *dev, struct flite_frame *f)
+{
+	enum v4l2_mbus_pixelcode pixelcode = dev->fmt->mbus_code;
+	unsigned int i = ARRAY_SIZE(src_pixfmt_map);
+	u32 cfg;
+
+	while (i-- >= 0) {
+		if (src_pixfmt_map[i][0] == pixelcode)
+			break;
+	}
+
+	if (i == 0 && src_pixfmt_map[i][0] != pixelcode) {
+		v4l2_err(dev->vfd,
+			 "Unsupported pixel code, falling back to %#08x\n",
+			 src_pixfmt_map[i][0]);
+	}
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg &= ~FLITE_REG_CIGCTRL_FMT_MASK;
+	cfg |= src_pixfmt_map[i][2];
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	cfg = readl(dev->regs + FLITE_REG_CISRCSIZE);
+	cfg &= ~(FLITE_REG_CISRCSIZE_ORDER422_MASK |
+		 FLITE_REG_CISRCSIZE_SIZE_CAM_MASK);
+	cfg |= (f->f_width << 16) | f->f_height;
+	cfg |= src_pixfmt_map[i][1];
+	writel(cfg, dev->regs + FLITE_REG_CISRCSIZE);
+}
+
+/* Set the camera host input window offsets (cropping) */
+void flite_hw_set_window_offset(struct fimc_lite *dev, struct flite_frame *f)
+{
+	u32 hoff2, voff2;
+	u32 cfg;
+
+	cfg = readl(dev->regs + FLITE_REG_CIWDOFST);
+	cfg &= ~FLITE_REG_CIWDOFST_OFST_MASK;
+	cfg |= (f->rect.left << 16) | f->rect.top;
+	cfg |= FLITE_REG_CIWDOFST_WINOFSEN;
+	writel(cfg, dev->regs + FLITE_REG_CIWDOFST);
+
+	hoff2 = f->f_width - f->rect.width - f->rect.left;
+	voff2 = f->f_height - f->rect.height - f->rect.top;
+
+	cfg = (hoff2 << 16) | voff2;
+	writel(cfg, dev->regs + FLITE_REG_CIWDOFST2);
+}
+
+/* Select camera port (A, B) */
+static void flite_hw_set_camera_port(struct fimc_lite *dev, int id)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGENERAL);
+	if (id == 0)
+		cfg &= ~FLITE_REG_CIGENERAL_CAM_B;
+	else
+		cfg |= FLITE_REG_CIGENERAL_CAM_B;
+	writel(cfg, dev->regs + FLITE_REG_CIGENERAL);
+}
+
+/* Select serial or parallel bus, camera port (A,B) and set signals polarity */
+void flite_hw_set_camera_bus(struct fimc_lite *dev,
+			     struct s5p_fimc_isp_info *s_info)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	unsigned int flags = s_info->flags;
+
+	if (s_info->bus_type != FIMC_MIPI_CSI2) {
+		cfg &= ~(FLITE_REG_CIGCTRL_SELCAM_MIPI |
+			 FLITE_REG_CIGCTRL_INVPOLPCLK |
+			 FLITE_REG_CIGCTRL_INVPOLVSYNC |
+			 FLITE_REG_CIGCTRL_INVPOLHREF);
+
+		if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLPCLK;
+
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLVSYNC;
+
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLHREF;
+	} else {
+		cfg |= FLITE_REG_CIGCTRL_SELCAM_MIPI;
+	}
+
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	flite_hw_set_camera_port(dev, s_info->mux_id);
+}
+
+void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
+{
+	static const u32 pixcode[4][2] = {
+		{ V4L2_MBUS_FMT_YUYV8_2X8, FLITE_REG_CIODMAFMT_YCBYCR },
+		{ V4L2_MBUS_FMT_YVYU8_2X8, FLITE_REG_CIODMAFMT_YCRYCB },
+		{ V4L2_MBUS_FMT_UYVY8_2X8, FLITE_REG_CIODMAFMT_CBYCRY },
+		{ V4L2_MBUS_FMT_VYUY8_2X8, FLITE_REG_CIODMAFMT_CRYCBY },
+	};
+	u32 cfg = readl(dev->regs + FLITE_REG_CIODMAFMT);
+	unsigned int i = ARRAY_SIZE(pixcode);
+
+	while (i-- >= 0)
+		if (pixcode[i][0] == dev->fmt->mbus_code)
+			break;
+	cfg &= ~FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK;
+	writel(cfg | pixcode[i][1], dev->regs + FLITE_REG_CIODMAFMT);
+}
+
+void flite_hw_set_dma_window(struct fimc_lite *dev, struct flite_frame *f)
+{
+	u32 cfg;
+
+	/* Maximum output pixel size */
+	cfg = readl(dev->regs + FLITE_REG_CIOCAN);
+	cfg &= ~FLITE_REG_CIOCAN_MASK;
+	cfg = (f->f_height << 16) | f->f_width;
+	writel(cfg, dev->regs + FLITE_REG_CIOCAN);
+
+	/* DMA offsets */
+	cfg = readl(dev->regs + FLITE_REG_CIOOFF);
+	cfg &= ~FLITE_REG_CIOOFF_MASK;
+	cfg |= (f->rect.top << 16) | f->rect.left;
+	writel(cfg, dev->regs + FLITE_REG_CIOOFF);
+}
+
+/* Enable/disable output DMA, set output pixel size and offsets (composition) */
+void flite_hw_set_output_dma(struct fimc_lite *dev, struct flite_frame *f,
+			     bool enable)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+
+	if (!enable) {
+		cfg |= FLITE_REG_CIGCTRL_ODMA_DISABLE;
+		writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+		return;
+	}
+
+	cfg &= ~FLITE_REG_CIGCTRL_ODMA_DISABLE;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	flite_hw_set_out_order(dev, f);
+	flite_hw_set_dma_window(dev, f);
+}
+
+void flite_hw_dump_regs(struct fimc_lite *dev, const char *label)
+{
+	struct {
+		u32 offset;
+		const char * const name;
+	} registers[] = {
+		{ 0x00, "CISRCSIZE" },
+		{ 0x04, "CIGCTRL" },
+		{ 0x08, "CIIMGCPT" },
+		{ 0x0c, "CICPTSEQ" },
+		{ 0x10, "CIWDOFST" },
+		{ 0x14, "CIWDOFST2" },
+		{ 0x18, "CIODMAFMT" },
+		{ 0x20, "CIOCAN" },
+		{ 0x24, "CIOOFF" },
+		{ 0x30, "CIOSA" },
+		{ 0x40, "CISTATUS" },
+		{ 0x44, "CISTATUS2" },
+		{ 0xf0, "CITHOLD" },
+		{ 0xfc, "CIGENERAL" },
+	};
+	u32 i;
+
+	pr_info("--- %s ---\n", label);
+	for (i = 0; i < ARRAY_SIZE(registers); i++) {
+		u32 cfg = readl(dev->regs + registers[i].offset);
+		pr_info("%s: %s:\t0x%08x\n", __func__, registers[i].name, cfg);
+	}
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-lite-reg.h b/drivers/media/video/s5p-fimc/fimc-lite-reg.h
new file mode 100644
index 000000000000..adb9e9e6f3c2
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite-reg.h
@@ -0,0 +1,150 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *
+ * 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 FIMC_LITE_REG_H_
+#define FIMC_LITE_REG_H_
+
+#include "fimc-lite.h"
+
+/* Camera Source size */
+#define FLITE_REG_CISRCSIZE			0x00
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_YCBYCR	(0 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_YCRYCB	(1 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_CBYCRY	(2 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_CRYCBY	(3 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_MASK	(0x3 << 14)
+#define FLITE_REG_CISRCSIZE_SIZE_CAM_MASK	(0x3fff << 16 | 0x3fff)
+
+/* Global control */
+#define FLITE_REG_CIGCTRL			0x04
+#define FLITE_REG_CIGCTRL_YUV422_1P		(0x1e << 24)
+#define FLITE_REG_CIGCTRL_RAW8			(0x2a << 24)
+#define FLITE_REG_CIGCTRL_RAW10			(0x2b << 24)
+#define FLITE_REG_CIGCTRL_RAW12			(0x2c << 24)
+#define FLITE_REG_CIGCTRL_RAW14			(0x2d << 24)
+/* User defined formats. x = 0...15 */
+#define FLITE_REG_CIGCTRL_USER(x)		((0x30 + x - 1) << 24)
+#define FLITE_REG_CIGCTRL_FMT_MASK		(0x3f << 24)
+#define FLITE_REG_CIGCTRL_SHADOWMASK_DISABLE	(1 << 21)
+#define FLITE_REG_CIGCTRL_ODMA_DISABLE		(1 << 20)
+#define FLITE_REG_CIGCTRL_SWRST_REQ		(1 << 19)
+#define FLITE_REG_CIGCTRL_SWRST_RDY		(1 << 18)
+#define FLITE_REG_CIGCTRL_SWRST			(1 << 17)
+#define FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR	(1 << 15)
+#define FLITE_REG_CIGCTRL_INVPOLPCLK		(1 << 14)
+#define FLITE_REG_CIGCTRL_INVPOLVSYNC		(1 << 13)
+#define FLITE_REG_CIGCTRL_INVPOLHREF		(1 << 12)
+/* Interrupts mask bits (1 disables an interrupt) */
+#define FLITE_REG_CIGCTRL_IRQ_LASTEN		(1 << 8)
+#define FLITE_REG_CIGCTRL_IRQ_ENDEN		(1 << 7)
+#define FLITE_REG_CIGCTRL_IRQ_STARTEN		(1 << 6)
+#define FLITE_REG_CIGCTRL_IRQ_OVFEN		(1 << 5)
+#define FLITE_REG_CIGCTRL_IRQ_DISABLE_MASK	(0xf << 5)
+#define FLITE_REG_CIGCTRL_SELCAM_MIPI		(1 << 3)
+
+/* Image Capture Enable */
+#define FLITE_REG_CIIMGCPT			0x08
+#define FLITE_REG_CIIMGCPT_IMGCPTEN		(1 << 31)
+#define FLITE_REG_CIIMGCPT_CPT_FREN		(1 << 25)
+#define FLITE_REG_CIIMGCPT_CPT_MOD_FRCNT	(1 << 18)
+#define FLITE_REG_CIIMGCPT_CPT_MOD_FREN		(0 << 18)
+
+/* Capture Sequence */
+#define FLITE_REG_CICPTSEQ			0x0c
+
+/* Camera Window Offset */
+#define FLITE_REG_CIWDOFST			0x10
+#define FLITE_REG_CIWDOFST_WINOFSEN		(1 << 31)
+#define FLITE_REG_CIWDOFST_CLROVIY		(1 << 31)
+#define FLITE_REG_CIWDOFST_CLROVFICB		(1 << 15)
+#define FLITE_REG_CIWDOFST_CLROVFICR		(1 << 14)
+#define FLITE_REG_CIWDOFST_OFST_MASK		((0x1fff << 16) | 0x1fff)
+
+/* Camera Window Offset2 */
+#define FLITE_REG_CIWDOFST2			0x14
+
+/* Camera Output DMA Format */
+#define FLITE_REG_CIODMAFMT			0x18
+#define FLITE_REG_CIODMAFMT_RAW_CON		(1 << 15)
+#define FLITE_REG_CIODMAFMT_PACK12		(1 << 14)
+#define FLITE_REG_CIODMAFMT_CRYCBY		(0 << 4)
+#define FLITE_REG_CIODMAFMT_CBYCRY		(1 << 4)
+#define FLITE_REG_CIODMAFMT_YCRYCB		(2 << 4)
+#define FLITE_REG_CIODMAFMT_YCBYCR		(3 << 4)
+#define FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK	(0x3 << 4)
+
+/* Camera Output Canvas */
+#define FLITE_REG_CIOCAN			0x20
+#define FLITE_REG_CIOCAN_MASK			((0x3fff << 16) | 0x3fff)
+
+/* Camera Output DMA Offset */
+#define FLITE_REG_CIOOFF			0x24
+#define FLITE_REG_CIOOFF_MASK			((0x3fff << 16) | 0x3fff)
+
+/* Camera Output DMA Start Address */
+#define FLITE_REG_CIOSA				0x30
+
+/* Camera Status */
+#define FLITE_REG_CISTATUS			0x40
+#define FLITE_REG_CISTATUS_MIPI_VVALID		(1 << 22)
+#define FLITE_REG_CISTATUS_MIPI_HVALID		(1 << 21)
+#define FLITE_REG_CISTATUS_MIPI_DVALID		(1 << 20)
+#define FLITE_REG_CISTATUS_ITU_VSYNC		(1 << 14)
+#define FLITE_REG_CISTATUS_ITU_HREFF		(1 << 13)
+#define FLITE_REG_CISTATUS_OVFIY		(1 << 10)
+#define FLITE_REG_CISTATUS_OVFICB		(1 << 9)
+#define FLITE_REG_CISTATUS_OVFICR		(1 << 8)
+#define FLITE_REG_CISTATUS_IRQ_SRC_OVERFLOW	(1 << 7)
+#define FLITE_REG_CISTATUS_IRQ_SRC_LASTCAPEND	(1 << 6)
+#define FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART	(1 << 5)
+#define FLITE_REG_CISTATUS_IRQ_SRC_FRMEND	(1 << 4)
+#define FLITE_REG_CISTATUS_IRQ_CAM		(1 << 0)
+#define FLITE_REG_CISTATUS_IRQ_MASK		(0xf << 4)
+
+/* Camera Status2 */
+#define FLITE_REG_CISTATUS2			0x44
+#define FLITE_REG_CISTATUS2_LASTCAPEND		(1 << 1)
+#define FLITE_REG_CISTATUS2_FRMEND		(1 << 0)
+
+/* Qos Threshold */
+#define FLITE_REG_CITHOLD			0xf0
+#define FLITE_REG_CITHOLD_W_QOS_EN		(1 << 30)
+
+/* Camera General Purpose */
+#define FLITE_REG_CIGENERAL			0xfc
+/* b0: 1 - camera B, 0 - camera A */
+#define FLITE_REG_CIGENERAL_CAM_B		(1 << 0)
+
+/* ----------------------------------------------------------------------------
+ * Function declarations
+ */
+void flite_hw_reset(struct fimc_lite *dev);
+void flite_hw_clear_pending_irq(struct fimc_lite *dev);
+u32 flite_hw_get_interrupt_source(struct fimc_lite *dev);
+void flite_hw_clear_last_capture_end(struct fimc_lite *dev);
+void flite_hw_set_interrupt_mask(struct fimc_lite *dev);
+void flite_hw_capture_start(struct fimc_lite *dev);
+void flite_hw_capture_stop(struct fimc_lite *dev);
+void flite_hw_set_camera_bus(struct fimc_lite *dev,
+			     struct s5p_fimc_isp_info *s_info);
+void flite_hw_set_camera_polarity(struct fimc_lite *dev,
+				  struct s5p_fimc_isp_info *cam);
+void flite_hw_set_window_offset(struct fimc_lite *dev, struct flite_frame *f);
+void flite_hw_set_source_format(struct fimc_lite *dev, struct flite_frame *f);
+
+void flite_hw_set_output_dma(struct fimc_lite *dev, struct flite_frame *f,
+			     bool enable);
+void flite_hw_set_dma_window(struct fimc_lite *dev, struct flite_frame *f);
+void flite_hw_set_test_pattern(struct fimc_lite *dev, bool on);
+void flite_hw_dump_regs(struct fimc_lite *dev, const char *label);
+
+static inline void flite_hw_set_output_addr(struct fimc_lite *dev, u32 paddr)
+{
+	writel(paddr, dev->regs + FLITE_REG_CIOSA);
+}
+#endif /* FIMC_LITE_REG_H */
diff --git a/drivers/media/video/s5p-fimc/fimc-lite.c b/drivers/media/video/s5p-fimc/fimc-lite.c
new file mode 100644
index 000000000000..400d701aef04
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite.c
@@ -0,0 +1,1576 @@
+/*
+ * Samsung EXYNOS FIMC-LITE (camera host interface) driver
+*
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ * 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
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bug.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "fimc-mdevice.h"
+#include "fimc-core.h"
+#include "fimc-lite-reg.h"
+
+static int debug;
+module_param(debug, int, 0644);
+
+static const struct fimc_fmt fimc_lite_formats[] = {
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCBYCR422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CBYCRY422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CRYCBY422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCRYCB422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
+	}, {
+		.name		= "RAW8 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG8,
+		.depth		= { 8 },
+		.color		= FIMC_FMT_RAW8,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_SGRBG8_1X8,
+	}, {
+		.name		= "RAW10 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG10,
+		.depth		= { 10 },
+		.color		= FIMC_FMT_RAW10,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_SGRBG10_1X10,
+	}, {
+		.name		= "RAW12 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG12,
+		.depth		= { 12 },
+		.color		= FIMC_FMT_RAW12,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_SGRBG12_1X12,
+	},
+};
+
+/**
+ * fimc_lite_find_format - lookup fimc color format by fourcc or media bus code
+ * @pixelformat: fourcc to match, ignored if null
+ * @mbus_code: media bus code to match, ignored if null
+ * @index: index to the fimc_lite_formats array, ignored if negative
+ */
+static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
+					const u32 *mbus_code, int index)
+{
+	const struct fimc_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+	int id = 0;
+
+	if (index >= (int)ARRAY_SIZE(fimc_lite_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_lite_formats); ++i) {
+		fmt = &fimc_lite_formats[i];
+		if (pixelformat && fmt->fourcc == *pixelformat)
+			return fmt;
+		if (mbus_code && fmt->mbus_code == *mbus_code)
+			return fmt;
+		if (index == id)
+			def_fmt = fmt;
+		id++;
+	}
+	return def_fmt;
+}
+
+static int fimc_lite_hw_init(struct fimc_lite *fimc)
+{
+	struct fimc_pipeline *pipeline = &fimc->pipeline;
+	struct fimc_sensor_info *sensor;
+	unsigned long flags;
+
+	if (pipeline->subdevs[IDX_SENSOR] == NULL)
+		return -ENXIO;
+
+	if (fimc->fmt == NULL)
+		return -EINVAL;
+
+	sensor = v4l2_get_subdev_hostdata(pipeline->subdevs[IDX_SENSOR]);
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	flite_hw_set_camera_bus(fimc, sensor->pdata);
+	flite_hw_set_source_format(fimc, &fimc->inp_frame);
+	flite_hw_set_window_offset(fimc, &fimc->inp_frame);
+	flite_hw_set_output_dma(fimc, &fimc->out_frame, true);
+	flite_hw_set_interrupt_mask(fimc);
+	flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
+
+	if (debug > 0)
+		flite_hw_dump_regs(fimc, __func__);
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return 0;
+}
+
+/*
+ * Reinitialize the driver so it is ready to start the streaming again.
+ * Set fimc->state to indicate stream off and the hardware shut down state.
+ * If not suspending (@suspend is false), return any buffers to videobuf2.
+ * Otherwise put any owned buffers onto the pending buffers queue, so they
+ * can be re-spun when the device is being resumed. Also perform FIMC
+ * software reset and disable streaming on the whole pipeline if required.
+ */
+static int fimc_lite_reinit(struct fimc_lite *fimc, bool suspend)
+{
+	struct flite_buffer *buf;
+	unsigned long flags;
+	bool streaming;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	streaming = fimc->state & (1 << ST_SENSOR_STREAM);
+
+	fimc->state &= ~(1 << ST_FLITE_RUN | 1 << ST_FLITE_OFF |
+			 1 << ST_FLITE_STREAM | 1 << ST_SENSOR_STREAM);
+	if (suspend)
+		fimc->state |= (1 << ST_FLITE_SUSPENDED);
+	else
+		fimc->state &= ~(1 << ST_FLITE_PENDING |
+				 1 << ST_FLITE_SUSPENDED);
+
+	/* Release unused buffers */
+	while (!suspend && !list_empty(&fimc->pending_buf_q)) {
+		buf = fimc_lite_pending_queue_pop(fimc);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+	/* If suspending put unused buffers onto pending queue */
+	while (!list_empty(&fimc->active_buf_q)) {
+		buf = fimc_lite_active_queue_pop(fimc);
+		if (suspend)
+			fimc_lite_pending_queue_add(fimc, buf);
+		else
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	flite_hw_reset(fimc);
+
+	if (!streaming)
+		return 0;
+
+	return fimc_pipeline_s_stream(&fimc->pipeline, 0);
+}
+
+static int fimc_lite_stop_capture(struct fimc_lite *fimc, bool suspend)
+{
+	unsigned long flags;
+
+	if (!fimc_lite_active(fimc))
+		return 0;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	set_bit(ST_FLITE_OFF, &fimc->state);
+	flite_hw_capture_stop(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	wait_event_timeout(fimc->irq_queue,
+			   !test_bit(ST_FLITE_OFF, &fimc->state),
+			   (2*HZ/10)); /* 200 ms */
+
+	return fimc_lite_reinit(fimc, suspend);
+}
+
+/* Must be called  with fimc.slock spinlock held. */
+static void fimc_lite_config_update(struct fimc_lite *fimc)
+{
+	flite_hw_set_window_offset(fimc, &fimc->inp_frame);
+	flite_hw_set_dma_window(fimc, &fimc->out_frame);
+	flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
+	clear_bit(ST_FLITE_CONFIG, &fimc->state);
+}
+
+static irqreturn_t flite_irq_handler(int irq, void *priv)
+{
+	struct fimc_lite *fimc = priv;
+	struct flite_buffer *vbuf;
+	unsigned long flags;
+	struct timeval *tv;
+	struct timespec ts;
+	u32 intsrc;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	intsrc = flite_hw_get_interrupt_source(fimc);
+	flite_hw_clear_pending_irq(fimc);
+
+	if (test_and_clear_bit(ST_FLITE_OFF, &fimc->state)) {
+		wake_up(&fimc->irq_queue);
+		goto done;
+	}
+
+	if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_OVERFLOW) {
+		clear_bit(ST_FLITE_RUN, &fimc->state);
+		fimc->events.data_overflow++;
+	}
+
+	if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_LASTCAPEND) {
+		flite_hw_clear_last_capture_end(fimc);
+		clear_bit(ST_FLITE_STREAM, &fimc->state);
+		wake_up(&fimc->irq_queue);
+	}
+
+	if (fimc->out_path != FIMC_IO_DMA)
+		goto done;
+
+	if ((intsrc & FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART) &&
+	    test_bit(ST_FLITE_RUN, &fimc->state) &&
+	    !list_empty(&fimc->active_buf_q) &&
+	    !list_empty(&fimc->pending_buf_q)) {
+		vbuf = fimc_lite_active_queue_pop(fimc);
+		ktime_get_ts(&ts);
+		tv = &vbuf->vb.v4l2_buf.timestamp;
+		tv->tv_sec = ts.tv_sec;
+		tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+		vbuf->vb.v4l2_buf.sequence = fimc->frame_count++;
+		vb2_buffer_done(&vbuf->vb, VB2_BUF_STATE_DONE);
+
+		vbuf = fimc_lite_pending_queue_pop(fimc);
+		flite_hw_set_output_addr(fimc, vbuf->paddr);
+		fimc_lite_active_queue_add(fimc, vbuf);
+	}
+
+	if (test_bit(ST_FLITE_CONFIG, &fimc->state))
+		fimc_lite_config_update(fimc);
+
+	if (list_empty(&fimc->pending_buf_q)) {
+		flite_hw_capture_stop(fimc);
+		clear_bit(ST_FLITE_STREAM, &fimc->state);
+	}
+done:
+	set_bit(ST_FLITE_RUN, &fimc->state);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return IRQ_HANDLED;
+}
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fimc_lite *fimc = q->drv_priv;
+	int ret;
+
+	fimc->frame_count = 0;
+
+	ret = fimc_lite_hw_init(fimc);
+	if (ret) {
+		fimc_lite_reinit(fimc, false);
+		return ret;
+	}
+
+	set_bit(ST_FLITE_PENDING, &fimc->state);
+
+	if (!list_empty(&fimc->active_buf_q) &&
+	    !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
+		flite_hw_capture_start(fimc);
+
+		if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
+	}
+	if (debug > 0)
+		flite_hw_dump_regs(fimc, __func__);
+
+	return 0;
+}
+
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_lite *fimc = q->drv_priv;
+
+	if (!fimc_lite_active(fimc))
+		return -EINVAL;
+
+	return fimc_lite_stop_capture(fimc, false);
+}
+
+static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
+		       unsigned int *num_buffers, unsigned int *num_planes,
+		       unsigned int sizes[], void *allocators[])
+{
+	const struct v4l2_pix_format_mplane *pixm = NULL;
+	struct fimc_lite *fimc = vq->drv_priv;
+	struct flite_frame *frame = &fimc->out_frame;
+	const struct fimc_fmt *fmt = fimc->fmt;
+	unsigned long wh;
+	int i;
+
+	if (pfmt) {
+		pixm = &pfmt->fmt.pix_mp;
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, -1);
+		wh = pixm->width * pixm->height;
+	} else {
+		wh = frame->f_width * frame->f_height;
+	}
+
+	if (fmt == NULL)
+		return -EINVAL;
+
+	*num_planes = fmt->memplanes;
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		unsigned int size = (wh * fmt->depth[i]) / 8;
+		if (pixm)
+			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+		else
+			sizes[i] = size;
+		allocators[i] = fimc->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct fimc_lite *fimc = vq->drv_priv;
+	int i;
+
+	if (fimc->fmt == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < fimc->fmt->memplanes; i++) {
+		unsigned long size = fimc->payload[i];
+
+		if (vb2_plane_size(vb, i) < size) {
+			v4l2_err(fimc->vfd,
+				 "User buffer too small (%ld < %ld)\n",
+				 vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct flite_buffer *buf
+		= container_of(vb, struct flite_buffer, vb);
+	struct fimc_lite *fimc = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	buf->paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (!test_bit(ST_FLITE_SUSPENDED, &fimc->state) &&
+	    !test_bit(ST_FLITE_STREAM, &fimc->state) &&
+	    list_empty(&fimc->active_buf_q)) {
+		flite_hw_set_output_addr(fimc, buf->paddr);
+		fimc_lite_active_queue_add(fimc, buf);
+	} else {
+		fimc_lite_pending_queue_add(fimc, buf);
+	}
+
+	if (vb2_is_streaming(&fimc->vb_queue) &&
+	    !list_empty(&fimc->pending_buf_q) &&
+	    !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
+		flite_hw_capture_start(fimc);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+
+		if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
+		return;
+	}
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static void fimc_lock(struct vb2_queue *vq)
+{
+	struct fimc_lite *fimc = vb2_get_drv_priv(vq);
+	mutex_lock(&fimc->lock);
+}
+
+static void fimc_unlock(struct vb2_queue *vq)
+{
+	struct fimc_lite *fimc = vb2_get_drv_priv(vq);
+	mutex_unlock(&fimc->lock);
+}
+
+static const struct vb2_ops fimc_lite_qops = {
+	.queue_setup	 = queue_setup,
+	.buf_prepare	 = buffer_prepare,
+	.buf_queue	 = buffer_queue,
+	.wait_prepare	 = fimc_unlock,
+	.wait_finish	 = fimc_lock,
+	.start_streaming = start_streaming,
+	.stop_streaming	 = stop_streaming,
+};
+
+static void fimc_lite_clear_event_counters(struct fimc_lite *fimc)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	memset(&fimc->events, 0, sizeof(fimc->events));
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static int fimc_lite_open(struct file *file)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	int ret = v4l2_fh_open(file);
+
+	if (ret)
+		return ret;
+
+	set_bit(ST_FLITE_IN_USE, &fimc->state);
+	pm_runtime_get_sync(&fimc->pdev->dev);
+
+	if (++fimc->ref_count != 1 || fimc->out_path != FIMC_IO_DMA)
+		return ret;
+
+	ret = fimc_pipeline_initialize(&fimc->pipeline, &fimc->vfd->entity,
+				       true);
+	if (ret < 0) {
+		v4l2_err(fimc->vfd, "Video pipeline initialization failed\n");
+		pm_runtime_put_sync(&fimc->pdev->dev);
+		fimc->ref_count--;
+		v4l2_fh_release(file);
+		clear_bit(ST_FLITE_IN_USE, &fimc->state);
+	}
+
+	fimc_lite_clear_event_counters(fimc);
+	return ret;
+}
+
+static int fimc_lite_close(struct file *file)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	if (--fimc->ref_count == 0 && fimc->out_path == FIMC_IO_DMA) {
+		clear_bit(ST_FLITE_IN_USE, &fimc->state);
+		fimc_lite_stop_capture(fimc, false);
+		fimc_pipeline_shutdown(&fimc->pipeline);
+		clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+	}
+
+	pm_runtime_put(&fimc->pdev->dev);
+
+	if (fimc->ref_count == 0)
+		vb2_queue_release(&fimc->vb_queue);
+
+	return v4l2_fh_release(file);
+}
+
+static unsigned int fimc_lite_poll(struct file *file,
+				   struct poll_table_struct *wait)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	return vb2_poll(&fimc->vb_queue, file, wait);
+}
+
+static int fimc_lite_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	return vb2_mmap(&fimc->vb_queue, vma);
+}
+
+static const struct v4l2_file_operations fimc_lite_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fimc_lite_open,
+	.release	= fimc_lite_close,
+	.poll		= fimc_lite_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= fimc_lite_mmap,
+};
+
+/*
+ * Format and crop negotiation helpers
+ */
+
+static const struct fimc_fmt *fimc_lite_try_format(struct fimc_lite *fimc,
+					u32 *width, u32 *height,
+					u32 *code, u32 *fourcc, int pad)
+{
+	struct flite_variant *variant = fimc->variant;
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_lite_find_format(fourcc, code, 0);
+	if (WARN_ON(!fmt))
+		return NULL;
+
+	if (code)
+		*code = fmt->mbus_code;
+	if (fourcc)
+		*fourcc = fmt->fourcc;
+
+	if (pad == FLITE_SD_PAD_SINK) {
+		v4l_bound_align_image(width, 8, variant->max_width,
+				      ffs(variant->out_width_align) - 1,
+				      height, 0, variant->max_height, 0, 0);
+	} else {
+		v4l_bound_align_image(width, 8, fimc->inp_frame.rect.width,
+				      ffs(variant->out_width_align) - 1,
+				      height, 0, fimc->inp_frame.rect.height,
+				      0, 0);
+	}
+
+	v4l2_dbg(1, debug, &fimc->subdev, "code: 0x%x, %dx%d\n",
+		 code ? *code : 0, *width, *height);
+
+	return fmt;
+}
+
+static void fimc_lite_try_crop(struct fimc_lite *fimc, struct v4l2_rect *r)
+{
+	struct flite_frame *frame = &fimc->inp_frame;
+
+	v4l_bound_align_image(&r->width, 0, frame->f_width, 0,
+			      &r->height, 0, frame->f_height, 0, 0);
+
+	/* Adjust left/top if cropping rectangle got out of bounds */
+	r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
+	r->left = round_down(r->left, fimc->variant->win_hor_offs_align);
+	r->top  = clamp_t(u32, r->top, 0, frame->f_height - r->height);
+
+	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, sink fmt: %dx%d",
+		 r->left, r->top, r->width, r->height,
+		 frame->f_width, frame->f_height);
+}
+
+static void fimc_lite_try_compose(struct fimc_lite *fimc, struct v4l2_rect *r)
+{
+	struct flite_frame *frame = &fimc->out_frame;
+	struct v4l2_rect *crop_rect = &fimc->inp_frame.rect;
+
+	/* Scaling is not supported so we enforce compose rectangle size
+	   same as size of the sink crop rectangle. */
+	r->width = crop_rect->width;
+	r->height = crop_rect->height;
+
+	/* Adjust left/top if the composing rectangle got out of bounds */
+	r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
+	r->left = round_down(r->left, fimc->variant->out_hor_offs_align);
+	r->top  = clamp_t(u32, r->top, 0, fimc->out_frame.f_height - r->height);
+
+	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, source fmt: %dx%d",
+		 r->left, r->top, r->width, r->height,
+		 frame->f_width, frame->f_height);
+}
+
+/*
+ * Video node ioctl operations
+ */
+static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
+					struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, FIMC_LITE_DRV_NAME, sizeof(cap->driver));
+	cap->bus_info[0] = 0;
+	cap->card[0] = 0;
+	cap->capabilities = V4L2_CAP_STREAMING;
+	return 0;
+}
+
+static int fimc_lite_enum_fmt_mplane(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	const struct fimc_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(fimc_lite_formats))
+		return -EINVAL;
+
+	fmt = &fimc_lite_formats[f->index];
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int fimc_lite_g_fmt_mplane(struct file *file, void *fh,
+				  struct v4l2_format *f)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt = &pixm->plane_fmt[0];
+	struct flite_frame *frame = &fimc->out_frame;
+	const struct fimc_fmt *fmt = fimc->fmt;
+
+	plane_fmt->bytesperline = (frame->f_width * fmt->depth[0]) / 8;
+	plane_fmt->sizeimage = plane_fmt->bytesperline * frame->f_height;
+
+	pixm->num_planes = fmt->memplanes;
+	pixm->pixelformat = fmt->fourcc;
+	pixm->width = frame->f_width;
+	pixm->height = frame->f_height;
+	pixm->field = V4L2_FIELD_NONE;
+	pixm->colorspace = V4L2_COLORSPACE_JPEG;
+	return 0;
+}
+
+static int fimc_lite_try_fmt(struct fimc_lite *fimc,
+			     struct v4l2_pix_format_mplane *pixm,
+			     const struct fimc_fmt **ffmt)
+{
+	struct flite_variant *variant = fimc->variant;
+	u32 bpl = pixm->plane_fmt[0].bytesperline;
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0);
+	if (WARN_ON(fmt == NULL))
+		return -EINVAL;
+	if (ffmt)
+		*ffmt = fmt;
+	v4l_bound_align_image(&pixm->width, 8, variant->max_width,
+			      ffs(variant->out_width_align) - 1,
+			      &pixm->height, 0, variant->max_height, 0, 0);
+
+	if ((bpl == 0 || ((bpl * 8) / fmt->depth[0]) < pixm->width))
+		pixm->plane_fmt[0].bytesperline = (pixm->width *
+						   fmt->depth[0]) / 8;
+
+	if (pixm->plane_fmt[0].sizeimage == 0)
+		pixm->plane_fmt[0].sizeimage = (pixm->width * pixm->height *
+						fmt->depth[0]) / 8;
+	pixm->num_planes = fmt->memplanes;
+	pixm->pixelformat = fmt->fourcc;
+	pixm->colorspace = V4L2_COLORSPACE_JPEG;
+	pixm->field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+static int fimc_lite_try_fmt_mplane(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	return fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, NULL);
+}
+
+static int fimc_lite_s_fmt_mplane(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *frame = &fimc->out_frame;
+	const struct fimc_fmt *fmt = NULL;
+	int ret;
+
+	if (vb2_is_busy(&fimc->vb_queue))
+		return -EBUSY;
+
+	ret = fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, &fmt);
+	if (ret < 0)
+		return ret;
+
+	fimc->fmt = fmt;
+	fimc->payload[0] = max((pixm->width * pixm->height * fmt->depth[0]) / 8,
+			       pixm->plane_fmt[0].sizeimage);
+	frame->f_width = pixm->width;
+	frame->f_height = pixm->height;
+
+	return 0;
+}
+
+static int fimc_pipeline_validate(struct fimc_lite *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->subdev;
+	struct v4l2_subdev_format sink_fmt, src_fmt;
+	struct media_pad *pad;
+	int ret;
+
+	while (1) {
+		/* Retrieve format at the sink pad */
+		pad = &sd->entity.pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+		/* Don't call FIMC subdev operation to avoid nested locking */
+		if (sd == &fimc->subdev) {
+			struct flite_frame *ff = &fimc->out_frame;
+			sink_fmt.format.width = ff->f_width;
+			sink_fmt.format.height = ff->f_height;
+			sink_fmt.format.code = fimc->fmt->mbus_code;
+		} else {
+			sink_fmt.pad = pad->index;
+			sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+			ret = v4l2_subdev_call(sd, pad, get_fmt, NULL,
+					       &sink_fmt);
+			if (ret < 0 && ret != -ENOIOCTLCMD)
+				return -EPIPE;
+		}
+		/* Retrieve format at the source pad */
+		pad = media_entity_remote_source(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+		src_fmt.pad = pad->index;
+		src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return -EPIPE;
+
+		if (src_fmt.format.width != sink_fmt.format.width ||
+		    src_fmt.format.height != sink_fmt.format.height ||
+		    src_fmt.format.code != sink_fmt.format.code)
+			return -EPIPE;
+	}
+	return 0;
+}
+
+static int fimc_lite_streamon(struct file *file, void *priv,
+			      enum v4l2_buf_type type)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct v4l2_subdev *sensor = fimc->pipeline.subdevs[IDX_SENSOR];
+	struct fimc_pipeline *p = &fimc->pipeline;
+	int ret;
+
+	if (fimc_lite_active(fimc))
+		return -EBUSY;
+
+	media_entity_pipeline_start(&sensor->entity, p->m_pipeline);
+
+	ret = fimc_pipeline_validate(fimc);
+	if (ret) {
+		media_entity_pipeline_stop(&sensor->entity);
+		return ret;
+	}
+
+	return vb2_streamon(&fimc->vb_queue, type);
+}
+
+static int fimc_lite_streamoff(struct file *file, void *priv,
+			       enum v4l2_buf_type type)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
+	int ret;
+
+	ret = vb2_streamoff(&fimc->vb_queue, type);
+	if (ret == 0)
+		media_entity_pipeline_stop(&sd->entity);
+	return ret;
+}
+
+static int fimc_lite_reqbufs(struct file *file, void *priv,
+			     struct v4l2_requestbuffers *reqbufs)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	int ret;
+
+	reqbufs->count = max_t(u32, FLITE_REQ_BUFS_MIN, reqbufs->count);
+	ret = vb2_reqbufs(&fimc->vb_queue, reqbufs);
+	if (!ret < 0)
+		fimc->reqbufs_count = reqbufs->count;
+
+	return ret;
+}
+
+static int fimc_lite_querybuf(struct file *file, void *priv,
+			      struct v4l2_buffer *buf)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	return vb2_querybuf(&fimc->vb_queue, buf);
+}
+
+static int fimc_lite_qbuf(struct file *file, void *priv,
+			  struct v4l2_buffer *buf)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	return vb2_qbuf(&fimc->vb_queue, buf);
+}
+
+static int fimc_lite_dqbuf(struct file *file, void *priv,
+			   struct v4l2_buffer *buf)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	return vb2_dqbuf(&fimc->vb_queue, buf, file->f_flags & O_NONBLOCK);
+}
+
+static int fimc_lite_create_bufs(struct file *file, void *priv,
+				 struct v4l2_create_buffers *create)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	return vb2_create_bufs(&fimc->vb_queue, create);
+}
+
+static int fimc_lite_prepare_buf(struct file *file, void *priv,
+				 struct v4l2_buffer *b)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	return vb2_prepare_buf(&fimc->vb_queue, b);
+}
+
+/* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
+static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int fimc_lite_g_selection(struct file *file, void *fh,
+				 struct v4l2_selection *sel)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *f = &fimc->out_frame;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = f->f_width;
+		sel->r.height = f->f_height;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+		sel->r = f->rect;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int fimc_lite_s_selection(struct file *file, void *fh,
+				 struct v4l2_selection *sel)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *f = &fimc->out_frame;
+	struct v4l2_rect rect = sel->r;
+	unsigned long flags;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
+	    sel->target != V4L2_SEL_TGT_COMPOSE_ACTIVE)
+		return -EINVAL;
+
+	fimc_lite_try_compose(fimc, &rect);
+
+	if ((sel->flags & V4L2_SEL_FLAG_LE) &&
+	    !enclosed_rectangle(&rect, &sel->r))
+		return -ERANGE;
+
+	if ((sel->flags & V4L2_SEL_FLAG_GE) &&
+	    !enclosed_rectangle(&sel->r, &rect))
+		return -ERANGE;
+
+	sel->r = rect;
+	spin_lock_irqsave(&fimc->slock, flags);
+	f->rect = rect;
+	set_bit(ST_FLITE_CONFIG, &fimc->state);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops fimc_lite_ioctl_ops = {
+	.vidioc_querycap		= fimc_vidioc_querycap_capture,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_lite_enum_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_lite_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_lite_s_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_lite_g_fmt_mplane,
+	.vidioc_g_selection		= fimc_lite_g_selection,
+	.vidioc_s_selection		= fimc_lite_s_selection,
+	.vidioc_reqbufs			= fimc_lite_reqbufs,
+	.vidioc_querybuf		= fimc_lite_querybuf,
+	.vidioc_prepare_buf		= fimc_lite_prepare_buf,
+	.vidioc_create_bufs		= fimc_lite_create_bufs,
+	.vidioc_qbuf			= fimc_lite_qbuf,
+	.vidioc_dqbuf			= fimc_lite_dqbuf,
+	.vidioc_streamon		= fimc_lite_streamon,
+	.vidioc_streamoff		= fimc_lite_streamoff,
+};
+
+/* Capture subdev media entity operations */
+static int fimc_lite_link_setup(struct media_entity *entity,
+				const struct media_pad *local,
+				const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	unsigned int remote_ent_type = media_entity_type(remote->entity);
+
+	if (WARN_ON(fimc == NULL))
+		return 0;
+
+	v4l2_dbg(1, debug, sd, "%s: %s --> %s, flags: 0x%x. source_id: 0x%x",
+		 __func__, local->entity->name, remote->entity->name,
+		 flags, fimc->source_subdev_grp_id);
+
+	switch (local->index) {
+	case FIMC_SD_PAD_SINK:
+		if (remote_ent_type != MEDIA_ENT_T_V4L2_SUBDEV)
+			return -EINVAL;
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (fimc->source_subdev_grp_id != 0)
+				return -EBUSY;
+			fimc->source_subdev_grp_id = sd->grp_id;
+			return 0;
+		}
+
+		fimc->source_subdev_grp_id = 0;
+		break;
+
+	case FIMC_SD_PAD_SOURCE:
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			fimc->out_path = FIMC_IO_NONE;
+			return 0;
+		}
+		if (remote_ent_type == MEDIA_ENT_T_V4L2_SUBDEV)
+			fimc->out_path = FIMC_IO_ISP;
+		else
+			fimc->out_path = FIMC_IO_DMA;
+		break;
+
+	default:
+		v4l2_err(sd, "Invalid pad index\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct media_entity_operations fimc_lite_subdev_media_ops = {
+	.link_setup = fimc_lite_link_setup,
+};
+
+static int fimc_lite_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+					   struct v4l2_subdev_fh *fh,
+					   struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_lite_find_format(NULL, NULL, code->index);
+	if (!fmt)
+		return -EINVAL;
+	code->code = fmt->mbus_code;
+	return 0;
+}
+
+static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_fh *fh,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct flite_frame *f = &fimc->out_frame;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		fmt->format = *mf;
+		return 0;
+	}
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	mutex_lock(&fimc->lock);
+	mf->code = fimc->fmt->mbus_code;
+
+	if (fmt->pad == FLITE_SD_PAD_SINK) {
+		/* full camera input frame size */
+		mf->width = f->f_width;
+		mf->height = f->f_height;
+	} else {
+		/* crop size */
+		mf->width = f->rect.width;
+		mf->height = f->rect.height;
+	}
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_fh *fh,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct flite_frame *sink = &fimc->inp_frame;
+	const struct fimc_fmt *ffmt;
+
+	v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %dx%d",
+		 fmt->pad, mf->code, mf->width, mf->height);
+
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mutex_lock(&fimc->lock);
+
+	if ((fimc->out_path == FIMC_IO_ISP && sd->entity.stream_count > 0) ||
+	    (fimc->out_path == FIMC_IO_DMA && vb2_is_busy(&fimc->vb_queue))) {
+		mutex_unlock(&fimc->lock);
+		return -EBUSY;
+	}
+
+	ffmt = fimc_lite_try_format(fimc, &mf->width, &mf->height,
+				    &mf->code, NULL, fmt->pad);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		*mf = fmt->format;
+		mutex_unlock(&fimc->lock);
+		return 0;
+	}
+
+	if (fmt->pad == FLITE_SD_PAD_SINK) {
+		sink->f_width = mf->width;
+		sink->f_height = mf->height;
+		fimc->fmt = ffmt;
+		/* Set sink crop rectangle */
+		sink->rect.width = mf->width;
+		sink->rect.height = mf->height;
+		sink->rect.left = 0;
+		sink->rect.top = 0;
+		/* Reset source crop rectangle */
+		fimc->out_frame.rect = sink->rect;
+	} else {
+		/* Allow changing format only on sink pad */
+		mf->code = fimc->fmt->mbus_code;
+		mf->width = sink->rect.width;
+		mf->height = sink->rect.height;
+	}
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_lite_subdev_get_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_fh *fh,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct flite_frame *f = &fimc->inp_frame;
+
+	if ((sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL &&
+	     sel->target != V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS) ||
+	    sel->pad != FLITE_SD_PAD_SINK)
+		return -EINVAL;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sel->r = *v4l2_subdev_get_try_crop(fh, sel->pad);
+		return 0;
+	}
+
+	mutex_lock(&fimc->lock);
+	if (sel->target == V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL) {
+		sel->r = f->rect;
+	} else {
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = f->f_width;
+		sel->r.height = f->f_height;
+	}
+	mutex_unlock(&fimc->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d",
+		 __func__, f->rect.left, f->rect.top, f->rect.width,
+		 f->rect.height, f->f_width, f->f_height);
+
+	return 0;
+}
+
+static int fimc_lite_subdev_set_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_fh *fh,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct flite_frame *f = &fimc->inp_frame;
+	int ret = 0;
+
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != FLITE_SD_PAD_SINK)
+		return -EINVAL;
+
+	mutex_lock(&fimc->lock);
+	fimc_lite_try_crop(fimc, &sel->r);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_crop(fh, sel->pad) = sel->r;
+	} else {
+		unsigned long flags;
+		spin_lock_irqsave(&fimc->slock, flags);
+		f->rect = sel->r;
+		/* Same crop rectangle on the source pad */
+		fimc->out_frame.rect = sel->r;
+		set_bit(ST_FLITE_CONFIG, &fimc->state);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+	}
+	mutex_unlock(&fimc->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d",
+		 __func__, f->rect.left, f->rect.top, f->rect.width,
+		 f->rect.height, f->f_width, f->f_height);
+
+	return ret;
+}
+
+static int fimc_lite_subdev_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc->out_path == FIMC_IO_DMA)
+		return -ENOIOCTLCMD;
+
+	/* TODO: */
+
+	return 0;
+}
+
+static int fimc_lite_subdev_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc->out_path == FIMC_IO_DMA)
+		return -ENOIOCTLCMD;
+
+	/* TODO: */
+
+	return 0;
+}
+
+static int fimc_lite_log_status(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	flite_hw_dump_regs(fimc, __func__);
+	return 0;
+}
+
+static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct vb2_queue *q = &fimc->vb_queue;
+	struct video_device *vfd;
+	int ret;
+
+	fimc->fmt = &fimc_lite_formats[0];
+	fimc->out_path = FIMC_IO_DMA;
+
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(sd->v4l2_dev, "Failed to allocate video device\n");
+		return -ENOMEM;
+	}
+
+	snprintf(vfd->name, sizeof(vfd->name), "fimc-lite.%d.capture",
+		 fimc->index);
+
+	vfd->fops = &fimc_lite_fops;
+	vfd->ioctl_ops = &fimc_lite_ioctl_ops;
+	vfd->v4l2_dev = sd->v4l2_dev;
+	vfd->minor = -1;
+	vfd->release = video_device_release;
+	vfd->lock = &fimc->lock;
+	fimc->vfd = vfd;
+	fimc->ref_count = 0;
+	fimc->reqbufs_count = 0;
+
+	INIT_LIST_HEAD(&fimc->pending_buf_q);
+	INIT_LIST_HEAD(&fimc->active_buf_q);
+
+	memset(q, 0, sizeof(*q));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->ops = &fimc_lite_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct flite_buffer);
+	q->drv_priv = fimc;
+
+	vb2_queue_init(q);
+
+	fimc->vd_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vfd->entity, 1, &fimc->vd_pad, 0);
+	if (ret)
+		goto err;
+
+	video_set_drvdata(vfd, fimc);
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_vd;
+
+	v4l2_info(sd->v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+	return 0;
+
+ err_vd:
+	media_entity_cleanup(&vfd->entity);
+ err:
+	video_device_release(vfd);
+	return ret;
+}
+
+static void fimc_lite_subdev_unregistered(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc == NULL)
+		return;
+
+	if (fimc->vfd) {
+		video_unregister_device(fimc->vfd);
+		media_entity_cleanup(&fimc->vfd->entity);
+		fimc->vfd = NULL;
+	}
+}
+
+static const struct v4l2_subdev_internal_ops fimc_lite_subdev_internal_ops = {
+	.registered = fimc_lite_subdev_registered,
+	.unregistered = fimc_lite_subdev_unregistered,
+};
+
+static const struct v4l2_subdev_pad_ops fimc_lite_subdev_pad_ops = {
+	.enum_mbus_code = fimc_lite_subdev_enum_mbus_code,
+	.get_selection = fimc_lite_subdev_get_selection,
+	.set_selection = fimc_lite_subdev_set_selection,
+	.get_fmt = fimc_lite_subdev_get_fmt,
+	.set_fmt = fimc_lite_subdev_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops fimc_lite_subdev_video_ops = {
+	.s_stream = fimc_lite_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops fimc_lite_core_ops = {
+	.s_power = fimc_lite_subdev_s_power,
+	.log_status = fimc_lite_log_status,
+};
+
+static struct v4l2_subdev_ops fimc_lite_subdev_ops = {
+	.core = &fimc_lite_core_ops,
+	.video = &fimc_lite_subdev_video_ops,
+	.pad = &fimc_lite_subdev_pad_ops,
+};
+
+static int fimc_lite_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fimc_lite *fimc = container_of(ctrl->handler, struct fimc_lite,
+					      ctrl_handler);
+	set_bit(ST_FLITE_CONFIG, &fimc->state);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops fimc_lite_ctrl_ops = {
+	.s_ctrl	= fimc_lite_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config fimc_lite_ctrl = {
+	.ops	= &fimc_lite_ctrl_ops,
+	.id	= V4L2_CTRL_CLASS_USER | 0x1001,
+	.type	= V4L2_CTRL_TYPE_BOOLEAN,
+	.name	= "Test Pattern 640x480",
+};
+
+static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
+{
+	struct v4l2_ctrl_handler *handler = &fimc->ctrl_handler;
+	struct v4l2_subdev *sd = &fimc->subdev;
+	int ret;
+
+	v4l2_subdev_init(sd, &fimc_lite_subdev_ops);
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC-LITE.%d", fimc->index);
+
+	fimc->subdev_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	fimc->subdev_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
+				fimc->subdev_pads, 0);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_handler_init(handler, 1);
+	fimc->test_pattern = v4l2_ctrl_new_custom(handler, &fimc_lite_ctrl,
+						  NULL);
+	if (handler->error) {
+		media_entity_cleanup(&sd->entity);
+		return handler->error;
+	}
+
+	sd->ctrl_handler = handler;
+	sd->internal_ops = &fimc_lite_subdev_internal_ops;
+	sd->entity.ops = &fimc_lite_subdev_media_ops;
+	v4l2_set_subdevdata(sd, fimc);
+
+	return 0;
+}
+
+static void fimc_lite_unregister_capture_subdev(struct fimc_lite *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&fimc->ctrl_handler);
+	v4l2_set_subdevdata(sd, NULL);
+}
+
+static void fimc_lite_clk_put(struct fimc_lite *fimc)
+{
+	if (IS_ERR_OR_NULL(fimc->clock))
+		return;
+
+	clk_unprepare(fimc->clock);
+	clk_put(fimc->clock);
+	fimc->clock = NULL;
+}
+
+static int fimc_lite_clk_get(struct fimc_lite *fimc)
+{
+	int ret;
+
+	fimc->clock = clk_get(&fimc->pdev->dev, FLITE_CLK_NAME);
+	if (IS_ERR(fimc->clock))
+		return PTR_ERR(fimc->clock);
+
+	ret = clk_prepare(fimc->clock);
+	if (ret < 0) {
+		clk_put(fimc->clock);
+		fimc->clock = NULL;
+	}
+	return ret;
+}
+
+static int __devinit fimc_lite_probe(struct platform_device *pdev)
+{
+	struct flite_drvdata *drv_data = fimc_lite_get_drvdata(pdev);
+	struct fimc_lite *fimc;
+	struct resource *res;
+	int ret;
+
+	fimc = devm_kzalloc(&pdev->dev, sizeof(*fimc), GFP_KERNEL);
+	if (!fimc)
+		return -ENOMEM;
+
+	fimc->index = pdev->id;
+	fimc->variant = drv_data->variant[fimc->index];
+	fimc->pdev = pdev;
+
+	init_waitqueue_head(&fimc->irq_queue);
+	spin_lock_init(&fimc->slock);
+	mutex_init(&fimc->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	fimc->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (fimc->regs == NULL) {
+		dev_err(&pdev->dev, "Failed to obtain io memory\n");
+		return -ENOENT;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
+		return -ENXIO;
+	}
+
+	ret = fimc_lite_clk_get(fimc);
+	if (ret)
+		return ret;
+
+	ret = devm_request_irq(&pdev->dev, res->start, flite_irq_handler,
+			       0, dev_name(&pdev->dev), fimc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
+		goto err_clk;
+	}
+
+	/* The video node will be created within the subdev's registered() op */
+	ret = fimc_lite_create_capture_subdev(fimc);
+	if (ret)
+		goto err_clk;
+
+	platform_set_drvdata(pdev, fimc);
+	pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0)
+		goto err_sd;
+
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(fimc->alloc_ctx)) {
+		ret = PTR_ERR(fimc->alloc_ctx);
+		goto err_pm;
+	}
+	pm_runtime_put(&pdev->dev);
+
+	dev_dbg(&pdev->dev, "FIMC-LITE.%d registered successfully\n",
+		fimc->index);
+	return 0;
+err_pm:
+	pm_runtime_put(&pdev->dev);
+err_sd:
+	fimc_lite_unregister_capture_subdev(fimc);
+err_clk:
+	fimc_lite_clk_put(fimc);
+	return ret;
+}
+
+static int fimc_lite_runtime_resume(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+
+	clk_enable(fimc->clock);
+	return 0;
+}
+
+static int fimc_lite_runtime_suspend(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+
+	clk_disable(fimc->clock);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_lite_resume(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	struct flite_buffer *buf;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	if (!test_and_clear_bit(ST_LPM, &fimc->state) ||
+	    !test_bit(ST_FLITE_IN_USE, &fimc->state)) {
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		return 0;
+	}
+	flite_hw_reset(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	if (!test_and_clear_bit(ST_FLITE_SUSPENDED, &fimc->state))
+		return 0;
+
+	INIT_LIST_HEAD(&fimc->active_buf_q);
+	fimc_pipeline_initialize(&fimc->pipeline, &fimc->vfd->entity, false);
+	fimc_lite_hw_init(fimc);
+	clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+
+	for (i = 0; i < fimc->reqbufs_count; i++) {
+		if (list_empty(&fimc->pending_buf_q))
+			break;
+		buf = fimc_lite_pending_queue_pop(fimc);
+		buffer_queue(&buf->vb);
+	}
+	return 0;
+}
+
+static int fimc_lite_suspend(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	bool suspend = test_bit(ST_FLITE_IN_USE, &fimc->state);
+	int ret;
+
+	if (test_and_set_bit(ST_LPM, &fimc->state))
+		return 0;
+
+	ret = fimc_lite_stop_capture(fimc, suspend);
+	if (ret)
+		return ret;
+
+	return fimc_pipeline_shutdown(&fimc->pipeline);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int __devexit fimc_lite_remove(struct platform_device *pdev)
+{
+	struct fimc_lite *fimc = platform_get_drvdata(pdev);
+	struct device *dev = &pdev->dev;
+
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	fimc_lite_unregister_capture_subdev(fimc);
+	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
+	fimc_lite_clk_put(fimc);
+
+	dev_info(dev, "Driver unloaded\n");
+	return 0;
+}
+
+static struct flite_variant fimc_lite0_variant_exynos4 = {
+	.max_width		= 8192,
+	.max_height		= 8192,
+	.out_width_align	= 8,
+	.win_hor_offs_align	= 2,
+	.out_hor_offs_align	= 8,
+};
+
+/* EXYNOS4212, EXYNOS4412 */
+static struct flite_drvdata fimc_lite_drvdata_exynos4 = {
+	.variant = {
+		[0] = &fimc_lite0_variant_exynos4,
+		[1] = &fimc_lite0_variant_exynos4,
+	},
+};
+
+static struct platform_device_id fimc_lite_driver_ids[] = {
+	{
+		.name		= "exynos-fimc-lite",
+		.driver_data	= (unsigned long)&fimc_lite_drvdata_exynos4,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(platform, fimc_lite_driver_ids);
+
+static const struct dev_pm_ops fimc_lite_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_lite_suspend, fimc_lite_resume)
+	SET_RUNTIME_PM_OPS(fimc_lite_runtime_suspend, fimc_lite_runtime_resume,
+			   NULL)
+};
+
+static struct platform_driver fimc_lite_driver = {
+	.probe		= fimc_lite_probe,
+	.remove		= __devexit_p(fimc_lite_remove),
+	.id_table	= fimc_lite_driver_ids,
+	.driver = {
+		.name		= FIMC_LITE_DRV_NAME,
+		.owner		= THIS_MODULE,
+		.pm		= &fimc_lite_pm_ops,
+	}
+};
+module_platform_driver(fimc_lite_driver);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" FIMC_LITE_DRV_NAME);
diff --git a/drivers/media/video/s5p-fimc/fimc-lite.h b/drivers/media/video/s5p-fimc/fimc-lite.h
new file mode 100644
index 000000000000..44424eee81d8
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite.h
@@ -0,0 +1,213 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *
+ * 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 FIMC_LITE_H_
+#define FIMC_LITE_H_
+
+#include <asm/sizes.h>
+#include <linux/io.h>
+#include <linux/irqreturn.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/videobuf2-core.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/s5p_fimc.h>
+
+#include "fimc-core.h"
+
+#define FIMC_LITE_DRV_NAME	"exynos-fimc-lite"
+#define FLITE_CLK_NAME		"flite"
+#define FIMC_LITE_MAX_DEVS	2
+#define FLITE_REQ_BUFS_MIN	2
+
+/* Bit index definitions for struct fimc_lite::state */
+enum {
+	ST_FLITE_LPM,
+	ST_FLITE_PENDING,
+	ST_FLITE_RUN,
+	ST_FLITE_STREAM,
+	ST_FLITE_SUSPENDED,
+	ST_FLITE_OFF,
+	ST_FLITE_IN_USE,
+	ST_FLITE_CONFIG,
+	ST_SENSOR_STREAM,
+};
+
+#define FLITE_SD_PAD_SINK	0
+#define FLITE_SD_PAD_SOURCE	1
+#define FLITE_SD_PADS_NUM	2
+
+struct flite_variant {
+	unsigned short max_width;
+	unsigned short max_height;
+	unsigned short out_width_align;
+	unsigned short win_hor_offs_align;
+	unsigned short out_hor_offs_align;
+};
+
+struct flite_drvdata {
+	struct flite_variant *variant[FIMC_LITE_MAX_DEVS];
+};
+
+#define fimc_lite_get_drvdata(_pdev) \
+	((struct flite_drvdata *) platform_get_device_id(_pdev)->driver_data)
+
+struct fimc_lite_events {
+	unsigned int data_overflow;
+};
+
+#define FLITE_MAX_PLANES	1
+
+/**
+ * struct flite_frame - source/target frame properties
+ * @f_width: full pixel width
+ * @f_height: full pixel height
+ * @rect: crop/composition rectangle
+ */
+struct flite_frame {
+	u16 f_width;
+	u16 f_height;
+	struct v4l2_rect rect;
+};
+
+/**
+ * struct flite_buffer - video buffer structure
+ * @vb:    vb2 buffer
+ * @list:  list head for the buffers queue
+ * @paddr: precalculated physical address
+ */
+struct flite_buffer {
+	struct vb2_buffer vb;
+	struct list_head list;
+	dma_addr_t paddr;
+};
+
+/**
+ * struct fimc_lite - fimc lite structure
+ * @pdev: pointer to FIMC-LITE platform device
+ * @variant: variant information for this IP
+ * @v4l2_dev: pointer to top the level v4l2_device
+ * @vfd: video device node
+ * @fh: v4l2 file handle
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @subdev: FIMC-LITE subdev
+ * @vd_pad: media (sink) pad for the capture video node
+ * @subdev_pads: the subdev media pads
+ * @ctrl_handler: v4l2 control handler
+ * @test_pattern: test pattern controls
+ * @index: FIMC-LITE platform device index
+ * @pipeline: video capture pipeline data structure
+ * @slock: spinlock protecting this data structure and the hw registers
+ * @lock: mutex serializing video device and the subdev operations
+ * @clock: FIMC-LITE gate clock
+ * @regs: memory mapped io registers
+ * @irq_queue: interrupt handler waitqueue
+ * @fmt: pointer to color format description structure
+ * @payload: image size in bytes (w x h x bpp)
+ * @inp_frame: camera input frame structure
+ * @out_frame: DMA output frame structure
+ * @out_path: output data path (DMA or FIFO)
+ * @source_subdev_grp_id: source subdev group id
+ * @state: driver state flags
+ * @pending_buf_q: pending buffers queue head
+ * @active_buf_q: the queue head of buffers scheduled in hardware
+ * @vb_queue: vb2 buffers queue
+ * @active_buf_count: number of video buffers scheduled in hardware
+ * @frame_count: the captured frames counter
+ * @reqbufs_count: the number of buffers requested with REQBUFS ioctl
+ * @ref_count: driver's private reference counter
+ */
+struct fimc_lite {
+	struct platform_device	*pdev;
+	struct flite_variant	*variant;
+	struct v4l2_device	*v4l2_dev;
+	struct video_device	*vfd;
+	struct v4l2_fh		fh;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	struct v4l2_subdev	subdev;
+	struct media_pad	vd_pad;
+	struct media_pad	subdev_pads[FLITE_SD_PADS_NUM];
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl	*test_pattern;
+	u32			index;
+	struct fimc_pipeline	pipeline;
+
+	struct mutex		lock;
+	spinlock_t		slock;
+
+	struct clk		*clock;
+	void __iomem		*regs;
+	wait_queue_head_t	irq_queue;
+
+	const struct fimc_fmt	*fmt;
+	unsigned long		payload[FLITE_MAX_PLANES];
+	struct flite_frame	inp_frame;
+	struct flite_frame	out_frame;
+	enum fimc_datapath	out_path;
+	unsigned int		source_subdev_grp_id;
+
+	unsigned long		state;
+	struct list_head	pending_buf_q;
+	struct list_head	active_buf_q;
+	struct vb2_queue	vb_queue;
+	unsigned int		frame_count;
+	unsigned int		reqbufs_count;
+	int			ref_count;
+
+	struct fimc_lite_events	events;
+};
+
+static inline bool fimc_lite_active(struct fimc_lite *fimc)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	ret = fimc->state & (1 << ST_FLITE_RUN) ||
+		fimc->state & (1 << ST_FLITE_PENDING);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return ret;
+}
+
+static inline void fimc_lite_active_queue_add(struct fimc_lite *dev,
+					 struct flite_buffer *buf)
+{
+	list_add_tail(&buf->list, &dev->active_buf_q);
+}
+
+static inline struct flite_buffer *fimc_lite_active_queue_pop(
+					struct fimc_lite *dev)
+{
+	struct flite_buffer *buf = list_entry(dev->active_buf_q.next,
+					      struct flite_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+static inline void fimc_lite_pending_queue_add(struct fimc_lite *dev,
+					struct flite_buffer *buf)
+{
+	list_add_tail(&buf->list, &dev->pending_buf_q);
+}
+
+static inline struct flite_buffer *fimc_lite_pending_queue_pop(
+					struct fimc_lite *dev)
+{
+	struct flite_buffer *buf = list_entry(dev->pending_buf_q.next,
+					      struct flite_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+#endif /* FIMC_LITE_H_ */
diff --git a/drivers/media/video/s5p-fimc/fimc-m2m.c b/drivers/media/video/s5p-fimc/fimc-m2m.c
new file mode 100644
index 000000000000..4c58e0570962
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-m2m.c
@@ -0,0 +1,824 @@
+/*
+ * Samsung S5P/EXYNOS4 SoC series FIMC (video postprocessor) driver
+ *
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ * 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 as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/bug.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "fimc-core.h"
+#include "fimc-reg.h"
+#include "fimc-mdevice.h"
+
+
+static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
+{
+	if (stream_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return FMT_FLAGS_M2M_IN;
+	else
+		return FMT_FLAGS_M2M_OUT;
+}
+
+void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
+{
+	struct vb2_buffer *src_vb, *dst_vb;
+
+	if (!ctx || !ctx->m2m_ctx)
+		return;
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+		v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
+				    ctx->m2m_ctx);
+	}
+}
+
+/* Complete the transaction which has been scheduled for execution. */
+static int fimc_m2m_shutdown(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	if (!fimc_m2m_pending(fimc))
+		return 0;
+
+	fimc_ctx_state_set(FIMC_CTX_SHUT, ctx);
+
+	ret = wait_event_timeout(fimc->irq_queue,
+			   !fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
+			   FIMC_SHUTDOWN_TIMEOUT);
+
+	return ret == 0 ? -ETIMEDOUT : ret;
+}
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = pm_runtime_get_sync(&ctx->fimc_dev->pdev->dev);
+	return ret > 0 ? 0 : ret;
+}
+
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = fimc_m2m_shutdown(ctx);
+	if (ret == -ETIMEDOUT)
+		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	pm_runtime_put(&ctx->fimc_dev->pdev->dev);
+	return 0;
+}
+
+static void fimc_device_run(void *priv)
+{
+	struct vb2_buffer *vb = NULL;
+	struct fimc_ctx *ctx = priv;
+	struct fimc_frame *sf, *df;
+	struct fimc_dev *fimc;
+	unsigned long flags;
+	u32 ret;
+
+	if (WARN(!ctx, "Null context\n"))
+		return;
+
+	fimc = ctx->fimc_dev;
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	set_bit(ST_M2M_PEND, &fimc->state);
+	sf = &ctx->s_frame;
+	df = &ctx->d_frame;
+
+	if (ctx->state & FIMC_PARAMS) {
+		/* Prepare the DMA offsets for scaler */
+		fimc_prepare_dma_offset(ctx, sf);
+		fimc_prepare_dma_offset(ctx, df);
+	}
+
+	vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	ret = fimc_prepare_addr(ctx, vb, sf, &sf->paddr);
+	if (ret)
+		goto dma_unlock;
+
+	vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	ret = fimc_prepare_addr(ctx, vb, df, &df->paddr);
+	if (ret)
+		goto dma_unlock;
+
+	/* Reconfigure hardware if the context has changed. */
+	if (fimc->m2m.ctx != ctx) {
+		ctx->state |= FIMC_PARAMS;
+		fimc->m2m.ctx = ctx;
+	}
+
+	if (ctx->state & FIMC_PARAMS) {
+		fimc_set_yuv_order(ctx);
+		fimc_hw_set_input_path(ctx);
+		fimc_hw_set_in_dma(ctx);
+		ret = fimc_set_scaler_info(ctx);
+		if (ret)
+			goto dma_unlock;
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
+		fimc_hw_set_target_format(ctx);
+		fimc_hw_set_rotation(ctx);
+		fimc_hw_set_effect(ctx);
+		fimc_hw_set_out_dma(ctx);
+		if (fimc->variant->has_alpha)
+			fimc_hw_set_rgb_alpha(ctx);
+		fimc_hw_set_output_path(ctx);
+	}
+	fimc_hw_set_input_addr(fimc, &sf->paddr);
+	fimc_hw_set_output_addr(fimc, &df->paddr, -1);
+
+	fimc_activate_capture(ctx);
+	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP |
+		       FIMC_SRC_FMT | FIMC_DST_FMT);
+	fimc_hw_activate_input_dma(fimc, true);
+
+dma_unlock:
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static void fimc_job_abort(void *priv)
+{
+	fimc_m2m_shutdown(priv);
+}
+
+static int fimc_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+			    unsigned int *num_buffers, unsigned int *num_planes,
+			    unsigned int sizes[], void *allocators[])
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	struct fimc_frame *f;
+	int i;
+
+	f = ctx_get_frame(ctx, vq->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+	/*
+	 * Return number of non-contigous planes (plane buffers)
+	 * depending on the configured color format.
+	 */
+	if (!f->fmt)
+		return -EINVAL;
+
+	*num_planes = f->fmt->memplanes;
+	for (i = 0; i < f->fmt->memplanes; i++) {
+		sizes[i] = (f->f_width * f->f_height * f->fmt->depth[i]) / 8;
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+	return 0;
+}
+
+static int fimc_buf_prepare(struct vb2_buffer *vb)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_frame *frame;
+	int i;
+
+	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	for (i = 0; i < frame->fmt->memplanes; i++)
+		vb2_set_plane_payload(vb, i, frame->payload[i]);
+
+	return 0;
+}
+
+static void fimc_buf_queue(struct vb2_buffer *vb)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	dbg("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
+
+	if (ctx->m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
+}
+
+static void fimc_lock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_lock(&ctx->fimc_dev->lock);
+}
+
+static void fimc_unlock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_unlock(&ctx->fimc_dev->lock);
+}
+
+static struct vb2_ops fimc_qops = {
+	.queue_setup	 = fimc_queue_setup,
+	.buf_prepare	 = fimc_buf_prepare,
+	.buf_queue	 = fimc_buf_queue,
+	.wait_prepare	 = fimc_unlock,
+	.wait_finish	 = fimc_lock,
+	.stop_streaming	 = stop_streaming,
+	.start_streaming = start_streaming,
+};
+
+/*
+ * V4L2 ioctl handlers
+ */
+static int fimc_m2m_querycap(struct file *file, void *fh,
+			     struct v4l2_capability *cap)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	strncpy(cap->driver, fimc->pdev->name, sizeof(cap->driver) - 1);
+	strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
+	cap->bus_info[0] = 0;
+	cap->capabilities = V4L2_CAP_STREAMING |
+		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+
+	return 0;
+}
+
+static int fimc_m2m_enum_fmt_mplane(struct file *file, void *priv,
+				    struct v4l2_fmtdesc *f)
+{
+	struct fimc_fmt *fmt;
+
+	fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
+			       f->index);
+	if (!fmt)
+		return -EINVAL;
+
+	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
+
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	return fimc_fill_format(frame, f);
+}
+
+static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_variant *variant = fimc->variant;
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct fimc_fmt *fmt;
+	u32 max_w, mod_x, mod_y;
+
+	if (!IS_M2M(f->type))
+		return -EINVAL;
+
+	dbg("w: %d, h: %d", pix->width, pix->height);
+
+	fmt = fimc_find_format(&pix->pixelformat, NULL,
+			       get_m2m_fmt_flags(f->type), 0);
+	if (WARN(fmt == NULL, "Pixel format lookup failed"))
+		return -EINVAL;
+
+	if (pix->field == V4L2_FIELD_ANY)
+		pix->field = V4L2_FIELD_NONE;
+	else if (pix->field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		max_w = variant->pix_limit->scaler_dis_w;
+		mod_x = ffs(variant->min_inp_pixsize) - 1;
+	} else {
+		max_w = variant->pix_limit->out_rot_dis_w;
+		mod_x = ffs(variant->min_out_pixsize) - 1;
+	}
+
+	if (tiled_fmt(fmt)) {
+		mod_x = 6; /* 64 x 32 pixels tile */
+		mod_y = 5;
+	} else {
+		if (variant->min_vsize_align == 1)
+			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
+		else
+			mod_y = ffs(variant->min_vsize_align) - 1;
+	}
+
+	v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
+		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
+
+	fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
+	return 0;
+}
+
+static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return fimc_try_fmt_mplane(ctx, f);
+}
+
+static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct vb2_queue *vq;
+	struct fimc_frame *frame;
+	struct v4l2_pix_format_mplane *pix;
+	int i, ret = 0;
+
+	ret = fimc_try_fmt_mplane(ctx, f);
+	if (ret)
+		return ret;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(fimc->m2m.vfd, "queue (%d) busy\n", f->type);
+		return -EBUSY;
+	}
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		frame = &ctx->s_frame;
+	else
+		frame = &ctx->d_frame;
+
+	pix = &f->fmt.pix_mp;
+	frame->fmt = fimc_find_format(&pix->pixelformat, NULL,
+				      get_m2m_fmt_flags(f->type), 0);
+	if (!frame->fmt)
+		return -EINVAL;
+
+	/* Update RGB Alpha control state and value range */
+	fimc_alpha_ctrl_update(ctx);
+
+	for (i = 0; i < frame->fmt->colplanes; i++) {
+		frame->payload[i] =
+			(pix->width * pix->height * frame->fmt->depth[i]) / 8;
+	}
+
+	fimc_fill_frame(frame, f);
+
+	ctx->scaler.enabled = 1;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		fimc_ctx_state_set(FIMC_PARAMS | FIMC_DST_FMT, ctx);
+	else
+		fimc_ctx_state_set(FIMC_PARAMS | FIMC_SRC_FMT, ctx);
+
+	dbg("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
+
+	return 0;
+}
+
+static int fimc_m2m_reqbufs(struct file *file, void *fh,
+			    struct v4l2_requestbuffers *reqbufs)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
+}
+
+static int fimc_m2m_querybuf(struct file *file, void *fh,
+			     struct v4l2_buffer *buf)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
+}
+
+static int fimc_m2m_qbuf(struct file *file, void *fh,
+			 struct v4l2_buffer *buf)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int fimc_m2m_dqbuf(struct file *file, void *fh,
+			  struct v4l2_buffer *buf)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int fimc_m2m_streamon(struct file *file, void *fh,
+			     enum v4l2_buf_type type)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	/* The source and target color format need to be set */
+	if (V4L2_TYPE_IS_OUTPUT(type)) {
+		if (!fimc_ctx_state_is_set(FIMC_SRC_FMT, ctx))
+			return -EINVAL;
+	} else if (!fimc_ctx_state_is_set(FIMC_DST_FMT, ctx)) {
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
+}
+
+static int fimc_m2m_streamoff(struct file *file, void *fh,
+			    enum v4l2_buf_type type)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
+}
+
+static int fimc_m2m_cropcap(struct file *file, void *fh,
+			    struct v4l2_cropcap *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame;
+
+	frame = ctx_get_frame(ctx, cr->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	cr->bounds.left = 0;
+	cr->bounds.top = 0;
+	cr->bounds.width = frame->o_width;
+	cr->bounds.height = frame->o_height;
+	cr->defrect = cr->bounds;
+
+	return 0;
+}
+
+static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame;
+
+	frame = ctx_get_frame(ctx, cr->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	cr->c.left = frame->offs_h;
+	cr->c.top = frame->offs_v;
+	cr->c.width = frame->width;
+	cr->c.height = frame->height;
+
+	return 0;
+}
+
+static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_frame *f;
+	u32 min_size, halign, depth = 0;
+	int i;
+
+	if (cr->c.top < 0 || cr->c.left < 0) {
+		v4l2_err(fimc->m2m.vfd,
+			"doesn't support negative values for top & left\n");
+		return -EINVAL;
+	}
+	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		f = &ctx->d_frame;
+	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		f = &ctx->s_frame;
+	else
+		return -EINVAL;
+
+	min_size = (f == &ctx->s_frame) ?
+		fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
+
+	/* Get pixel alignment constraints. */
+	if (fimc->variant->min_vsize_align == 1)
+		halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
+	else
+		halign = ffs(fimc->variant->min_vsize_align) - 1;
+
+	for (i = 0; i < f->fmt->colplanes; i++)
+		depth += f->fmt->depth[i];
+
+	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
+			      ffs(min_size) - 1,
+			      &cr->c.height, min_size, f->o_height,
+			      halign, 64/(ALIGN(depth, 8)));
+
+	/* adjust left/top if cropping rectangle is out of bounds */
+	if (cr->c.left + cr->c.width > f->o_width)
+		cr->c.left = f->o_width - cr->c.width;
+	if (cr->c.top + cr->c.height > f->o_height)
+		cr->c.top = f->o_height - cr->c.height;
+
+	cr->c.left = round_down(cr->c.left, min_size);
+	cr->c.top  = round_down(cr->c.top, fimc->variant->hor_offs_align);
+
+	dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
+	    cr->c.left, cr->c.top, cr->c.width, cr->c.height,
+	    f->f_width, f->f_height);
+
+	return 0;
+}
+
+static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_frame *f;
+	int ret;
+
+	ret = fimc_m2m_try_crop(ctx, cr);
+	if (ret)
+		return ret;
+
+	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,
+					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);
+		}
+		if (ret) {
+			v4l2_err(fimc->m2m.vfd, "Out of scaler range\n");
+			return -EINVAL;
+		}
+	}
+
+	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);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
+	.vidioc_querycap		= fimc_m2m_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_m2m_enum_fmt_mplane,
+	.vidioc_enum_fmt_vid_out_mplane	= fimc_m2m_enum_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_m2m_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= fimc_m2m_g_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_m2m_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= fimc_m2m_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_reqbufs			= fimc_m2m_reqbufs,
+	.vidioc_querybuf		= fimc_m2m_querybuf,
+	.vidioc_qbuf			= fimc_m2m_qbuf,
+	.vidioc_dqbuf			= fimc_m2m_dqbuf,
+	.vidioc_streamon		= fimc_m2m_streamon,
+	.vidioc_streamoff		= fimc_m2m_streamoff,
+	.vidioc_g_crop			= fimc_m2m_g_crop,
+	.vidioc_s_crop			= fimc_m2m_s_crop,
+	.vidioc_cropcap			= fimc_m2m_cropcap
+
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct fimc_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &fimc_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &fimc_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int fimc_m2m_open(struct file *file)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_ctx *ctx;
+	int ret;
+
+	dbg("pid: %d, state: 0x%lx, refcnt: %d",
+	    task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
+
+	/*
+	 * Return if the corresponding video capture node
+	 * is already opened.
+	 */
+	if (fimc->vid_cap.refcnt > 0)
+		return -EBUSY;
+
+	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+	v4l2_fh_init(&ctx->fh, fimc->m2m.vfd);
+	ctx->fimc_dev = fimc;
+
+	/* Default color format */
+	ctx->s_frame.fmt = fimc_get_format(0);
+	ctx->d_frame.fmt = fimc_get_format(0);
+
+	ret = fimc_ctrls_create(ctx);
+	if (ret)
+		goto error_fh;
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrls.handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	/* Setup the device context for memory-to-memory mode */
+	ctx->state = FIMC_CTX_M2M;
+	ctx->flags = 0;
+	ctx->in_path = FIMC_IO_DMA;
+	ctx->out_path = FIMC_IO_DMA;
+
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->m2m_ctx)) {
+		ret = PTR_ERR(ctx->m2m_ctx);
+		goto error_c;
+	}
+
+	if (fimc->m2m.refcnt++ == 0)
+		set_bit(ST_M2M_RUN, &fimc->state);
+	return 0;
+
+error_c:
+	fimc_ctrls_delete(ctx);
+error_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	return ret;
+}
+
+static int fimc_m2m_release(struct file *file)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	dbg("pid: %d, state: 0x%lx, refcnt= %d",
+		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
+
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	fimc_ctrls_delete(ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	if (--fimc->m2m.refcnt <= 0)
+		clear_bit(ST_M2M_RUN, &fimc->state);
+	kfree(ctx);
+	return 0;
+}
+
+static unsigned int fimc_m2m_poll(struct file *file,
+				  struct poll_table_struct *wait)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+
+	return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
+}
+
+
+static int fimc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+
+	return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
+}
+
+static const struct v4l2_file_operations fimc_m2m_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fimc_m2m_open,
+	.release	= fimc_m2m_release,
+	.poll		= fimc_m2m_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= fimc_m2m_mmap,
+};
+
+static struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= fimc_device_run,
+	.job_abort	= fimc_job_abort,
+};
+
+int fimc_register_m2m_device(struct fimc_dev *fimc,
+			     struct v4l2_device *v4l2_dev)
+{
+	struct video_device *vfd;
+	struct platform_device *pdev;
+	int ret = 0;
+
+	if (!fimc)
+		return -ENODEV;
+
+	pdev = fimc->pdev;
+	fimc->v4l2_dev = v4l2_dev;
+
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(v4l2_dev, "Failed to allocate video device\n");
+		return -ENOMEM;
+	}
+
+	vfd->fops = &fimc_m2m_fops;
+	vfd->ioctl_ops = &fimc_m2m_ioctl_ops;
+	vfd->v4l2_dev = v4l2_dev;
+	vfd->minor = -1;
+	vfd->release = video_device_release;
+	vfd->lock = &fimc->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
+
+	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
+	video_set_drvdata(vfd, fimc);
+
+	fimc->m2m.vfd = vfd;
+	fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(fimc->m2m.m2m_dev)) {
+		v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
+		ret = PTR_ERR(fimc->m2m.m2m_dev);
+		goto err_init;
+	}
+
+	ret = media_entity_init(&vfd->entity, 0, NULL, 0);
+	if (ret)
+		goto err_me;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_vd;
+
+	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+	return 0;
+
+err_vd:
+	media_entity_cleanup(&vfd->entity);
+err_me:
+	v4l2_m2m_release(fimc->m2m.m2m_dev);
+err_init:
+	video_device_release(fimc->m2m.vfd);
+	return ret;
+}
+
+void fimc_unregister_m2m_device(struct fimc_dev *fimc)
+{
+	if (!fimc)
+		return;
+
+	if (fimc->m2m.m2m_dev)
+		v4l2_m2m_release(fimc->m2m.m2m_dev);
+	if (fimc->m2m.vfd) {
+		media_entity_cleanup(&fimc->m2m.vfd->entity);
+		/* Can also be called if video device wasn't registered */
+		video_unregister_device(fimc->m2m.vfd);
+	}
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-mdevice.c b/drivers/media/video/s5p-fimc/fimc-mdevice.c
index 62ed37e40149..6753c45631b8 100644
--- a/drivers/media/video/s5p-fimc/fimc-mdevice.c
+++ b/drivers/media/video/s5p-fimc/fimc-mdevice.c
@@ -25,6 +25,7 @@
 #include <media/media-device.h>
 
 #include "fimc-core.h"
+#include "fimc-lite.h"
 #include "fimc-mdevice.h"
 #include "mipi-csis.h"
 
@@ -37,22 +38,46 @@ static int __fimc_md_set_camclk(struct fimc_md *fmd,
  *
  * Caller holds the graph mutex.
  */
-void fimc_pipeline_prepare(struct fimc_dev *fimc, struct media_entity *me)
+void fimc_pipeline_prepare(struct fimc_pipeline *p, struct media_entity *me)
 {
-	struct media_entity_graph graph;
+	struct media_pad *pad = &me->pads[0];
 	struct v4l2_subdev *sd;
+	int i;
 
-	media_entity_graph_walk_start(&graph, me);
+	for (i = 0; i < IDX_MAX; i++)
+		p->subdevs[i] = NULL;
 
-	while ((me = media_entity_graph_walk_next(&graph))) {
-		if (media_entity_type(me) != MEDIA_ENT_T_V4L2_SUBDEV)
-			continue;
-		sd = media_entity_to_v4l2_subdev(me);
+	while (1) {
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		/* source pad */
+		pad = media_entity_remote_source(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
 
-		if (sd->grp_id == SENSOR_GROUP_ID)
-			fimc->pipeline.sensor = sd;
-		else if (sd->grp_id == CSIS_GROUP_ID)
-			fimc->pipeline.csis = sd;
+		switch (sd->grp_id) {
+		case SENSOR_GROUP_ID:
+			p->subdevs[IDX_SENSOR] = sd;
+			break;
+		case CSIS_GROUP_ID:
+			p->subdevs[IDX_CSIS] = sd;
+			break;
+		case FLITE_GROUP_ID:
+			p->subdevs[IDX_FLITE] = sd;
+			break;
+		case FIMC_GROUP_ID:
+			/* No need to control FIMC subdev through subdev ops */
+			break;
+		default:
+			pr_warn("%s: Unknown subdev grp_id: %#x\n",
+				__func__, sd->grp_id);
+		}
+		/* sink pad */
+		pad = &sd->entity.pads[0];
 	}
 }
 
@@ -85,30 +110,27 @@ static int __subdev_set_power(struct v4l2_subdev *sd, int on)
 /**
  * fimc_pipeline_s_power - change power state of all pipeline subdevs
  * @fimc: fimc device terminating the pipeline
- * @state: 1 to enable power or 0 for power down
+ * @state: true to power on, false to power off
  *
- * Need to be called with the graph mutex held.
+ * Needs to be called with the graph mutex held.
  */
-int fimc_pipeline_s_power(struct fimc_dev *fimc, int state)
+int fimc_pipeline_s_power(struct fimc_pipeline *p, bool state)
 {
-	int ret = 0;
+	unsigned int i;
+	int ret;
 
-	if (fimc->pipeline.sensor == NULL)
+	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENXIO;
 
-	if (state) {
-		ret = __subdev_set_power(fimc->pipeline.csis, 1);
-		if (ret && ret != -ENXIO)
+	for (i = 0; i < IDX_MAX; i++) {
+		unsigned int idx = state ? (IDX_MAX - 1) - i : i;
+
+		ret = __subdev_set_power(p->subdevs[idx], state);
+		if (ret < 0 && ret != -ENXIO)
 			return ret;
-		return __subdev_set_power(fimc->pipeline.sensor, 1);
 	}
 
-	ret = __subdev_set_power(fimc->pipeline.sensor, 0);
-	if (ret)
-		return ret;
-	ret = __subdev_set_power(fimc->pipeline.csis, 0);
-
-	return ret == -ENXIO ? 0 : ret;
+	return 0;
 }
 
 /**
@@ -119,32 +141,36 @@ int fimc_pipeline_s_power(struct fimc_dev *fimc, int state)
  *
  * This function must be called with the graph mutex held.
  */
-static int __fimc_pipeline_initialize(struct fimc_dev *fimc,
+static int __fimc_pipeline_initialize(struct fimc_pipeline *p,
 				      struct media_entity *me, bool prep)
 {
 	int ret;
 
 	if (prep)
-		fimc_pipeline_prepare(fimc, me);
-	if (fimc->pipeline.sensor == NULL)
+		fimc_pipeline_prepare(p, me);
+
+	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -EINVAL;
-	ret = fimc_md_set_camclk(fimc->pipeline.sensor, true);
+
+	ret = fimc_md_set_camclk(p->subdevs[IDX_SENSOR], true);
 	if (ret)
 		return ret;
-	return fimc_pipeline_s_power(fimc, 1);
+
+	return fimc_pipeline_s_power(p, 1);
 }
 
-int fimc_pipeline_initialize(struct fimc_dev *fimc, struct media_entity *me,
+int fimc_pipeline_initialize(struct fimc_pipeline *p, struct media_entity *me,
 			     bool prep)
 {
 	int ret;
 
 	mutex_lock(&me->parent->graph_mutex);
-	ret =  __fimc_pipeline_initialize(fimc, me, prep);
+	ret =  __fimc_pipeline_initialize(p, me, prep);
 	mutex_unlock(&me->parent->graph_mutex);
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(fimc_pipeline_initialize);
 
 /**
  * __fimc_pipeline_shutdown - disable the sensor clock and pipeline power
@@ -154,52 +180,55 @@ int fimc_pipeline_initialize(struct fimc_dev *fimc, struct media_entity *me,
  * sensor clock.
  * Called with the graph mutex held.
  */
-int __fimc_pipeline_shutdown(struct fimc_dev *fimc)
+int __fimc_pipeline_shutdown(struct fimc_pipeline *p)
 {
 	int ret = 0;
 
-	if (fimc->pipeline.sensor) {
-		ret = fimc_pipeline_s_power(fimc, 0);
-		fimc_md_set_camclk(fimc->pipeline.sensor, false);
+	if (p->subdevs[IDX_SENSOR]) {
+		ret = fimc_pipeline_s_power(p, 0);
+		fimc_md_set_camclk(p->subdevs[IDX_SENSOR], false);
 	}
 	return ret == -ENXIO ? 0 : ret;
 }
 
-int fimc_pipeline_shutdown(struct fimc_dev *fimc)
+int fimc_pipeline_shutdown(struct fimc_pipeline *p)
 {
-	struct media_entity *me = &fimc->vid_cap.vfd->entity;
+	struct media_entity *me = &p->subdevs[IDX_SENSOR]->entity;
 	int ret;
 
 	mutex_lock(&me->parent->graph_mutex);
-	ret = __fimc_pipeline_shutdown(fimc);
+	ret = __fimc_pipeline_shutdown(p);
 	mutex_unlock(&me->parent->graph_mutex);
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(fimc_pipeline_shutdown);
 
 /**
  * fimc_pipeline_s_stream - invoke s_stream on pipeline subdevs
- * @fimc: fimc device terminating the pipeline
+ * @pipeline: video pipeline structure
  * @on: passed as the s_stream call argument
  */
-int fimc_pipeline_s_stream(struct fimc_dev *fimc, int on)
+int fimc_pipeline_s_stream(struct fimc_pipeline *p, bool on)
 {
-	struct fimc_pipeline *p = &fimc->pipeline;
-	int ret = 0;
+	int i, ret;
 
-	if (p->sensor == NULL)
+	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENODEV;
 
-	if ((on && p->csis) || !on)
-		ret = v4l2_subdev_call(on ? p->csis : p->sensor,
-				       video, s_stream, on);
-	if (ret < 0 && ret != -ENOIOCTLCMD)
-		return ret;
-	if ((!on && p->csis) || on)
-		ret = v4l2_subdev_call(on ? p->sensor : p->csis,
-				       video, s_stream, on);
-	return ret == -ENOIOCTLCMD ? 0 : ret;
+	for (i = 0; i < IDX_MAX; i++) {
+		unsigned int idx = on ? (IDX_MAX - 1) - i : i;
+
+		ret = v4l2_subdev_call(p->subdevs[idx], video, s_stream, on);
+
+		if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+			return ret;
+	}
+
+	return 0;
+
 }
+EXPORT_SYMBOL_GPL(fimc_pipeline_s_stream);
 
 /*
  * Sensor subdevice helper functions
@@ -214,14 +243,20 @@ static struct v4l2_subdev *fimc_md_register_sensor(struct fimc_md *fmd,
 		return NULL;
 
 	adapter = i2c_get_adapter(s_info->pdata->i2c_bus_num);
-	if (!adapter)
-		return NULL;
+	if (!adapter) {
+		v4l2_warn(&fmd->v4l2_dev,
+			  "Failed to get I2C adapter %d, deferring probe\n",
+			  s_info->pdata->i2c_bus_num);
+		return ERR_PTR(-EPROBE_DEFER);
+	}
 	sd = v4l2_i2c_new_subdev_board(&fmd->v4l2_dev, adapter,
 				       s_info->pdata->board_info, NULL);
 	if (IS_ERR_OR_NULL(sd)) {
 		i2c_put_adapter(adapter);
-		v4l2_err(&fmd->v4l2_dev, "Failed to acquire subdev\n");
-		return NULL;
+		v4l2_warn(&fmd->v4l2_dev,
+			  "Failed to acquire subdev %s, deferring probe\n",
+			  s_info->pdata->board_info->type);
+		return ERR_PTR(-EPROBE_DEFER);
 	}
 	v4l2_set_subdev_hostdata(sd, s_info);
 	sd->grp_id = SENSOR_GROUP_ID;
@@ -269,13 +304,22 @@ static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 
 	fmd->num_sensors = num_clients;
 	for (i = 0; i < num_clients; i++) {
+		struct v4l2_subdev *sd;
+
 		fmd->sensor[i].pdata = &pdata->isp_info[i];
 		ret = __fimc_md_set_camclk(fmd, &fmd->sensor[i], true);
 		if (ret)
 			break;
-		fmd->sensor[i].subdev =
-			fimc_md_register_sensor(fmd, &fmd->sensor[i]);
+		sd = fimc_md_register_sensor(fmd, &fmd->sensor[i]);
 		ret = __fimc_md_set_camclk(fmd, &fmd->sensor[i], false);
+
+		if (!IS_ERR(sd)) {
+			fmd->sensor[i].subdev = sd;
+		} else {
+			fmd->sensor[i].subdev = NULL;
+			ret = PTR_ERR(sd);
+			break;
+		}
 		if (ret)
 			break;
 	}
@@ -289,21 +333,50 @@ static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 static int fimc_register_callback(struct device *dev, void *p)
 {
 	struct fimc_dev *fimc = dev_get_drvdata(dev);
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
 	struct fimc_md *fmd = p;
-	int ret;
+	int ret = 0;
 
 	if (!fimc || !fimc->pdev)
 		return 0;
+
 	if (fimc->pdev->id < 0 || fimc->pdev->id >= FIMC_MAX_DEVS)
 		return 0;
 
 	fmd->fimc[fimc->pdev->id] = fimc;
-	ret = fimc_register_m2m_device(fimc, &fmd->v4l2_dev);
-	if (ret)
-		return ret;
-	ret = fimc_register_capture_device(fimc, &fmd->v4l2_dev);
-	if (!ret)
-		fimc->vid_cap.user_subdev_api = fmd->user_subdev_api;
+	sd->grp_id = FIMC_GROUP_ID;
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (ret) {
+		v4l2_err(&fmd->v4l2_dev, "Failed to register FIMC.%d (%d)\n",
+			 fimc->id, ret);
+	}
+
+	return ret;
+}
+
+static int fimc_lite_register_callback(struct device *dev, void *p)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	struct v4l2_subdev *sd = &fimc->subdev;
+	struct fimc_md *fmd = p;
+	int ret;
+
+	if (fimc == NULL)
+		return 0;
+
+	if (fimc->index >= FIMC_LITE_MAX_DEVS)
+		return 0;
+
+	fmd->fimc_lite[fimc->index] = fimc;
+	sd->grp_id = FLITE_GROUP_ID;
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (ret) {
+		v4l2_err(&fmd->v4l2_dev,
+			 "Failed to register FIMC-LITE.%d (%d)\n",
+			 fimc->index, ret);
+	}
 	return ret;
 }
 
@@ -336,22 +409,56 @@ static int csis_register_callback(struct device *dev, void *p)
  */
 static int fimc_md_register_platform_entities(struct fimc_md *fmd)
 {
+	struct s5p_platform_fimc *pdata = fmd->pdev->dev.platform_data;
 	struct device_driver *driver;
-	int ret;
+	int ret, i;
 
 	driver = driver_find(FIMC_MODULE_NAME, &platform_bus_type);
-	if (!driver)
-		return -ENODEV;
+	if (!driver) {
+		v4l2_warn(&fmd->v4l2_dev,
+			 "%s driver not found, deffering probe\n",
+			 FIMC_MODULE_NAME);
+		return -EPROBE_DEFER;
+	}
+
 	ret = driver_for_each_device(driver, NULL, fmd,
 				     fimc_register_callback);
 	if (ret)
 		return ret;
 
-	driver = driver_find(CSIS_DRIVER_NAME, &platform_bus_type);
-	if (driver)
+	driver = driver_find(FIMC_LITE_DRV_NAME, &platform_bus_type);
+	if (driver && try_module_get(driver->owner)) {
 		ret = driver_for_each_device(driver, NULL, fmd,
-					     csis_register_callback);
-	return ret;
+					     fimc_lite_register_callback);
+		if (ret)
+			return ret;
+		module_put(driver->owner);
+	}
+	/*
+	 * Check if there is any sensor on the MIPI-CSI2 bus and
+	 * if not skip the s5p-csis module loading.
+	 */
+	if (pdata == NULL)
+		return 0;
+	for (i = 0; i < pdata->num_clients; i++) {
+		if (pdata->isp_info[i].bus_type == FIMC_MIPI_CSI2) {
+			ret = 1;
+			break;
+		}
+	}
+	if (!ret)
+		return 0;
+
+	driver = driver_find(CSIS_DRIVER_NAME, &platform_bus_type);
+	if (!driver || !try_module_get(driver->owner)) {
+		v4l2_warn(&fmd->v4l2_dev,
+			 "%s driver not found, deffering probe\n",
+			 CSIS_DRIVER_NAME);
+		return -EPROBE_DEFER;
+	}
+
+	return driver_for_each_device(driver, NULL, fmd,
+				      csis_register_callback);
 }
 
 static void fimc_md_unregister_entities(struct fimc_md *fmd)
@@ -361,14 +468,20 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (fmd->fimc[i] == NULL)
 			continue;
-		fimc_unregister_m2m_device(fmd->fimc[i]);
-		fimc_unregister_capture_device(fmd->fimc[i]);
+		v4l2_device_unregister_subdev(&fmd->fimc[i]->vid_cap.subdev);
 		fmd->fimc[i] = NULL;
 	}
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		if (fmd->fimc_lite[i] == NULL)
+			continue;
+		v4l2_device_unregister_subdev(&fmd->fimc_lite[i]->subdev);
+		fmd->fimc_lite[i] = NULL;
+	}
 	for (i = 0; i < CSIS_MAX_ENTITIES; i++) {
 		if (fmd->csis[i].sd == NULL)
 			continue;
 		v4l2_device_unregister_subdev(fmd->csis[i].sd);
+		module_put(fmd->csis[i].sd->owner);
 		fmd->csis[i].sd = NULL;
 	}
 	for (i = 0; i < fmd->num_sensors; i++) {
@@ -379,35 +492,6 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 	}
 }
 
-static int fimc_md_register_video_nodes(struct fimc_md *fmd)
-{
-	struct video_device *vdev;
-	int i, ret = 0;
-
-	for (i = 0; i < FIMC_MAX_DEVS && !ret; i++) {
-		if (!fmd->fimc[i])
-			continue;
-
-		vdev = fmd->fimc[i]->m2m.vfd;
-		if (vdev) {
-			ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
-			if (ret)
-				break;
-			v4l2_info(&fmd->v4l2_dev, "Registered %s as /dev/%s\n",
-				  vdev->name, video_device_node_name(vdev));
-		}
-
-		vdev = fmd->fimc[i]->vid_cap.vfd;
-		if (vdev == NULL)
-			continue;
-		ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
-		v4l2_info(&fmd->v4l2_dev, "Registered %s as /dev/%s\n",
-			  vdev->name, video_device_node_name(vdev));
-	}
-
-	return ret;
-}
-
 /**
  * __fimc_md_create_fimc_links - create links to all FIMC entities
  * @fmd: fimc media device
@@ -416,29 +500,29 @@ static int fimc_md_register_video_nodes(struct fimc_md *fmd)
  * @pad: the source entity pad index
  * @fimc_id: index of the fimc device for which link should be enabled
  */
-static int __fimc_md_create_fimc_links(struct fimc_md *fmd,
-				       struct media_entity *source,
-				       struct v4l2_subdev *sensor,
-				       int pad, int fimc_id)
+static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
+					    struct media_entity *source,
+					    struct v4l2_subdev *sensor,
+					    int pad, int fimc_id)
 {
 	struct fimc_sensor_info *s_info;
 	struct media_entity *sink;
-	unsigned int flags;
+	unsigned int flags = 0;
 	int ret, i;
 
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
-			break;
+			continue;
 		/*
 		 * Some FIMC variants are not fitted with camera capture
 		 * interface. Skip creating a link from sensor for those.
 		 */
-		if (sensor->grp_id == SENSOR_GROUP_ID &&
-		    !fmd->fimc[i]->variant->has_cam_if)
+		if (!fmd->fimc[i]->variant->has_cam_if)
 			continue;
 
 		flags = (i == fimc_id) ? MEDIA_LNK_FL_ENABLED : 0;
-		sink = &fmd->fimc[i]->vid_cap.subdev->entity;
+
+		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
 		ret = media_entity_create_link(source, pad, sink,
 					      FIMC_SD_PAD_SINK, flags);
 		if (ret)
@@ -453,7 +537,7 @@ static int __fimc_md_create_fimc_links(struct fimc_md *fmd,
 		v4l2_info(&fmd->v4l2_dev, "created link [%s] %c> [%s]",
 			  source->name, flags ? '=' : '-', sink->name);
 
-		if (flags == 0)
+		if (flags == 0 || sensor == NULL)
 			continue;
 		s_info = v4l2_get_subdev_hostdata(sensor);
 		if (!WARN_ON(s_info == NULL)) {
@@ -463,9 +547,55 @@ static int __fimc_md_create_fimc_links(struct fimc_md *fmd,
 			spin_unlock_irqrestore(&fmd->slock, irq_flags);
 		}
 	}
+
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		if (!fmd->fimc_lite[i])
+			continue;
+
+		flags = (i == fimc_id) ? MEDIA_LNK_FL_ENABLED : 0;
+
+		sink = &fmd->fimc_lite[i]->subdev.entity;
+		ret = media_entity_create_link(source, pad, sink,
+					       FLITE_SD_PAD_SINK, flags);
+		if (ret)
+			return ret;
+
+		/* Notify FIMC-LITE subdev entity */
+		ret = media_entity_call(sink, link_setup, &sink->pads[0],
+					&source->pads[pad], flags);
+		if (ret)
+			break;
+
+		v4l2_info(&fmd->v4l2_dev, "created link [%s] %c> [%s]",
+			  source->name, flags ? '=' : '-', sink->name);
+	}
 	return 0;
 }
 
+/* Create links from FIMC-LITE source pads to other entities */
+static int __fimc_md_create_flite_source_links(struct fimc_md *fmd)
+{
+	struct media_entity *source, *sink;
+	unsigned int flags = MEDIA_LNK_FL_ENABLED;
+	int i, ret;
+
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		struct fimc_lite *fimc = fmd->fimc_lite[i];
+		if (fimc == NULL)
+			continue;
+		source = &fimc->subdev.entity;
+		sink = &fimc->vfd->entity;
+		/* FIMC-LITE's subdev and video node */
+		ret = media_entity_create_link(source, FIMC_SD_PAD_SOURCE,
+					       sink, 0, flags);
+		if (ret)
+			break;
+		/* TODO: create links to other entities */
+	}
+
+	return ret;
+}
+
 /**
  * fimc_md_create_links - create default links between registered entities
  *
@@ -522,8 +652,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 			v4l2_info(&fmd->v4l2_dev, "created link [%s] => [%s]",
 				  sensor->entity.name, csis->entity.name);
 
-			source = &csis->entity;
-			pad = CSIS_PAD_SOURCE;
+			source = NULL;
 			break;
 
 		case FIMC_ITU_601...FIMC_ITU_656:
@@ -539,15 +668,27 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 		if (source == NULL)
 			continue;
 
-		ret = __fimc_md_create_fimc_links(fmd, source, sensor, pad,
-						  fimc_id++);
+		ret = __fimc_md_create_fimc_sink_links(fmd, source, sensor,
+						       pad, fimc_id++);
 	}
+
+	fimc_id = 0;
+	for (i = 0; i < ARRAY_SIZE(fmd->csis); i++) {
+		if (fmd->csis[i].sd == NULL)
+			continue;
+		source = &fmd->csis[i].sd->entity;
+		pad = CSIS_PAD_SOURCE;
+
+		ret = __fimc_md_create_fimc_sink_links(fmd, source, NULL,
+						       pad, fimc_id++);
+	}
+
 	/* Create immutable links between each FIMC's subdev and video node */
 	flags = MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED;
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
 			continue;
-		source = &fmd->fimc[i]->vid_cap.subdev->entity;
+		source = &fmd->fimc[i]->vid_cap.subdev.entity;
 		sink = &fmd->fimc[i]->vid_cap.vfd->entity;
 		ret = media_entity_create_link(source, FIMC_SD_PAD_SOURCE,
 					      sink, 0, flags);
@@ -555,7 +696,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 			break;
 	}
 
-	return ret;
+	return __fimc_md_create_flite_source_links(fmd);
 }
 
 /*
@@ -663,24 +804,40 @@ int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on)
 static int fimc_md_link_notify(struct media_pad *source,
 			       struct media_pad *sink, u32 flags)
 {
+	struct fimc_lite *fimc_lite = NULL;
+	struct fimc_dev *fimc = NULL;
+	struct fimc_pipeline *pipeline;
 	struct v4l2_subdev *sd;
-	struct fimc_dev *fimc;
 	int ret = 0;
 
 	if (media_entity_type(sink->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 		return 0;
 
 	sd = media_entity_to_v4l2_subdev(sink->entity);
-	fimc = v4l2_get_subdevdata(sd);
 
-	if (!(flags & MEDIA_LNK_FL_ENABLED)) {
-		ret = __fimc_pipeline_shutdown(fimc);
-		fimc->pipeline.sensor = NULL;
-		fimc->pipeline.csis = NULL;
+	switch (sd->grp_id) {
+	case FLITE_GROUP_ID:
+		fimc_lite = v4l2_get_subdevdata(sd);
+		pipeline = &fimc_lite->pipeline;
+		break;
+	case FIMC_GROUP_ID:
+		fimc = v4l2_get_subdevdata(sd);
+		pipeline = &fimc->pipeline;
+		break;
+	default:
+		return 0;
+	}
 
-		mutex_lock(&fimc->lock);
-		fimc_ctrls_delete(fimc->vid_cap.ctx);
-		mutex_unlock(&fimc->lock);
+	if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+		ret = __fimc_pipeline_shutdown(pipeline);
+		pipeline->subdevs[IDX_SENSOR] = NULL;
+		pipeline->subdevs[IDX_CSIS] = NULL;
+
+		if (fimc) {
+			mutex_lock(&fimc->lock);
+			fimc_ctrls_delete(fimc->vid_cap.ctx);
+			mutex_unlock(&fimc->lock);
+		}
 		return ret;
 	}
 	/*
@@ -688,14 +845,23 @@ static int fimc_md_link_notify(struct media_pad *source,
 	 * pipeline is already in use, i.e. its video node is opened.
 	 * Recreate the controls destroyed during the link deactivation.
 	 */
-	mutex_lock(&fimc->lock);
-	if (fimc->vid_cap.refcnt > 0) {
-		ret = __fimc_pipeline_initialize(fimc, source->entity, true);
+	if (fimc) {
+		mutex_lock(&fimc->lock);
+		if (fimc->vid_cap.refcnt > 0) {
+			ret = __fimc_pipeline_initialize(pipeline,
+							 source->entity, true);
 		if (!ret)
 			ret = fimc_capture_ctrls_create(fimc);
+		}
+		mutex_unlock(&fimc->lock);
+	} else {
+		mutex_lock(&fimc_lite->lock);
+		if (fimc_lite->ref_count > 0) {
+			ret = __fimc_pipeline_initialize(pipeline,
+							 source->entity, true);
+		}
+		mutex_unlock(&fimc_lite->lock);
 	}
-	mutex_unlock(&fimc->lock);
-
 	return ret ? -EPIPE : ret;
 }
 
@@ -744,7 +910,7 @@ static ssize_t fimc_md_sysfs_store(struct device *dev,
 static DEVICE_ATTR(subdev_conf_mode, S_IWUSR | S_IRUGO,
 		   fimc_md_sysfs_show, fimc_md_sysfs_store);
 
-static int __devinit fimc_md_probe(struct platform_device *pdev)
+static int fimc_md_probe(struct platform_device *pdev)
 {
 	struct v4l2_device *v4l2_dev;
 	struct fimc_md *fmd;
@@ -776,42 +942,48 @@ static int __devinit fimc_md_probe(struct platform_device *pdev)
 	ret = media_device_register(&fmd->media_dev);
 	if (ret < 0) {
 		v4l2_err(v4l2_dev, "Failed to register media device: %d\n", ret);
-		goto err2;
+		goto err_md;
 	}
 	ret = fimc_md_get_clocks(fmd);
 	if (ret)
-		goto err3;
+		goto err_clk;
 
 	fmd->user_subdev_api = false;
+
+	/* Protect the media graph while we're registering entities */
+	mutex_lock(&fmd->media_dev.graph_mutex);
+
 	ret = fimc_md_register_platform_entities(fmd);
 	if (ret)
-		goto err3;
+		goto err_unlock;
 
 	if (pdev->dev.platform_data) {
 		ret = fimc_md_register_sensor_entities(fmd);
 		if (ret)
-			goto err3;
+			goto err_unlock;
 	}
 	ret = fimc_md_create_links(fmd);
 	if (ret)
-		goto err3;
+		goto err_unlock;
 	ret = v4l2_device_register_subdev_nodes(&fmd->v4l2_dev);
 	if (ret)
-		goto err3;
-	ret = fimc_md_register_video_nodes(fmd);
-	if (ret)
-		goto err3;
+		goto err_unlock;
 
 	ret = device_create_file(&pdev->dev, &dev_attr_subdev_conf_mode);
-	if (!ret) {
-		platform_set_drvdata(pdev, fmd);
-		return 0;
-	}
-err3:
+	if (ret)
+		goto err_unlock;
+
+	platform_set_drvdata(pdev, fmd);
+	mutex_unlock(&fmd->media_dev.graph_mutex);
+	return 0;
+
+err_unlock:
+	mutex_unlock(&fmd->media_dev.graph_mutex);
+err_clk:
 	media_device_unregister(&fmd->media_dev);
 	fimc_md_put_clocks(fmd);
 	fimc_md_unregister_entities(fmd);
-err2:
+err_md:
 	v4l2_device_unregister(&fmd->v4l2_dev);
 	return ret;
 }
@@ -841,10 +1013,12 @@ static struct platform_driver fimc_md_driver = {
 int __init fimc_md_init(void)
 {
 	int ret;
+
 	request_module("s5p-csis");
 	ret = fimc_register_driver();
 	if (ret)
 		return ret;
+
 	return platform_driver_register(&fimc_md_driver);
 }
 void __exit fimc_md_exit(void)
diff --git a/drivers/media/video/s5p-fimc/fimc-mdevice.h b/drivers/media/video/s5p-fimc/fimc-mdevice.h
index da3780823e7d..3b8a3492a176 100644
--- a/drivers/media/video/s5p-fimc/fimc-mdevice.h
+++ b/drivers/media/video/s5p-fimc/fimc-mdevice.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2011 - 2012 Samsung Electronics Co., Ltd.
  *
  * 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
@@ -18,12 +18,15 @@
 #include <media/v4l2-subdev.h>
 
 #include "fimc-core.h"
+#include "fimc-lite.h"
 #include "mipi-csis.h"
 
-/* Group IDs of sensor, MIPI CSIS and the writeback subdevs. */
+/* Group IDs of sensor, MIPI-CSIS, FIMC-LITE and the writeback subdevs. */
 #define SENSOR_GROUP_ID		(1 << 8)
 #define CSIS_GROUP_ID		(1 << 9)
 #define WRITEBACK_GROUP_ID	(1 << 10)
+#define FIMC_GROUP_ID		(1 << 11)
+#define FLITE_GROUP_ID		(1 << 12)
 
 #define FIMC_MAX_SENSORS	8
 #define FIMC_MAX_CAMCLKS	2
@@ -73,6 +76,7 @@ struct fimc_md {
 	struct fimc_sensor_info sensor[FIMC_MAX_SENSORS];
 	int num_sensors;
 	struct fimc_camclk_info camclk[FIMC_MAX_CAMCLKS];
+	struct fimc_lite *fimc_lite[FIMC_LITE_MAX_DEVS];
 	struct fimc_dev *fimc[FIMC_MAX_DEVS];
 	struct media_device media_dev;
 	struct v4l2_device v4l2_dev;
@@ -108,11 +112,11 @@ static inline void fimc_md_graph_unlock(struct fimc_dev *fimc)
 }
 
 int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on);
-void fimc_pipeline_prepare(struct fimc_dev *fimc, struct media_entity *me);
-int fimc_pipeline_initialize(struct fimc_dev *fimc, struct media_entity *me,
+void fimc_pipeline_prepare(struct fimc_pipeline *p, struct media_entity *me);
+int fimc_pipeline_initialize(struct fimc_pipeline *p, struct media_entity *me,
 			     bool resume);
-int fimc_pipeline_shutdown(struct fimc_dev *fimc);
-int fimc_pipeline_s_power(struct fimc_dev *fimc, int state);
-int fimc_pipeline_s_stream(struct fimc_dev *fimc, int state);
+int fimc_pipeline_shutdown(struct fimc_pipeline *p);
+int fimc_pipeline_s_power(struct fimc_pipeline *p, bool state);
+int fimc_pipeline_s_stream(struct fimc_pipeline *p, bool state);
 
 #endif
diff --git a/drivers/media/video/s5p-fimc/fimc-reg.c b/drivers/media/video/s5p-fimc/fimc-reg.c
index 15466d0529c1..1fc4ce8446f5 100644
--- a/drivers/media/video/s5p-fimc/fimc-reg.c
+++ b/drivers/media/video/s5p-fimc/fimc-reg.c
@@ -1,9 +1,8 @@
 /*
  * Register interface file for Samsung Camera Interface (FIMC) driver
  *
- * Copyright (c) 2010 Samsung Electronics
- *
- * Sylwester Nawrocki, s.nawrocki@samsung.com
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ * 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
@@ -12,9 +11,9 @@
 
 #include <linux/io.h>
 #include <linux/delay.h>
-#include <mach/map.h>
 #include <media/s5p_fimc.h>
 
+#include "fimc-reg.h"
 #include "fimc-core.h"
 
 
@@ -22,19 +21,19 @@ void fimc_hw_reset(struct fimc_dev *dev)
 {
 	u32 cfg;
 
-	cfg = readl(dev->regs + S5P_CISRCFMT);
-	cfg |= S5P_CISRCFMT_ITU601_8BIT;
-	writel(cfg, dev->regs + S5P_CISRCFMT);
+	cfg = readl(dev->regs + FIMC_REG_CISRCFMT);
+	cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
+	writel(cfg, dev->regs + FIMC_REG_CISRCFMT);
 
 	/* Software reset. */
-	cfg = readl(dev->regs + S5P_CIGCTRL);
-	cfg |= (S5P_CIGCTRL_SWRST | S5P_CIGCTRL_IRQ_LEVEL);
-	writel(cfg, dev->regs + S5P_CIGCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg |= (FIMC_REG_CIGCTRL_SWRST | FIMC_REG_CIGCTRL_IRQ_LEVEL);
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 	udelay(10);
 
-	cfg = readl(dev->regs + S5P_CIGCTRL);
-	cfg &= ~S5P_CIGCTRL_SWRST;
-	writel(cfg, dev->regs + S5P_CIGCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg &= ~FIMC_REG_CIGCTRL_SWRST;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 
 	if (dev->variant->out_buf_count > 4)
 		fimc_hw_set_dma_seq(dev, 0xF);
@@ -42,32 +41,32 @@ void fimc_hw_reset(struct fimc_dev *dev)
 
 static u32 fimc_hw_get_in_flip(struct fimc_ctx *ctx)
 {
-	u32 flip = S5P_MSCTRL_FLIP_NORMAL;
+	u32 flip = FIMC_REG_MSCTRL_FLIP_NORMAL;
 
 	if (ctx->hflip)
-		flip = S5P_MSCTRL_FLIP_X_MIRROR;
+		flip = FIMC_REG_MSCTRL_FLIP_X_MIRROR;
 	if (ctx->vflip)
-		flip = S5P_MSCTRL_FLIP_Y_MIRROR;
+		flip = FIMC_REG_MSCTRL_FLIP_Y_MIRROR;
 
 	if (ctx->rotation <= 90)
 		return flip;
 
-	return (flip ^ S5P_MSCTRL_FLIP_180) & S5P_MSCTRL_FLIP_180;
+	return (flip ^ FIMC_REG_MSCTRL_FLIP_180) & FIMC_REG_MSCTRL_FLIP_180;
 }
 
 static u32 fimc_hw_get_target_flip(struct fimc_ctx *ctx)
 {
-	u32 flip = S5P_CITRGFMT_FLIP_NORMAL;
+	u32 flip = FIMC_REG_CITRGFMT_FLIP_NORMAL;
 
 	if (ctx->hflip)
-		flip |= S5P_CITRGFMT_FLIP_X_MIRROR;
+		flip |= FIMC_REG_CITRGFMT_FLIP_X_MIRROR;
 	if (ctx->vflip)
-		flip |= S5P_CITRGFMT_FLIP_Y_MIRROR;
+		flip |= FIMC_REG_CITRGFMT_FLIP_Y_MIRROR;
 
 	if (ctx->rotation <= 90)
 		return flip;
 
-	return (flip ^ S5P_CITRGFMT_FLIP_180) & S5P_CITRGFMT_FLIP_180;
+	return (flip ^ FIMC_REG_CITRGFMT_FLIP_180) & FIMC_REG_CITRGFMT_FLIP_180;
 }
 
 void fimc_hw_set_rotation(struct fimc_ctx *ctx)
@@ -75,9 +74,9 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 	u32 cfg, flip;
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	cfg = readl(dev->regs + S5P_CITRGFMT);
-	cfg &= ~(S5P_CITRGFMT_INROT90 | S5P_CITRGFMT_OUTROT90 |
-		 S5P_CITRGFMT_FLIP_180);
+	cfg = readl(dev->regs + FIMC_REG_CITRGFMT);
+	cfg &= ~(FIMC_REG_CITRGFMT_INROT90 | FIMC_REG_CITRGFMT_OUTROT90 |
+		 FIMC_REG_CITRGFMT_FLIP_180);
 
 	/*
 	 * The input and output rotator cannot work simultaneously.
@@ -85,21 +84,21 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 	 * in direct fifo output mode.
 	 */
 	if (ctx->rotation == 90 || ctx->rotation == 270) {
-		if (ctx->out_path == FIMC_LCDFIFO)
-			cfg |= S5P_CITRGFMT_INROT90;
+		if (ctx->out_path == FIMC_IO_LCDFIFO)
+			cfg |= FIMC_REG_CITRGFMT_INROT90;
 		else
-			cfg |= S5P_CITRGFMT_OUTROT90;
+			cfg |= FIMC_REG_CITRGFMT_OUTROT90;
 	}
 
-	if (ctx->out_path == FIMC_DMA) {
+	if (ctx->out_path == FIMC_IO_DMA) {
 		cfg |= fimc_hw_get_target_flip(ctx);
-		writel(cfg, dev->regs + S5P_CITRGFMT);
+		writel(cfg, dev->regs + FIMC_REG_CITRGFMT);
 	} else {
 		/* LCD FIFO path */
-		flip = readl(dev->regs + S5P_MSCTRL);
-		flip &= ~S5P_MSCTRL_FLIP_MASK;
+		flip = readl(dev->regs + FIMC_REG_MSCTRL);
+		flip &= ~FIMC_REG_MSCTRL_FLIP_MASK;
 		flip |= fimc_hw_get_in_flip(ctx);
-		writel(flip, dev->regs + S5P_MSCTRL);
+		writel(flip, dev->regs + FIMC_REG_MSCTRL);
 	}
 }
 
@@ -110,43 +109,40 @@ void fimc_hw_set_target_format(struct fimc_ctx *ctx)
 	struct fimc_frame *frame = &ctx->d_frame;
 
 	dbg("w= %d, h= %d color: %d", frame->width,
-		frame->height, frame->fmt->color);
+	    frame->height, frame->fmt->color);
 
-	cfg = readl(dev->regs + S5P_CITRGFMT);
-	cfg &= ~(S5P_CITRGFMT_FMT_MASK | S5P_CITRGFMT_HSIZE_MASK |
-		  S5P_CITRGFMT_VSIZE_MASK);
+	cfg = readl(dev->regs + FIMC_REG_CITRGFMT);
+	cfg &= ~(FIMC_REG_CITRGFMT_FMT_MASK | FIMC_REG_CITRGFMT_HSIZE_MASK |
+		 FIMC_REG_CITRGFMT_VSIZE_MASK);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB444...S5P_FIMC_RGB888:
-		cfg |= S5P_CITRGFMT_RGB;
+	case FIMC_FMT_RGB444...FIMC_FMT_RGB888:
+		cfg |= FIMC_REG_CITRGFMT_RGB;
 		break;
-	case S5P_FIMC_YCBCR420:
-		cfg |= S5P_CITRGFMT_YCBCR420;
+	case FIMC_FMT_YCBCR420:
+		cfg |= FIMC_REG_CITRGFMT_YCBCR420;
 		break;
-	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+	case FIMC_FMT_YCBYCR422...FIMC_FMT_CRYCBY422:
 		if (frame->fmt->colplanes == 1)
-			cfg |= S5P_CITRGFMT_YCBCR422_1P;
+			cfg |= FIMC_REG_CITRGFMT_YCBCR422_1P;
 		else
-			cfg |= S5P_CITRGFMT_YCBCR422;
+			cfg |= FIMC_REG_CITRGFMT_YCBCR422;
 		break;
 	default:
 		break;
 	}
 
-	if (ctx->rotation == 90 || ctx->rotation == 270) {
-		cfg |= S5P_CITRGFMT_HSIZE(frame->height);
-		cfg |= S5P_CITRGFMT_VSIZE(frame->width);
-	} else {
-
-		cfg |= S5P_CITRGFMT_HSIZE(frame->width);
-		cfg |= S5P_CITRGFMT_VSIZE(frame->height);
-	}
+	if (ctx->rotation == 90 || ctx->rotation == 270)
+		cfg |= (frame->height << 16) | frame->width;
+	else
+		cfg |= (frame->width << 16) | frame->height;
 
-	writel(cfg, dev->regs + S5P_CITRGFMT);
+	writel(cfg, dev->regs + FIMC_REG_CITRGFMT);
 
-	cfg = readl(dev->regs + S5P_CITAREA) & ~S5P_CITAREA_MASK;
+	cfg = readl(dev->regs + FIMC_REG_CITAREA);
+	cfg &= ~FIMC_REG_CITAREA_MASK;
 	cfg |= (frame->width * frame->height);
-	writel(cfg, dev->regs + S5P_CITAREA);
+	writel(cfg, dev->regs + FIMC_REG_CITAREA);
 }
 
 static void fimc_hw_set_out_dma_size(struct fimc_ctx *ctx)
@@ -155,87 +151,82 @@ static void fimc_hw_set_out_dma_size(struct fimc_ctx *ctx)
 	struct fimc_frame *frame = &ctx->d_frame;
 	u32 cfg;
 
-	cfg = S5P_ORIG_SIZE_HOR(frame->f_width);
-	cfg |= S5P_ORIG_SIZE_VER(frame->f_height);
-	writel(cfg, dev->regs + S5P_ORGOSIZE);
+	cfg = (frame->f_height << 16) | frame->f_width;
+	writel(cfg, dev->regs + FIMC_REG_ORGOSIZE);
 
 	/* Select color space conversion equation (HD/SD size).*/
-	cfg = readl(dev->regs + S5P_CIGCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
 	if (frame->f_width >= 1280) /* HD */
-		cfg |= S5P_CIGCTRL_CSC_ITU601_709;
+		cfg |= FIMC_REG_CIGCTRL_CSC_ITU601_709;
 	else	/* SD */
-		cfg &= ~S5P_CIGCTRL_CSC_ITU601_709;
-	writel(cfg, dev->regs + S5P_CIGCTRL);
+		cfg &= ~FIMC_REG_CIGCTRL_CSC_ITU601_709;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 
 }
 
 void fimc_hw_set_out_dma(struct fimc_ctx *ctx)
 {
-	u32 cfg;
 	struct fimc_dev *dev = ctx->fimc_dev;
 	struct fimc_frame *frame = &ctx->d_frame;
 	struct fimc_dma_offset *offset = &frame->dma_offset;
 	struct fimc_fmt *fmt = frame->fmt;
+	u32 cfg;
 
 	/* Set the input dma offsets. */
-	cfg = 0;
-	cfg |= S5P_CIO_OFFS_HOR(offset->y_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->y_v);
-	writel(cfg, dev->regs + S5P_CIOYOFF);
+	cfg = (offset->y_v << 16) | offset->y_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOYOFF);
 
-	cfg = 0;
-	cfg |= S5P_CIO_OFFS_HOR(offset->cb_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cb_v);
-	writel(cfg, dev->regs + S5P_CIOCBOFF);
+	cfg = (offset->cb_v << 16) | offset->cb_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOCBOFF);
 
-	cfg = 0;
-	cfg |= S5P_CIO_OFFS_HOR(offset->cr_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cr_v);
-	writel(cfg, dev->regs + S5P_CIOCROFF);
+	cfg = (offset->cr_v << 16) | offset->cr_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOCROFF);
 
 	fimc_hw_set_out_dma_size(ctx);
 
 	/* Configure chroma components order. */
-	cfg = readl(dev->regs + S5P_CIOCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
 
-	cfg &= ~(S5P_CIOCTRL_ORDER2P_MASK | S5P_CIOCTRL_ORDER422_MASK |
-		 S5P_CIOCTRL_YCBCR_PLANE_MASK | S5P_CIOCTRL_RGB16FMT_MASK);
+	cfg &= ~(FIMC_REG_CIOCTRL_ORDER2P_MASK |
+		 FIMC_REG_CIOCTRL_ORDER422_MASK |
+		 FIMC_REG_CIOCTRL_YCBCR_PLANE_MASK |
+		 FIMC_REG_CIOCTRL_RGB16FMT_MASK);
 
 	if (fmt->colplanes == 1)
 		cfg |= ctx->out_order_1p;
 	else if (fmt->colplanes == 2)
-		cfg |= ctx->out_order_2p | S5P_CIOCTRL_YCBCR_2PLANE;
+		cfg |= ctx->out_order_2p | FIMC_REG_CIOCTRL_YCBCR_2PLANE;
 	else if (fmt->colplanes == 3)
-		cfg |= S5P_CIOCTRL_YCBCR_3PLANE;
+		cfg |= FIMC_REG_CIOCTRL_YCBCR_3PLANE;
 
-	if (fmt->color == S5P_FIMC_RGB565)
-		cfg |= S5P_CIOCTRL_RGB565;
-	else if (fmt->color == S5P_FIMC_RGB555)
-		cfg |= S5P_CIOCTRL_ARGB1555;
-	else if (fmt->color == S5P_FIMC_RGB444)
-		cfg |= S5P_CIOCTRL_ARGB4444;
+	if (fmt->color == FIMC_FMT_RGB565)
+		cfg |= FIMC_REG_CIOCTRL_RGB565;
+	else if (fmt->color == FIMC_FMT_RGB555)
+		cfg |= FIMC_REG_CIOCTRL_ARGB1555;
+	else if (fmt->color == FIMC_FMT_RGB444)
+		cfg |= FIMC_REG_CIOCTRL_ARGB4444;
 
-	writel(cfg, dev->regs + S5P_CIOCTRL);
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
 }
 
 static void fimc_hw_en_autoload(struct fimc_dev *dev, int enable)
 {
-	u32 cfg = readl(dev->regs + S5P_ORGISIZE);
+	u32 cfg = readl(dev->regs + FIMC_REG_ORGISIZE);
 	if (enable)
-		cfg |= S5P_CIREAL_ISIZE_AUTOLOAD_EN;
+		cfg |= FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
 	else
-		cfg &= ~S5P_CIREAL_ISIZE_AUTOLOAD_EN;
-	writel(cfg, dev->regs + S5P_ORGISIZE);
+		cfg &= ~FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
+	writel(cfg, dev->regs + FIMC_REG_ORGISIZE);
 }
 
 void fimc_hw_en_lastirq(struct fimc_dev *dev, int enable)
 {
-	u32 cfg = readl(dev->regs + S5P_CIOCTRL);
+	u32 cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
 	if (enable)
-		cfg |= S5P_CIOCTRL_LASTIRQ_ENABLE;
+		cfg |= FIMC_REG_CIOCTRL_LASTIRQ_ENABLE;
 	else
-		cfg &= ~S5P_CIOCTRL_LASTIRQ_ENABLE;
-	writel(cfg, dev->regs + S5P_CIOCTRL);
+		cfg &= ~FIMC_REG_CIOCTRL_LASTIRQ_ENABLE;
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
 }
 
 void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
@@ -245,15 +236,13 @@ void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
 	u32 cfg, shfactor;
 
 	shfactor = 10 - (sc->hfactor + sc->vfactor);
+	cfg = shfactor << 28;
 
-	cfg = S5P_CISCPRERATIO_SHFACTOR(shfactor);
-	cfg |= S5P_CISCPRERATIO_HOR(sc->pre_hratio);
-	cfg |= S5P_CISCPRERATIO_VER(sc->pre_vratio);
-	writel(cfg, dev->regs + S5P_CISCPRERATIO);
+	cfg |= (sc->pre_hratio << 16) | sc->pre_vratio;
+	writel(cfg, dev->regs + FIMC_REG_CISCPRERATIO);
 
-	cfg = S5P_CISCPREDST_WIDTH(sc->pre_dst_width);
-	cfg |= S5P_CISCPREDST_HEIGHT(sc->pre_dst_height);
-	writel(cfg, dev->regs + S5P_CISCPREDST);
+	cfg = (sc->pre_dst_width << 16) | sc->pre_dst_height;
+	writel(cfg, dev->regs + FIMC_REG_CISCPREDST);
 }
 
 static void fimc_hw_set_scaler(struct fimc_ctx *ctx)
@@ -263,93 +252,95 @@ static void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 	struct fimc_frame *src_frame = &ctx->s_frame;
 	struct fimc_frame *dst_frame = &ctx->d_frame;
 
-	u32 cfg = readl(dev->regs + S5P_CISCCTRL);
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
 
-	cfg &= ~(S5P_CISCCTRL_CSCR2Y_WIDE | S5P_CISCCTRL_CSCY2R_WIDE |
-		 S5P_CISCCTRL_SCALEUP_H | S5P_CISCCTRL_SCALEUP_V |
-		 S5P_CISCCTRL_SCALERBYPASS | S5P_CISCCTRL_ONE2ONE |
-		 S5P_CISCCTRL_INRGB_FMT_MASK | S5P_CISCCTRL_OUTRGB_FMT_MASK |
-		 S5P_CISCCTRL_INTERLACE | S5P_CISCCTRL_RGB_EXT);
+	cfg &= ~(FIMC_REG_CISCCTRL_CSCR2Y_WIDE | FIMC_REG_CISCCTRL_CSCY2R_WIDE |
+		 FIMC_REG_CISCCTRL_SCALEUP_H | FIMC_REG_CISCCTRL_SCALEUP_V |
+		 FIMC_REG_CISCCTRL_SCALERBYPASS | FIMC_REG_CISCCTRL_ONE2ONE |
+		 FIMC_REG_CISCCTRL_INRGB_FMT_MASK | FIMC_REG_CISCCTRL_OUTRGB_FMT_MASK |
+		 FIMC_REG_CISCCTRL_INTERLACE | FIMC_REG_CISCCTRL_RGB_EXT);
 
 	if (!(ctx->flags & FIMC_COLOR_RANGE_NARROW))
-		cfg |= (S5P_CISCCTRL_CSCR2Y_WIDE | S5P_CISCCTRL_CSCY2R_WIDE);
+		cfg |= (FIMC_REG_CISCCTRL_CSCR2Y_WIDE |
+			FIMC_REG_CISCCTRL_CSCY2R_WIDE);
 
 	if (!sc->enabled)
-		cfg |= S5P_CISCCTRL_SCALERBYPASS;
+		cfg |= FIMC_REG_CISCCTRL_SCALERBYPASS;
 
 	if (sc->scaleup_h)
-		cfg |= S5P_CISCCTRL_SCALEUP_H;
+		cfg |= FIMC_REG_CISCCTRL_SCALEUP_H;
 
 	if (sc->scaleup_v)
-		cfg |= S5P_CISCCTRL_SCALEUP_V;
+		cfg |= FIMC_REG_CISCCTRL_SCALEUP_V;
 
 	if (sc->copy_mode)
-		cfg |= S5P_CISCCTRL_ONE2ONE;
+		cfg |= FIMC_REG_CISCCTRL_ONE2ONE;
 
-	if (ctx->in_path == FIMC_DMA) {
+	if (ctx->in_path == FIMC_IO_DMA) {
 		switch (src_frame->fmt->color) {
-		case S5P_FIMC_RGB565:
-			cfg |= S5P_CISCCTRL_INRGB_FMT_RGB565;
+		case FIMC_FMT_RGB565:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB565;
 			break;
-		case S5P_FIMC_RGB666:
-			cfg |= S5P_CISCCTRL_INRGB_FMT_RGB666;
+		case FIMC_FMT_RGB666:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB666;
 			break;
-		case S5P_FIMC_RGB888:
-			cfg |= S5P_CISCCTRL_INRGB_FMT_RGB888;
+		case FIMC_FMT_RGB888:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB888;
 			break;
 		}
 	}
 
-	if (ctx->out_path == FIMC_DMA) {
+	if (ctx->out_path == FIMC_IO_DMA) {
 		u32 color = dst_frame->fmt->color;
 
-		if (color >= S5P_FIMC_RGB444 && color <= S5P_FIMC_RGB565)
-			cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB565;
-		else if (color == S5P_FIMC_RGB666)
-			cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB666;
-		else if (color == S5P_FIMC_RGB888)
-			cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB888;
+		if (color >= FIMC_FMT_RGB444 && color <= FIMC_FMT_RGB565)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB565;
+		else if (color == FIMC_FMT_RGB666)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB666;
+		else if (color == FIMC_FMT_RGB888)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888;
 	} else {
-		cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB888;
+		cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888;
 
 		if (ctx->flags & FIMC_SCAN_MODE_INTERLACED)
-			cfg |= S5P_CISCCTRL_INTERLACE;
+			cfg |= FIMC_REG_CISCCTRL_INTERLACE;
 	}
 
-	writel(cfg, dev->regs + S5P_CISCCTRL);
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 }
 
 void fimc_hw_set_mainscaler(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
-	struct samsung_fimc_variant *variant = dev->variant;
+	struct fimc_variant *variant = dev->variant;
 	struct fimc_scaler *sc = &ctx->scaler;
 	u32 cfg;
 
 	dbg("main_hratio= 0x%X  main_vratio= 0x%X",
-		sc->main_hratio, sc->main_vratio);
+	    sc->main_hratio, sc->main_vratio);
 
 	fimc_hw_set_scaler(ctx);
 
-	cfg = readl(dev->regs + S5P_CISCCTRL);
-	cfg &= ~(S5P_CISCCTRL_MHRATIO_MASK | S5P_CISCCTRL_MVRATIO_MASK);
+	cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	cfg &= ~(FIMC_REG_CISCCTRL_MHRATIO_MASK |
+		 FIMC_REG_CISCCTRL_MVRATIO_MASK);
 
 	if (variant->has_mainscaler_ext) {
-		cfg |= S5P_CISCCTRL_MHRATIO_EXT(sc->main_hratio);
-		cfg |= S5P_CISCCTRL_MVRATIO_EXT(sc->main_vratio);
-		writel(cfg, dev->regs + S5P_CISCCTRL);
+		cfg |= FIMC_REG_CISCCTRL_MHRATIO_EXT(sc->main_hratio);
+		cfg |= FIMC_REG_CISCCTRL_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 
-		cfg = readl(dev->regs + S5P_CIEXTEN);
+		cfg = readl(dev->regs + FIMC_REG_CIEXTEN);
 
-		cfg &= ~(S5P_CIEXTEN_MVRATIO_EXT_MASK |
-			 S5P_CIEXTEN_MHRATIO_EXT_MASK);
-		cfg |= S5P_CIEXTEN_MHRATIO_EXT(sc->main_hratio);
-		cfg |= S5P_CIEXTEN_MVRATIO_EXT(sc->main_vratio);
-		writel(cfg, dev->regs + S5P_CIEXTEN);
+		cfg &= ~(FIMC_REG_CIEXTEN_MVRATIO_EXT_MASK |
+			 FIMC_REG_CIEXTEN_MHRATIO_EXT_MASK);
+		cfg |= FIMC_REG_CIEXTEN_MHRATIO_EXT(sc->main_hratio);
+		cfg |= FIMC_REG_CIEXTEN_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CIEXTEN);
 	} else {
-		cfg |= S5P_CISCCTRL_MHRATIO(sc->main_hratio);
-		cfg |= S5P_CISCCTRL_MVRATIO(sc->main_vratio);
-		writel(cfg, dev->regs + S5P_CISCCTRL);
+		cfg |= FIMC_REG_CISCCTRL_MHRATIO(sc->main_hratio);
+		cfg |= FIMC_REG_CISCCTRL_MVRATIO(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 	}
 }
 
@@ -357,40 +348,41 @@ void fimc_hw_en_capture(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	u32 cfg = readl(dev->regs + S5P_CIIMGCPT);
+	u32 cfg = readl(dev->regs + FIMC_REG_CIIMGCPT);
 
-	if (ctx->out_path == FIMC_DMA) {
+	if (ctx->out_path == FIMC_IO_DMA) {
 		/* one shot mode */
-		cfg |= S5P_CIIMGCPT_CPT_FREN_ENABLE | S5P_CIIMGCPT_IMGCPTEN;
+		cfg |= FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE |
+			FIMC_REG_CIIMGCPT_IMGCPTEN;
 	} else {
 		/* Continuous frame capture mode (freerun). */
-		cfg &= ~(S5P_CIIMGCPT_CPT_FREN_ENABLE |
-			 S5P_CIIMGCPT_CPT_FRMOD_CNT);
-		cfg |= S5P_CIIMGCPT_IMGCPTEN;
+		cfg &= ~(FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE |
+			 FIMC_REG_CIIMGCPT_CPT_FRMOD_CNT);
+		cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN;
 	}
 
 	if (ctx->scaler.enabled)
-		cfg |= S5P_CIIMGCPT_IMGCPTEN_SC;
+		cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN_SC;
 
-	writel(cfg | S5P_CIIMGCPT_IMGCPTEN, dev->regs + S5P_CIIMGCPT);
+	cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FIMC_REG_CIIMGCPT);
 }
 
-void fimc_hw_set_effect(struct fimc_ctx *ctx, bool active)
+void fimc_hw_set_effect(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 	struct fimc_effect *effect = &ctx->effect;
 	u32 cfg = 0;
 
-	if (active) {
-		cfg |= S5P_CIIMGEFF_IE_SC_AFTER | S5P_CIIMGEFF_IE_ENABLE;
+	if (effect->type != FIMC_REG_CIIMGEFF_FIN_BYPASS) {
+		cfg |= FIMC_REG_CIIMGEFF_IE_SC_AFTER |
+			FIMC_REG_CIIMGEFF_IE_ENABLE;
 		cfg |= effect->type;
-		if (effect->type == S5P_FIMC_EFFECT_ARBITRARY) {
-			cfg |= S5P_CIIMGEFF_PAT_CB(effect->pat_cb);
-			cfg |= S5P_CIIMGEFF_PAT_CR(effect->pat_cr);
-		}
+		if (effect->type == FIMC_REG_CIIMGEFF_FIN_ARBITRARY)
+			cfg |= (effect->pat_cb << 13) | effect->pat_cr;
 	}
 
-	writel(cfg, dev->regs + S5P_CIIMGEFF);
+	writel(cfg, dev->regs + FIMC_REG_CIIMGEFF);
 }
 
 void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx)
@@ -402,10 +394,10 @@ void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx)
 	if (!(frame->fmt->flags & FMT_HAS_ALPHA))
 		return;
 
-	cfg = readl(dev->regs + S5P_CIOCTRL);
-	cfg &= ~S5P_CIOCTRL_ALPHA_OUT_MASK;
+	cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
+	cfg &= ~FIMC_REG_CIOCTRL_ALPHA_OUT_MASK;
 	cfg |= (frame->alpha << 4);
-	writel(cfg, dev->regs + S5P_CIOCTRL);
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
 }
 
 static void fimc_hw_set_in_dma_size(struct fimc_ctx *ctx)
@@ -415,16 +407,14 @@ static void fimc_hw_set_in_dma_size(struct fimc_ctx *ctx)
 	u32 cfg_o = 0;
 	u32 cfg_r = 0;
 
-	if (FIMC_LCDFIFO == ctx->out_path)
-		cfg_r |= S5P_CIREAL_ISIZE_AUTOLOAD_EN;
+	if (FIMC_IO_LCDFIFO == ctx->out_path)
+		cfg_r |= FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
 
-	cfg_o |= S5P_ORIG_SIZE_HOR(frame->f_width);
-	cfg_o |= S5P_ORIG_SIZE_VER(frame->f_height);
-	cfg_r |= S5P_CIREAL_ISIZE_WIDTH(frame->width);
-	cfg_r |= S5P_CIREAL_ISIZE_HEIGHT(frame->height);
+	cfg_o |= (frame->f_height << 16) | frame->f_width;
+	cfg_r |= (frame->height << 16) | frame->width;
 
-	writel(cfg_o, dev->regs + S5P_ORGISIZE);
-	writel(cfg_r, dev->regs + S5P_CIREAL_ISIZE);
+	writel(cfg_o, dev->regs + FIMC_REG_ORGISIZE);
+	writel(cfg_r, dev->regs + FIMC_REG_CIREAL_ISIZE);
 }
 
 void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
@@ -435,80 +425,77 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 	u32 cfg;
 
 	/* Set the pixel offsets. */
-	cfg = S5P_CIO_OFFS_HOR(offset->y_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->y_v);
-	writel(cfg, dev->regs + S5P_CIIYOFF);
+	cfg = (offset->y_v << 16) | offset->y_h;
+	writel(cfg, dev->regs + FIMC_REG_CIIYOFF);
 
-	cfg = S5P_CIO_OFFS_HOR(offset->cb_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cb_v);
-	writel(cfg, dev->regs + S5P_CIICBOFF);
+	cfg = (offset->cb_v << 16) | offset->cb_h;
+	writel(cfg, dev->regs + FIMC_REG_CIICBOFF);
 
-	cfg = S5P_CIO_OFFS_HOR(offset->cr_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cr_v);
-	writel(cfg, dev->regs + S5P_CIICROFF);
+	cfg = (offset->cr_v << 16) | offset->cr_h;
+	writel(cfg, dev->regs + FIMC_REG_CIICROFF);
 
 	/* Input original and real size. */
 	fimc_hw_set_in_dma_size(ctx);
 
 	/* Use DMA autoload only in FIFO mode. */
-	fimc_hw_en_autoload(dev, ctx->out_path == FIMC_LCDFIFO);
+	fimc_hw_en_autoload(dev, ctx->out_path == FIMC_IO_LCDFIFO);
 
 	/* Set the input DMA to process single frame only. */
-	cfg = readl(dev->regs + S5P_MSCTRL);
-	cfg &= ~(S5P_MSCTRL_INFORMAT_MASK
-		| S5P_MSCTRL_IN_BURST_COUNT_MASK
-		| S5P_MSCTRL_INPUT_MASK
-		| S5P_MSCTRL_C_INT_IN_MASK
-		| S5P_MSCTRL_2P_IN_ORDER_MASK);
+	cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	cfg &= ~(FIMC_REG_MSCTRL_INFORMAT_MASK
+		 | FIMC_REG_MSCTRL_IN_BURST_COUNT_MASK
+		 | FIMC_REG_MSCTRL_INPUT_MASK
+		 | FIMC_REG_MSCTRL_C_INT_IN_MASK
+		 | FIMC_REG_MSCTRL_2P_IN_ORDER_MASK);
 
-	cfg |= (S5P_MSCTRL_IN_BURST_COUNT(4)
-		| S5P_MSCTRL_INPUT_MEMORY
-		| S5P_MSCTRL_FIFO_CTRL_FULL);
+	cfg |= (FIMC_REG_MSCTRL_IN_BURST_COUNT(4)
+		| FIMC_REG_MSCTRL_INPUT_MEMORY
+		| FIMC_REG_MSCTRL_FIFO_CTRL_FULL);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB565...S5P_FIMC_RGB888:
-		cfg |= S5P_MSCTRL_INFORMAT_RGB;
+	case FIMC_FMT_RGB565...FIMC_FMT_RGB888:
+		cfg |= FIMC_REG_MSCTRL_INFORMAT_RGB;
 		break;
-	case S5P_FIMC_YCBCR420:
-		cfg |= S5P_MSCTRL_INFORMAT_YCBCR420;
+	case FIMC_FMT_YCBCR420:
+		cfg |= FIMC_REG_MSCTRL_INFORMAT_YCBCR420;
 
 		if (frame->fmt->colplanes == 2)
-			cfg |= ctx->in_order_2p | S5P_MSCTRL_C_INT_IN_2PLANE;
+			cfg |= ctx->in_order_2p | FIMC_REG_MSCTRL_C_INT_IN_2PLANE;
 		else
-			cfg |= S5P_MSCTRL_C_INT_IN_3PLANE;
+			cfg |= FIMC_REG_MSCTRL_C_INT_IN_3PLANE;
 
 		break;
-	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+	case FIMC_FMT_YCBYCR422...FIMC_FMT_CRYCBY422:
 		if (frame->fmt->colplanes == 1) {
 			cfg |= ctx->in_order_1p
-				| S5P_MSCTRL_INFORMAT_YCBCR422_1P;
+				| FIMC_REG_MSCTRL_INFORMAT_YCBCR422_1P;
 		} else {
-			cfg |= S5P_MSCTRL_INFORMAT_YCBCR422;
+			cfg |= FIMC_REG_MSCTRL_INFORMAT_YCBCR422;
 
 			if (frame->fmt->colplanes == 2)
 				cfg |= ctx->in_order_2p
-					| S5P_MSCTRL_C_INT_IN_2PLANE;
+					| FIMC_REG_MSCTRL_C_INT_IN_2PLANE;
 			else
-				cfg |= S5P_MSCTRL_C_INT_IN_3PLANE;
+				cfg |= FIMC_REG_MSCTRL_C_INT_IN_3PLANE;
 		}
 		break;
 	default:
 		break;
 	}
 
-	writel(cfg, dev->regs + S5P_MSCTRL);
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
 
 	/* Input/output DMA linear/tiled mode. */
-	cfg = readl(dev->regs + S5P_CIDMAPARAM);
-	cfg &= ~S5P_CIDMAPARAM_TILE_MASK;
+	cfg = readl(dev->regs + FIMC_REG_CIDMAPARAM);
+	cfg &= ~FIMC_REG_CIDMAPARAM_TILE_MASK;
 
 	if (tiled_fmt(ctx->s_frame.fmt))
-		cfg |= S5P_CIDMAPARAM_R_64X32;
+		cfg |= FIMC_REG_CIDMAPARAM_R_64X32;
 
 	if (tiled_fmt(ctx->d_frame.fmt))
-		cfg |= S5P_CIDMAPARAM_W_64X32;
+		cfg |= FIMC_REG_CIDMAPARAM_W_64X32;
 
-	writel(cfg, dev->regs + S5P_CIDMAPARAM);
+	writel(cfg, dev->regs + FIMC_REG_CIDMAPARAM);
 }
 
 
@@ -516,40 +503,40 @@ void fimc_hw_set_input_path(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	u32 cfg = readl(dev->regs + S5P_MSCTRL);
-	cfg &= ~S5P_MSCTRL_INPUT_MASK;
+	u32 cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	cfg &= ~FIMC_REG_MSCTRL_INPUT_MASK;
 
-	if (ctx->in_path == FIMC_DMA)
-		cfg |= S5P_MSCTRL_INPUT_MEMORY;
+	if (ctx->in_path == FIMC_IO_DMA)
+		cfg |= FIMC_REG_MSCTRL_INPUT_MEMORY;
 	else
-		cfg |= S5P_MSCTRL_INPUT_EXTCAM;
+		cfg |= FIMC_REG_MSCTRL_INPUT_EXTCAM;
 
-	writel(cfg, dev->regs + S5P_MSCTRL);
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
 }
 
 void fimc_hw_set_output_path(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	u32 cfg = readl(dev->regs + S5P_CISCCTRL);
-	cfg &= ~S5P_CISCCTRL_LCDPATHEN_FIFO;
-	if (ctx->out_path == FIMC_LCDFIFO)
-		cfg |= S5P_CISCCTRL_LCDPATHEN_FIFO;
-	writel(cfg, dev->regs + S5P_CISCCTRL);
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	cfg &= ~FIMC_REG_CISCCTRL_LCDPATHEN_FIFO;
+	if (ctx->out_path == FIMC_IO_LCDFIFO)
+		cfg |= FIMC_REG_CISCCTRL_LCDPATHEN_FIFO;
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 }
 
 void fimc_hw_set_input_addr(struct fimc_dev *dev, struct fimc_addr *paddr)
 {
-	u32 cfg = readl(dev->regs + S5P_CIREAL_ISIZE);
-	cfg |= S5P_CIREAL_ISIZE_ADDR_CH_DIS;
-	writel(cfg, dev->regs + S5P_CIREAL_ISIZE);
+	u32 cfg = readl(dev->regs + FIMC_REG_CIREAL_ISIZE);
+	cfg |= FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS;
+	writel(cfg, dev->regs + FIMC_REG_CIREAL_ISIZE);
 
-	writel(paddr->y, dev->regs + S5P_CIIYSA(0));
-	writel(paddr->cb, dev->regs + S5P_CIICBSA(0));
-	writel(paddr->cr, dev->regs + S5P_CIICRSA(0));
+	writel(paddr->y, dev->regs + FIMC_REG_CIIYSA(0));
+	writel(paddr->cb, dev->regs + FIMC_REG_CIICBSA(0));
+	writel(paddr->cr, dev->regs + FIMC_REG_CIICRSA(0));
 
-	cfg &= ~S5P_CIREAL_ISIZE_ADDR_CH_DIS;
-	writel(cfg, dev->regs + S5P_CIREAL_ISIZE);
+	cfg &= ~FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS;
+	writel(cfg, dev->regs + FIMC_REG_CIREAL_ISIZE);
 }
 
 void fimc_hw_set_output_addr(struct fimc_dev *dev,
@@ -557,9 +544,9 @@ void fimc_hw_set_output_addr(struct fimc_dev *dev,
 {
 	int i = (index == -1) ? 0 : index;
 	do {
-		writel(paddr->y, dev->regs + S5P_CIOYSA(i));
-		writel(paddr->cb, dev->regs + S5P_CIOCBSA(i));
-		writel(paddr->cr, dev->regs + S5P_CIOCRSA(i));
+		writel(paddr->y, dev->regs + FIMC_REG_CIOYSA(i));
+		writel(paddr->cb, dev->regs + FIMC_REG_CIOCBSA(i));
+		writel(paddr->cr, dev->regs + FIMC_REG_CIOCRSA(i));
 		dbg("dst_buf[%d]: 0x%X, cb: 0x%X, cr: 0x%X",
 		    i, paddr->y, paddr->cb, paddr->cr);
 	} while (index == -1 && ++i < FIMC_MAX_OUT_BUFS);
@@ -568,32 +555,45 @@ void fimc_hw_set_output_addr(struct fimc_dev *dev,
 int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
 				struct s5p_fimc_isp_info *cam)
 {
-	u32 cfg = readl(fimc->regs + S5P_CIGCTRL);
+	u32 cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
 
-	cfg &= ~(S5P_CIGCTRL_INVPOLPCLK | S5P_CIGCTRL_INVPOLVSYNC |
-		 S5P_CIGCTRL_INVPOLHREF | S5P_CIGCTRL_INVPOLHSYNC |
-		 S5P_CIGCTRL_INVPOLFIELD);
+	cfg &= ~(FIMC_REG_CIGCTRL_INVPOLPCLK | FIMC_REG_CIGCTRL_INVPOLVSYNC |
+		 FIMC_REG_CIGCTRL_INVPOLHREF | FIMC_REG_CIGCTRL_INVPOLHSYNC |
+		 FIMC_REG_CIGCTRL_INVPOLFIELD);
 
 	if (cam->flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
-		cfg |= S5P_CIGCTRL_INVPOLPCLK;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLPCLK;
 
 	if (cam->flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLVSYNC;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLVSYNC;
 
 	if (cam->flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLHREF;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLHREF;
 
 	if (cam->flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLHSYNC;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLHSYNC;
 
 	if (cam->flags & V4L2_MBUS_FIELD_EVEN_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLFIELD;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLFIELD;
 
-	writel(cfg, fimc->regs + S5P_CIGCTRL);
+	writel(cfg, fimc->regs + FIMC_REG_CIGCTRL);
 
 	return 0;
 }
 
+struct mbus_pixfmt_desc {
+	u32 pixelcode;
+	u32 cisrcfmt;
+	u16 bus_width;
+};
+
+static const struct mbus_pixfmt_desc pix_desc[] = {
+	{ V4L2_MBUS_FMT_YUYV8_2X8, FIMC_REG_CISRCFMT_ORDER422_YCBYCR, 8 },
+	{ V4L2_MBUS_FMT_YVYU8_2X8, FIMC_REG_CISRCFMT_ORDER422_YCRYCB, 8 },
+	{ V4L2_MBUS_FMT_VYUY8_2X8, FIMC_REG_CISRCFMT_ORDER422_CRYCBY, 8 },
+	{ V4L2_MBUS_FMT_UYVY8_2X8, FIMC_REG_CISRCFMT_ORDER422_CBYCRY, 8 },
+};
+
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 			      struct s5p_fimc_isp_info *cam)
 {
@@ -602,18 +602,6 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 	u32 bus_width;
 	int i;
 
-	static const struct {
-		u32 pixelcode;
-		u32 cisrcfmt;
-		u16 bus_width;
-	} pix_desc[] = {
-		{ V4L2_MBUS_FMT_YUYV8_2X8, S5P_CISRCFMT_ORDER422_YCBYCR, 8 },
-		{ V4L2_MBUS_FMT_YVYU8_2X8, S5P_CISRCFMT_ORDER422_YCRYCB, 8 },
-		{ V4L2_MBUS_FMT_VYUY8_2X8, S5P_CISRCFMT_ORDER422_CRYCBY, 8 },
-		{ V4L2_MBUS_FMT_UYVY8_2X8, S5P_CISRCFMT_ORDER422_CBYCRY, 8 },
-		/* TODO: Add pixel codes for 16-bit bus width */
-	};
-
 	if (cam->bus_type == FIMC_ITU_601 || cam->bus_type == FIMC_ITU_656) {
 		for (i = 0; i < ARRAY_SIZE(pix_desc); i++) {
 			if (fimc->vid_cap.mf.code == pix_desc[i].pixelcode) {
@@ -632,41 +620,37 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 
 		if (cam->bus_type == FIMC_ITU_601) {
 			if (bus_width == 8)
-				cfg |= S5P_CISRCFMT_ITU601_8BIT;
+				cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
 			else if (bus_width == 16)
-				cfg |= S5P_CISRCFMT_ITU601_16BIT;
+				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))
-			cfg |= S5P_CISRCFMT_ITU601_8BIT;
+			cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
 	}
 
-	cfg |= S5P_CISRCFMT_HSIZE(f->o_width) | S5P_CISRCFMT_VSIZE(f->o_height);
-	writel(cfg, fimc->regs + S5P_CISRCFMT);
+	cfg |= (f->o_width << 16) | f->o_height;
+	writel(cfg, fimc->regs + FIMC_REG_CISRCFMT);
 	return 0;
 }
 
-
-int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
+void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
 {
 	u32 hoff2, voff2;
 
-	u32 cfg = readl(fimc->regs + S5P_CIWDOFST);
+	u32 cfg = readl(fimc->regs + FIMC_REG_CIWDOFST);
 
-	cfg &= ~(S5P_CIWDOFST_HOROFF_MASK | S5P_CIWDOFST_VEROFF_MASK);
-	cfg |=  S5P_CIWDOFST_OFF_EN |
-		S5P_CIWDOFST_HOROFF(f->offs_h) |
-		S5P_CIWDOFST_VEROFF(f->offs_v);
+	cfg &= ~(FIMC_REG_CIWDOFST_HOROFF_MASK | FIMC_REG_CIWDOFST_VEROFF_MASK);
+	cfg |=  FIMC_REG_CIWDOFST_OFF_EN |
+		(f->offs_h << 16) | f->offs_v;
 
-	writel(cfg, fimc->regs + S5P_CIWDOFST);
+	writel(cfg, fimc->regs + FIMC_REG_CIWDOFST);
 
 	/* See CIWDOFSTn register description in the datasheet for details. */
 	hoff2 = f->o_width - f->width - f->offs_h;
 	voff2 = f->o_height - f->height - f->offs_v;
-	cfg = S5P_CIWDOFST2_HOROFF(hoff2) | S5P_CIWDOFST2_VEROFF(voff2);
-
-	writel(cfg, fimc->regs + S5P_CIWDOFST2);
-	return 0;
+	cfg = (hoff2 << 16) | voff2;
+	writel(cfg, fimc->regs + FIMC_REG_CIWDOFST2);
 }
 
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
@@ -674,28 +658,29 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 {
 	u32 cfg, tmp;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	u32 csis_data_alignment = 32;
 
-	cfg = readl(fimc->regs + S5P_CIGCTRL);
+	cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
 
 	/* Select ITU B interface, disable Writeback path and test pattern. */
-	cfg &= ~(S5P_CIGCTRL_TESTPAT_MASK | S5P_CIGCTRL_SELCAM_ITU_A |
-		S5P_CIGCTRL_SELCAM_MIPI | S5P_CIGCTRL_CAMIF_SELWB |
-		S5P_CIGCTRL_SELCAM_MIPI_A | S5P_CIGCTRL_CAM_JPEG);
+	cfg &= ~(FIMC_REG_CIGCTRL_TESTPAT_MASK | FIMC_REG_CIGCTRL_SELCAM_ITU_A |
+		FIMC_REG_CIGCTRL_SELCAM_MIPI | FIMC_REG_CIGCTRL_CAMIF_SELWB |
+		FIMC_REG_CIGCTRL_SELCAM_MIPI_A | FIMC_REG_CIGCTRL_CAM_JPEG);
 
 	if (cam->bus_type == FIMC_MIPI_CSI2) {
-		cfg |= S5P_CIGCTRL_SELCAM_MIPI;
+		cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI;
 
 		if (cam->mux_id == 0)
-			cfg |= S5P_CIGCTRL_SELCAM_MIPI_A;
+			cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI_A;
 
 		/* TODO: add remaining supported formats. */
 		switch (vid_cap->mf.code) {
 		case V4L2_MBUS_FMT_VYUY8_2X8:
-			tmp = S5P_CSIIMGFMT_YCBCR422_8BIT;
+			tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
 			break;
 		case V4L2_MBUS_FMT_JPEG_1X8:
-			tmp = S5P_CSIIMGFMT_USER(1);
-			cfg |= S5P_CIGCTRL_CAM_JPEG;
+			tmp = FIMC_REG_CSIIMGFMT_USER(1);
+			cfg |= FIMC_REG_CIGCTRL_CAM_JPEG;
 			break;
 		default:
 			v4l2_err(fimc->vid_cap.vfd,
@@ -703,21 +688,86 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 				 vid_cap->mf.code);
 			return -EINVAL;
 		}
-		tmp |= (cam->csi_data_align == 32) << 8;
+		tmp |= (csis_data_alignment == 32) << 8;
 
-		writel(tmp, fimc->regs + S5P_CSIIMGFMT);
+		writel(tmp, fimc->regs + FIMC_REG_CSIIMGFMT);
 
 	} else if (cam->bus_type == FIMC_ITU_601 ||
 		   cam->bus_type == FIMC_ITU_656) {
 		if (cam->mux_id == 0) /* ITU-A, ITU-B: 0, 1 */
-			cfg |= S5P_CIGCTRL_SELCAM_ITU_A;
+			cfg |= FIMC_REG_CIGCTRL_SELCAM_ITU_A;
 	} else if (cam->bus_type == FIMC_LCD_WB) {
-		cfg |= S5P_CIGCTRL_CAMIF_SELWB;
+		cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
 	} else {
 		err("invalid camera bus type selected\n");
 		return -EINVAL;
 	}
-	writel(cfg, fimc->regs + S5P_CIGCTRL);
+	writel(cfg, fimc->regs + FIMC_REG_CIGCTRL);
 
 	return 0;
 }
+
+void fimc_hw_clear_irq(struct fimc_dev *dev)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg |= FIMC_REG_CIGCTRL_IRQ_CLR;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
+}
+
+void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	if (on)
+		cfg |= FIMC_REG_CISCCTRL_SCALERSTART;
+	else
+		cfg &= ~FIMC_REG_CISCCTRL_SCALERSTART;
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
+}
+
+void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	if (on)
+		cfg |= FIMC_REG_MSCTRL_ENVID;
+	else
+		cfg &= ~FIMC_REG_MSCTRL_ENVID;
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
+}
+
+void fimc_hw_dis_capture(struct fimc_dev *dev)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIIMGCPT);
+	cfg &= ~(FIMC_REG_CIIMGCPT_IMGCPTEN | FIMC_REG_CIIMGCPT_IMGCPTEN_SC);
+	writel(cfg, dev->regs + FIMC_REG_CIIMGCPT);
+}
+
+/* Return an index to the buffer actually being written. */
+u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
+{
+	u32 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_CISTATUS);
+
+	return (reg & FIMC_REG_CISTATUS_FRAMECNT_MASK) >>
+		FIMC_REG_CISTATUS_FRAMECNT_SHIFT;
+}
+
+/* Locking: the caller holds fimc->slock */
+void fimc_activate_capture(struct fimc_ctx *ctx)
+{
+	fimc_hw_enable_scaler(ctx->fimc_dev, ctx->scaler.enabled);
+	fimc_hw_en_capture(ctx);
+}
+
+void fimc_deactivate_capture(struct fimc_dev *fimc)
+{
+	fimc_hw_en_lastirq(fimc, true);
+	fimc_hw_dis_capture(fimc);
+	fimc_hw_enable_scaler(fimc, false);
+	fimc_hw_en_lastirq(fimc, false);
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-reg.h b/drivers/media/video/s5p-fimc/fimc-reg.h
new file mode 100644
index 000000000000..579ac8ac03de
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-reg.h
@@ -0,0 +1,326 @@
+/*
+ * Samsung camera host interface (FIMC) registers definition
+ *
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ *
+ * 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 FIMC_REG_H_
+#define FIMC_REG_H_
+
+#include "fimc-core.h"
+
+/* Input source format */
+#define FIMC_REG_CISRCFMT			0x00
+#define FIMC_REG_CISRCFMT_ITU601_8BIT		(1 << 31)
+#define FIMC_REG_CISRCFMT_ITU601_16BIT		(1 << 29)
+#define FIMC_REG_CISRCFMT_ORDER422_YCBYCR	(0 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_YCRYCB	(1 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_CBYCRY	(2 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_CRYCBY	(3 << 14)
+
+/* Window offset */
+#define FIMC_REG_CIWDOFST			0x04
+#define FIMC_REG_CIWDOFST_OFF_EN		(1 << 31)
+#define FIMC_REG_CIWDOFST_CLROVFIY		(1 << 30)
+#define FIMC_REG_CIWDOFST_CLROVRLB		(1 << 29)
+#define FIMC_REG_CIWDOFST_HOROFF_MASK		(0x7ff << 16)
+#define FIMC_REG_CIWDOFST_CLROVFICB		(1 << 15)
+#define FIMC_REG_CIWDOFST_CLROVFICR		(1 << 14)
+#define FIMC_REG_CIWDOFST_VEROFF_MASK		(0xfff << 0)
+
+/* Global control */
+#define FIMC_REG_CIGCTRL			0x08
+#define FIMC_REG_CIGCTRL_SWRST			(1 << 31)
+#define FIMC_REG_CIGCTRL_CAMRST_A		(1 << 30)
+#define FIMC_REG_CIGCTRL_SELCAM_ITU_A		(1 << 29)
+#define FIMC_REG_CIGCTRL_TESTPAT_NORMAL		(0 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_COLOR_BAR	(1 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_HOR_INC	(2 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_VER_INC	(3 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_MASK		(3 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_SHIFT		27
+#define FIMC_REG_CIGCTRL_INVPOLPCLK		(1 << 26)
+#define FIMC_REG_CIGCTRL_INVPOLVSYNC		(1 << 25)
+#define FIMC_REG_CIGCTRL_INVPOLHREF		(1 << 24)
+#define FIMC_REG_CIGCTRL_IRQ_OVFEN		(1 << 22)
+#define FIMC_REG_CIGCTRL_HREF_MASK		(1 << 21)
+#define FIMC_REG_CIGCTRL_IRQ_LEVEL		(1 << 20)
+#define FIMC_REG_CIGCTRL_IRQ_CLR		(1 << 19)
+#define FIMC_REG_CIGCTRL_IRQ_ENABLE		(1 << 16)
+#define FIMC_REG_CIGCTRL_SHDW_DISABLE		(1 << 12)
+#define FIMC_REG_CIGCTRL_CAM_JPEG		(1 << 8)
+#define FIMC_REG_CIGCTRL_SELCAM_MIPI_A		(1 << 7)
+#define FIMC_REG_CIGCTRL_CAMIF_SELWB		(1 << 6)
+/* 0 - ITU601; 1 - ITU709 */
+#define FIMC_REG_CIGCTRL_CSC_ITU601_709		(1 << 5)
+#define FIMC_REG_CIGCTRL_INVPOLHSYNC		(1 << 4)
+#define FIMC_REG_CIGCTRL_SELCAM_MIPI		(1 << 3)
+#define FIMC_REG_CIGCTRL_INVPOLFIELD		(1 << 1)
+#define FIMC_REG_CIGCTRL_INTERLACE		(1 << 0)
+
+/* Window offset 2 */
+#define FIMC_REG_CIWDOFST2			0x14
+#define FIMC_REG_CIWDOFST2_HOROFF_MASK		(0xfff << 16)
+#define FIMC_REG_CIWDOFST2_VEROFF_MASK		(0xfff << 0)
+
+/* Output DMA Y/Cb/Cr plane start addresses */
+#define FIMC_REG_CIOYSA(n)			(0x18 + (n) * 4)
+#define FIMC_REG_CIOCBSA(n)			(0x28 + (n) * 4)
+#define FIMC_REG_CIOCRSA(n)			(0x38 + (n) * 4)
+
+/* Target image format */
+#define FIMC_REG_CITRGFMT			0x48
+#define FIMC_REG_CITRGFMT_INROT90		(1 << 31)
+#define FIMC_REG_CITRGFMT_YCBCR420		(0 << 29)
+#define FIMC_REG_CITRGFMT_YCBCR422		(1 << 29)
+#define FIMC_REG_CITRGFMT_YCBCR422_1P		(2 << 29)
+#define FIMC_REG_CITRGFMT_RGB			(3 << 29)
+#define FIMC_REG_CITRGFMT_FMT_MASK		(3 << 29)
+#define FIMC_REG_CITRGFMT_HSIZE_MASK		(0xfff << 16)
+#define FIMC_REG_CITRGFMT_FLIP_SHIFT		14
+#define FIMC_REG_CITRGFMT_FLIP_NORMAL		(0 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_X_MIRROR		(1 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_Y_MIRROR		(2 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_180		(3 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_MASK		(3 << 14)
+#define FIMC_REG_CITRGFMT_OUTROT90		(1 << 13)
+#define FIMC_REG_CITRGFMT_VSIZE_MASK		(0xfff << 0)
+
+/* Output DMA control */
+#define FIMC_REG_CIOCTRL			0x4c
+#define FIMC_REG_CIOCTRL_ORDER422_MASK		(3 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CRYCBY	(0 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CBYCRY	(1 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_YCRYCB	(2 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_YCBYCR	(3 << 0)
+#define FIMC_REG_CIOCTRL_LASTIRQ_ENABLE		(1 << 2)
+#define FIMC_REG_CIOCTRL_YCBCR_3PLANE		(0 << 3)
+#define FIMC_REG_CIOCTRL_YCBCR_2PLANE		(1 << 3)
+#define FIMC_REG_CIOCTRL_YCBCR_PLANE_MASK	(1 << 3)
+#define FIMC_REG_CIOCTRL_ALPHA_OUT_MASK		(0xff << 4)
+#define FIMC_REG_CIOCTRL_RGB16FMT_MASK		(3 << 16)
+#define FIMC_REG_CIOCTRL_RGB565			(0 << 16)
+#define FIMC_REG_CIOCTRL_ARGB1555		(1 << 16)
+#define FIMC_REG_CIOCTRL_ARGB4444		(2 << 16)
+#define FIMC_REG_CIOCTRL_ORDER2P_SHIFT		24
+#define FIMC_REG_CIOCTRL_ORDER2P_MASK		(3 << 24)
+#define FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB	(0 << 24)
+
+/* Pre-scaler control 1 */
+#define FIMC_REG_CISCPRERATIO			0x50
+
+#define FIMC_REG_CISCPREDST			0x54
+
+/* Main scaler control */
+#define FIMC_REG_CISCCTRL			0x58
+#define FIMC_REG_CISCCTRL_SCALERBYPASS		(1 << 31)
+#define FIMC_REG_CISCCTRL_SCALEUP_H		(1 << 30)
+#define FIMC_REG_CISCCTRL_SCALEUP_V		(1 << 29)
+#define FIMC_REG_CISCCTRL_CSCR2Y_WIDE		(1 << 28)
+#define FIMC_REG_CISCCTRL_CSCY2R_WIDE		(1 << 27)
+#define FIMC_REG_CISCCTRL_LCDPATHEN_FIFO	(1 << 26)
+#define FIMC_REG_CISCCTRL_INTERLACE		(1 << 25)
+#define FIMC_REG_CISCCTRL_SCALERSTART		(1 << 15)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB565	(0 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB666	(1 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB888	(2 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_MASK	(3 << 13)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB565	(0 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB666	(1 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888	(2 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_MASK	(3 << 11)
+#define FIMC_REG_CISCCTRL_RGB_EXT		(1 << 10)
+#define FIMC_REG_CISCCTRL_ONE2ONE		(1 << 9)
+#define FIMC_REG_CISCCTRL_MHRATIO(x)		((x) << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO(x)		((x) << 0)
+#define FIMC_REG_CISCCTRL_MHRATIO_MASK		(0x1ff << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO_MASK		(0x1ff << 0)
+#define FIMC_REG_CISCCTRL_MHRATIO_EXT(x)	(((x) >> 6) << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO_EXT(x)	(((x) >> 6) << 0)
+
+/* Target area */
+#define FIMC_REG_CITAREA			0x5c
+#define FIMC_REG_CITAREA_MASK			0x0fffffff
+
+/* General status */
+#define FIMC_REG_CISTATUS			0x64
+#define FIMC_REG_CISTATUS_OVFIY			(1 << 31)
+#define FIMC_REG_CISTATUS_OVFICB		(1 << 30)
+#define FIMC_REG_CISTATUS_OVFICR		(1 << 29)
+#define FIMC_REG_CISTATUS_VSYNC			(1 << 28)
+#define FIMC_REG_CISTATUS_FRAMECNT_MASK		(3 << 26)
+#define FIMC_REG_CISTATUS_FRAMECNT_SHIFT	26
+#define FIMC_REG_CISTATUS_WINOFF_EN		(1 << 25)
+#define FIMC_REG_CISTATUS_IMGCPT_EN		(1 << 22)
+#define FIMC_REG_CISTATUS_IMGCPT_SCEN		(1 << 21)
+#define FIMC_REG_CISTATUS_VSYNC_A		(1 << 20)
+#define FIMC_REG_CISTATUS_VSYNC_B		(1 << 19)
+#define FIMC_REG_CISTATUS_OVRLB			(1 << 18)
+#define FIMC_REG_CISTATUS_FRAME_END		(1 << 17)
+#define FIMC_REG_CISTATUS_LASTCAPT_END		(1 << 16)
+#define FIMC_REG_CISTATUS_VVALID_A		(1 << 15)
+#define FIMC_REG_CISTATUS_VVALID_B		(1 << 14)
+
+/* Indexes to the last and the currently processed buffer. */
+#define FIMC_REG_CISTATUS2			0x68
+
+/* Image capture control */
+#define FIMC_REG_CIIMGCPT			0xc0
+#define FIMC_REG_CIIMGCPT_IMGCPTEN		(1 << 31)
+#define FIMC_REG_CIIMGCPT_IMGCPTEN_SC		(1 << 30)
+#define FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE	(1 << 25)
+#define FIMC_REG_CIIMGCPT_CPT_FRMOD_CNT		(1 << 18)
+
+/* Frame capture sequence */
+#define FIMC_REG_CICPTSEQ			0xc4
+
+/* Image effect */
+#define FIMC_REG_CIIMGEFF			0xd0
+#define FIMC_REG_CIIMGEFF_IE_ENABLE		(1 << 30)
+#define FIMC_REG_CIIMGEFF_IE_SC_BEFORE		(0 << 29)
+#define FIMC_REG_CIIMGEFF_IE_SC_AFTER		(1 << 29)
+#define FIMC_REG_CIIMGEFF_FIN_BYPASS		(0 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_ARBITRARY		(1 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_NEGATIVE		(2 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_ARTFREEZE		(3 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_EMBOSSING		(4 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_SILHOUETTE	(5 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_MASK		(7 << 26)
+#define FIMC_REG_CIIMGEFF_PAT_CBCR_MASK		((0xff << 13) | 0xff)
+
+/* Input DMA Y/Cb/Cr plane start address 0/1 */
+#define FIMC_REG_CIIYSA(n)			(0xd4 + (n) * 0x70)
+#define FIMC_REG_CIICBSA(n)			(0xd8 + (n) * 0x70)
+#define FIMC_REG_CIICRSA(n)			(0xdc + (n) * 0x70)
+
+/* Real input DMA image size */
+#define FIMC_REG_CIREAL_ISIZE			0xf8
+#define FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN	(1 << 31)
+#define FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS	(1 << 30)
+
+/* Input DMA control */
+#define FIMC_REG_MSCTRL				0xfc
+#define FIMC_REG_MSCTRL_IN_BURST_COUNT_MASK	(0xf << 24)
+#define FIMC_REG_MSCTRL_2P_IN_ORDER_MASK	(3 << 16)
+#define FIMC_REG_MSCTRL_2P_IN_ORDER_SHIFT	16
+#define FIMC_REG_MSCTRL_C_INT_IN_3PLANE		(0 << 15)
+#define FIMC_REG_MSCTRL_C_INT_IN_2PLANE		(1 << 15)
+#define FIMC_REG_MSCTRL_C_INT_IN_MASK		(1 << 15)
+#define FIMC_REG_MSCTRL_FLIP_SHIFT		13
+#define FIMC_REG_MSCTRL_FLIP_MASK		(3 << 13)
+#define FIMC_REG_MSCTRL_FLIP_NORMAL		(0 << 13)
+#define FIMC_REG_MSCTRL_FLIP_X_MIRROR		(1 << 13)
+#define FIMC_REG_MSCTRL_FLIP_Y_MIRROR		(2 << 13)
+#define FIMC_REG_MSCTRL_FLIP_180		(3 << 13)
+#define FIMC_REG_MSCTRL_FIFO_CTRL_FULL		(1 << 12)
+#define FIMC_REG_MSCTRL_ORDER422_SHIFT		4
+#define FIMC_REG_MSCTRL_ORDER422_YCBYCR		(0 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_CBYCRY		(1 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_YCRYCB		(2 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_CRYCBY		(3 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_MASK		(3 << 4)
+#define FIMC_REG_MSCTRL_INPUT_EXTCAM		(0 << 3)
+#define FIMC_REG_MSCTRL_INPUT_MEMORY		(1 << 3)
+#define FIMC_REG_MSCTRL_INPUT_MASK		(1 << 3)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR420	(0 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR422	(1 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR422_1P	(2 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_RGB		(3 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_MASK		(3 << 1)
+#define FIMC_REG_MSCTRL_ENVID			(1 << 0)
+#define FIMC_REG_MSCTRL_IN_BURST_COUNT(x)	((x) << 24)
+
+/* Output DMA Y/Cb/Cr offset */
+#define FIMC_REG_CIOYOFF			0x168
+#define FIMC_REG_CIOCBOFF			0x16c
+#define FIMC_REG_CIOCROFF			0x170
+
+/* Input DMA Y/Cb/Cr offset */
+#define FIMC_REG_CIIYOFF			0x174
+#define FIMC_REG_CIICBOFF			0x178
+#define FIMC_REG_CIICROFF			0x17c
+
+/* Input DMA original image size */
+#define FIMC_REG_ORGISIZE			0x180
+
+/* Output DMA original image size */
+#define FIMC_REG_ORGOSIZE			0x184
+
+/* Real output DMA image size (extension register) */
+#define FIMC_REG_CIEXTEN			0x188
+#define FIMC_REG_CIEXTEN_MHRATIO_EXT(x)		(((x) & 0x3f) << 10)
+#define FIMC_REG_CIEXTEN_MVRATIO_EXT(x)		((x) & 0x3f)
+#define FIMC_REG_CIEXTEN_MHRATIO_EXT_MASK	(0x3f << 10)
+#define FIMC_REG_CIEXTEN_MVRATIO_EXT_MASK	0x3f
+
+#define FIMC_REG_CIDMAPARAM			0x18c
+#define FIMC_REG_CIDMAPARAM_R_LINEAR		(0 << 29)
+#define FIMC_REG_CIDMAPARAM_R_64X32		(3 << 29)
+#define FIMC_REG_CIDMAPARAM_W_LINEAR		(0 << 13)
+#define FIMC_REG_CIDMAPARAM_W_64X32		(3 << 13)
+#define FIMC_REG_CIDMAPARAM_TILE_MASK		((3 << 29) | (3 << 13))
+
+/* MIPI CSI image format */
+#define FIMC_REG_CSIIMGFMT			0x194
+#define FIMC_REG_CSIIMGFMT_YCBCR422_8BIT	0x1e
+#define FIMC_REG_CSIIMGFMT_RAW8			0x2a
+#define FIMC_REG_CSIIMGFMT_RAW10		0x2b
+#define FIMC_REG_CSIIMGFMT_RAW12		0x2c
+/* User defined formats. x = 0...16. */
+#define FIMC_REG_CSIIMGFMT_USER(x)		(0x30 + x - 1)
+
+/* Output frame buffer sequence mask */
+#define FIMC_REG_CIFCNTSEQ			0x1fc
+
+/*
+ * Function declarations
+ */
+void fimc_hw_reset(struct fimc_dev *fimc);
+void fimc_hw_set_rotation(struct fimc_ctx *ctx);
+void fimc_hw_set_target_format(struct fimc_ctx *ctx);
+void fimc_hw_set_out_dma(struct fimc_ctx *ctx);
+void fimc_hw_en_lastirq(struct fimc_dev *fimc, int enable);
+void fimc_hw_en_irq(struct fimc_dev *fimc, int enable);
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx);
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx);
+void fimc_hw_en_capture(struct fimc_ctx *ctx);
+void fimc_hw_set_effect(struct fimc_ctx *ctx);
+void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx);
+void fimc_hw_set_in_dma(struct fimc_ctx *ctx);
+void fimc_hw_set_input_path(struct fimc_ctx *ctx);
+void fimc_hw_set_output_path(struct fimc_ctx *ctx);
+void fimc_hw_set_input_addr(struct fimc_dev *fimc, struct fimc_addr *paddr);
+void fimc_hw_set_output_addr(struct fimc_dev *fimc, struct fimc_addr *paddr,
+			     int index);
+int fimc_hw_set_camera_source(struct fimc_dev *fimc,
+			      struct s5p_fimc_isp_info *cam);
+void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f);
+int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
+				struct s5p_fimc_isp_info *cam);
+int fimc_hw_set_camera_type(struct fimc_dev *fimc,
+			    struct s5p_fimc_isp_info *cam);
+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);
+void fimc_activate_capture(struct fimc_ctx *ctx);
+void fimc_deactivate_capture(struct fimc_dev *fimc);
+
+/**
+ * fimc_hw_set_dma_seq - configure output DMA buffer sequence
+ * @mask: bitmask for the DMA output buffer registers, set to 0 to skip buffer
+ * This function masks output DMA ring buffers, it allows to select which of
+ * the 32 available output buffer address registers will be used by the DMA
+ * engine.
+ */
+static inline void fimc_hw_set_dma_seq(struct fimc_dev *dev, u32 mask)
+{
+	writel(mask, dev->regs + FIMC_REG_CIFCNTSEQ);
+}
+
+#endif /* FIMC_REG_H_ */
diff --git a/drivers/media/video/s5p-fimc/mipi-csis.c b/drivers/media/video/s5p-fimc/mipi-csis.c
index f44f690397f7..2f73d9e3d0b7 100644
--- a/drivers/media/video/s5p-fimc/mipi-csis.c
+++ b/drivers/media/video/s5p-fimc/mipi-csis.c
@@ -127,20 +127,24 @@ struct csis_state {
  *                       multiple of 2^pix_width_alignment
  * @code: corresponding media bus code
  * @fmt_reg: S5PCSIS_CONFIG register value
+ * @data_alignment: MIPI-CSI data alignment in bits
  */
 struct csis_pix_format {
 	unsigned int pix_width_alignment;
 	enum v4l2_mbus_pixelcode code;
 	u32 fmt_reg;
+	u8 data_alignment;
 };
 
 static const struct csis_pix_format s5pcsis_formats[] = {
 	{
 		.code = V4L2_MBUS_FMT_VYUY8_2X8,
 		.fmt_reg = S5PCSIS_CFG_FMT_YCBCR422_8BIT,
+		.data_alignment = 32,
 	}, {
 		.code = V4L2_MBUS_FMT_JPEG_1X8,
 		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
+		.data_alignment = 32,
 	},
 };
 
@@ -239,7 +243,7 @@ static void s5pcsis_set_params(struct csis_state *state)
 	s5pcsis_set_hsync_settle(state, pdata->hs_settle);
 
 	val = s5pcsis_read(state, S5PCSIS_CTRL);
-	if (pdata->alignment == 32)
+	if (state->csis_fmt->data_alignment == 32)
 		val |= S5PCSIS_CTRL_ALIGN_32BIT;
 	else /* 24-bits */
 		val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
@@ -711,19 +715,8 @@ static struct platform_driver s5pcsis_driver = {
 	},
 };
 
-static int __init s5pcsis_init(void)
-{
-	return platform_driver_probe(&s5pcsis_driver, s5pcsis_probe);
-}
-
-static void __exit s5pcsis_exit(void)
-{
-	platform_driver_unregister(&s5pcsis_driver);
-}
-
-module_init(s5pcsis_init);
-module_exit(s5pcsis_exit);
+module_platform_driver(s5pcsis_driver);
 
 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
-MODULE_DESCRIPTION("S5P/EXYNOS4 MIPI CSI receiver driver");
+MODULE_DESCRIPTION("Samsung S5P/EXYNOS SoC MIPI-CSI2 receiver driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/s5p-fimc/regs-fimc.h b/drivers/media/video/s5p-fimc/regs-fimc.h
deleted file mode 100644
index c7a5bc51d571..000000000000
--- a/drivers/media/video/s5p-fimc/regs-fimc.h
+++ /dev/null
@@ -1,301 +0,0 @@
-/*
- * Register definition file for Samsung Camera Interface (FIMC) driver
- *
- * Copyright (c) 2010 Samsung Electronics
- *
- * 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_FIMC_H_
-#define REGS_FIMC_H_
-
-/* Input source format */
-#define S5P_CISRCFMT			0x00
-#define S5P_CISRCFMT_ITU601_8BIT	(1 << 31)
-#define S5P_CISRCFMT_ITU601_16BIT	(1 << 29)
-#define S5P_CISRCFMT_ORDER422_YCBYCR	(0 << 14)
-#define S5P_CISRCFMT_ORDER422_YCRYCB	(1 << 14)
-#define S5P_CISRCFMT_ORDER422_CBYCRY	(2 << 14)
-#define S5P_CISRCFMT_ORDER422_CRYCBY	(3 << 14)
-#define S5P_CISRCFMT_HSIZE(x)		((x) << 16)
-#define S5P_CISRCFMT_VSIZE(x)		((x) << 0)
-
-/* Window offset */
-#define S5P_CIWDOFST			0x04
-#define S5P_CIWDOFST_OFF_EN		(1 << 31)
-#define S5P_CIWDOFST_CLROVFIY		(1 << 30)
-#define S5P_CIWDOFST_CLROVRLB		(1 << 29)
-#define S5P_CIWDOFST_HOROFF_MASK	(0x7ff << 16)
-#define S5P_CIWDOFST_CLROVFICB		(1 << 15)
-#define S5P_CIWDOFST_CLROVFICR		(1 << 14)
-#define S5P_CIWDOFST_HOROFF(x)		((x) << 16)
-#define S5P_CIWDOFST_VEROFF(x)		((x) << 0)
-#define S5P_CIWDOFST_VEROFF_MASK	(0xfff << 0)
-
-/* Global control */
-#define S5P_CIGCTRL			0x08
-#define S5P_CIGCTRL_SWRST		(1 << 31)
-#define S5P_CIGCTRL_CAMRST_A		(1 << 30)
-#define S5P_CIGCTRL_SELCAM_ITU_A	(1 << 29)
-#define S5P_CIGCTRL_TESTPAT_NORMAL	(0 << 27)
-#define S5P_CIGCTRL_TESTPAT_COLOR_BAR	(1 << 27)
-#define S5P_CIGCTRL_TESTPAT_HOR_INC	(2 << 27)
-#define S5P_CIGCTRL_TESTPAT_VER_INC	(3 << 27)
-#define S5P_CIGCTRL_TESTPAT_MASK	(3 << 27)
-#define S5P_CIGCTRL_TESTPAT_SHIFT	(27)
-#define S5P_CIGCTRL_INVPOLPCLK		(1 << 26)
-#define S5P_CIGCTRL_INVPOLVSYNC		(1 << 25)
-#define S5P_CIGCTRL_INVPOLHREF		(1 << 24)
-#define S5P_CIGCTRL_IRQ_OVFEN		(1 << 22)
-#define S5P_CIGCTRL_HREF_MASK		(1 << 21)
-#define S5P_CIGCTRL_IRQ_LEVEL		(1 << 20)
-#define S5P_CIGCTRL_IRQ_CLR		(1 << 19)
-#define S5P_CIGCTRL_IRQ_ENABLE		(1 << 16)
-#define S5P_CIGCTRL_SHDW_DISABLE	(1 << 12)
-#define S5P_CIGCTRL_CAM_JPEG		(1 << 8)
-#define S5P_CIGCTRL_SELCAM_MIPI_A	(1 << 7)
-#define S5P_CIGCTRL_CAMIF_SELWB		(1 << 6)
-/* 0 - ITU601; 1 - ITU709 */
-#define S5P_CIGCTRL_CSC_ITU601_709	(1 << 5)
-#define S5P_CIGCTRL_INVPOLHSYNC		(1 << 4)
-#define S5P_CIGCTRL_SELCAM_MIPI		(1 << 3)
-#define S5P_CIGCTRL_INVPOLFIELD		(1 << 1)
-#define S5P_CIGCTRL_INTERLACE		(1 << 0)
-
-/* Window offset 2 */
-#define S5P_CIWDOFST2			0x14
-#define S5P_CIWDOFST2_HOROFF_MASK	(0xfff << 16)
-#define S5P_CIWDOFST2_VEROFF_MASK	(0xfff << 0)
-#define S5P_CIWDOFST2_HOROFF(x)		((x) << 16)
-#define S5P_CIWDOFST2_VEROFF(x)		((x) << 0)
-
-/* Output DMA Y/Cb/Cr plane start addresses */
-#define S5P_CIOYSA(n)			(0x18 + (n) * 4)
-#define S5P_CIOCBSA(n)			(0x28 + (n) * 4)
-#define S5P_CIOCRSA(n)			(0x38 + (n) * 4)
-
-/* Target image format */
-#define S5P_CITRGFMT			0x48
-#define S5P_CITRGFMT_INROT90		(1 << 31)
-#define S5P_CITRGFMT_YCBCR420		(0 << 29)
-#define S5P_CITRGFMT_YCBCR422		(1 << 29)
-#define S5P_CITRGFMT_YCBCR422_1P	(2 << 29)
-#define S5P_CITRGFMT_RGB		(3 << 29)
-#define S5P_CITRGFMT_FMT_MASK		(3 << 29)
-#define S5P_CITRGFMT_HSIZE_MASK		(0xfff << 16)
-#define S5P_CITRGFMT_FLIP_SHIFT		(14)
-#define S5P_CITRGFMT_FLIP_NORMAL	(0 << 14)
-#define S5P_CITRGFMT_FLIP_X_MIRROR	(1 << 14)
-#define S5P_CITRGFMT_FLIP_Y_MIRROR	(2 << 14)
-#define S5P_CITRGFMT_FLIP_180		(3 << 14)
-#define S5P_CITRGFMT_FLIP_MASK		(3 << 14)
-#define S5P_CITRGFMT_OUTROT90		(1 << 13)
-#define S5P_CITRGFMT_VSIZE_MASK		(0xfff << 0)
-#define S5P_CITRGFMT_HSIZE(x)		((x) << 16)
-#define S5P_CITRGFMT_VSIZE(x)		((x) << 0)
-
-/* Output DMA control */
-#define S5P_CIOCTRL			0x4c
-#define S5P_CIOCTRL_ORDER422_MASK	(3 << 0)
-#define S5P_CIOCTRL_ORDER422_CRYCBY	(0 << 0)
-#define S5P_CIOCTRL_ORDER422_CBYCRY	(1 << 0)
-#define S5P_CIOCTRL_ORDER422_YCRYCB	(2 << 0)
-#define S5P_CIOCTRL_ORDER422_YCBYCR	(3 << 0)
-#define S5P_CIOCTRL_LASTIRQ_ENABLE	(1 << 2)
-#define S5P_CIOCTRL_YCBCR_3PLANE	(0 << 3)
-#define S5P_CIOCTRL_YCBCR_2PLANE	(1 << 3)
-#define S5P_CIOCTRL_YCBCR_PLANE_MASK	(1 << 3)
-#define S5P_CIOCTRL_ALPHA_OUT_MASK	(0xff << 4)
-#define S5P_CIOCTRL_RGB16FMT_MASK	(3 << 16)
-#define S5P_CIOCTRL_RGB565		(0 << 16)
-#define S5P_CIOCTRL_ARGB1555		(1 << 16)
-#define S5P_CIOCTRL_ARGB4444		(2 << 16)
-#define S5P_CIOCTRL_ORDER2P_SHIFT	(24)
-#define S5P_CIOCTRL_ORDER2P_MASK	(3 << 24)
-#define S5P_CIOCTRL_ORDER422_2P_LSB_CRCB (0 << 24)
-
-/* Pre-scaler control 1 */
-#define S5P_CISCPRERATIO		0x50
-#define S5P_CISCPRERATIO_SHFACTOR(x)	((x) << 28)
-#define S5P_CISCPRERATIO_HOR(x)		((x) << 16)
-#define S5P_CISCPRERATIO_VER(x)		((x) << 0)
-
-#define S5P_CISCPREDST			0x54
-#define S5P_CISCPREDST_WIDTH(x)		((x) << 16)
-#define S5P_CISCPREDST_HEIGHT(x)	((x) << 0)
-
-/* Main scaler control */
-#define S5P_CISCCTRL			0x58
-#define S5P_CISCCTRL_SCALERBYPASS	(1 << 31)
-#define S5P_CISCCTRL_SCALEUP_H		(1 << 30)
-#define S5P_CISCCTRL_SCALEUP_V		(1 << 29)
-#define S5P_CISCCTRL_CSCR2Y_WIDE	(1 << 28)
-#define S5P_CISCCTRL_CSCY2R_WIDE	(1 << 27)
-#define S5P_CISCCTRL_LCDPATHEN_FIFO	(1 << 26)
-#define S5P_CISCCTRL_INTERLACE		(1 << 25)
-#define S5P_CISCCTRL_SCALERSTART	(1 << 15)
-#define S5P_CISCCTRL_INRGB_FMT_RGB565	(0 << 13)
-#define S5P_CISCCTRL_INRGB_FMT_RGB666	(1 << 13)
-#define S5P_CISCCTRL_INRGB_FMT_RGB888	(2 << 13)
-#define S5P_CISCCTRL_INRGB_FMT_MASK	(3 << 13)
-#define S5P_CISCCTRL_OUTRGB_FMT_RGB565	(0 << 11)
-#define S5P_CISCCTRL_OUTRGB_FMT_RGB666	(1 << 11)
-#define S5P_CISCCTRL_OUTRGB_FMT_RGB888	(2 << 11)
-#define S5P_CISCCTRL_OUTRGB_FMT_MASK	(3 << 11)
-#define S5P_CISCCTRL_RGB_EXT		(1 << 10)
-#define S5P_CISCCTRL_ONE2ONE		(1 << 9)
-#define S5P_CISCCTRL_MHRATIO(x)		((x) << 16)
-#define S5P_CISCCTRL_MVRATIO(x)		((x) << 0)
-#define S5P_CISCCTRL_MHRATIO_MASK	(0x1ff << 16)
-#define S5P_CISCCTRL_MVRATIO_MASK	(0x1ff << 0)
-#define S5P_CISCCTRL_MHRATIO_EXT(x)	(((x) >> 6) << 16)
-#define S5P_CISCCTRL_MVRATIO_EXT(x)	(((x) >> 6) << 0)
-
-/* Target area */
-#define S5P_CITAREA			0x5c
-#define S5P_CITAREA_MASK		0x0fffffff
-
-/* General status */
-#define S5P_CISTATUS			0x64
-#define S5P_CISTATUS_OVFIY		(1 << 31)
-#define S5P_CISTATUS_OVFICB		(1 << 30)
-#define S5P_CISTATUS_OVFICR		(1 << 29)
-#define S5P_CISTATUS_VSYNC		(1 << 28)
-#define S5P_CISTATUS_FRAMECNT_MASK	(3 << 26)
-#define S5P_CISTATUS_FRAMECNT_SHIFT	26
-#define S5P_CISTATUS_WINOFF_EN		(1 << 25)
-#define S5P_CISTATUS_IMGCPT_EN		(1 << 22)
-#define S5P_CISTATUS_IMGCPT_SCEN	(1 << 21)
-#define S5P_CISTATUS_VSYNC_A		(1 << 20)
-#define S5P_CISTATUS_VSYNC_B		(1 << 19)
-#define S5P_CISTATUS_OVRLB		(1 << 18)
-#define S5P_CISTATUS_FRAME_END		(1 << 17)
-#define S5P_CISTATUS_LASTCAPT_END	(1 << 16)
-#define S5P_CISTATUS_VVALID_A		(1 << 15)
-#define S5P_CISTATUS_VVALID_B		(1 << 14)
-
-/* Indexes to the last and the currently processed buffer. */
-#define S5P_CISTATUS2			0x68
-
-/* Image capture control */
-#define S5P_CIIMGCPT			0xc0
-#define S5P_CIIMGCPT_IMGCPTEN		(1 << 31)
-#define S5P_CIIMGCPT_IMGCPTEN_SC	(1 << 30)
-#define S5P_CIIMGCPT_CPT_FREN_ENABLE	(1 << 25)
-#define S5P_CIIMGCPT_CPT_FRMOD_CNT	(1 << 18)
-
-/* Frame capture sequence */
-#define S5P_CICPTSEQ			0xc4
-
-/* Image effect */
-#define S5P_CIIMGEFF			0xd0
-#define S5P_CIIMGEFF_IE_ENABLE		(1 << 30)
-#define S5P_CIIMGEFF_IE_SC_BEFORE	(0 << 29)
-#define S5P_CIIMGEFF_IE_SC_AFTER	(1 << 29)
-#define S5P_CIIMGEFF_FIN_BYPASS		(0 << 26)
-#define S5P_CIIMGEFF_FIN_ARBITRARY	(1 << 26)
-#define S5P_CIIMGEFF_FIN_NEGATIVE	(2 << 26)
-#define S5P_CIIMGEFF_FIN_ARTFREEZE	(3 << 26)
-#define S5P_CIIMGEFF_FIN_EMBOSSING	(4 << 26)
-#define S5P_CIIMGEFF_FIN_SILHOUETTE	(5 << 26)
-#define S5P_CIIMGEFF_FIN_MASK		(7 << 26)
-#define S5P_CIIMGEFF_PAT_CBCR_MASK	((0xff < 13) | (0xff < 0))
-#define S5P_CIIMGEFF_PAT_CB(x)		((x) << 13)
-#define S5P_CIIMGEFF_PAT_CR(x)		((x) << 0)
-
-/* Input DMA Y/Cb/Cr plane start address 0/1 */
-#define S5P_CIIYSA(n)			(0xd4 + (n) * 0x70)
-#define S5P_CIICBSA(n)			(0xd8 + (n) * 0x70)
-#define S5P_CIICRSA(n)			(0xdc + (n) * 0x70)
-
-/* Real input DMA image size */
-#define S5P_CIREAL_ISIZE		0xf8
-#define S5P_CIREAL_ISIZE_AUTOLOAD_EN	(1 << 31)
-#define S5P_CIREAL_ISIZE_ADDR_CH_DIS	(1 << 30)
-#define S5P_CIREAL_ISIZE_HEIGHT(x)	((x) << 16)
-#define S5P_CIREAL_ISIZE_WIDTH(x)	((x) << 0)
-
-
-/* Input DMA control */
-#define S5P_MSCTRL			0xfc
-#define S5P_MSCTRL_IN_BURST_COUNT_MASK	(0xF << 24)
-#define S5P_MSCTRL_2P_IN_ORDER_MASK	(3 << 16)
-#define S5P_MSCTRL_2P_IN_ORDER_SHIFT	16
-#define S5P_MSCTRL_C_INT_IN_3PLANE	(0 << 15)
-#define S5P_MSCTRL_C_INT_IN_2PLANE	(1 << 15)
-#define S5P_MSCTRL_C_INT_IN_MASK	(1 << 15)
-#define S5P_MSCTRL_FLIP_SHIFT		13
-#define S5P_MSCTRL_FLIP_MASK		(3 << 13)
-#define S5P_MSCTRL_FLIP_NORMAL		(0 << 13)
-#define S5P_MSCTRL_FLIP_X_MIRROR	(1 << 13)
-#define S5P_MSCTRL_FLIP_Y_MIRROR	(2 << 13)
-#define S5P_MSCTRL_FLIP_180		(3 << 13)
-#define S5P_MSCTRL_FIFO_CTRL_FULL	(1 << 12)
-#define S5P_MSCTRL_ORDER422_SHIFT	4
-#define S5P_MSCTRL_ORDER422_YCBYCR	(0 << 4)
-#define S5P_MSCTRL_ORDER422_CBYCRY	(1 << 4)
-#define S5P_MSCTRL_ORDER422_YCRYCB	(2 << 4)
-#define S5P_MSCTRL_ORDER422_CRYCBY	(3 << 4)
-#define S5P_MSCTRL_ORDER422_MASK	(3 << 4)
-#define S5P_MSCTRL_INPUT_EXTCAM		(0 << 3)
-#define S5P_MSCTRL_INPUT_MEMORY		(1 << 3)
-#define S5P_MSCTRL_INPUT_MASK		(1 << 3)
-#define S5P_MSCTRL_INFORMAT_YCBCR420	(0 << 1)
-#define S5P_MSCTRL_INFORMAT_YCBCR422	(1 << 1)
-#define S5P_MSCTRL_INFORMAT_YCBCR422_1P	(2 << 1)
-#define S5P_MSCTRL_INFORMAT_RGB		(3 << 1)
-#define S5P_MSCTRL_INFORMAT_MASK	(3 << 1)
-#define S5P_MSCTRL_ENVID		(1 << 0)
-#define S5P_MSCTRL_IN_BURST_COUNT(x)	((x) << 24)
-
-/* Output DMA Y/Cb/Cr offset */
-#define S5P_CIOYOFF			0x168
-#define S5P_CIOCBOFF			0x16c
-#define S5P_CIOCROFF			0x170
-
-/* Input DMA Y/Cb/Cr offset */
-#define S5P_CIIYOFF			0x174
-#define S5P_CIICBOFF			0x178
-#define S5P_CIICROFF			0x17c
-
-#define S5P_CIO_OFFS_VER(x)		((x) << 16)
-#define S5P_CIO_OFFS_HOR(x)		((x) << 0)
-
-/* Input DMA original image size */
-#define S5P_ORGISIZE			0x180
-
-/* Output DMA original image size */
-#define S5P_ORGOSIZE			0x184
-
-#define S5P_ORIG_SIZE_VER(x)		((x) << 16)
-#define S5P_ORIG_SIZE_HOR(x)		((x) << 0)
-
-/* Real output DMA image size (extension register) */
-#define S5P_CIEXTEN			0x188
-#define S5P_CIEXTEN_MHRATIO_EXT(x)	(((x) & 0x3f) << 10)
-#define S5P_CIEXTEN_MVRATIO_EXT(x)	((x) & 0x3f)
-#define S5P_CIEXTEN_MHRATIO_EXT_MASK	(0x3f << 10)
-#define S5P_CIEXTEN_MVRATIO_EXT_MASK	0x3f
-
-#define S5P_CIDMAPARAM			0x18c
-#define S5P_CIDMAPARAM_R_LINEAR		(0 << 29)
-#define S5P_CIDMAPARAM_R_64X32		(3 << 29)
-#define S5P_CIDMAPARAM_W_LINEAR		(0 << 13)
-#define S5P_CIDMAPARAM_W_64X32		(3 << 13)
-#define S5P_CIDMAPARAM_TILE_MASK	((3 << 29) | (3 << 13))
-
-/* MIPI CSI image format */
-#define S5P_CSIIMGFMT			0x194
-#define S5P_CSIIMGFMT_YCBCR422_8BIT	0x1e
-#define S5P_CSIIMGFMT_RAW8		0x2a
-#define S5P_CSIIMGFMT_RAW10		0x2b
-#define S5P_CSIIMGFMT_RAW12		0x2c
-/* User defined formats. x = 0...16. */
-#define S5P_CSIIMGFMT_USER(x)		(0x30 + x - 1)
-
-/* Output frame buffer sequence mask */
-#define S5P_CIFCNTSEQ			0x1FC
-
-#endif /* REGS_FIMC_H_ */
diff --git a/drivers/media/video/s5p-g2d/g2d.c b/drivers/media/video/s5p-g2d/g2d.c
index 789de74014e5..7c98ee7377ee 100644
--- a/drivers/media/video/s5p-g2d/g2d.c
+++ b/drivers/media/video/s5p-g2d/g2d.c
@@ -65,7 +65,7 @@ static struct g2d_fmt formats[] = {
 };
 #define NUM_FORMATS ARRAY_SIZE(formats)
 
-struct g2d_frame def_frame = {
+static struct g2d_frame def_frame = {
 	.width		= DEFAULT_WIDTH,
 	.height		= DEFAULT_HEIGHT,
 	.c_width	= DEFAULT_WIDTH,
@@ -77,7 +77,7 @@ struct g2d_frame def_frame = {
 	.bottom		= DEFAULT_HEIGHT,
 };
 
-struct g2d_fmt *find_fmt(struct v4l2_format *f)
+static struct g2d_fmt *find_fmt(struct v4l2_format *f)
 {
 	unsigned int i;
 	for (i = 0; i < NUM_FORMATS; i++) {
@@ -202,7 +202,7 @@ static const struct v4l2_ctrl_ops g2d_ctrl_ops = {
 	.s_ctrl		= g2d_s_ctrl,
 };
 
-int g2d_setup_ctrls(struct g2d_ctx *ctx)
+static int g2d_setup_ctrls(struct g2d_ctx *ctx)
 {
 	struct g2d_dev *dev = ctx->dev;
 
@@ -546,11 +546,11 @@ static void job_abort(void *prv)
 	struct g2d_dev *dev = ctx->dev;
 	int ret;
 
-	if (dev->curr == 0) /* No job currently running */
+	if (dev->curr == NULL) /* No job currently running */
 		return;
 
 	ret = wait_event_timeout(dev->irq_queue,
-		dev->curr == 0,
+		dev->curr == NULL,
 		msecs_to_jiffies(G2D_TIMEOUT));
 }
 
@@ -599,19 +599,19 @@ static irqreturn_t g2d_isr(int irq, void *prv)
 	g2d_clear_int(dev);
 	clk_disable(dev->gate);
 
-	BUG_ON(ctx == 0);
+	BUG_ON(ctx == NULL);
 
 	src = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
 	dst = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 
-	BUG_ON(src == 0);
-	BUG_ON(dst == 0);
+	BUG_ON(src == NULL);
+	BUG_ON(dst == NULL);
 
 	v4l2_m2m_buf_done(src, VB2_BUF_STATE_DONE);
 	v4l2_m2m_buf_done(dst, VB2_BUF_STATE_DONE);
 	v4l2_m2m_job_finish(dev->m2m_dev, ctx->m2m_ctx);
 
-	dev->curr = 0;
+	dev->curr = NULL;
 	wake_up(&dev->irq_queue);
 	return IRQ_HANDLED;
 }
@@ -674,42 +674,27 @@ static int g2d_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret = 0;
 
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
+
 	spin_lock_init(&dev->ctrl_lock);
 	mutex_init(&dev->mutex);
 	atomic_set(&dev->num_inst, 0);
 	init_waitqueue_head(&dev->irq_queue);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "failed to find registers\n");
-		ret = -ENOENT;
-		goto free_dev;
-	}
 
-	dev->res_regs = request_mem_region(res->start, resource_size(res),
-						dev_name(&pdev->dev));
-
-	if (!dev->res_regs) {
-		dev_err(&pdev->dev, "failed to obtain register region\n");
-		ret = -ENOENT;
-		goto free_dev;
-	}
-
-	dev->regs = ioremap(res->start, resource_size(res));
-	if (!dev->regs) {
-		dev_err(&pdev->dev, "failed to map registers\n");
-		ret = -ENOENT;
-		goto rel_res_regs;
+	dev->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (dev->regs == NULL) {
+			dev_err(&pdev->dev, "Failed to obtain io memory\n");
+			return -ENOENT;
 	}
 
 	dev->clk = clk_get(&pdev->dev, "sclk_fimg2d");
 	if (IS_ERR_OR_NULL(dev->clk)) {
 		dev_err(&pdev->dev, "failed to get g2d clock\n");
-		ret = -ENXIO;
-		goto unmap_regs;
+		return -ENXIO;
 	}
 
 	ret = clk_prepare(dev->clk);
@@ -740,7 +725,8 @@ static int g2d_probe(struct platform_device *pdev)
 
 	dev->irq = res->start;
 
-	ret = request_irq(dev->irq, g2d_isr, 0, pdev->name, dev);
+	ret = devm_request_irq(&pdev->dev, dev->irq, g2d_isr,
+						0, pdev->name, dev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to install IRQ\n");
 		goto put_clk_gate;
@@ -749,7 +735,7 @@ static int g2d_probe(struct platform_device *pdev)
 	dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(dev->alloc_ctx)) {
 		ret = PTR_ERR(dev->alloc_ctx);
-		goto rel_irq;
+		goto unprep_clk_gate;
 	}
 
 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
@@ -762,6 +748,10 @@ static int g2d_probe(struct platform_device *pdev)
 		goto unreg_v4l2_dev;
 	}
 	*vfd = g2d_videodev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &dev->mutex;
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
 	if (ret) {
@@ -793,8 +783,6 @@ unreg_v4l2_dev:
 	v4l2_device_unregister(&dev->v4l2_dev);
 alloc_ctx_cleanup:
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
-rel_irq:
-	free_irq(dev->irq, dev);
 unprep_clk_gate:
 	clk_unprepare(dev->gate);
 put_clk_gate:
@@ -803,12 +791,7 @@ unprep_clk:
 	clk_unprepare(dev->clk);
 put_clk:
 	clk_put(dev->clk);
-unmap_regs:
-	iounmap(dev->regs);
-rel_res_regs:
-	release_resource(dev->res_regs);
-free_dev:
-	kfree(dev);
+
 	return ret;
 }
 
@@ -821,14 +804,10 @@ static int g2d_remove(struct platform_device *pdev)
 	video_unregister_device(dev->vfd);
 	v4l2_device_unregister(&dev->v4l2_dev);
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
-	free_irq(dev->irq, dev);
 	clk_unprepare(dev->gate);
 	clk_put(dev->gate);
 	clk_unprepare(dev->clk);
 	clk_put(dev->clk);
-	iounmap(dev->regs);
-	release_resource(dev->res_regs);
-	kfree(dev);
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-g2d/g2d.h b/drivers/media/video/s5p-g2d/g2d.h
index 1b82065aeaef..6b765b0216c5 100644
--- a/drivers/media/video/s5p-g2d/g2d.h
+++ b/drivers/media/video/s5p-g2d/g2d.h
@@ -23,7 +23,6 @@ struct g2d_dev {
 	spinlock_t		ctrl_lock;
 	atomic_t		num_inst;
 	struct vb2_alloc_ctx	*alloc_ctx;
-	struct resource		*res_regs;
 	void __iomem		*regs;
 	struct clk		*clk;
 	struct clk		*gate;
diff --git a/drivers/media/video/s5p-jpeg/jpeg-core.c b/drivers/media/video/s5p-jpeg/jpeg-core.c
index 5a49c307f9c1..28b5225d94f5 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/video/s5p-jpeg/jpeg-core.c
@@ -813,7 +813,7 @@ static int s5p_jpeg_streamoff(struct file *file, void *priv,
 	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
 }
 
-int s5p_jpeg_g_selection(struct file *file, void *priv,
+static int s5p_jpeg_g_selection(struct file *file, void *priv,
 			 struct v4l2_selection *s)
 {
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
@@ -1290,7 +1290,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	int ret;
 
 	/* JPEG IP abstraction struct */
-	jpeg = kzalloc(sizeof(struct s5p_jpeg), GFP_KERNEL);
+	jpeg = devm_kzalloc(&pdev->dev, sizeof(struct s5p_jpeg), GFP_KERNEL);
 	if (!jpeg)
 		return -ENOMEM;
 
@@ -1300,43 +1300,25 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 
 	/* memory-mapped registers */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "cannot find IO resource\n");
-		ret = -ENOENT;
-		goto jpeg_alloc_rollback;
-	}
-
-	jpeg->ioarea = request_mem_region(res->start, resource_size(res),
-					  pdev->name);
-	if (!jpeg->ioarea) {
-		dev_err(&pdev->dev, "cannot request IO\n");
-		ret = -ENXIO;
-		goto jpeg_alloc_rollback;
-	}
 
-	jpeg->regs = ioremap(res->start, resource_size(res));
-	if (!jpeg->regs) {
-		dev_err(&pdev->dev, "cannot map IO\n");
-		ret = -ENXIO;
-		goto mem_region_rollback;
+	jpeg->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (jpeg->regs == NULL) {
+		dev_err(&pdev->dev, "Failed to obtain io memory\n");
+		return -ENOENT;
 	}
 
-	dev_dbg(&pdev->dev, "registers %p (%p, %p)\n",
-		jpeg->regs, jpeg->ioarea, res);
-
 	/* interrupt service routine registration */
 	jpeg->irq = ret = platform_get_irq(pdev, 0);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "cannot find IRQ\n");
-		goto ioremap_rollback;
+		return ret;
 	}
 
-	ret = request_irq(jpeg->irq, s5p_jpeg_irq, 0,
-			  dev_name(&pdev->dev), jpeg);
-
+	ret = devm_request_irq(&pdev->dev, jpeg->irq, s5p_jpeg_irq, 0,
+			dev_name(&pdev->dev), jpeg);
 	if (ret) {
 		dev_err(&pdev->dev, "cannot claim IRQ %d\n", jpeg->irq);
-		goto ioremap_rollback;
+		return ret;
 	}
 
 	/* clocks */
@@ -1344,7 +1326,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	if (IS_ERR(jpeg->clk)) {
 		dev_err(&pdev->dev, "cannot get clock\n");
 		ret = PTR_ERR(jpeg->clk);
-		goto request_irq_rollback;
+		return ret;
 	}
 	dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
 	clk_enable(jpeg->clk);
@@ -1386,6 +1368,10 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	jpeg->vfd_encoder->release	= video_device_release;
 	jpeg->vfd_encoder->lock		= &jpeg->lock;
 	jpeg->vfd_encoder->v4l2_dev	= &jpeg->v4l2_dev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &jpeg->vfd_encoder->flags);
 
 	ret = video_register_device(jpeg->vfd_encoder, VFL_TYPE_GRABBER, -1);
 	if (ret) {
@@ -1413,6 +1399,10 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	jpeg->vfd_decoder->release	= video_device_release;
 	jpeg->vfd_decoder->lock		= &jpeg->lock;
 	jpeg->vfd_decoder->v4l2_dev	= &jpeg->v4l2_dev;
+	/* Locking in file operations other than ioctl should be done by the driver,
+	   not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &jpeg->vfd_decoder->flags);
 
 	ret = video_register_device(jpeg->vfd_decoder, VFL_TYPE_GRABBER, -1);
 	if (ret) {
@@ -1456,18 +1446,6 @@ clk_get_rollback:
 	clk_disable(jpeg->clk);
 	clk_put(jpeg->clk);
 
-request_irq_rollback:
-	free_irq(jpeg->irq, jpeg);
-
-ioremap_rollback:
-	iounmap(jpeg->regs);
-
-mem_region_rollback:
-	release_resource(jpeg->ioarea);
-	release_mem_region(jpeg->ioarea->start, resource_size(jpeg->ioarea));
-
-jpeg_alloc_rollback:
-	kfree(jpeg);
 	return ret;
 }
 
@@ -1488,14 +1466,6 @@ static int s5p_jpeg_remove(struct platform_device *pdev)
 	clk_disable(jpeg->clk);
 	clk_put(jpeg->clk);
 
-	free_irq(jpeg->irq, jpeg);
-
-	iounmap(jpeg->regs);
-
-	release_resource(jpeg->ioarea);
-	release_mem_region(jpeg->ioarea->start, resource_size(jpeg->ioarea));
-	kfree(jpeg);
-
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-jpeg/jpeg-core.h b/drivers/media/video/s5p-jpeg/jpeg-core.h
index 38d7367f7a6d..9d0cd2b76f61 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-core.h
+++ b/drivers/media/video/s5p-jpeg/jpeg-core.h
@@ -54,7 +54,6 @@
  * @vfd_encoder:	video device node for encoder mem2mem mode
  * @vfd_decoder:	video device node for decoder mem2mem mode
  * @m2m_dev:		v4l2 mem2mem device data
- * @ioarea:		JPEG IP memory region
  * @regs:		JPEG IP registers mapping
  * @irq:		JPEG IP irq
  * @clk:		JPEG IP clock
@@ -70,7 +69,6 @@ struct s5p_jpeg {
 	struct video_device	*vfd_decoder;
 	struct v4l2_m2m_dev	*m2m_dev;
 
-	struct resource		*ioarea;
 	void __iomem		*regs;
 	unsigned int		irq;
 	struct clk		*clk;
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc.c b/drivers/media/video/s5p-mfc/s5p_mfc.c
index 83fe461af263..9bb68e7b5ae8 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc.c
@@ -70,7 +70,7 @@ static void wake_up_dev(struct s5p_mfc_dev *dev, unsigned int reason,
 	wake_up(&dev->queue);
 }
 
-void s5p_mfc_watchdog(unsigned long arg)
+static void s5p_mfc_watchdog(unsigned long arg)
 {
 	struct s5p_mfc_dev *dev = (struct s5p_mfc_dev *)arg;
 
@@ -373,7 +373,7 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
 
 	/* If no context is available then all necessary
 	 * processing has been done. */
-	if (ctx == 0)
+	if (ctx == NULL)
 		return;
 
 	dev = ctx->dev;
@@ -429,7 +429,7 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 	struct s5p_mfc_dev *dev;
 	unsigned int guard_width, guard_height;
 
-	if (ctx == 0)
+	if (ctx == NULL)
 		return;
 	dev = ctx->dev;
 	if (ctx->c_ops->post_seq_start) {
@@ -496,7 +496,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 	struct s5p_mfc_dev *dev;
 	unsigned long flags;
 
-	if (ctx == 0)
+	if (ctx == NULL)
 		return;
 	dev = ctx->dev;
 	s5p_mfc_clear_int_flags(dev);
@@ -772,7 +772,7 @@ err_queue_init:
 err_init_hw:
 	s5p_mfc_release_firmware(dev);
 err_alloc_fw:
-	dev->ctx[ctx->num] = 0;
+	dev->ctx[ctx->num] = NULL;
 	del_timer_sync(&dev->watchdog_timer);
 	s5p_mfc_clock_off();
 err_pwr_enable:
@@ -849,7 +849,7 @@ static int s5p_mfc_release(struct file *file)
 	}
 	mfc_debug(2, "Shutting down clock\n");
 	s5p_mfc_clock_off();
-	dev->ctx[ctx->num] = 0;
+	dev->ctx[ctx->num] = NULL;
 	s5p_mfc_dec_ctrls_delete(ctx);
 	v4l2_fh_del(&ctx->fh);
 	v4l2_fh_exit(&ctx->fh);
@@ -948,7 +948,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	int ret;
 
 	pr_debug("%s++\n", __func__);
-	dev = kzalloc(sizeof *dev, GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof *dev, GFP_KERNEL);
 	if (!dev) {
 		dev_err(&pdev->dev, "Not enough memory for MFC device\n");
 		return -ENOMEM;
@@ -959,49 +959,35 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	dev->plat_dev = pdev;
 	if (!dev->plat_dev) {
 		dev_err(&pdev->dev, "No platform data specified\n");
-		ret = -ENODEV;
-		goto err_dev;
+		return -ENODEV;
 	}
 
 	ret = s5p_mfc_init_pm(dev);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to get mfc clock source\n");
-		goto err_clk;
+		return ret;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "failed to get memory region resource\n");
-		ret = -ENOENT;
-		goto err_res;
-	}
 
-	dev->mfc_mem = request_mem_region(res->start, resource_size(res),
-					  pdev->name);
-	if (dev->mfc_mem == NULL) {
-		dev_err(&pdev->dev, "failed to get memory region\n");
-		ret = -ENOENT;
-		goto err_mem_reg;
-	}
-	dev->regs_base = ioremap(dev->mfc_mem->start, resource_size(dev->mfc_mem));
+	dev->regs_base = devm_request_and_ioremap(&pdev->dev, res);
 	if (dev->regs_base == NULL) {
-		dev_err(&pdev->dev, "failed to ioremap address region\n");
-		ret = -ENOENT;
-		goto err_ioremap;
+		dev_err(&pdev->dev, "Failed to obtain io memory\n");
+		return -ENOENT;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res == NULL) {
 		dev_err(&pdev->dev, "failed to get irq resource\n");
 		ret = -ENOENT;
-		goto err_get_res;
+		goto err_res;
 	}
 	dev->irq = res->start;
-	ret = request_irq(dev->irq, s5p_mfc_irq, IRQF_DISABLED, pdev->name,
-									dev);
+	ret = devm_request_irq(&pdev->dev, dev->irq, s5p_mfc_irq,
+					IRQF_DISABLED, pdev->name, dev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
-		goto err_req_irq;
+		goto err_res;
 	}
 
 	dev->mem_dev_l = device_find_child(&dev->plat_dev->dev, "s5p-mfc-l",
@@ -1009,20 +995,20 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	if (!dev->mem_dev_l) {
 		mfc_err("Mem child (L) device get failed\n");
 		ret = -ENODEV;
-		goto err_find_child;
+		goto err_res;
 	}
 	dev->mem_dev_r = device_find_child(&dev->plat_dev->dev, "s5p-mfc-r",
 					   match_child);
 	if (!dev->mem_dev_r) {
 		mfc_err("Mem child (R) device get failed\n");
 		ret = -ENODEV;
-		goto err_find_child;
+		goto err_res;
 	}
 
 	dev->alloc_ctx[0] = vb2_dma_contig_init_ctx(dev->mem_dev_l);
 	if (IS_ERR_OR_NULL(dev->alloc_ctx[0])) {
 		ret = PTR_ERR(dev->alloc_ctx[0]);
-		goto err_mem_init_ctx_0;
+		goto err_res;
 	}
 	dev->alloc_ctx[1] = vb2_dma_contig_init_ctx(dev->mem_dev_r);
 	if (IS_ERR_OR_NULL(dev->alloc_ctx[1])) {
@@ -1048,6 +1034,10 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	vfd->ioctl_ops	= get_dec_v4l2_ioctl_ops();
 	vfd->release	= video_device_release,
 	vfd->lock	= &dev->mfc_mutex;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->v4l2_dev	= &dev->v4l2_dev;
 	snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_DEC_NAME);
 	dev->vfd_dec	= vfd;
@@ -1072,6 +1062,8 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	vfd->ioctl_ops	= get_enc_v4l2_ioctl_ops();
 	vfd->release	= video_device_release,
 	vfd->lock	= &dev->mfc_mutex;
+	/* This should not be necessary */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->v4l2_dev	= &dev->v4l2_dev;
 	snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_ENC_NAME);
 	dev->vfd_enc	= vfd;
@@ -1110,22 +1102,9 @@ err_v4l2_dev_reg:
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
 err_mem_init_ctx_1:
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
-err_mem_init_ctx_0:
-err_find_child:
-	free_irq(dev->irq, dev);
-err_req_irq:
-err_get_res:
-	iounmap(dev->regs_base);
-	dev->regs_base = NULL;
-err_ioremap:
-	release_resource(dev->mfc_mem);
-	kfree(dev->mfc_mem);
-err_mem_reg:
 err_res:
 	s5p_mfc_final_pm(dev);
-err_clk:
-err_dev:
-	kfree(dev);
+
 	pr_debug("%s-- with error\n", __func__);
 	return ret;
 
@@ -1148,15 +1127,7 @@ static int __devexit s5p_mfc_remove(struct platform_device *pdev)
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
 
-	free_irq(dev->irq, dev);
-	iounmap(dev->regs_base);
-	if (dev->mfc_mem) {
-		release_resource(dev->mfc_mem);
-		kfree(dev->mfc_mem);
-		dev->mfc_mem = NULL;
-	}
 	s5p_mfc_final_pm(dev);
-	kfree(dev);
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_common.h b/drivers/media/video/s5p-mfc/s5p_mfc_common.h
index 91146fa622e4..bd5706a6bad1 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_common.h
@@ -185,7 +185,6 @@ struct s5p_mfc_pm {
  * @mem_dev_r:		child device of the right memory bank (1)
  * @regs_base:		base address of the MFC hw registers
  * @irq:		irq resource
- * @mfc_mem:		MFC registers memory resource
  * @dec_ctrl_handler:	control framework handler for decoding
  * @enc_ctrl_handler:	control framework handler for encoding
  * @pm:			power management control
@@ -221,7 +220,6 @@ struct s5p_mfc_dev {
 	struct device		*mem_dev_r;
 	void __iomem		*regs_base;
 	int			irq;
-	struct resource		*mfc_mem;
 	struct v4l2_ctrl_handler dec_ctrl_handler;
 	struct v4l2_ctrl_handler enc_ctrl_handler;
 	struct s5p_mfc_pm	pm;
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
index f2481a85e0a2..08a5cfeaa59e 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
@@ -52,7 +52,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 	s5p_mfc_bitproc_buf = vb2_dma_contig_memops.alloc(
 		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], dev->fw_size);
 	if (IS_ERR(s5p_mfc_bitproc_buf)) {
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		mfc_err("Allocating bitprocessor buffer failed\n");
 		release_firmware(fw_blob);
 		return -ENOMEM;
@@ -63,7 +63,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 		mfc_err("The base memory for bank 1 is not aligned to 128KB\n");
 		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
 		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		release_firmware(fw_blob);
 		return -EIO;
 	}
@@ -72,7 +72,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 		mfc_err("Bitprocessor memory remap failed\n");
 		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
 		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		release_firmware(fw_blob);
 		return -EIO;
 	}
@@ -82,7 +82,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 	if (IS_ERR(b_base)) {
 		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
 		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		mfc_err("Allocating bank2 base failed\n");
 	release_firmware(fw_blob);
 		return -ENOMEM;
@@ -94,7 +94,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 		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 = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		release_firmware(fw_blob);
 		return -EIO;
 	}
@@ -126,7 +126,7 @@ int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev)
 		release_firmware(fw_blob);
 		return -ENOMEM;
 	}
-	if (s5p_mfc_bitproc_buf == 0 || s5p_mfc_bitproc_phys == 0) {
+	if (s5p_mfc_bitproc_buf == NULL || s5p_mfc_bitproc_phys == 0) {
 		mfc_err("MFC firmware is not allocated or was not mapped correctly\n");
 		release_firmware(fw_blob);
 		return -EINVAL;
@@ -146,9 +146,9 @@ int s5p_mfc_release_firmware(struct s5p_mfc_dev *dev)
 	if (!s5p_mfc_bitproc_buf)
 		return -EINVAL;
 	vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-	s5p_mfc_bitproc_virt =  0;
+	s5p_mfc_bitproc_virt = NULL;
 	s5p_mfc_bitproc_phys = 0;
-	s5p_mfc_bitproc_buf = 0;
+	s5p_mfc_bitproc_buf = NULL;
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
index dff9dc798795..acedb2004be3 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
@@ -1436,7 +1436,8 @@ static const struct v4l2_ctrl_ops s5p_mfc_enc_ctrl_ops = {
 	.s_ctrl = s5p_mfc_enc_s_ctrl,
 };
 
-int vidioc_s_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
+static int vidioc_s_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *a)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 
@@ -1452,7 +1453,8 @@ int vidioc_s_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
 	return 0;
 }
 
-int vidioc_g_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
+static int vidioc_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *a)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_opr.c b/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
index e08b21c50ebf..e6217cbfa4a3 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
@@ -43,7 +43,7 @@ int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
 	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 = 0;
+		ctx->desc_buf = NULL;
 		mfc_err("Allocating DESC buffer failed\n");
 		return -ENOMEM;
 	}
@@ -54,7 +54,7 @@ int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
 	if (desc_virt == NULL) {
 		vb2_dma_contig_memops.put(ctx->desc_buf);
 		ctx->desc_phys = 0;
-		ctx->desc_buf = 0;
+		ctx->desc_buf = NULL;
 		mfc_err("Remapping DESC buffer failed\n");
 		return -ENOMEM;
 	}
@@ -69,7 +69,7 @@ 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 = 0;
+		ctx->desc_buf = NULL;
 	}
 }
 
@@ -186,7 +186,7 @@ int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
 		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;
+			ctx->bank1_buf = NULL;
 			printk(KERN_ERR
 			       "Buf alloc for decoding failed (port A)\n");
 			return -ENOMEM;
@@ -200,7 +200,7 @@ int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
 		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 = 0;
+			ctx->bank2_buf = NULL;
 			mfc_err("Buf alloc for decoding failed (port B)\n");
 			return -ENOMEM;
 		}
@@ -216,13 +216,13 @@ 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 = 0;
+		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 = 0;
+		ctx->bank2_buf = NULL;
 		ctx->bank2_phys = 0;
 		ctx->bank2_size = 0;
 	}
@@ -244,7 +244,7 @@ int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
 	if (IS_ERR(ctx->ctx_buf)) {
 		mfc_err("Allocating context buffer failed\n");
 		ctx->ctx_phys = 0;
-		ctx->ctx_buf = 0;
+		ctx->ctx_buf = NULL;
 		return -ENOMEM;
 	}
 	ctx->ctx_phys = s5p_mfc_mem_cookie(
@@ -256,7 +256,7 @@ int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
 		mfc_err("Remapping instance buffer failed\n");
 		vb2_dma_contig_memops.put(ctx->ctx_buf);
 		ctx->ctx_phys = 0;
-		ctx->ctx_buf = 0;
+		ctx->ctx_buf = NULL;
 		return -ENOMEM;
 	}
 	/* Zero content of the allocated memory */
@@ -265,7 +265,7 @@ int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
 	if (s5p_mfc_init_shm(ctx) < 0) {
 		vb2_dma_contig_memops.put(ctx->ctx_buf);
 		ctx->ctx_phys = 0;
-		ctx->ctx_buf = 0;
+		ctx->ctx_buf = NULL;
 		return -ENOMEM;
 	}
 	return 0;
@@ -277,12 +277,12 @@ 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 = 0;
+		ctx->ctx_buf = NULL;
 	}
 	if (ctx->shm_alloc) {
 		vb2_dma_contig_memops.put(ctx->shm_alloc);
-		ctx->shm_alloc = 0;
-		ctx->shm = 0;
+		ctx->shm_alloc = NULL;
+		ctx->shm = NULL;
 	}
 }
 
@@ -296,7 +296,7 @@ void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for shared buffer */
-void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
+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);
diff --git a/drivers/media/video/s5p-tv/hdmi_drv.c b/drivers/media/video/s5p-tv/hdmi_drv.c
index 4865d25a0e57..20cb6eef2979 100644
--- a/drivers/media/video/s5p-tv/hdmi_drv.c
+++ b/drivers/media/video/s5p-tv/hdmi_drv.c
@@ -42,7 +42,23 @@ MODULE_DESCRIPTION("Samsung HDMI");
 MODULE_LICENSE("GPL");
 
 /* default preset configured on probe */
-#define HDMI_DEFAULT_PRESET V4L2_DV_1080P60
+#define HDMI_DEFAULT_PRESET V4L2_DV_480P59_94
+
+struct hdmi_pulse {
+	u32 beg;
+	u32 end;
+};
+
+struct hdmi_timings {
+	struct hdmi_pulse hact;
+	u32 hsyn_pol; /* 0 - high, 1 - low */
+	struct hdmi_pulse hsyn;
+	u32 interlaced;
+	struct hdmi_pulse vact[2];
+	u32 vsyn_pol; /* 0 - high, 1 - low */
+	u32 vsyn_off;
+	struct hdmi_pulse vsyn[2];
+};
 
 struct hdmi_resources {
 	struct clk *hdmi;
@@ -70,64 +86,15 @@ struct hdmi_device {
 	/** subdev of MHL interface */
 	struct v4l2_subdev *mhl_sd;
 	/** configuration of current graphic mode */
-	const struct hdmi_preset_conf *cur_conf;
+	const struct hdmi_timings *cur_conf;
+	/** flag indicating that timings are dirty */
+	int cur_conf_dirty;
 	/** current preset */
 	u32 cur_preset;
 	/** other resources */
 	struct hdmi_resources res;
 };
 
-struct hdmi_tg_regs {
-	u8 cmd;
-	u8 h_fsz_l;
-	u8 h_fsz_h;
-	u8 hact_st_l;
-	u8 hact_st_h;
-	u8 hact_sz_l;
-	u8 hact_sz_h;
-	u8 v_fsz_l;
-	u8 v_fsz_h;
-	u8 vsync_l;
-	u8 vsync_h;
-	u8 vsync2_l;
-	u8 vsync2_h;
-	u8 vact_st_l;
-	u8 vact_st_h;
-	u8 vact_sz_l;
-	u8 vact_sz_h;
-	u8 field_chg_l;
-	u8 field_chg_h;
-	u8 vact_st2_l;
-	u8 vact_st2_h;
-	u8 vsync_top_hdmi_l;
-	u8 vsync_top_hdmi_h;
-	u8 vsync_bot_hdmi_l;
-	u8 vsync_bot_hdmi_h;
-	u8 field_top_hdmi_l;
-	u8 field_top_hdmi_h;
-	u8 field_bot_hdmi_l;
-	u8 field_bot_hdmi_h;
-};
-
-struct hdmi_core_regs {
-	u8 h_blank[2];
-	u8 v_blank[3];
-	u8 h_v_line[3];
-	u8 vsync_pol[1];
-	u8 int_pro_mode[1];
-	u8 v_blank_f[3];
-	u8 h_sync_gen[3];
-	u8 v_sync_gen1[3];
-	u8 v_sync_gen2[3];
-	u8 v_sync_gen3[3];
-};
-
-struct hdmi_preset_conf {
-	struct hdmi_core_regs core;
-	struct hdmi_tg_regs tg;
-	struct v4l2_mbus_framefmt mbus_fmt;
-};
-
 static struct platform_device_id hdmi_driver_types[] = {
 	{
 		.name		= "s5pv210-hdmi",
@@ -165,6 +132,21 @@ void hdmi_writeb(struct hdmi_device *hdev, u32 reg_id, u8 value)
 	writeb(value, hdev->regs + reg_id);
 }
 
+static inline
+void hdmi_writebn(struct hdmi_device *hdev, u32 reg_id, int n, u32 value)
+{
+	switch (n) {
+	default:
+		writeb(value >> 24, hdev->regs + reg_id + 12);
+	case 3:
+		writeb(value >> 16, hdev->regs + reg_id + 8);
+	case 2:
+		writeb(value >>  8, hdev->regs + reg_id + 4);
+	case 1:
+		writeb(value >>  0, hdev->regs + reg_id + 0);
+	}
+}
+
 static inline u32 hdmi_read(struct hdmi_device *hdev, u32 reg_id)
 {
 	return readl(hdev->regs + reg_id);
@@ -211,77 +193,72 @@ static void hdmi_reg_init(struct hdmi_device *hdev)
 }
 
 static void hdmi_timing_apply(struct hdmi_device *hdev,
-	const struct hdmi_preset_conf *conf)
+	const struct hdmi_timings *t)
 {
-	const struct hdmi_core_regs *core = &conf->core;
-	const struct hdmi_tg_regs *tg = &conf->tg;
-
 	/* setting core registers */
-	hdmi_writeb(hdev, HDMI_H_BLANK_0, core->h_blank[0]);
-	hdmi_writeb(hdev, HDMI_H_BLANK_1, core->h_blank[1]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_0, core->v_blank[0]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_1, core->v_blank[1]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_2, core->v_blank[2]);
-	hdmi_writeb(hdev, HDMI_H_V_LINE_0, core->h_v_line[0]);
-	hdmi_writeb(hdev, HDMI_H_V_LINE_1, core->h_v_line[1]);
-	hdmi_writeb(hdev, HDMI_H_V_LINE_2, core->h_v_line[2]);
-	hdmi_writeb(hdev, HDMI_VSYNC_POL, core->vsync_pol[0]);
-	hdmi_writeb(hdev, HDMI_INT_PRO_MODE, core->int_pro_mode[0]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_F_0, core->v_blank_f[0]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_F_1, core->v_blank_f[1]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_F_2, core->v_blank_f[2]);
-	hdmi_writeb(hdev, HDMI_H_SYNC_GEN_0, core->h_sync_gen[0]);
-	hdmi_writeb(hdev, HDMI_H_SYNC_GEN_1, core->h_sync_gen[1]);
-	hdmi_writeb(hdev, HDMI_H_SYNC_GEN_2, core->h_sync_gen[2]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_0, core->v_sync_gen1[0]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_1, core->v_sync_gen1[1]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_2, core->v_sync_gen1[2]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_0, core->v_sync_gen2[0]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_1, core->v_sync_gen2[1]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_2, core->v_sync_gen2[2]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_0, core->v_sync_gen3[0]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_1, core->v_sync_gen3[1]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_2, core->v_sync_gen3[2]);
+	hdmi_writebn(hdev, HDMI_H_BLANK_0, 2, t->hact.beg);
+	hdmi_writebn(hdev, HDMI_H_SYNC_GEN_0, 3,
+		(t->hsyn_pol << 20) | (t->hsyn.end << 10) | t->hsyn.beg);
+	hdmi_writeb(hdev, HDMI_VSYNC_POL, t->vsyn_pol);
+	hdmi_writebn(hdev, HDMI_V_BLANK_0, 3,
+		(t->vact[0].beg << 11) | t->vact[0].end);
+	hdmi_writebn(hdev, HDMI_V_SYNC_GEN_1_0, 3,
+		(t->vsyn[0].beg << 12) | t->vsyn[0].end);
+	if (t->interlaced) {
+		u32 vsyn_trans = t->hsyn.beg + t->vsyn_off;
+
+		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 1);
+		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
+			(t->hact.end << 12) | t->vact[1].end);
+		hdmi_writebn(hdev, HDMI_V_BLANK_F_0, 3,
+			(t->vact[1].end << 11) | t->vact[1].beg);
+		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_2_0, 3,
+			(t->vsyn[1].beg << 12) | t->vsyn[1].end);
+		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_3_0, 3,
+			(vsyn_trans << 12) | vsyn_trans);
+	} else {
+		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 0);
+		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
+			(t->hact.end << 12) | t->vact[0].end);
+	}
+
 	/* Timing generator registers */
-	hdmi_writeb(hdev, HDMI_TG_H_FSZ_L, tg->h_fsz_l);
-	hdmi_writeb(hdev, HDMI_TG_H_FSZ_H, tg->h_fsz_h);
-	hdmi_writeb(hdev, HDMI_TG_HACT_ST_L, tg->hact_st_l);
-	hdmi_writeb(hdev, HDMI_TG_HACT_ST_H, tg->hact_st_h);
-	hdmi_writeb(hdev, HDMI_TG_HACT_SZ_L, tg->hact_sz_l);
-	hdmi_writeb(hdev, HDMI_TG_HACT_SZ_H, tg->hact_sz_h);
-	hdmi_writeb(hdev, HDMI_TG_V_FSZ_L, tg->v_fsz_l);
-	hdmi_writeb(hdev, HDMI_TG_V_FSZ_H, tg->v_fsz_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_L, tg->vsync_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_H, tg->vsync_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC2_L, tg->vsync2_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC2_H, tg->vsync2_h);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST_L, tg->vact_st_l);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST_H, tg->vact_st_h);
-	hdmi_writeb(hdev, HDMI_TG_VACT_SZ_L, tg->vact_sz_l);
-	hdmi_writeb(hdev, HDMI_TG_VACT_SZ_H, tg->vact_sz_h);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_CHG_L, tg->field_chg_l);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_CHG_H, tg->field_chg_h);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST2_L, tg->vact_st2_l);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST2_H, tg->vact_st2_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_TOP_HDMI_L, tg->vsync_top_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_TOP_HDMI_H, tg->vsync_top_hdmi_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_BOT_HDMI_L, tg->vsync_bot_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_BOT_HDMI_H, tg->vsync_bot_hdmi_h);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_TOP_HDMI_L, tg->field_top_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_TOP_HDMI_H, tg->field_top_hdmi_h);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_BOT_HDMI_L, tg->field_bot_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_BOT_HDMI_H, tg->field_bot_hdmi_h);
+	hdmi_writebn(hdev, HDMI_TG_H_FSZ_L, 2, t->hact.end);
+	hdmi_writebn(hdev, HDMI_TG_HACT_ST_L, 2, t->hact.beg);
+	hdmi_writebn(hdev, HDMI_TG_HACT_SZ_L, 2, t->hact.end - t->hact.beg);
+	hdmi_writebn(hdev, HDMI_TG_VSYNC_L, 2, t->vsyn[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VACT_ST_L, 2, t->vact[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VACT_SZ_L, 2,
+		t->vact[0].end - t->vact[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VSYNC_TOP_HDMI_L, 2, t->vsyn[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_FIELD_TOP_HDMI_L, 2, t->vsyn[0].beg);
+	if (t->interlaced) {
+		hdmi_write_mask(hdev, HDMI_TG_CMD, ~0, HDMI_TG_FIELD_EN);
+		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[1].end);
+		hdmi_writebn(hdev, HDMI_TG_VSYNC2_L, 2, t->vsyn[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_FIELD_CHG_L, 2, t->vact[0].end);
+		hdmi_writebn(hdev, HDMI_TG_VACT_ST2_L, 2, t->vact[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_VSYNC_BOT_HDMI_L, 2, t->vsyn[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_FIELD_BOT_HDMI_L, 2, t->vsyn[1].beg);
+	} else {
+		hdmi_write_mask(hdev, HDMI_TG_CMD, 0, HDMI_TG_FIELD_EN);
+		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[0].end);
+	}
 }
 
 static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 {
 	struct device *dev = hdmi_dev->dev;
-	const struct hdmi_preset_conf *conf = hdmi_dev->cur_conf;
+	const struct hdmi_timings *conf = hdmi_dev->cur_conf;
 	struct v4l2_dv_preset preset;
 	int ret;
 
 	dev_dbg(dev, "%s\n", __func__);
 
+	/* skip if conf is already synchronized with HW */
+	if (!hdmi_dev->cur_conf_dirty)
+		return 0;
+
 	/* reset hdmiphy */
 	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT, ~0, HDMI_PHY_SW_RSTOUT);
 	mdelay(10);
@@ -307,6 +284,8 @@ static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 	/* setting core registers */
 	hdmi_timing_apply(hdmi_dev, conf);
 
+	hdmi_dev->cur_conf_dirty = 0;
+
 	return 0;
 }
 
@@ -398,156 +377,126 @@ static void hdmi_dumpregs(struct hdmi_device *hdev, char *prefix)
 #undef DUMPREG
 }
 
-static const struct hdmi_preset_conf hdmi_conf_480p = {
-	.core = {
-		.h_blank = {0x8a, 0x00},
-		.v_blank = {0x0d, 0x6a, 0x01},
-		.h_v_line = {0x0d, 0xa2, 0x35},
-		.vsync_pol = {0x01},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00},
-		.h_sync_gen = {0x0e, 0x30, 0x11},
-		.v_sync_gen1 = {0x0f, 0x90, 0x00},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x5a, 0x03, /* h_fsz */
-		0x8a, 0x00, 0xd0, 0x02, /* hact */
-		0x0d, 0x02, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x2d, 0x00, 0xe0, 0x01, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-	.mbus_fmt = {
-		.width = 720,
-		.height = 480,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_480p = {
+	.hact = { .beg = 138, .end = 858 },
+	.hsyn_pol = 1,
+	.hsyn = { .beg = 16, .end = 16 + 62 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 42 + 3, .end = 522 + 3 },
+	.vsyn_pol = 1,
+	.vsyn[0] = { .beg = 6 + 3, .end = 12 + 3},
 };
 
-static const struct hdmi_preset_conf hdmi_conf_720p60 = {
-	.core = {
-		.h_blank = {0x72, 0x01},
-		.v_blank = {0xee, 0xf2, 0x00},
-		.h_v_line = {0xee, 0x22, 0x67},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
-		.h_sync_gen = {0x6c, 0x50, 0x02},
-		.v_sync_gen1 = {0x0a, 0x50, 0x00},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x72, 0x06, /* h_fsz */
-		0x72, 0x01, 0x00, 0x05, /* hact */
-		0xee, 0x02, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x1e, 0x00, 0xd0, 0x02, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-	.mbus_fmt = {
-		.width = 1280,
-		.height = 720,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_576p50 = {
+	.hact = { .beg = 144, .end = 864 },
+	.hsyn_pol = 1,
+	.hsyn = { .beg = 12, .end = 12 + 64 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 44 + 5, .end = 620 + 5 },
+	.vsyn_pol = 1,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
 };
 
-static const struct hdmi_preset_conf hdmi_conf_1080p50 = {
-	.core = {
-		.h_blank = {0xd0, 0x02},
-		.v_blank = {0x65, 0x6c, 0x01},
-		.h_v_line = {0x65, 0x04, 0xa5},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
-		.h_sync_gen = {0x0e, 0xea, 0x08},
-		.v_sync_gen1 = {0x09, 0x40, 0x00},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x98, 0x08, /* h_fsz */
-		0x18, 0x01, 0x80, 0x07, /* hact */
-		0x65, 0x04, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x2d, 0x00, 0x38, 0x04, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-	.mbus_fmt = {
-		.width = 1920,
-		.height = 1080,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_720p60 = {
+	.hact = { .beg = 370, .end = 1650 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 110, .end = 110 + 40 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
 };
 
-static const struct hdmi_preset_conf hdmi_conf_1080p60 = {
-	.core = {
-		.h_blank = {0x18, 0x01},
-		.v_blank = {0x65, 0x6c, 0x01},
-		.h_v_line = {0x65, 0x84, 0x89},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
-		.h_sync_gen = {0x56, 0x08, 0x02},
-		.v_sync_gen1 = {0x09, 0x40, 0x00},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x98, 0x08, /* h_fsz */
-		0x18, 0x01, 0x80, 0x07, /* hact */
-		0x65, 0x04, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x2d, 0x00, 0x38, 0x04, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x48, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-	.mbus_fmt = {
-		.width = 1920,
-		.height = 1080,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_720p50 = {
+	.hact = { .beg = 700, .end = 1980 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 440, .end = 440 + 40 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p24 = {
+	.hact = { .beg = 830, .end = 2750 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 638, .end = 638 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p60 = {
+	.hact = { .beg = 280, .end = 2200 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 88, .end = 88 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
+};
+
+static const struct hdmi_timings hdmi_timings_1080i60 = {
+	.hact = { .beg = 280, .end = 2200 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 88, .end = 88 + 44 },
+	.interlaced = 1,
+	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
+	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
+	.vsyn_pol = 0,
+	.vsyn_off = 1100,
+	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
+	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
+};
+
+static const struct hdmi_timings hdmi_timings_1080i50 = {
+	.hact = { .beg = 720, .end = 2640 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 528, .end = 528 + 44 },
+	.interlaced = 1,
+	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
+	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
+	.vsyn_pol = 0,
+	.vsyn_off = 1320,
+	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
+	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p50 = {
+	.hact = { .beg = 720, .end = 2640 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 528, .end = 528 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
 };
 
 static const struct {
 	u32 preset;
-	const struct hdmi_preset_conf *conf;
-} hdmi_conf[] = {
-	{ V4L2_DV_480P59_94, &hdmi_conf_480p },
-	{ V4L2_DV_720P59_94, &hdmi_conf_720p60 },
-	{ V4L2_DV_1080P50, &hdmi_conf_1080p50 },
-	{ V4L2_DV_1080P30, &hdmi_conf_1080p60 },
-	{ V4L2_DV_1080P60, &hdmi_conf_1080p60 },
+	const struct hdmi_timings *timings;
+} hdmi_timings[] = {
+	{ V4L2_DV_480P59_94, &hdmi_timings_480p },
+	{ V4L2_DV_576P50, &hdmi_timings_576p50 },
+	{ V4L2_DV_720P50, &hdmi_timings_720p50 },
+	{ V4L2_DV_720P59_94, &hdmi_timings_720p60 },
+	{ V4L2_DV_720P60, &hdmi_timings_720p60 },
+	{ V4L2_DV_1080P24, &hdmi_timings_1080p24 },
+	{ V4L2_DV_1080P30, &hdmi_timings_1080p60 },
+	{ V4L2_DV_1080P50, &hdmi_timings_1080p50 },
+	{ V4L2_DV_1080I50, &hdmi_timings_1080i50 },
+	{ V4L2_DV_1080I60, &hdmi_timings_1080i60 },
+	{ V4L2_DV_1080P60, &hdmi_timings_1080p60 },
 };
 
-static const struct hdmi_preset_conf *hdmi_preset2conf(u32 preset)
+static const struct hdmi_timings *hdmi_preset2timings(u32 preset)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(hdmi_conf); ++i)
-		if (hdmi_conf[i].preset == preset)
-			return  hdmi_conf[i].conf;
+	for (i = 0; i < ARRAY_SIZE(hdmi_timings); ++i)
+		if (hdmi_timings[i].preset == preset)
+			return  hdmi_timings[i].timings;
 	return NULL;
 }
 
@@ -559,6 +508,10 @@ static int hdmi_streamon(struct hdmi_device *hdev)
 
 	dev_dbg(dev, "%s\n", __func__);
 
+	ret = hdmi_conf_apply(hdev);
+	if (ret)
+		return ret;
+
 	ret = v4l2_subdev_call(hdev->phy_sd, video, s_stream, 1);
 	if (ret)
 		return ret;
@@ -671,14 +624,15 @@ static int hdmi_s_dv_preset(struct v4l2_subdev *sd,
 {
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
 	struct device *dev = hdev->dev;
-	const struct hdmi_preset_conf *conf;
+	const struct hdmi_timings *conf;
 
-	conf = hdmi_preset2conf(preset->preset);
+	conf = hdmi_preset2timings(preset->preset);
 	if (conf == NULL) {
 		dev_err(dev, "preset (%u) not supported\n", preset->preset);
 		return -EINVAL;
 	}
 	hdev->cur_conf = conf;
+	hdev->cur_conf_dirty = 1;
 	hdev->cur_preset = preset->preset;
 	return 0;
 }
@@ -695,21 +649,32 @@ static int hdmi_g_mbus_fmt(struct v4l2_subdev *sd,
 	  struct v4l2_mbus_framefmt *fmt)
 {
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-	struct device *dev = hdev->dev;
+	const struct hdmi_timings *t = hdev->cur_conf;
 
-	dev_dbg(dev, "%s\n", __func__);
+	dev_dbg(hdev->dev, "%s\n", __func__);
 	if (!hdev->cur_conf)
 		return -EINVAL;
-	*fmt = hdev->cur_conf->mbus_fmt;
+	memset(fmt, 0, sizeof *fmt);
+	fmt->width = t->hact.end - t->hact.beg;
+	fmt->height = t->vact[0].end - t->vact[0].beg;
+	fmt->code = V4L2_MBUS_FMT_FIXED; /* means RGB888 */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	if (t->interlaced) {
+		fmt->field = V4L2_FIELD_INTERLACED;
+		fmt->height *= 2;
+	} else {
+		fmt->field = V4L2_FIELD_NONE;
+	}
 	return 0;
 }
 
 static int hdmi_enum_dv_presets(struct v4l2_subdev *sd,
 	struct v4l2_dv_enum_preset *preset)
 {
-	if (preset->index >= ARRAY_SIZE(hdmi_conf))
+	if (preset->index >= ARRAY_SIZE(hdmi_timings))
 		return -EINVAL;
-	return v4l_fill_dv_preset_info(hdmi_conf[preset->index].preset, preset);
+	return v4l_fill_dv_preset_info(hdmi_timings[preset->index].preset,
+		preset);
 }
 
 static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
@@ -737,6 +702,8 @@ static int hdmi_runtime_suspend(struct device *dev)
 	dev_dbg(dev, "%s\n", __func__);
 	v4l2_subdev_call(hdev->mhl_sd, core, s_power, 0);
 	hdmi_resource_poweroff(&hdev->res);
+	/* flag that device context is lost */
+	hdev->cur_conf_dirty = 1;
 	return 0;
 }
 
@@ -750,10 +717,6 @@ static int hdmi_runtime_resume(struct device *dev)
 
 	hdmi_resource_poweron(&hdev->res);
 
-	ret = hdmi_conf_apply(hdev);
-	if (ret)
-		goto fail;
-
 	/* starting MHL */
 	ret = v4l2_subdev_call(hdev->mhl_sd, core, s_power, 1);
 	if (hdev->mhl_sd && ret)
@@ -993,7 +956,8 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 	strlcpy(sd->name, "s5p-hdmi", sizeof sd->name);
 	hdmi_dev->cur_preset = HDMI_DEFAULT_PRESET;
 	/* FIXME: missing fail preset is not supported */
-	hdmi_dev->cur_conf = hdmi_preset2conf(hdmi_dev->cur_preset);
+	hdmi_dev->cur_conf = hdmi_preset2timings(hdmi_dev->cur_preset);
+	hdmi_dev->cur_conf_dirty = 1;
 
 	/* storing subdev for call that have only access to struct device */
 	dev_set_drvdata(dev, sd);
diff --git a/drivers/media/video/s5p-tv/hdmiphy_drv.c b/drivers/media/video/s5p-tv/hdmiphy_drv.c
index 0afef77747e5..f67b38631801 100644
--- a/drivers/media/video/s5p-tv/hdmiphy_drv.c
+++ b/drivers/media/video/s5p-tv/hdmiphy_drv.c
@@ -26,53 +26,188 @@ MODULE_DESCRIPTION("Samsung HDMI Physical interface driver");
 MODULE_LICENSE("GPL");
 
 struct hdmiphy_conf {
-	u32 preset;
+	unsigned long pixclk;
 	const u8 *data;
 };
 
-static const u8 hdmiphy_conf27[32] = {
-	0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
-	0x6B, 0x10, 0x02, 0x51, 0xDf, 0xF2, 0x54, 0x87,
-	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-	0x22, 0x40, 0xe3, 0x26, 0x00, 0x00, 0x00, 0x00,
+struct hdmiphy_ctx {
+	struct v4l2_subdev sd;
+	const struct hdmiphy_conf *conf_tab;
 };
 
-static const u8 hdmiphy_conf74_175[32] = {
-	0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xef, 0x5B,
-	0x6D, 0x10, 0x01, 0x51, 0xef, 0xF3, 0x54, 0xb9,
-	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-	0x22, 0x40, 0xa5, 0x26, 0x01, 0x00, 0x00, 0x00,
+static const struct hdmiphy_conf hdmiphy_conf_s5pv210[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
+		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x10, 0x01, 0x52, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA5, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
+		0x6A, 0x10, 0x01, 0x52, 0xFF, 0xF1, 0x54, 0xBA,
+		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA4, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-static const u8 hdmiphy_conf74_25[32] = {
-	0x01, 0x05, 0x00, 0xd8, 0x10, 0x9c, 0xf8, 0x40,
-	0x6a, 0x10, 0x01, 0x51, 0xff, 0xf1, 0x54, 0xba,
-	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xe0,
-	0x22, 0x40, 0xa4, 0x26, 0x01, 0x00, 0x00, 0x00,
+static const struct hdmiphy_conf hdmiphy_conf_exynos4210[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+		0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
+		0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x10, 0x01, 0x51, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA5, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
+		0x6A, 0x10, 0x01, 0x51, 0xFF, 0xF1, 0x54, 0xBA,
+		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA4, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 148352000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x18, 0x00, 0x51, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x11, 0x40, 0xA5, 0x26, 0x02, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
+		0x6A, 0x18, 0x00, 0x51, 0xFF, 0xF1, 0x54, 0xBA,
+		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x11, 0x40, 0xA4, 0x26, 0x02, 0x00, 0x00, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-static const u8 hdmiphy_conf148_5[32] = {
-	0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xf8, 0x40,
-	0x6A, 0x18, 0x00, 0x51, 0xff, 0xF1, 0x54, 0xba,
-	0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
-	0x22, 0x40, 0xa4, 0x26, 0x02, 0x00, 0x00, 0x00,
+static const struct hdmiphy_conf hdmiphy_conf_exynos4212[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x11, 0x2D, 0x75, 0x00, 0x01, 0x00, 0x08,
+		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
+		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x91, 0x2D, 0x72, 0x00, 0x64, 0x12, 0x08,
+		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
+		0x54, 0xE2, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x35, 0x00, 0x5B, 0xDE, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
+		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x35, 0x00, 0x40, 0xF0, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
+		0x54, 0xA4, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x15, 0x00, 0x40, 0xF0, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0xA4,
+		0x54, 0x4A, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-static const struct hdmiphy_conf hdmiphy_conf[] = {
-	{ V4L2_DV_480P59_94, hdmiphy_conf27 },
-	{ V4L2_DV_1080P30, hdmiphy_conf74_175 },
-	{ V4L2_DV_720P59_94, hdmiphy_conf74_175 },
-	{ V4L2_DV_720P60, hdmiphy_conf74_25 },
-	{ V4L2_DV_1080P50, hdmiphy_conf148_5 },
-	{ V4L2_DV_1080P60, hdmiphy_conf148_5 },
+static const struct hdmiphy_conf hdmiphy_conf_exynos4412[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x11, 0x2D, 0x75, 0x40, 0x01, 0x00, 0x08,
+		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xE4, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x91, 0x2D, 0x72, 0x40, 0x64, 0x12, 0x08,
+		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x10, 0x40, 0x5B, 0xEF, 0x08,
+		0x81, 0x20, 0xB9, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xA6, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x10, 0x40, 0x40, 0xF8, 0x08,
+		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x00, 0x40, 0x40, 0xF8, 0x08,
+		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0x4B, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-const u8 *hdmiphy_preset2conf(u32 preset)
+static inline struct hdmiphy_ctx *sd_to_ctx(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct hdmiphy_ctx, sd);
+}
+
+static unsigned long hdmiphy_preset_to_pixclk(u32 preset)
+{
+	static const unsigned long pixclk[] = {
+		[V4L2_DV_480P59_94] =  27000000,
+		[V4L2_DV_576P50]    =  27000000,
+		[V4L2_DV_720P59_94] =  74176000,
+		[V4L2_DV_720P50]    =  74250000,
+		[V4L2_DV_720P60]    =  74250000,
+		[V4L2_DV_1080P24]   =  74250000,
+		[V4L2_DV_1080P30]   =  74250000,
+		[V4L2_DV_1080I50]   =  74250000,
+		[V4L2_DV_1080I60]   =  74250000,
+		[V4L2_DV_1080P50]   = 148500000,
+		[V4L2_DV_1080P60]   = 148500000,
+	};
+	if (preset < ARRAY_SIZE(pixclk))
+		return pixclk[preset];
+	else
+		return 0;
+}
+
+static const u8 *hdmiphy_find_conf(u32 preset, const struct hdmiphy_conf *conf)
 {
-	int i;
-	for (i = 0; i < ARRAY_SIZE(hdmiphy_conf); ++i)
-		if (hdmiphy_conf[i].preset == preset)
-			return hdmiphy_conf[i].data;
+	unsigned long pixclk;
+
+	pixclk = hdmiphy_preset_to_pixclk(preset);
+	if (!pixclk)
+		return NULL;
+
+	for (; conf->pixclk; ++conf)
+		if (conf->pixclk == pixclk)
+			return conf->data;
 	return NULL;
 }
 
@@ -88,11 +223,12 @@ static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
 	const u8 *data;
 	u8 buffer[32];
 	int ret;
+	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct device *dev = &client->dev;
 
 	dev_info(dev, "s_dv_preset(preset = %d)\n", preset->preset);
-	data = hdmiphy_preset2conf(preset->preset);
+	data = hdmiphy_find_conf(preset->preset, ctx->conf_tab);
 	if (!data) {
 		dev_err(dev, "format not supported\n");
 		return -EINVAL;
@@ -146,21 +282,36 @@ static const struct v4l2_subdev_ops hdmiphy_ops = {
 static int __devinit hdmiphy_probe(struct i2c_client *client,
 	const struct i2c_device_id *id)
 {
-	static struct v4l2_subdev sd;
+	struct hdmiphy_ctx *ctx;
+
+	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->conf_tab = (struct hdmiphy_conf *)id->driver_data;
+	v4l2_i2c_subdev_init(&ctx->sd, client, &hdmiphy_ops);
 
-	v4l2_i2c_subdev_init(&sd, client, &hdmiphy_ops);
 	dev_info(&client->dev, "probe successful\n");
 	return 0;
 }
 
 static int __devexit hdmiphy_remove(struct i2c_client *client)
 {
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
+
+	kfree(ctx);
 	dev_info(&client->dev, "remove successful\n");
+
 	return 0;
 }
 
 static const struct i2c_device_id hdmiphy_id[] = {
-	{ "hdmiphy", 0 },
+	{ "hdmiphy", (unsigned long)hdmiphy_conf_exynos4210 },
+	{ "hdmiphy-s5pv210", (unsigned long)hdmiphy_conf_s5pv210 },
+	{ "hdmiphy-exynos4210", (unsigned long)hdmiphy_conf_exynos4210 },
+	{ "hdmiphy-exynos4212", (unsigned long)hdmiphy_conf_exynos4212 },
+	{ "hdmiphy-exynos4412", (unsigned long)hdmiphy_conf_exynos4412 },
 	{ },
 };
 MODULE_DEVICE_TABLE(i2c, hdmiphy_id);
diff --git a/drivers/media/video/s5p-tv/mixer.h b/drivers/media/video/s5p-tv/mixer.h
index 1597078c4a50..ddb422e23550 100644
--- a/drivers/media/video/s5p-tv/mixer.h
+++ b/drivers/media/video/s5p-tv/mixer.h
@@ -226,6 +226,7 @@ struct mxr_resources {
 /* event flags used  */
 enum mxr_devide_flags {
 	MXR_EVENT_VSYNC = 0,
+	MXR_EVENT_TOP = 1,
 };
 
 /** drivers instance */
@@ -293,7 +294,7 @@ int __devinit mxr_acquire_video(struct mxr_device *mdev,
 	struct mxr_output_conf *output_cont, int output_count);
 
 /** releasing common video resources */
-void __devexit mxr_release_video(struct mxr_device *mdev);
+void mxr_release_video(struct mxr_device *mdev);
 
 struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx);
 struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx);
diff --git a/drivers/media/video/s5p-tv/mixer_drv.c b/drivers/media/video/s5p-tv/mixer_drv.c
index a2c0c25ad130..edca06592883 100644
--- a/drivers/media/video/s5p-tv/mixer_drv.c
+++ b/drivers/media/video/s5p-tv/mixer_drv.c
@@ -461,7 +461,7 @@ static struct platform_driver mxr_driver __refdata = {
 static int __init mxr_init(void)
 {
 	int i, ret;
-	static const char banner[] __initdata = KERN_INFO
+	static const char banner[] __initconst = KERN_INFO
 		"Samsung TV Mixer driver, "
 		"(c) 2010-2011 Samsung Electronics Co., Ltd.\n";
 	printk(banner);
diff --git a/drivers/media/video/s5p-tv/mixer_reg.c b/drivers/media/video/s5p-tv/mixer_reg.c
index 4800a3cbb297..3b1670a045f4 100644
--- a/drivers/media/video/s5p-tv/mixer_reg.c
+++ b/drivers/media/video/s5p-tv/mixer_reg.c
@@ -296,21 +296,25 @@ irqreturn_t mxr_irq_handler(int irq, void *dev_data)
 	/* wake up process waiting for VSYNC */
 	if (val & MXR_INT_STATUS_VSYNC) {
 		set_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
+		/* toggle TOP field event if working in interlaced mode */
+		if (~mxr_read(mdev, MXR_CFG) & MXR_CFG_SCAN_PROGRASSIVE)
+			change_bit(MXR_EVENT_TOP, &mdev->event_flags);
 		wake_up(&mdev->event_queue);
-	}
-
-	/* clear interrupts */
-	if (~val & MXR_INT_EN_VSYNC) {
 		/* vsync interrupt use different bit for read and clear */
-		val &= ~MXR_INT_EN_VSYNC;
+		val &= ~MXR_INT_STATUS_VSYNC;
 		val |= MXR_INT_CLEAR_VSYNC;
 	}
+
+	/* clear interrupts */
 	mxr_write(mdev, MXR_INT_STATUS, val);
 
 	spin_unlock(&mdev->reg_slock);
 	/* leave on non-vsync event */
 	if (~val & MXR_INT_CLEAR_VSYNC)
 		return IRQ_HANDLED;
+	/* skip layer update on bottom field */
+	if (!test_bit(MXR_EVENT_TOP, &mdev->event_flags))
+		return IRQ_HANDLED;
 	for (i = 0; i < MXR_MAX_LAYERS; ++i)
 		mxr_irq_layer_handle(mdev->layer[i]);
 	return IRQ_HANDLED;
@@ -333,6 +337,7 @@ void mxr_reg_streamon(struct mxr_device *mdev)
 
 	/* start MIXER */
 	mxr_write_mask(mdev, MXR_STATUS, ~0, MXR_STATUS_REG_RUN);
+	set_bit(MXR_EVENT_TOP, &mdev->event_flags);
 
 	spin_unlock_irqrestore(&mdev->reg_slock, flags);
 }
diff --git a/drivers/media/video/s5p-tv/mixer_video.c b/drivers/media/video/s5p-tv/mixer_video.c
index f7ca5cc143c6..33fde2a763ec 100644
--- a/drivers/media/video/s5p-tv/mixer_video.c
+++ b/drivers/media/video/s5p-tv/mixer_video.c
@@ -140,7 +140,7 @@ fail:
 	return ret;
 }
 
-void __devexit mxr_release_video(struct mxr_device *mdev)
+void mxr_release_video(struct mxr_device *mdev)
 {
 	int i;
 
@@ -853,8 +853,8 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 	*nplanes = fmt->num_subframes;
 	for (i = 0; i < fmt->num_subframes; ++i) {
 		alloc_ctxs[i] = layer->mdev->alloc_ctx;
-		sizes[i] = PAGE_ALIGN(planes[i].sizeimage);
-		mxr_dbg(mdev, "size[%d] = %08lx\n", i, sizes[i]);
+		sizes[i] = planes[i].sizeimage;
+		mxr_dbg(mdev, "size[%d] = %08x\n", i, sizes[i]);
 	}
 
 	if (*nbuffers == 0)
@@ -1069,6 +1069,10 @@ struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
 	set_bit(V4L2_FL_USE_FH_PRIO, &layer->vfd.flags);
 
 	video_set_drvdata(&layer->vfd, layer);
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &layer->vfd.flags);
 	layer->vfd.lock = &layer->mutex;
 	layer->vfd.v4l2_dev = &mdev->v4l2_dev;
 
diff --git a/drivers/media/video/s5p-tv/regs-hdmi.h b/drivers/media/video/s5p-tv/regs-hdmi.h
index 33247d13e4c0..a889d1f57f28 100644
--- a/drivers/media/video/s5p-tv/regs-hdmi.h
+++ b/drivers/media/video/s5p-tv/regs-hdmi.h
@@ -140,6 +140,7 @@
 #define HDMI_MODE_MASK			(3 << 0)
 
 /* HDMI_TG_CMD */
+#define HDMI_TG_FIELD_EN		(1 << 1)
 #define HDMI_TG_EN			(1 << 0)
 
 #endif /* SAMSUNG_REGS_HDMI_H */
diff --git a/drivers/media/video/saa7134/saa7134-cards.c b/drivers/media/video/saa7134/saa7134-cards.c
index 53aae5968ffb..bc08f1dbc293 100644
--- a/drivers/media/video/saa7134/saa7134-cards.c
+++ b/drivers/media/video/saa7134/saa7134-cards.c
@@ -5080,6 +5080,36 @@ struct saa7134_board saa7134_boards[] = {
 			.gpio = 0x0200000,
 		},
 	},
+	[SAA7134_BOARD_ASUSTeK_PS3_100] = {
+		.name           = "Asus My Cinema PS3-100",
+		.audio_clock    = 0x00187de7,
+		.tuner_type     = TUNER_PHILIPS_TDA8290,
+		.radio_type     = UNSET,
+		.tuner_addr     = ADDR_UNSET,
+		.radio_addr     = ADDR_UNSET,
+		.tuner_config   = 2,
+		.gpiomask       = 1 << 21,
+		.mpeg           = SAA7134_MPEG_DVB,
+		.inputs         = {{
+			.name = name_tv,
+			.vmux = 1,
+			.amux = TV,
+			.tv   = 1,
+		}, {
+			.name = name_comp,
+			.vmux = 0,
+			.amux = LINE2,
+		}, {
+			.name = name_svideo,
+			.vmux = 8,
+			.amux = LINE2,
+		} },
+		.radio = {
+			.name = name_radio,
+			.amux = TV,
+			.gpio = 0x0200000,
+		},
+	},
 	[SAA7134_BOARD_REAL_ANGEL_220] = {
 		.name           = "Zogis Real Angel 220",
 		.audio_clock    = 0x00187de7,
@@ -6877,6 +6907,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
 		.driver_data  = SAA7134_BOARD_ASUSTeK_TIGER_3IN1,
 	}, {
 		.vendor       = PCI_VENDOR_ID_PHILIPS,
+		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
+		.subvendor    = 0x1043,
+		.subdevice    = 0x48cd,
+		.driver_data  = SAA7134_BOARD_ASUSTeK_PS3_100,
+	}, {
+		.vendor       = PCI_VENDOR_ID_PHILIPS,
 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
 		.subvendor    = 0x17de,
 		.subdevice    = 0x7128,
@@ -7347,6 +7383,7 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 	case SAA7134_BOARD_KWORLD_TERMINATOR:
 	case SAA7134_BOARD_SEDNA_PC_TV_CARDBUS:
 	case SAA7134_BOARD_FLYDVBT_LR301:
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
 	case SAA7134_BOARD_ASUSTeK_P7131_DUAL:
 	case SAA7134_BOARD_ASUSTeK_P7131_HYBRID_LNA:
 	case SAA7134_BOARD_ASUSTeK_P7131_ANALOG:
@@ -7811,6 +7848,14 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		i2c_transfer(&dev->i2c_adap, &msg, 1);
 		break;
 	}
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
+	{
+		u8 data[] = { 0x3c, 0x33, 0x60};
+		struct i2c_msg msg = {.addr = 0x0b, .flags = 0, .buf = data,
+						       .len = sizeof(data)};
+		i2c_transfer(&dev->i2c_adap, &msg, 1);
+		break;
+	}
 	case SAA7134_BOARD_FLYDVB_TRIO:
 	{
 		u8 temp = 0;
diff --git a/drivers/media/video/saa7134/saa7134-dvb.c b/drivers/media/video/saa7134/saa7134-dvb.c
index aaa5c97a7216..5dfd826d734e 100644
--- a/drivers/media/video/saa7134/saa7134-dvb.c
+++ b/drivers/media/video/saa7134/saa7134-dvb.c
@@ -881,6 +881,20 @@ static struct tda1004x_config asus_tiger_3in1_config = {
 	.request_firmware = philips_tda1004x_request_firmware
 };
 
+static struct tda1004x_config asus_ps3_100_config = {
+	.demod_address = 0x0b,
+	.invert        = 1,
+	.invert_oclk   = 0,
+	.xtal_freq     = TDA10046_XTAL_16M,
+	.agc_config    = TDA10046_AGC_TDA827X,
+	.gpio_config   = TDA10046_GP11_I,
+	.if_freq       = TDA10046_FREQ_045,
+	.i2c_gate      = 0x4b,
+	.tuner_address = 0x61,
+	.antenna_switch = 1,
+	.request_firmware = philips_tda1004x_request_firmware
+};
+
 /* ------------------------------------------------------------------
  * special case: this card uses saa713x GPIO22 for the mode switch
  */
@@ -1647,6 +1661,31 @@ static int dvb_init(struct saa7134_dev *dev)
 						&dev->i2c_adap, 0, 0) == NULL) {
 					wprintk("%s: Asus Tiger 3in1, no lnbp21"
 						" found!\n", __func__);
+				       goto dettach_frontend;
+			       }
+		       }
+	       }
+	       break;
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
+		if (!use_frontend) {     /* terrestrial */
+			if (configure_tda827x_fe(dev, &asus_ps3_100_config,
+						 &tda827x_cfg_2) < 0)
+				goto dettach_frontend;
+	       } else {                /* satellite */
+			fe0->dvb.frontend = dvb_attach(tda10086_attach,
+						       &flydvbs, &dev->i2c_adap);
+			if (fe0->dvb.frontend) {
+				if (dvb_attach(tda826x_attach,
+					       fe0->dvb.frontend, 0x60,
+					       &dev->i2c_adap, 0) == NULL) {
+					wprintk("%s: Asus My Cinema PS3-100, no "
+						"tda826x found!\n", __func__);
+					goto dettach_frontend;
+				}
+				if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
+					       &dev->i2c_adap, 0, 0) == NULL) {
+					wprintk("%s: Asus My Cinema PS3-100, no lnbp21"
+						" found!\n", __func__);
 					goto dettach_frontend;
 				}
 			}
diff --git a/drivers/media/video/saa7134/saa7134-input.c b/drivers/media/video/saa7134/saa7134-input.c
index 48d2878699b7..05c6e217d8a7 100644
--- a/drivers/media/video/saa7134/saa7134-input.c
+++ b/drivers/media/video/saa7134/saa7134-input.c
@@ -753,6 +753,13 @@ int saa7134_input_init1(struct saa7134_dev *dev)
 		mask_keycode = 0xffff;
 		raw_decode   = true;
 		break;
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
+		ir_codes     = RC_MAP_ASUS_PS3_100;
+		mask_keydown = 0x0040000;
+		mask_keyup   = 0x0040000;
+		mask_keycode = 0xffff;
+		raw_decode   = true;
+		break;
 	case SAA7134_BOARD_ENCORE_ENLTV:
 	case SAA7134_BOARD_ENCORE_ENLTV_FM:
 		ir_codes     = RC_MAP_ENCORE_ENLTV;
diff --git a/drivers/media/video/saa7134/saa7134-video.c b/drivers/media/video/saa7134/saa7134-video.c
index 417034eb6ad2..6de10b1e7251 100644
--- a/drivers/media/video/saa7134/saa7134-video.c
+++ b/drivers/media/video/saa7134/saa7134-video.c
@@ -2036,7 +2036,7 @@ static int saa7134_s_tuner(struct file *file, void *priv,
 	mode = dev->thread.mode;
 	if (UNSET == mode) {
 		rx   = saa7134_tvaudio_getstereo(dev);
-		mode = saa7134_tvaudio_rx2mode(t->rxsubchans);
+		mode = saa7134_tvaudio_rx2mode(rx);
 	}
 	if (mode != t->audmode)
 		dev->thread.mode = t->audmode;
diff --git a/drivers/media/video/saa7134/saa7134.h b/drivers/media/video/saa7134/saa7134.h
index f625060e6a0f..89c8333736a2 100644
--- a/drivers/media/video/saa7134/saa7134.h
+++ b/drivers/media/video/saa7134/saa7134.h
@@ -332,6 +332,7 @@ struct saa7134_card_ir {
 #define SAA7134_BOARD_BEHOLD_503FM          187
 #define SAA7134_BOARD_SENSORAY811_911       188
 #define SAA7134_BOARD_KWORLD_PC150U         189
+#define SAA7134_BOARD_ASUSTeK_PS3_100      190
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
diff --git a/drivers/media/video/saa7164/saa7164-vbi.c b/drivers/media/video/saa7164/saa7164-vbi.c
index 273cf807401c..d8e6c8f14079 100644
--- a/drivers/media/video/saa7164/saa7164-vbi.c
+++ b/drivers/media/video/saa7164/saa7164-vbi.c
@@ -952,7 +952,7 @@ static int saa7164_vbi_start_streaming(struct saa7164_port *port)
 
 		/* Stop the hardware, regardless */
 		result = saa7164_vbi_stop_port(port);
-		if ((result != SAA_OK) && (result != SAA_ERR_ALREADY_STOPPED)) {
+		if (result != SAA_OK) {
 			printk(KERN_ERR "%s() pause/forced stop transition "
 				"failed, res = 0x%x\n", __func__, result);
 		}
@@ -971,7 +971,7 @@ static int saa7164_vbi_start_streaming(struct saa7164_port *port)
 		/* Stop the hardware, regardless */
 		result = saa7164_vbi_acquire_port(port);
 		result = saa7164_vbi_stop_port(port);
-		if ((result != SAA_OK) && (result != SAA_ERR_ALREADY_STOPPED)) {
+		if (result != SAA_OK) {
 			printk(KERN_ERR "%s() run/forced stop transition "
 				"failed, res = 0x%x\n", __func__, result);
 		}
diff --git a/drivers/media/video/saa7164/saa7164.h b/drivers/media/video/saa7164/saa7164.h
index 742b34103b5d..8d120e3baf70 100644
--- a/drivers/media/video/saa7164/saa7164.h
+++ b/drivers/media/video/saa7164/saa7164.h
@@ -611,11 +611,6 @@ extern unsigned int saa_debug;
 		printk(KERN_WARNING "%s: " fmt, dev->name, ## arg);\
 	} while (0)
 
-#define log_err(fmt, arg...)\
-	do { \
-		printk(KERN_ERROR "%s: " fmt, dev->name, ## arg);\
-	} while (0)
-
 #define saa7164_readl(reg) readl(dev->lmmio + ((reg) >> 2))
 #define saa7164_writel(reg, value) writel((value), dev->lmmio + ((reg) >> 2))
 
diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 424dfacd263a..0baaf94db7e0 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -210,27 +210,33 @@ static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
 	struct soc_camera_device *icd = container_of(vq, struct soc_camera_device, vb2_vidq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	int bytes_per_line;
-	unsigned int height;
 
 	if (fmt) {
 		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
 								fmt->fmt.pix.pixelformat);
+		unsigned int bytes_per_line;
+		int ret;
+
 		if (!xlate)
 			return -EINVAL;
-		bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
-							 xlate->host_fmt);
-		height = fmt->fmt.pix.height;
+
+		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+					      xlate->host_fmt);
+		if (ret < 0)
+			return ret;
+
+		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
+
+		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
+					  fmt->fmt.pix.height);
+		if (ret < 0)
+			return ret;
+
+		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
 	} else {
 		/* Called from VIDIOC_REQBUFS or in compatibility mode */
-		bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-		height = icd->user_height;
+		sizes[0] = icd->sizeimage;
 	}
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	sizes[0] = bytes_per_line * height;
 
 	alloc_ctxs[0] = pcdev->alloc_ctx;
 
@@ -336,21 +342,15 @@ static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 
 	ceu_write(pcdev, top1, phys_addr_top);
 	if (V4L2_FIELD_NONE != pcdev->field) {
-		if (planar)
-			phys_addr_bottom = phys_addr_top + icd->user_width;
-		else
-			phys_addr_bottom = phys_addr_top +
-				soc_mbus_bytes_per_line(icd->user_width,
-							icd->current_fmt->host_fmt);
+		phys_addr_bottom = phys_addr_top + icd->bytesperline;
 		ceu_write(pcdev, bottom1, phys_addr_bottom);
 	}
 
 	if (planar) {
-		phys_addr_top += icd->user_width *
-			icd->user_height;
+		phys_addr_top += icd->bytesperline * icd->user_height;
 		ceu_write(pcdev, top2, phys_addr_top);
 		if (V4L2_FIELD_NONE != pcdev->field) {
-			phys_addr_bottom = phys_addr_top + icd->user_width;
+			phys_addr_bottom = phys_addr_top + icd->bytesperline;
 			ceu_write(pcdev, bottom2, phys_addr_bottom);
 		}
 	}
@@ -377,13 +377,8 @@ static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
 	struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 	unsigned long size;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
 
-	if (bytes_per_line < 0)
-		goto error;
-
-	size = icd->user_height * bytes_per_line;
+	size = icd->sizeimage;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		dev_err(icd->parent, "Buffer #%d too small (%lu < %lu)\n",
@@ -682,10 +677,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 			in_width *= 2;
 			left_offset *= 2;
 		}
-		cdwdr_width = width;
 	} else {
-		int bytes_per_line = soc_mbus_bytes_per_line(width,
-						icd->current_fmt->host_fmt);
 		unsigned int w_factor;
 
 		switch (icd->current_fmt->host_fmt->packing) {
@@ -698,13 +690,10 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 
 		in_width = cam->width * w_factor;
 		left_offset *= w_factor;
-
-		if (bytes_per_line < 0)
-			cdwdr_width = width;
-		else
-			cdwdr_width = bytes_per_line;
 	}
 
+	cdwdr_width = icd->bytesperline;
+
 	height = icd->user_height;
 	in_height = cam->height;
 	if (V4L2_FIELD_NONE != pcdev->field) {
@@ -881,11 +870,13 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd)
 
 	value |= common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
 	value |= common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
-	value |= pcdev->is_16bit ? 1 << 12 : 0;
 
-	/* CSI2 mode */
-	if (pcdev->pdata->csi2)
+	if (pcdev->pdata->csi2) /* CSI2 mode */
 		value |= 3 << 12;
+	else if (pcdev->is_16bit)
+		value |= 1 << 12;
+	else if (pcdev->pdata->flags & SH_CEU_FLAG_LOWER_8BIT)
+		value |= 2 << 12;
 
 	ceu_write(pcdev, CAMCR, value);
 
@@ -964,24 +955,28 @@ static const struct soc_mbus_pixelfmt sh_mobile_ceu_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_C,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_NV21,
 		.name			= "NV21",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_C,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_NV16,
 		.name			= "NV16",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_NV61,
 		.name			= "NV61",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
 	},
 };
 
@@ -1845,6 +1840,8 @@ static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
 	return 0;
 }
 
+#define CEU_CHDW_MAX	8188U	/* Maximum line stride */
+
 static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 				 struct v4l2_format *f)
 {
@@ -1863,8 +1860,12 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 	if (!xlate) {
-		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
-		return -EINVAL;
+		xlate = icd->current_fmt;
+		dev_dbg(icd->parent, "Format %x not found, keeping %x\n",
+			pixfmt, xlate->host_fmt->fourcc);
+		pixfmt = xlate->host_fmt->fourcc;
+		pix->pixelformat = pixfmt;
+		pix->colorspace = icd->colorspace;
 	}
 
 	/* FIXME: calculate using depth and bus width */
@@ -1923,10 +1924,20 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 			pix->width = width;
 		if (mf.height > height)
 			pix->height = height;
+
+		pix->bytesperline = max(pix->bytesperline, pix->width);
+		pix->bytesperline = min(pix->bytesperline, CEU_CHDW_MAX);
+		pix->bytesperline &= ~3;
+		break;
+
+	default:
+		/* Configurable stride isn't supported in pass-through mode. */
+		pix->bytesperline  = 0;
 	}
 
 	pix->width	&= ~3;
 	pix->height	&= ~3;
+	pix->sizeimage	= 0;
 
 	dev_geo(icd->parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
 		__func__, ret, pix->pixelformat, pix->width, pix->height);
@@ -2145,6 +2156,7 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
 	pcdev->ici.nr = pdev->id;
 	pcdev->ici.drv_name = dev_name(&pdev->dev);
 	pcdev->ici.ops = &sh_mobile_ceu_host_ops;
+	pcdev->ici.capabilities = SOCAM_HOST_CAP_STRIDE;
 
 	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(pcdev->alloc_ctx)) {
diff --git a/drivers/media/video/sh_vou.c b/drivers/media/video/sh_vou.c
index 9644bd861abc..8fd1874382c6 100644
--- a/drivers/media/video/sh_vou.c
+++ b/drivers/media/video/sh_vou.c
@@ -1390,6 +1390,10 @@ static int __devinit sh_vou_probe(struct platform_device *pdev)
 	vdev->v4l2_dev = &vou_dev->v4l2_dev;
 	vdev->release = video_device_release;
 	vdev->lock = &vou_dev->fop_lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags);
 
 	vou_dev->vdev = vdev;
 	video_set_drvdata(vdev, vou_dev);
diff --git a/drivers/media/video/smiapp-pll.c b/drivers/media/video/smiapp-pll.c
new file mode 100644
index 000000000000..a2e41a21dc65
--- /dev/null
+++ b/drivers/media/video/smiapp-pll.c
@@ -0,0 +1,418 @@
+/*
+ * drivers/media/video/smiapp-pll.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/gcd.h>
+#include <linux/lcm.h>
+#include <linux/module.h>
+
+#include "smiapp-pll.h"
+
+/* Return an even number or one. */
+static inline uint32_t clk_div_even(uint32_t a)
+{
+	return max_t(uint32_t, 1, a & ~1);
+}
+
+/* Return an even number or one. */
+static inline uint32_t clk_div_even_up(uint32_t a)
+{
+	if (a == 1)
+		return 1;
+	return (a + 1) & ~1;
+}
+
+static inline uint32_t is_one_or_even(uint32_t a)
+{
+	if (a == 1)
+		return 1;
+	if (a & 1)
+		return 0;
+
+	return 1;
+}
+
+static int bounds_check(struct device *dev, uint32_t val,
+			uint32_t min, uint32_t max, char *str)
+{
+	if (val >= min && val <= max)
+		return 0;
+
+	dev_warn(dev, "%s out of bounds: %d (%d--%d)\n", str, val, min, max);
+
+	return -EINVAL;
+}
+
+static void print_pll(struct device *dev, struct smiapp_pll *pll)
+{
+	dev_dbg(dev, "pre_pll_clk_div\t%d\n",  pll->pre_pll_clk_div);
+	dev_dbg(dev, "pll_multiplier \t%d\n",  pll->pll_multiplier);
+	if (pll->flags != SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
+		dev_dbg(dev, "op_sys_clk_div \t%d\n", pll->op_sys_clk_div);
+		dev_dbg(dev, "op_pix_clk_div \t%d\n", pll->op_pix_clk_div);
+	}
+	dev_dbg(dev, "vt_sys_clk_div \t%d\n",  pll->vt_sys_clk_div);
+	dev_dbg(dev, "vt_pix_clk_div \t%d\n",  pll->vt_pix_clk_div);
+
+	dev_dbg(dev, "ext_clk_freq_hz \t%d\n", pll->ext_clk_freq_hz);
+	dev_dbg(dev, "pll_ip_clk_freq_hz \t%d\n", pll->pll_ip_clk_freq_hz);
+	dev_dbg(dev, "pll_op_clk_freq_hz \t%d\n", pll->pll_op_clk_freq_hz);
+	if (pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
+		dev_dbg(dev, "op_sys_clk_freq_hz \t%d\n",
+			pll->op_sys_clk_freq_hz);
+		dev_dbg(dev, "op_pix_clk_freq_hz \t%d\n",
+			pll->op_pix_clk_freq_hz);
+	}
+	dev_dbg(dev, "vt_sys_clk_freq_hz \t%d\n", pll->vt_sys_clk_freq_hz);
+	dev_dbg(dev, "vt_pix_clk_freq_hz \t%d\n", pll->vt_pix_clk_freq_hz);
+}
+
+int smiapp_pll_calculate(struct device *dev, struct smiapp_pll_limits *limits,
+			 struct smiapp_pll *pll)
+{
+	uint32_t sys_div;
+	uint32_t best_pix_div = INT_MAX >> 1;
+	uint32_t vt_op_binning_div;
+	uint32_t lane_op_clock_ratio;
+	uint32_t mul, div;
+	uint32_t more_mul_min, more_mul_max;
+	uint32_t more_mul_factor;
+	uint32_t min_vt_div, max_vt_div, vt_div;
+	uint32_t min_sys_div, max_sys_div;
+	unsigned int i;
+	int rval;
+
+	if (pll->flags & SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE)
+		lane_op_clock_ratio = pll->lanes;
+	else
+		lane_op_clock_ratio = 1;
+	dev_dbg(dev, "lane_op_clock_ratio: %d\n", lane_op_clock_ratio);
+
+	dev_dbg(dev, "binning: %dx%d\n", pll->binning_horizontal,
+		pll->binning_vertical);
+
+	/* CSI transfers 2 bits per clock per lane; thus times 2 */
+	pll->pll_op_clk_freq_hz = pll->link_freq * 2
+		* (pll->lanes / lane_op_clock_ratio);
+
+	/* Figure out limits for pre-pll divider based on extclk */
+	dev_dbg(dev, "min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+	limits->max_pre_pll_clk_div =
+		min_t(uint16_t, limits->max_pre_pll_clk_div,
+		      clk_div_even(pll->ext_clk_freq_hz /
+				   limits->min_pll_ip_freq_hz));
+	limits->min_pre_pll_clk_div =
+		max_t(uint16_t, limits->min_pre_pll_clk_div,
+		      clk_div_even_up(
+			      DIV_ROUND_UP(pll->ext_clk_freq_hz,
+					   limits->max_pll_ip_freq_hz)));
+	dev_dbg(dev, "pre-pll check: min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+
+	i = gcd(pll->pll_op_clk_freq_hz, pll->ext_clk_freq_hz);
+	mul = div_u64(pll->pll_op_clk_freq_hz, i);
+	div = pll->ext_clk_freq_hz / i;
+	dev_dbg(dev, "mul %d / div %d\n", mul, div);
+
+	limits->min_pre_pll_clk_div =
+		max_t(uint16_t, limits->min_pre_pll_clk_div,
+		      clk_div_even_up(
+			      DIV_ROUND_UP(mul * pll->ext_clk_freq_hz,
+					   limits->max_pll_op_freq_hz)));
+	dev_dbg(dev, "pll_op check: min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+
+	if (limits->min_pre_pll_clk_div > limits->max_pre_pll_clk_div) {
+		dev_err(dev, "unable to compute pre_pll divisor\n");
+		return -EINVAL;
+	}
+
+	pll->pre_pll_clk_div = limits->min_pre_pll_clk_div;
+
+	/*
+	 * Get pre_pll_clk_div so that our pll_op_clk_freq_hz won't be
+	 * too high.
+	 */
+	dev_dbg(dev, "pre_pll_clk_div %d\n", pll->pre_pll_clk_div);
+
+	/* Don't go above max pll multiplier. */
+	more_mul_max = limits->max_pll_multiplier / mul;
+	dev_dbg(dev, "more_mul_max: max_pll_multiplier check: %d\n",
+		more_mul_max);
+	/* Don't go above max pll op frequency. */
+	more_mul_max =
+		min_t(int,
+		      more_mul_max,
+		      limits->max_pll_op_freq_hz
+		      / (pll->ext_clk_freq_hz / pll->pre_pll_clk_div * mul));
+	dev_dbg(dev, "more_mul_max: max_pll_op_freq_hz check: %d\n",
+		more_mul_max);
+	/* Don't go above the division capability of op sys clock divider. */
+	more_mul_max = min(more_mul_max,
+			   limits->max_op_sys_clk_div * pll->pre_pll_clk_div
+			   / div);
+	dev_dbg(dev, "more_mul_max: max_op_sys_clk_div check: %d\n",
+		more_mul_max);
+	/* Ensure we won't go above min_pll_multiplier. */
+	more_mul_max = min(more_mul_max,
+			   DIV_ROUND_UP(limits->max_pll_multiplier, mul));
+	dev_dbg(dev, "more_mul_max: min_pll_multiplier check: %d\n",
+		more_mul_max);
+
+	/* Ensure we won't go below min_pll_op_freq_hz. */
+	more_mul_min = DIV_ROUND_UP(limits->min_pll_op_freq_hz,
+				    pll->ext_clk_freq_hz / pll->pre_pll_clk_div
+				    * mul);
+	dev_dbg(dev, "more_mul_min: min_pll_op_freq_hz check: %d\n",
+		more_mul_min);
+	/* Ensure we won't go below min_pll_multiplier. */
+	more_mul_min = max(more_mul_min,
+			   DIV_ROUND_UP(limits->min_pll_multiplier, mul));
+	dev_dbg(dev, "more_mul_min: min_pll_multiplier check: %d\n",
+		more_mul_min);
+
+	if (more_mul_min > more_mul_max) {
+		dev_warn(dev,
+			 "unable to compute more_mul_min and more_mul_max");
+		return -EINVAL;
+	}
+
+	more_mul_factor = lcm(div, pll->pre_pll_clk_div) / div;
+	dev_dbg(dev, "more_mul_factor: %d\n", more_mul_factor);
+	more_mul_factor = lcm(more_mul_factor, limits->min_op_sys_clk_div);
+	dev_dbg(dev, "more_mul_factor: min_op_sys_clk_div: %d\n",
+		more_mul_factor);
+	i = roundup(more_mul_min, more_mul_factor);
+	if (!is_one_or_even(i))
+		i <<= 1;
+
+	dev_dbg(dev, "final more_mul: %d\n", i);
+	if (i > more_mul_max) {
+		dev_warn(dev, "final more_mul is bad, max %d", more_mul_max);
+		return -EINVAL;
+	}
+
+	pll->pll_multiplier = mul * i;
+	pll->op_sys_clk_div = div * i / pll->pre_pll_clk_div;
+	dev_dbg(dev, "op_sys_clk_div: %d\n", pll->op_sys_clk_div);
+
+	pll->pll_ip_clk_freq_hz = pll->ext_clk_freq_hz
+		/ pll->pre_pll_clk_div;
+
+	pll->pll_op_clk_freq_hz = pll->pll_ip_clk_freq_hz
+		* pll->pll_multiplier;
+
+	/* Derive pll_op_clk_freq_hz. */
+	pll->op_sys_clk_freq_hz =
+		pll->pll_op_clk_freq_hz / pll->op_sys_clk_div;
+
+	pll->op_pix_clk_div = pll->bits_per_pixel;
+	dev_dbg(dev, "op_pix_clk_div: %d\n", pll->op_pix_clk_div);
+
+	pll->op_pix_clk_freq_hz =
+		pll->op_sys_clk_freq_hz / pll->op_pix_clk_div;
+
+	/*
+	 * Some sensors perform analogue binning and some do this
+	 * digitally. The ones doing this digitally can be roughly be
+	 * found out using this formula. The ones doing this digitally
+	 * should run at higher clock rate, so smaller divisor is used
+	 * on video timing side.
+	 */
+	if (limits->min_line_length_pck_bin > limits->min_line_length_pck
+	    / pll->binning_horizontal)
+		vt_op_binning_div = pll->binning_horizontal;
+	else
+		vt_op_binning_div = 1;
+	dev_dbg(dev, "vt_op_binning_div: %d\n", vt_op_binning_div);
+
+	/*
+	 * Profile 2 supports vt_pix_clk_div E [4, 10]
+	 *
+	 * Horizontal binning can be used as a base for difference in
+	 * divisors. One must make sure that horizontal blanking is
+	 * enough to accommodate the CSI-2 sync codes.
+	 *
+	 * Take scaling factor into account as well.
+	 *
+	 * Find absolute limits for the factor of vt divider.
+	 */
+	dev_dbg(dev, "scale_m: %d\n", pll->scale_m);
+	min_vt_div = DIV_ROUND_UP(pll->op_pix_clk_div * pll->op_sys_clk_div
+				  * pll->scale_n,
+				  lane_op_clock_ratio * vt_op_binning_div
+				  * pll->scale_m);
+
+	/* Find smallest and biggest allowed vt divisor. */
+	dev_dbg(dev, "min_vt_div: %d\n", min_vt_div);
+	min_vt_div = max(min_vt_div,
+			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				      limits->max_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "min_vt_div: max_vt_pix_clk_freq_hz: %d\n",
+		min_vt_div);
+	min_vt_div = max_t(uint32_t, min_vt_div,
+			   limits->min_vt_pix_clk_div
+			   * limits->min_vt_sys_clk_div);
+	dev_dbg(dev, "min_vt_div: min_vt_clk_div: %d\n", min_vt_div);
+
+	max_vt_div = limits->max_vt_sys_clk_div * limits->max_vt_pix_clk_div;
+	dev_dbg(dev, "max_vt_div: %d\n", max_vt_div);
+	max_vt_div = min(max_vt_div,
+			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				      limits->min_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "max_vt_div: min_vt_pix_clk_freq_hz: %d\n",
+		max_vt_div);
+
+	/*
+	 * Find limitsits for sys_clk_div. Not all values are possible
+	 * with all values of pix_clk_div.
+	 */
+	min_sys_div = limits->min_vt_sys_clk_div;
+	dev_dbg(dev, "min_sys_div: %d\n", min_sys_div);
+	min_sys_div = max(min_sys_div,
+			  DIV_ROUND_UP(min_vt_div,
+				       limits->max_vt_pix_clk_div));
+	dev_dbg(dev, "min_sys_div: max_vt_pix_clk_div: %d\n", min_sys_div);
+	min_sys_div = max(min_sys_div,
+			  pll->pll_op_clk_freq_hz
+			  / limits->max_vt_sys_clk_freq_hz);
+	dev_dbg(dev, "min_sys_div: max_pll_op_clk_freq_hz: %d\n", min_sys_div);
+	min_sys_div = clk_div_even_up(min_sys_div);
+	dev_dbg(dev, "min_sys_div: one or even: %d\n", min_sys_div);
+
+	max_sys_div = limits->max_vt_sys_clk_div;
+	dev_dbg(dev, "max_sys_div: %d\n", max_sys_div);
+	max_sys_div = min(max_sys_div,
+			  DIV_ROUND_UP(max_vt_div,
+				       limits->min_vt_pix_clk_div));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_div: %d\n", max_sys_div);
+	max_sys_div = min(max_sys_div,
+			  DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				       limits->min_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_freq_hz: %d\n", max_sys_div);
+
+	/*
+	 * Find pix_div such that a legal pix_div * sys_div results
+	 * into a value which is not smaller than div, the desired
+	 * divisor.
+	 */
+	for (vt_div = min_vt_div; vt_div <= max_vt_div;
+	     vt_div += 2 - (vt_div & 1)) {
+		for (sys_div = min_sys_div;
+		     sys_div <= max_sys_div;
+		     sys_div += 2 - (sys_div & 1)) {
+			int pix_div = DIV_ROUND_UP(vt_div, sys_div);
+
+			if (pix_div < limits->min_vt_pix_clk_div
+			    || pix_div > limits->max_vt_pix_clk_div) {
+				dev_dbg(dev,
+					"pix_div %d too small or too big (%d--%d)\n",
+					pix_div,
+					limits->min_vt_pix_clk_div,
+					limits->max_vt_pix_clk_div);
+				continue;
+			}
+
+			/* Check if this one is better. */
+			if (pix_div * sys_div
+			    <= roundup(min_vt_div, best_pix_div))
+				best_pix_div = pix_div;
+		}
+		if (best_pix_div < INT_MAX >> 1)
+			break;
+	}
+
+	pll->vt_sys_clk_div = DIV_ROUND_UP(min_vt_div, best_pix_div);
+	pll->vt_pix_clk_div = best_pix_div;
+
+	pll->vt_sys_clk_freq_hz =
+		pll->pll_op_clk_freq_hz / pll->vt_sys_clk_div;
+	pll->vt_pix_clk_freq_hz =
+		pll->vt_sys_clk_freq_hz / pll->vt_pix_clk_div;
+
+	pll->pixel_rate_csi =
+		pll->op_pix_clk_freq_hz * lane_op_clock_ratio;
+
+	print_pll(dev, pll);
+
+	rval = bounds_check(dev, pll->pre_pll_clk_div,
+			    limits->min_pre_pll_clk_div,
+			    limits->max_pre_pll_clk_div, "pre_pll_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_ip_clk_freq_hz,
+			limits->min_pll_ip_freq_hz, limits->max_pll_ip_freq_hz,
+			"pll_ip_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_multiplier,
+			limits->min_pll_multiplier, limits->max_pll_multiplier,
+			"pll_multiplier");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_op_clk_freq_hz,
+			limits->min_pll_op_freq_hz, limits->max_pll_op_freq_hz,
+			"pll_op_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_sys_clk_div,
+			limits->min_op_sys_clk_div, limits->max_op_sys_clk_div,
+			"op_sys_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_pix_clk_div,
+			limits->min_op_pix_clk_div, limits->max_op_pix_clk_div,
+			"op_pix_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_sys_clk_freq_hz,
+			limits->min_op_sys_clk_freq_hz,
+			limits->max_op_sys_clk_freq_hz,
+			"op_sys_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_pix_clk_freq_hz,
+			limits->min_op_pix_clk_freq_hz,
+			limits->max_op_pix_clk_freq_hz,
+			"op_pix_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->vt_sys_clk_freq_hz,
+			limits->min_vt_sys_clk_freq_hz,
+			limits->max_vt_sys_clk_freq_hz,
+			"vt_sys_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->vt_pix_clk_freq_hz,
+			limits->min_vt_pix_clk_freq_hz,
+			limits->max_vt_pix_clk_freq_hz,
+			"vt_pix_clk_freq_hz");
+
+	return rval;
+}
+EXPORT_SYMBOL_GPL(smiapp_pll_calculate);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>");
+MODULE_DESCRIPTION("Generic SMIA/SMIA++ PLL calculator");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/smiapp-pll.h b/drivers/media/video/smiapp-pll.h
new file mode 100644
index 000000000000..9eab63f23afb
--- /dev/null
+++ b/drivers/media/video/smiapp-pll.h
@@ -0,0 +1,103 @@
+/*
+ * drivers/media/video/smiapp-pll.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef SMIAPP_PLL_H
+#define SMIAPP_PLL_H
+
+#include <linux/device.h>
+
+struct smiapp_pll {
+	uint8_t lanes;
+	uint8_t binning_horizontal;
+	uint8_t binning_vertical;
+	uint8_t scale_m;
+	uint8_t scale_n;
+	uint8_t bits_per_pixel;
+	uint16_t flags;
+	uint32_t link_freq;
+
+	uint16_t pre_pll_clk_div;
+	uint16_t pll_multiplier;
+	uint16_t op_sys_clk_div;
+	uint16_t op_pix_clk_div;
+	uint16_t vt_sys_clk_div;
+	uint16_t vt_pix_clk_div;
+
+	uint32_t ext_clk_freq_hz;
+	uint32_t pll_ip_clk_freq_hz;
+	uint32_t pll_op_clk_freq_hz;
+	uint32_t op_sys_clk_freq_hz;
+	uint32_t op_pix_clk_freq_hz;
+	uint32_t vt_sys_clk_freq_hz;
+	uint32_t vt_pix_clk_freq_hz;
+
+	uint32_t pixel_rate_csi;
+};
+
+struct smiapp_pll_limits {
+	/* Strict PLL limits */
+	uint32_t min_ext_clk_freq_hz;
+	uint32_t max_ext_clk_freq_hz;
+	uint16_t min_pre_pll_clk_div;
+	uint16_t max_pre_pll_clk_div;
+	uint32_t min_pll_ip_freq_hz;
+	uint32_t max_pll_ip_freq_hz;
+	uint16_t min_pll_multiplier;
+	uint16_t max_pll_multiplier;
+	uint32_t min_pll_op_freq_hz;
+	uint32_t max_pll_op_freq_hz;
+
+	uint16_t min_vt_sys_clk_div;
+	uint16_t max_vt_sys_clk_div;
+	uint32_t min_vt_sys_clk_freq_hz;
+	uint32_t max_vt_sys_clk_freq_hz;
+	uint16_t min_vt_pix_clk_div;
+	uint16_t max_vt_pix_clk_div;
+	uint32_t min_vt_pix_clk_freq_hz;
+	uint32_t max_vt_pix_clk_freq_hz;
+
+	uint16_t min_op_sys_clk_div;
+	uint16_t max_op_sys_clk_div;
+	uint32_t min_op_sys_clk_freq_hz;
+	uint32_t max_op_sys_clk_freq_hz;
+	uint16_t min_op_pix_clk_div;
+	uint16_t max_op_pix_clk_div;
+	uint32_t min_op_pix_clk_freq_hz;
+	uint32_t max_op_pix_clk_freq_hz;
+
+	/* Other relevant limits */
+	uint32_t min_line_length_pck_bin;
+	uint32_t min_line_length_pck;
+};
+
+/* op pix clock is for all lanes in total normally */
+#define SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE			(1 << 0)
+#define SMIAPP_PLL_FLAG_NO_OP_CLOCKS				(1 << 1)
+
+struct device;
+
+int smiapp_pll_calculate(struct device *dev, struct smiapp_pll_limits *limits,
+			 struct smiapp_pll *pll);
+
+#endif /* SMIAPP_PLL_H */
diff --git a/drivers/media/video/smiapp/Kconfig b/drivers/media/video/smiapp/Kconfig
new file mode 100644
index 000000000000..f7b35ff443bf
--- /dev/null
+++ b/drivers/media/video/smiapp/Kconfig
@@ -0,0 +1,6 @@
+config VIDEO_SMIAPP
+	tristate "SMIA++/SMIA sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	select VIDEO_SMIAPP_PLL
+	---help---
+	  This is a generic driver for SMIA++/SMIA camera modules.
diff --git a/drivers/media/video/smiapp/Makefile b/drivers/media/video/smiapp/Makefile
new file mode 100644
index 000000000000..36b0cfa2c541
--- /dev/null
+++ b/drivers/media/video/smiapp/Makefile
@@ -0,0 +1,5 @@
+smiapp-objs			+= smiapp-core.o smiapp-regs.o \
+				   smiapp-quirk.o smiapp-limits.o
+obj-$(CONFIG_VIDEO_SMIAPP)	+= smiapp.o
+
+ccflags-y += -Idrivers/media/video
diff --git a/drivers/media/video/smiapp/smiapp-core.c b/drivers/media/video/smiapp/smiapp-core.c
new file mode 100644
index 000000000000..f518026cb67b
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-core.c
@@ -0,0 +1,2894 @@
+/*
+ * drivers/media/video/smiapp/smiapp-core.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * Based on smiapp driver by Vimarsh Zutshi
+ * Based on jt8ev1.c by Vimarsh Zutshi
+ * Based on smia-sensor.c by Tuukka Toivonen <tuukkat76@gmail.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-device.h>
+
+#include "smiapp.h"
+
+#define SMIAPP_ALIGN_DIM(dim, flags)		\
+	((flags) & V4L2_SUBDEV_SEL_FLAG_SIZE_GE	\
+	 ? ALIGN((dim), 2)			\
+	 : (dim) & ~1)
+
+/*
+ * smiapp_module_idents - supported camera modules
+ */
+static const struct smiapp_module_ident smiapp_module_idents[] = {
+	SMIAPP_IDENT_L(0x01, 0x022b, -1, "vs6555"),
+	SMIAPP_IDENT_L(0x01, 0x022e, -1, "vw6558"),
+	SMIAPP_IDENT_L(0x07, 0x7698, -1, "ovm7698"),
+	SMIAPP_IDENT_L(0x0b, 0x4242, -1, "smiapp-003"),
+	SMIAPP_IDENT_L(0x0c, 0x208a, -1, "tcm8330md"),
+	SMIAPP_IDENT_LQ(0x0c, 0x2134, -1, "tcm8500md", &smiapp_tcm8500md_quirk),
+	SMIAPP_IDENT_L(0x0c, 0x213e, -1, "et8en2"),
+	SMIAPP_IDENT_L(0x0c, 0x2184, -1, "tcm8580md"),
+	SMIAPP_IDENT_LQ(0x0c, 0x560f, -1, "jt8ew9", &smiapp_jt8ew9_quirk),
+	SMIAPP_IDENT_LQ(0x10, 0x4141, -1, "jt8ev1", &smiapp_jt8ev1_quirk),
+	SMIAPP_IDENT_LQ(0x10, 0x4241, -1, "imx125es", &smiapp_imx125es_quirk),
+};
+
+/*
+ *
+ * Dynamic Capability Identification
+ *
+ */
+
+static int smiapp_read_frame_fmt(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	u32 fmt_model_type, fmt_model_subtype, ncol_desc, nrow_desc;
+	unsigned int i;
+	int rval;
+	int line_count = 0;
+	int embedded_start = -1, embedded_end = -1;
+	int image_start = 0;
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE,
+			   &fmt_model_type);
+	if (rval)
+		return rval;
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE,
+			   &fmt_model_subtype);
+	if (rval)
+		return rval;
+
+	ncol_desc = (fmt_model_subtype
+		     & SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_MASK)
+		>> SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_SHIFT;
+	nrow_desc = fmt_model_subtype
+		& SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NROWS_MASK;
+
+	dev_dbg(&client->dev, "format_model_type %s\n",
+		fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE
+		? "2 byte" :
+		fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE
+		? "4 byte" : "is simply bad");
+
+	for (i = 0; i < ncol_desc + nrow_desc; i++) {
+		u32 desc;
+		u32 pixelcode;
+		u32 pixels;
+		char *which;
+		char *what;
+
+		if (fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(i),
+				&desc);
+			if (rval)
+				return rval;
+
+			pixelcode =
+				(desc
+				 & SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_MASK)
+				>> SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_SHIFT;
+			pixels = desc & SMIAPP_FRAME_FORMAT_DESC_2_PIXELS_MASK;
+		} else if (fmt_model_type
+			   == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(i),
+				&desc);
+			if (rval)
+				return rval;
+
+			pixelcode =
+				(desc
+				 & SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_MASK)
+				>> SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_SHIFT;
+			pixels = desc & SMIAPP_FRAME_FORMAT_DESC_4_PIXELS_MASK;
+		} else {
+			dev_dbg(&client->dev,
+				"invalid frame format model type %d\n",
+				fmt_model_type);
+			return -EINVAL;
+		}
+
+		if (i < ncol_desc)
+			which = "columns";
+		else
+			which = "rows";
+
+		switch (pixelcode) {
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED:
+			what = "embedded";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DUMMY:
+			what = "dummy";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_BLACK:
+			what = "black";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DARK:
+			what = "dark";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE:
+			what = "visible";
+			break;
+		default:
+			what = "invalid";
+			dev_dbg(&client->dev, "pixelcode %d\n", pixelcode);
+			break;
+		}
+
+		dev_dbg(&client->dev, "%s pixels: %d %s\n",
+			what, pixels, which);
+
+		if (i < ncol_desc)
+			continue;
+
+		/* Handle row descriptors */
+		if (pixelcode
+		    == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED) {
+			embedded_start = line_count;
+		} else {
+			if (pixelcode == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE
+			    || pixels >= sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES] / 2)
+				image_start = line_count;
+			if (embedded_start != -1 && embedded_end == -1)
+				embedded_end = line_count;
+		}
+		line_count += pixels;
+	}
+
+	if (embedded_start == -1 || embedded_end == -1) {
+		embedded_start = 0;
+		embedded_end = 0;
+	}
+
+	dev_dbg(&client->dev, "embedded data from lines %d to %d\n",
+		embedded_start, embedded_end);
+	dev_dbg(&client->dev, "image data starts at line %d\n", image_start);
+
+	return 0;
+}
+
+static int smiapp_pll_configure(struct smiapp_sensor *sensor)
+{
+	struct smiapp_pll *pll = &sensor->pll;
+	int rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_VT_PIX_CLK_DIV, pll->vt_pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_VT_SYS_CLK_DIV, pll->vt_sys_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_PRE_PLL_CLK_DIV, pll->pre_pll_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_PLL_MULTIPLIER, pll->pll_multiplier);
+	if (rval < 0)
+		return rval;
+
+	/* Lane op clock ratio does not apply here. */
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS,
+		DIV_ROUND_UP(pll->op_sys_clk_freq_hz, 1000000 / 256 / 256));
+	if (rval < 0 || sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_OP_PIX_CLK_DIV, pll->op_pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	return smiapp_write(
+		sensor, SMIAPP_REG_U16_OP_SYS_CLK_DIV, pll->op_sys_clk_div);
+}
+
+static int smiapp_pll_update(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct smiapp_pll_limits lim = {
+		.min_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_PRE_PLL_CLK_DIV],
+		.max_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_PRE_PLL_CLK_DIV],
+		.min_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ],
+		.max_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_IP_FREQ_HZ],
+		.min_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MIN_PLL_MULTIPLIER],
+		.max_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MAX_PLL_MULTIPLIER],
+		.min_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_OP_FREQ_HZ],
+		.max_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_OP_FREQ_HZ],
+
+		.min_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV],
+		.max_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV],
+		.min_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV],
+		.max_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV],
+		.min_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_FREQ_HZ],
+		.max_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_FREQ_HZ],
+		.min_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_FREQ_HZ],
+		.max_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_FREQ_HZ],
+
+		.min_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_DIV],
+		.max_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_DIV],
+		.min_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_DIV],
+		.max_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_DIV],
+		.min_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_FREQ_HZ],
+		.max_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_FREQ_HZ],
+		.min_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_FREQ_HZ],
+		.max_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_FREQ_HZ],
+
+		.min_line_length_pck_bin = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN],
+		.min_line_length_pck = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK],
+	};
+	struct smiapp_pll *pll = &sensor->pll;
+	int rval;
+
+	memset(&sensor->pll, 0, sizeof(sensor->pll));
+
+	pll->lanes = sensor->platform_data->lanes;
+	pll->ext_clk_freq_hz = sensor->platform_data->ext_clk;
+
+	if (sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0) {
+		/*
+		 * Fill in operational clock divisors limits from the
+		 * video timing ones. On profile 0 sensors the
+		 * requirements regarding them are essentially the
+		 * same as on VT ones.
+		 */
+		lim.min_op_sys_clk_div = lim.min_vt_sys_clk_div;
+		lim.max_op_sys_clk_div = lim.max_vt_sys_clk_div;
+		lim.min_op_pix_clk_div = lim.min_vt_pix_clk_div;
+		lim.max_op_pix_clk_div = lim.max_vt_pix_clk_div;
+		lim.min_op_sys_clk_freq_hz = lim.min_vt_sys_clk_freq_hz;
+		lim.max_op_sys_clk_freq_hz = lim.max_vt_sys_clk_freq_hz;
+		lim.min_op_pix_clk_freq_hz = lim.min_vt_pix_clk_freq_hz;
+		lim.max_op_pix_clk_freq_hz = lim.max_vt_pix_clk_freq_hz;
+		/* Profile 0 sensors have no separate OP clock branch. */
+		pll->flags |= SMIAPP_PLL_FLAG_NO_OP_CLOCKS;
+	}
+
+	if (smiapp_needs_quirk(sensor,
+			       SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE))
+		pll->flags |= SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE;
+
+	pll->binning_horizontal = sensor->binning_horizontal;
+	pll->binning_vertical = sensor->binning_vertical;
+	pll->link_freq =
+		sensor->link_freq->qmenu_int[sensor->link_freq->val];
+	pll->scale_m = sensor->scale_m;
+	pll->scale_n = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+	pll->bits_per_pixel = sensor->csi_format->compressed;
+
+	rval = smiapp_pll_calculate(&client->dev, &lim, pll);
+	if (rval < 0)
+		return rval;
+
+	sensor->pixel_rate_parray->cur.val64 = pll->vt_pix_clk_freq_hz;
+	sensor->pixel_rate_csi->cur.val64 = pll->pixel_rate_csi;
+
+	return 0;
+}
+
+
+/*
+ *
+ * V4L2 Controls handling
+ *
+ */
+
+static void __smiapp_update_exposure_limits(struct smiapp_sensor *sensor)
+{
+	struct v4l2_ctrl *ctrl = sensor->exposure;
+	int max;
+
+	max = sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+		+ sensor->vblank->val
+		- sensor->limits[SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN];
+
+	ctrl->maximum = max;
+	if (ctrl->default_value > max)
+		ctrl->default_value = max;
+	if (ctrl->val > max)
+		ctrl->val = max;
+	if (ctrl->cur.val > max)
+		ctrl->cur.val = max;
+}
+
+/*
+ * Order matters.
+ *
+ * 1. Bits-per-pixel, descending.
+ * 2. Bits-per-pixel compressed, descending.
+ * 3. Pixel order, same as in pixel_order_str. Formats for all four pixel
+ *    orders must be defined.
+ */
+static const struct smiapp_csi_data_format smiapp_csi_data_formats[] = {
+	{ V4L2_MBUS_FMT_SGRBG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GBRG, },
+};
+
+const char *pixel_order_str[] = { "GRBG", "RGGB", "BGGR", "GBRG" };
+
+#define to_csi_format_idx(fmt) (((unsigned long)(fmt)			\
+				 - (unsigned long)smiapp_csi_data_formats) \
+				/ sizeof(*smiapp_csi_data_formats))
+
+static u32 smiapp_pixel_order(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int flip = 0;
+
+	if (sensor->hflip) {
+		if (sensor->hflip->val)
+			flip |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
+
+		if (sensor->vflip->val)
+			flip |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
+	}
+
+	flip ^= sensor->hvflip_inv_mask;
+
+	dev_dbg(&client->dev, "flip %d\n", flip);
+	return sensor->default_pixel_order ^ flip;
+}
+
+static void smiapp_update_mbus_formats(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int csi_format_idx =
+		to_csi_format_idx(sensor->csi_format) & ~3;
+	unsigned int internal_csi_format_idx =
+		to_csi_format_idx(sensor->internal_csi_format) & ~3;
+	unsigned int pixel_order = smiapp_pixel_order(sensor);
+
+	sensor->mbus_frame_fmts =
+		sensor->default_mbus_frame_fmts << pixel_order;
+	sensor->csi_format =
+		&smiapp_csi_data_formats[csi_format_idx + pixel_order];
+	sensor->internal_csi_format =
+		&smiapp_csi_data_formats[internal_csi_format_idx
+					 + pixel_order];
+
+	BUG_ON(max(internal_csi_format_idx, csi_format_idx) + pixel_order
+	       >= ARRAY_SIZE(smiapp_csi_data_formats));
+	BUG_ON(min(internal_csi_format_idx, csi_format_idx) < 0);
+
+	dev_dbg(&client->dev, "new pixel order %s\n",
+		pixel_order_str[pixel_order]);
+}
+
+static int smiapp_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct smiapp_sensor *sensor =
+		container_of(ctrl->handler, struct smiapp_subdev, ctrl_handler)
+			->sensor;
+	u32 orient = 0;
+	int exposure;
+	int rval;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		return smiapp_write(
+			sensor,
+			SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL, ctrl->val);
+
+	case V4L2_CID_EXPOSURE:
+		return smiapp_write(
+			sensor,
+			SMIAPP_REG_U16_COARSE_INTEGRATION_TIME, ctrl->val);
+
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		if (sensor->hflip->val)
+			orient |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
+
+		if (sensor->vflip->val)
+			orient |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
+
+		orient ^= sensor->hvflip_inv_mask;
+		rval = smiapp_write(sensor,
+				    SMIAPP_REG_U8_IMAGE_ORIENTATION,
+				    orient);
+		if (rval < 0)
+			return rval;
+
+		smiapp_update_mbus_formats(sensor);
+
+		return 0;
+
+	case V4L2_CID_VBLANK:
+		exposure = sensor->exposure->val;
+
+		__smiapp_update_exposure_limits(sensor);
+
+		if (exposure > sensor->exposure->maximum) {
+			sensor->exposure->val =
+				sensor->exposure->maximum;
+			rval = smiapp_set_ctrl(
+				sensor->exposure);
+			if (rval < 0)
+				return rval;
+		}
+
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_FRAME_LENGTH_LINES,
+			sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+			+ ctrl->val);
+
+	case V4L2_CID_HBLANK:
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_LINE_LENGTH_PCK,
+			sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
+			+ ctrl->val);
+
+	case V4L2_CID_LINK_FREQ:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		return smiapp_pll_update(sensor);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops smiapp_ctrl_ops = {
+	.s_ctrl = smiapp_set_ctrl,
+};
+
+static int smiapp_init_controls(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int max;
+	int rval;
+
+	rval = v4l2_ctrl_handler_init(&sensor->pixel_array->ctrl_handler, 7);
+	if (rval)
+		return rval;
+	sensor->pixel_array->ctrl_handler.lock = &sensor->mutex;
+
+	sensor->analog_gain = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_ANALOGUE_GAIN,
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN],
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX],
+		max(sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_STEP], 1U),
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN]);
+
+	/* Exposure limits will be updated soon, use just something here. */
+	sensor->exposure = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_EXPOSURE, 0, 0, 1, 0);
+
+	sensor->hflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sensor->vflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	sensor->vblank = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_VBLANK, 0, 1, 1, 0);
+
+	if (sensor->vblank)
+		sensor->vblank->flags |= V4L2_CTRL_FLAG_UPDATE;
+
+	sensor->hblank = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_HBLANK, 0, 1, 1, 0);
+
+	if (sensor->hblank)
+		sensor->hblank->flags |= V4L2_CTRL_FLAG_UPDATE;
+
+	sensor->pixel_rate_parray = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
+
+	if (sensor->pixel_array->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"pixel array controls initialization failed (%d)\n",
+			sensor->pixel_array->ctrl_handler.error);
+		rval = sensor->pixel_array->ctrl_handler.error;
+		goto error;
+	}
+
+	sensor->pixel_array->sd.ctrl_handler =
+		&sensor->pixel_array->ctrl_handler;
+
+	v4l2_ctrl_cluster(2, &sensor->hflip);
+
+	rval = v4l2_ctrl_handler_init(&sensor->src->ctrl_handler, 0);
+	if (rval)
+		goto error;
+	sensor->src->ctrl_handler.lock = &sensor->mutex;
+
+	for (max = 0; sensor->platform_data->op_sys_clock[max + 1]; max++);
+
+	sensor->link_freq = v4l2_ctrl_new_int_menu(
+		&sensor->src->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_LINK_FREQ, max, 0,
+		sensor->platform_data->op_sys_clock);
+
+	sensor->pixel_rate_csi = v4l2_ctrl_new_std(
+		&sensor->src->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
+
+	if (sensor->src->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"src controls initialization failed (%d)\n",
+			sensor->src->ctrl_handler.error);
+		rval = sensor->src->ctrl_handler.error;
+		goto error;
+	}
+
+	sensor->src->sd.ctrl_handler =
+		&sensor->src->ctrl_handler;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(&sensor->pixel_array->ctrl_handler);
+	v4l2_ctrl_handler_free(&sensor->src->ctrl_handler);
+
+	return rval;
+}
+
+static void smiapp_free_controls(struct smiapp_sensor *sensor)
+{
+	unsigned int i;
+
+	for (i = 0; i < sensor->ssds_used; i++)
+		v4l2_ctrl_handler_free(&sensor->ssds[i].ctrl_handler);
+}
+
+static int smiapp_get_limits(struct smiapp_sensor *sensor, int const *limit,
+			     unsigned int n)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int i;
+	u32 val;
+	int rval;
+
+	for (i = 0; i < n; i++) {
+		rval = smiapp_read(
+			sensor, smiapp_reg_limits[limit[i]].addr, &val);
+		if (rval)
+			return rval;
+		sensor->limits[limit[i]] = val;
+		dev_dbg(&client->dev, "0x%8.8x \"%s\" = %d, 0x%x\n",
+			smiapp_reg_limits[limit[i]].addr,
+			smiapp_reg_limits[limit[i]].what, val, val);
+	}
+
+	return 0;
+}
+
+static int smiapp_get_all_limits(struct smiapp_sensor *sensor)
+{
+	unsigned int i;
+	int rval;
+
+	for (i = 0; i < SMIAPP_LIMIT_LAST; i++) {
+		rval = smiapp_get_limits(sensor, &i, 1);
+		if (rval < 0)
+			return rval;
+	}
+
+	if (sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] == 0)
+		smiapp_replace_limit(sensor, SMIAPP_LIMIT_SCALER_N_MIN, 16);
+
+	return 0;
+}
+
+static int smiapp_get_limits_binning(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	static u32 const limits[] = {
+		SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN,
+		SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN,
+		SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN,
+		SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN,
+		SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN_BIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN,
+	};
+	static u32 const limits_replace[] = {
+		SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES,
+		SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES,
+		SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK,
+		SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK,
+		SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN,
+	};
+	unsigned int i;
+	int rval;
+
+	if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY] ==
+	    SMIAPP_BINNING_CAPABILITY_NO) {
+		for (i = 0; i < ARRAY_SIZE(limits); i++)
+			sensor->limits[limits[i]] =
+				sensor->limits[limits_replace[i]];
+
+		return 0;
+	}
+
+	rval = smiapp_get_limits(sensor, limits, ARRAY_SIZE(limits));
+	if (rval < 0)
+		return rval;
+
+	/*
+	 * Sanity check whether the binning limits are valid. If not,
+	 * use the non-binning ones.
+	 */
+	if (sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN]
+	    && sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN]
+	    && sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(limits); i++) {
+		dev_dbg(&client->dev,
+			"replace limit 0x%8.8x \"%s\" = %d, 0x%x\n",
+			smiapp_reg_limits[limits[i]].addr,
+			smiapp_reg_limits[limits[i]].what,
+			sensor->limits[limits_replace[i]],
+			sensor->limits[limits_replace[i]]);
+		sensor->limits[limits[i]] =
+			sensor->limits[limits_replace[i]];
+	}
+
+	return 0;
+}
+
+static int smiapp_get_mbus_formats(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int type, n;
+	unsigned int i, pixel_order;
+	int rval;
+
+	rval = smiapp_read(
+		sensor, SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE, &type);
+	if (rval)
+		return rval;
+
+	dev_dbg(&client->dev, "data_format_model_type %d\n", type);
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_PIXEL_ORDER,
+			   &pixel_order);
+	if (rval)
+		return rval;
+
+	if (pixel_order >= ARRAY_SIZE(pixel_order_str)) {
+		dev_dbg(&client->dev, "bad pixel order %d\n", pixel_order);
+		return -EINVAL;
+	}
+
+	dev_dbg(&client->dev, "pixel order %d (%s)\n", pixel_order,
+		pixel_order_str[pixel_order]);
+
+	switch (type) {
+	case SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL:
+		n = SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N;
+		break;
+	case SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED:
+		n = SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED_N;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	sensor->default_pixel_order = pixel_order;
+	sensor->mbus_frame_fmts = 0;
+
+	for (i = 0; i < n; i++) {
+		unsigned int fmt, j;
+
+		rval = smiapp_read(
+			sensor,
+			SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(i), &fmt);
+		if (rval)
+			return rval;
+
+		dev_dbg(&client->dev, "bpp %d, compressed %d\n",
+			fmt >> 8, (u8)fmt);
+
+		for (j = 0; j < ARRAY_SIZE(smiapp_csi_data_formats); j++) {
+			const struct smiapp_csi_data_format *f =
+				&smiapp_csi_data_formats[j];
+
+			if (f->pixel_order != SMIAPP_PIXEL_ORDER_GRBG)
+				continue;
+
+			if (f->width != fmt >> 8 || f->compressed != (u8)fmt)
+				continue;
+
+			dev_dbg(&client->dev, "jolly good! %d\n", j);
+
+			sensor->default_mbus_frame_fmts |= 1 << j;
+			if (!sensor->csi_format) {
+				sensor->csi_format = f;
+				sensor->internal_csi_format = f;
+			}
+		}
+	}
+
+	if (!sensor->csi_format) {
+		dev_err(&client->dev, "no supported mbus code found\n");
+		return -EINVAL;
+	}
+
+	smiapp_update_mbus_formats(sensor);
+
+	return 0;
+}
+
+static void smiapp_update_blanking(struct smiapp_sensor *sensor)
+{
+	struct v4l2_ctrl *vblank = sensor->vblank;
+	struct v4l2_ctrl *hblank = sensor->hblank;
+
+	vblank->minimum =
+		max_t(int,
+		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES],
+		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN] -
+		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height);
+	vblank->maximum =
+		sensor->limits[SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN] -
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height;
+
+	vblank->val = clamp_t(int, vblank->val,
+			      vblank->minimum, vblank->maximum);
+	vblank->default_value = vblank->minimum;
+	vblank->val = vblank->val;
+	vblank->cur.val = vblank->val;
+
+	hblank->minimum =
+		max_t(int,
+		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN] -
+		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width,
+		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN]);
+	hblank->maximum =
+		sensor->limits[SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN] -
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width;
+
+	hblank->val = clamp_t(int, hblank->val,
+			      hblank->minimum, hblank->maximum);
+	hblank->default_value = hblank->minimum;
+	hblank->val = hblank->val;
+	hblank->cur.val = hblank->val;
+
+	__smiapp_update_exposure_limits(sensor);
+}
+
+static int smiapp_update_mode(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int binning_mode;
+	int rval;
+
+	dev_dbg(&client->dev, "frame size: %dx%d\n",
+		sensor->src->crop[SMIAPP_PAD_SRC].width,
+		sensor->src->crop[SMIAPP_PAD_SRC].height);
+	dev_dbg(&client->dev, "csi format width: %d\n",
+		sensor->csi_format->width);
+
+	/* Binning has to be set up here; it affects limits */
+	if (sensor->binning_horizontal == 1 &&
+	    sensor->binning_vertical == 1) {
+		binning_mode = 0;
+	} else {
+		u8 binning_type =
+			(sensor->binning_horizontal << 4)
+			| sensor->binning_vertical;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U8_BINNING_TYPE, binning_type);
+		if (rval < 0)
+			return rval;
+
+		binning_mode = 1;
+	}
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_BINNING_MODE, binning_mode);
+	if (rval < 0)
+		return rval;
+
+	/* Get updated limits due to binning */
+	rval = smiapp_get_limits_binning(sensor);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_pll_update(sensor);
+	if (rval < 0)
+		return rval;
+
+	/* Output from pixel array, including blanking */
+	smiapp_update_blanking(sensor);
+
+	dev_dbg(&client->dev, "vblank\t\t%d\n", sensor->vblank->val);
+	dev_dbg(&client->dev, "hblank\t\t%d\n", sensor->hblank->val);
+
+	dev_dbg(&client->dev, "real timeperframe\t100/%d\n",
+		sensor->pll.vt_pix_clk_freq_hz /
+		((sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
+		  + sensor->hblank->val) *
+		 (sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+		  + sensor->vblank->val) / 100));
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ NVM handling
+ *
+ */
+static int smiapp_read_nvm(struct smiapp_sensor *sensor,
+			   unsigned char *nvm)
+{
+	u32 i, s, p, np, v;
+	int rval = 0, rval2;
+
+	np = sensor->nvm_size / SMIAPP_NVM_PAGE_SIZE;
+	for (p = 0; p < np; p++) {
+		rval = smiapp_write(
+			sensor,
+			SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT, p);
+		if (rval)
+			goto out;
+
+		rval = smiapp_write(sensor,
+				    SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL,
+				    SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN |
+				    SMIAPP_DATA_TRANSFER_IF_1_CTRL_RD_EN);
+		if (rval)
+			goto out;
+
+		for (i = 0; i < 1000; i++) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS, &s);
+
+			if (rval)
+				goto out;
+
+			if (s & SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY)
+				break;
+
+			if (--i == 0) {
+				rval = -ETIMEDOUT;
+				goto out;
+			}
+
+		}
+
+		for (i = 0; i < SMIAPP_NVM_PAGE_SIZE; i++) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0 + i,
+				&v);
+			if (rval)
+				goto out;
+
+			*nvm++ = v;
+		}
+	}
+
+out:
+	rval2 = smiapp_write(sensor, SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL, 0);
+	if (rval < 0)
+		return rval;
+	else
+		return rval2;
+}
+
+/*
+ *
+ * SMIA++ CCI address control
+ *
+ */
+static int smiapp_change_cci_addr(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+	u32 val;
+
+	client->addr = sensor->platform_data->i2c_addr_dfl;
+
+	rval = smiapp_write(sensor,
+			    SMIAPP_REG_U8_CCI_ADDRESS_CONTROL,
+			    sensor->platform_data->i2c_addr_alt << 1);
+	if (rval)
+		return rval;
+
+	client->addr = sensor->platform_data->i2c_addr_alt;
+
+	/* verify addr change went ok */
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_CCI_ADDRESS_CONTROL, &val);
+	if (rval)
+		return rval;
+
+	if (val != sensor->platform_data->i2c_addr_alt << 1)
+		return -ENODEV;
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ Mode Control
+ *
+ */
+static int smiapp_setup_flash_strobe(struct smiapp_sensor *sensor)
+{
+	struct smiapp_flash_strobe_parms *strobe_setup;
+	unsigned int ext_freq = sensor->platform_data->ext_clk;
+	u32 tmp;
+	u32 strobe_adjustment;
+	u32 strobe_width_high_rs;
+	int rval;
+
+	strobe_setup = sensor->platform_data->strobe_setup;
+
+	/*
+	 * How to calculate registers related to strobe length. Please
+	 * do not change, or if you do at least know what you're
+	 * doing. :-)
+	 *
+	 * Sakari Ailus <sakari.ailus@maxwell.research.nokia.com> 2010-10-25
+	 *
+	 * flash_strobe_length [us] / 10^6 = (tFlash_strobe_width_ctrl
+	 *	/ EXTCLK freq [Hz]) * flash_strobe_adjustment
+	 *
+	 * tFlash_strobe_width_ctrl E N, [1 - 0xffff]
+	 * flash_strobe_adjustment E N, [1 - 0xff]
+	 *
+	 * The formula above is written as below to keep it on one
+	 * line:
+	 *
+	 * l / 10^6 = w / e * a
+	 *
+	 * Let's mark w * a by x:
+	 *
+	 * x = w * a
+	 *
+	 * Thus, we get:
+	 *
+	 * x = l * e / 10^6
+	 *
+	 * The strobe width must be at least as long as requested,
+	 * thus rounding upwards is needed.
+	 *
+	 * x = (l * e + 10^6 - 1) / 10^6
+	 * -----------------------------
+	 *
+	 * Maximum possible accuracy is wanted at all times. Thus keep
+	 * a as small as possible.
+	 *
+	 * Calculate a, assuming maximum w, with rounding upwards:
+	 *
+	 * a = (x + (2^16 - 1) - 1) / (2^16 - 1)
+	 * -------------------------------------
+	 *
+	 * Thus, we also get w, with that a, with rounding upwards:
+	 *
+	 * w = (x + a - 1) / a
+	 * -------------------
+	 *
+	 * To get limits:
+	 *
+	 * x E [1, (2^16 - 1) * (2^8 - 1)]
+	 *
+	 * Substituting maximum x to the original formula (with rounding),
+	 * the maximum l is thus
+	 *
+	 * (2^16 - 1) * (2^8 - 1) * 10^6 = l * e + 10^6 - 1
+	 *
+	 * l = (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / e
+	 * --------------------------------------------------
+	 *
+	 * flash_strobe_length must be clamped between 1 and
+	 * (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / EXTCLK freq.
+	 *
+	 * Then,
+	 *
+	 * flash_strobe_adjustment = ((flash_strobe_length *
+	 *	EXTCLK freq + 10^6 - 1) / 10^6 + (2^16 - 1) - 1) / (2^16 - 1)
+	 *
+	 * tFlash_strobe_width_ctrl = ((flash_strobe_length *
+	 *	EXTCLK freq + 10^6 - 1) / 10^6 +
+	 *	flash_strobe_adjustment - 1) / flash_strobe_adjustment
+	 */
+	tmp = div_u64(1000000ULL * ((1 << 16) - 1) * ((1 << 8) - 1) -
+		      1000000 + 1, ext_freq);
+	strobe_setup->strobe_width_high_us =
+		clamp_t(u32, strobe_setup->strobe_width_high_us, 1, tmp);
+
+	tmp = div_u64(((u64)strobe_setup->strobe_width_high_us * (u64)ext_freq +
+			1000000 - 1), 1000000ULL);
+	strobe_adjustment = (tmp + (1 << 16) - 1 - 1) / ((1 << 16) - 1);
+	strobe_width_high_rs = (tmp + strobe_adjustment - 1) /
+				strobe_adjustment;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_MODE_RS,
+			    strobe_setup->mode);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT,
+			    strobe_adjustment);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL,
+		strobe_width_high_rs);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL,
+			    strobe_setup->strobe_delay);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_FLASH_STROBE_START_POINT,
+			    strobe_setup->stobe_start_point);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_TRIGGER_RS,
+			    strobe_setup->trigger);
+
+out:
+	sensor->platform_data->strobe_setup->trigger = 0;
+
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int smiapp_power_on(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int sleep;
+	int rval;
+
+	rval = regulator_enable(sensor->vana);
+	if (rval) {
+		dev_err(&client->dev, "failed to enable vana regulator\n");
+		return rval;
+	}
+	usleep_range(1000, 1000);
+
+	if (sensor->platform_data->set_xclk)
+		rval = sensor->platform_data->set_xclk(
+			&sensor->src->sd, sensor->platform_data->ext_clk);
+	else
+		rval = clk_enable(sensor->ext_clk);
+	if (rval < 0) {
+		dev_dbg(&client->dev, "failed to set xclk\n");
+		goto out_xclk_fail;
+	}
+	usleep_range(1000, 1000);
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 1);
+
+	sleep = SMIAPP_RESET_DELAY(sensor->platform_data->ext_clk);
+	usleep_range(sleep, sleep);
+
+	/*
+	 * Failures to respond to the address change command have been noticed.
+	 * Those failures seem to be caused by the sensor requiring a longer
+	 * boot time than advertised. An additional 10ms delay seems to work
+	 * around the issue, but the SMIA++ I2C write retry hack makes the delay
+	 * unnecessary. The failures need to be investigated to find a proper
+	 * fix, and a delay will likely need to be added here if the I2C write
+	 * retry hack is reverted before the root cause of the boot time issue
+	 * is found.
+	 */
+
+	if (sensor->platform_data->i2c_addr_alt) {
+		rval = smiapp_change_cci_addr(sensor);
+		if (rval) {
+			dev_err(&client->dev, "cci address change error\n");
+			goto out_cci_addr_fail;
+		}
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_SOFTWARE_RESET,
+			    SMIAPP_SOFTWARE_RESET);
+	if (rval < 0) {
+		dev_err(&client->dev, "software reset failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	if (sensor->platform_data->i2c_addr_alt) {
+		rval = smiapp_change_cci_addr(sensor);
+		if (rval) {
+			dev_err(&client->dev, "cci address change error\n");
+			goto out_cci_addr_fail;
+		}
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_COMPRESSION_MODE,
+			    SMIAPP_COMPRESSION_MODE_SIMPLE_PREDICTOR);
+	if (rval) {
+		dev_err(&client->dev, "compression mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ,
+		sensor->platform_data->ext_clk / (1000000 / (1 << 8)));
+	if (rval) {
+		dev_err(&client->dev, "extclk frequency set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_LANE_MODE,
+			    sensor->platform_data->lanes - 1);
+	if (rval) {
+		dev_err(&client->dev, "csi lane mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FAST_STANDBY_CTRL,
+			    SMIAPP_FAST_STANDBY_CTRL_IMMEDIATE);
+	if (rval) {
+		dev_err(&client->dev, "fast standby set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_SIGNALLING_MODE,
+			    sensor->platform_data->csi_signalling_mode);
+	if (rval) {
+		dev_err(&client->dev, "csi signalling mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	/* DPHY control done by sensor based on requested link rate */
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_DPHY_CTRL,
+			    SMIAPP_DPHY_CTRL_UI);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_call_quirk(sensor, post_poweron);
+	if (rval) {
+		dev_err(&client->dev, "post_poweron quirks failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	/* Are we still initialising...? If yes, return here. */
+	if (!sensor->pixel_array)
+		return 0;
+
+	rval = v4l2_ctrl_handler_setup(
+		&sensor->pixel_array->ctrl_handler);
+	if (rval)
+		goto out_cci_addr_fail;
+
+	rval = v4l2_ctrl_handler_setup(&sensor->src->ctrl_handler);
+	if (rval)
+		goto out_cci_addr_fail;
+
+	mutex_lock(&sensor->mutex);
+	rval = smiapp_update_mode(sensor);
+	mutex_unlock(&sensor->mutex);
+	if (rval < 0)
+		goto out_cci_addr_fail;
+
+	return 0;
+
+out_cci_addr_fail:
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 0);
+	if (sensor->platform_data->set_xclk)
+		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+	else
+		clk_disable(sensor->ext_clk);
+
+out_xclk_fail:
+	regulator_disable(sensor->vana);
+	return rval;
+}
+
+static void smiapp_power_off(struct smiapp_sensor *sensor)
+{
+	/*
+	 * Currently power/clock to lens are enable/disabled separately
+	 * but they are essentially the same signals. So if the sensor is
+	 * powered off while the lens is powered on the sensor does not
+	 * really see a power off and next time the cci address change
+	 * will fail. So do a soft reset explicitly here.
+	 */
+	if (sensor->platform_data->i2c_addr_alt)
+		smiapp_write(sensor,
+			     SMIAPP_REG_U8_SOFTWARE_RESET,
+			     SMIAPP_SOFTWARE_RESET);
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 0);
+	if (sensor->platform_data->set_xclk)
+		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+	else
+		clk_disable(sensor->ext_clk);
+	usleep_range(5000, 5000);
+	regulator_disable(sensor->vana);
+	sensor->streaming = 0;
+}
+
+static int smiapp_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int ret = 0;
+
+	mutex_lock(&sensor->power_mutex);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0
+	 * to 0, update the power state.
+	 */
+	if (!sensor->power_count == !on)
+		goto out;
+
+	if (on) {
+		/* Power on and perform initialisation. */
+		ret = smiapp_power_on(sensor);
+		if (ret < 0)
+			goto out;
+	} else {
+		smiapp_power_off(sensor);
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+
+out:
+	mutex_unlock(&sensor->power_mutex);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video stream management
+ */
+
+static int smiapp_start_streaming(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_CSI_DATA_FORMAT,
+			    (sensor->csi_format->width << 8) |
+			    sensor->csi_format->compressed);
+	if (rval)
+		goto out;
+
+	rval = smiapp_pll_configure(sensor);
+	if (rval)
+		goto out;
+
+	/* Analog crop start coordinates */
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_X_ADDR_START,
+			    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_ADDR_START,
+			    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top);
+	if (rval < 0)
+		goto out;
+
+	/* Analog crop end coordinates */
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_X_ADDR_END,
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left
+		+ sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width - 1);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_Y_ADDR_END,
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top
+		+ sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height - 1);
+	if (rval < 0)
+		goto out;
+
+	/*
+	 * Output from pixel array, including blanking, is set using
+	 * controls below. No need to set here.
+	 */
+
+	/* Digital crop */
+	if (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+	    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].left);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].top);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].width);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].height);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Scaling */
+	if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+	    != SMIAPP_SCALING_CAPABILITY_NONE) {
+		rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALING_MODE,
+				    sensor->scaling_mode);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALE_M,
+				    sensor->scale_m);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Output size from sensor */
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_X_OUTPUT_SIZE,
+			    sensor->src->crop[SMIAPP_PAD_SRC].width);
+	if (rval < 0)
+		goto out;
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_OUTPUT_SIZE,
+			    sensor->src->crop[SMIAPP_PAD_SRC].height);
+	if (rval < 0)
+		goto out;
+
+	if ((sensor->flash_capability &
+	     (SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE |
+	      SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE)) &&
+	    sensor->platform_data->strobe_setup != NULL &&
+	    sensor->platform_data->strobe_setup->trigger != 0) {
+		rval = smiapp_setup_flash_strobe(sensor);
+		if (rval)
+			goto out;
+	}
+
+	rval = smiapp_call_quirk(sensor, pre_streamon);
+	if (rval) {
+		dev_err(&client->dev, "pre_streamon quirks failed\n");
+		goto out;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
+			    SMIAPP_MODE_SELECT_STREAMING);
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static int smiapp_stop_streaming(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
+			    SMIAPP_MODE_SELECT_SOFTWARE_STANDBY);
+	if (rval)
+		goto out;
+
+	rval = smiapp_call_quirk(sensor, post_streamoff);
+	if (rval)
+		dev_err(&client->dev, "post_streamoff quirks failed\n");
+
+out:
+	mutex_unlock(&sensor->mutex);
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static int smiapp_set_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	if (sensor->streaming == enable)
+		return 0;
+
+	if (enable) {
+		sensor->streaming = 1;
+		rval = smiapp_start_streaming(sensor);
+		if (rval < 0)
+			sensor->streaming = 0;
+	} else {
+		rval = smiapp_stop_streaming(sensor);
+		sensor->streaming = 0;
+	}
+
+	return rval;
+}
+
+static int smiapp_enum_mbus_code(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+	int idx = -1;
+	int rval = -EINVAL;
+
+	mutex_lock(&sensor->mutex);
+
+	dev_err(&client->dev, "subdev %s, pad %d, index %d\n",
+		subdev->name, code->pad, code->index);
+
+	if (subdev != &sensor->src->sd || code->pad != SMIAPP_PAD_SRC) {
+		if (code->index)
+			goto out;
+
+		code->code = sensor->internal_csi_format->code;
+		rval = 0;
+		goto out;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i))
+			idx++;
+
+		if (idx == code->index) {
+			code->code = smiapp_csi_data_formats[i].code;
+			dev_err(&client->dev, "found index %d, i %d, code %x\n",
+				code->index, i, code->code);
+			rval = 0;
+			break;
+		}
+	}
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static u32 __smiapp_get_mbus_code(struct v4l2_subdev *subdev,
+				  unsigned int pad)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+
+	if (subdev == &sensor->src->sd && pad == SMIAPP_PAD_SRC)
+		return sensor->csi_format->code;
+	else
+		return sensor->internal_csi_format->code;
+}
+
+static int __smiapp_get_format(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt->format = *v4l2_subdev_get_try_format(fh, fmt->pad);
+	} else {
+		struct v4l2_rect *r;
+
+		if (fmt->pad == ssd->source_pad)
+			r = &ssd->crop[ssd->source_pad];
+		else
+			r = &ssd->sink_fmt;
+
+		fmt->format.code = __smiapp_get_mbus_code(subdev, fmt->pad);
+		fmt->format.width = r->width;
+		fmt->format.height = r->height;
+	}
+
+	return 0;
+}
+
+static int smiapp_get_format(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __smiapp_get_format(subdev, fh, fmt);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static void smiapp_get_crop_compose(struct v4l2_subdev *subdev,
+				    struct v4l2_subdev_fh *fh,
+				    struct v4l2_rect **crops,
+				    struct v4l2_rect **comps, int which)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	unsigned int i;
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		if (crops)
+			for (i = 0; i < subdev->entity.num_pads; i++)
+				crops[i] = &ssd->crop[i];
+		if (comps)
+			*comps = &ssd->compose;
+	} else {
+		if (crops) {
+			for (i = 0; i < subdev->entity.num_pads; i++) {
+				crops[i] = v4l2_subdev_get_try_crop(fh, i);
+				BUG_ON(!crops[i]);
+			}
+		}
+		if (comps) {
+			*comps = v4l2_subdev_get_try_compose(fh,
+							     SMIAPP_PAD_SINK);
+			BUG_ON(!*comps);
+		}
+	}
+}
+
+/* Changes require propagation only on sink pad. */
+static void smiapp_propagate(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh, int which,
+			     int target)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, which);
+
+	switch (target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		comp->width = crops[SMIAPP_PAD_SINK]->width;
+		comp->height = crops[SMIAPP_PAD_SINK]->height;
+		if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			if (ssd == sensor->scaler) {
+				sensor->scale_m =
+					sensor->limits[
+						SMIAPP_LIMIT_SCALER_N_MIN];
+				sensor->scaling_mode =
+					SMIAPP_SCALING_MODE_NONE;
+			} else if (ssd == sensor->binner) {
+				sensor->binning_horizontal = 1;
+				sensor->binning_vertical = 1;
+			}
+		}
+		/* Fall through */
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		*crops[SMIAPP_PAD_SRC] = *comp;
+		break;
+	default:
+		BUG();
+	}
+}
+
+static const struct smiapp_csi_data_format
+*smiapp_validate_csi_data_format(struct smiapp_sensor *sensor, u32 code)
+{
+	const struct smiapp_csi_data_format *csi_format = sensor->csi_format;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i)
+		    && smiapp_csi_data_formats[i].code == code)
+			return &smiapp_csi_data_formats[i];
+	}
+
+	return csi_format;
+}
+
+static int smiapp_set_format(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *crops[SMIAPP_PADS];
+
+	mutex_lock(&sensor->mutex);
+
+	/*
+	 * Media bus code is changeable on src subdev's source pad. On
+	 * other source pads we just get format here.
+	 */
+	if (fmt->pad == ssd->source_pad) {
+		u32 code = fmt->format.code;
+		int rval = __smiapp_get_format(subdev, fh, fmt);
+
+		if (!rval && subdev == &sensor->src->sd) {
+			const struct smiapp_csi_data_format *csi_format =
+				smiapp_validate_csi_data_format(sensor, code);
+			if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+				sensor->csi_format = csi_format;
+			fmt->format.code = csi_format->code;
+		}
+
+		mutex_unlock(&sensor->mutex);
+		return rval;
+	}
+
+	/* Sink pad. Width and height are changeable here. */
+	fmt->format.code = __smiapp_get_mbus_code(subdev, fmt->pad);
+	fmt->format.width &= ~1;
+	fmt->format.height &= ~1;
+
+	fmt->format.width =
+		clamp(fmt->format.width,
+		      sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
+		      sensor->limits[SMIAPP_LIMIT_MAX_X_OUTPUT_SIZE]);
+	fmt->format.height =
+		clamp(fmt->format.height,
+		      sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
+		      sensor->limits[SMIAPP_LIMIT_MAX_Y_OUTPUT_SIZE]);
+
+	smiapp_get_crop_compose(subdev, fh, crops, NULL, fmt->which);
+
+	crops[ssd->sink_pad]->left = 0;
+	crops[ssd->sink_pad]->top = 0;
+	crops[ssd->sink_pad]->width = fmt->format.width;
+	crops[ssd->sink_pad]->height = fmt->format.height;
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ssd->sink_fmt = *crops[ssd->sink_pad];
+	smiapp_propagate(subdev, fh, fmt->which,
+			 V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL);
+
+	mutex_unlock(&sensor->mutex);
+
+	return 0;
+}
+
+/*
+ * Calculate goodness of scaled image size compared to expected image
+ * size and flags provided.
+ */
+#define SCALING_GOODNESS		100000
+#define SCALING_GOODNESS_EXTREME	100000000
+static int scaling_goodness(struct v4l2_subdev *subdev, int w, int ask_w,
+			    int h, int ask_h, u32 flags)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	int val = 0;
+
+	w &= ~1;
+	ask_w &= ~1;
+	h &= ~1;
+	ask_h &= ~1;
+
+	if (flags & V4L2_SUBDEV_SEL_FLAG_SIZE_GE) {
+		if (w < ask_w)
+			val -= SCALING_GOODNESS;
+		if (h < ask_h)
+			val -= SCALING_GOODNESS;
+	}
+
+	if (flags & V4L2_SUBDEV_SEL_FLAG_SIZE_LE) {
+		if (w > ask_w)
+			val -= SCALING_GOODNESS;
+		if (h > ask_h)
+			val -= SCALING_GOODNESS;
+	}
+
+	val -= abs(w - ask_w);
+	val -= abs(h - ask_h);
+
+	if (w < sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE])
+		val -= SCALING_GOODNESS_EXTREME;
+
+	dev_dbg(&client->dev, "w %d ask_w %d h %d ask_h %d goodness %d\n",
+		w, ask_h, h, ask_h, val);
+
+	return val;
+}
+
+static void smiapp_set_compose_binner(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_fh *fh,
+				      struct v4l2_subdev_selection *sel,
+				      struct v4l2_rect **crops,
+				      struct v4l2_rect *comp)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+	unsigned int binh = 1, binv = 1;
+	unsigned int best = scaling_goodness(
+		subdev,
+		crops[SMIAPP_PAD_SINK]->width, sel->r.width,
+		crops[SMIAPP_PAD_SINK]->height, sel->r.height, sel->flags);
+
+	for (i = 0; i < sensor->nbinning_subtypes; i++) {
+		int this = scaling_goodness(
+			subdev,
+			crops[SMIAPP_PAD_SINK]->width
+			/ sensor->binning_subtypes[i].horizontal,
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height
+			/ sensor->binning_subtypes[i].vertical,
+			sel->r.height, sel->flags);
+
+		if (this > best) {
+			binh = sensor->binning_subtypes[i].horizontal;
+			binv = sensor->binning_subtypes[i].vertical;
+			best = this;
+		}
+	}
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sensor->binning_vertical = binv;
+		sensor->binning_horizontal = binh;
+	}
+
+	sel->r.width = (crops[SMIAPP_PAD_SINK]->width / binh) & ~1;
+	sel->r.height = (crops[SMIAPP_PAD_SINK]->height / binv) & ~1;
+}
+
+/*
+ * Calculate best scaling ratio and mode for given output resolution.
+ *
+ * Try all of these: horizontal ratio, vertical ratio and smallest
+ * size possible (horizontally).
+ *
+ * Also try whether horizontal scaler or full scaler gives a better
+ * result.
+ */
+static void smiapp_set_compose_scaler(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_fh *fh,
+				      struct v4l2_subdev_selection *sel,
+				      struct v4l2_rect **crops,
+				      struct v4l2_rect *comp)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	u32 min, max, a, b, max_m;
+	u32 scale_m = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+	int mode = SMIAPP_SCALING_MODE_HORIZONTAL;
+	u32 try[4];
+	u32 ntry = 0;
+	unsigned int i;
+	int best = INT_MIN;
+
+	sel->r.width = min_t(unsigned int, sel->r.width,
+			     crops[SMIAPP_PAD_SINK]->width);
+	sel->r.height = min_t(unsigned int, sel->r.height,
+			      crops[SMIAPP_PAD_SINK]->height);
+
+	a = crops[SMIAPP_PAD_SINK]->width
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.width;
+	b = crops[SMIAPP_PAD_SINK]->height
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.height;
+	max_m = crops[SMIAPP_PAD_SINK]->width
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]
+		/ sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE];
+
+	a = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		max(a, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+	b = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		max(b, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+	max_m = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		    max(max_m, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+
+	dev_dbg(&client->dev, "scaling: a %d b %d max_m %d\n", a, b, max_m);
+
+	min = min(max_m, min(a, b));
+	max = min(max_m, max(a, b));
+
+	try[ntry] = min;
+	ntry++;
+	if (min != max) {
+		try[ntry] = max;
+		ntry++;
+	}
+	if (max != max_m) {
+		try[ntry] = min + 1;
+		ntry++;
+		if (min != max) {
+			try[ntry] = max + 1;
+			ntry++;
+		}
+	}
+
+	for (i = 0; i < ntry; i++) {
+		int this = scaling_goodness(
+			subdev,
+			crops[SMIAPP_PAD_SINK]->width
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height,
+			sel->r.height,
+			sel->flags);
+
+		dev_dbg(&client->dev, "trying factor %d (%d)\n", try[i], i);
+
+		if (this > best) {
+			scale_m = try[i];
+			mode = SMIAPP_SCALING_MODE_HORIZONTAL;
+			best = this;
+		}
+
+		if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
+			continue;
+
+		this = scaling_goodness(
+			subdev, crops[SMIAPP_PAD_SINK]->width
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.height,
+			sel->flags);
+
+		if (this > best) {
+			scale_m = try[i];
+			mode = SMIAPP_SCALING_MODE_BOTH;
+			best = this;
+		}
+	}
+
+	sel->r.width =
+		(crops[SMIAPP_PAD_SINK]->width
+		 / scale_m
+		 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]) & ~1;
+	if (mode == SMIAPP_SCALING_MODE_BOTH)
+		sel->r.height =
+			(crops[SMIAPP_PAD_SINK]->height
+			 / scale_m
+			 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN])
+			& ~1;
+	else
+		sel->r.height = crops[SMIAPP_PAD_SINK]->height;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sensor->scale_m = scale_m;
+		sensor->scaling_mode = mode;
+	}
+}
+/* We're only called on source pads. This function sets scaling. */
+static int smiapp_set_compose(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, sel->which);
+
+	sel->r.top = 0;
+	sel->r.left = 0;
+
+	if (ssd == sensor->binner)
+		smiapp_set_compose_binner(subdev, fh, sel, crops, comp);
+	else
+		smiapp_set_compose_scaler(subdev, fh, sel, crops, comp);
+
+	*comp = sel->r;
+	smiapp_propagate(subdev, fh, sel->which,
+			 V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return smiapp_update_mode(sensor);
+
+	return 0;
+}
+
+static int __smiapp_sel_supported(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+
+	/* We only implement crop in three places. */
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		if (ssd == sensor->pixel_array
+		    && sel->pad == SMIAPP_PA_PAD_SRC)
+			return 0;
+		if (ssd == sensor->src
+		    && sel->pad == SMIAPP_PAD_SRC)
+			return 0;
+		if (ssd == sensor->scaler
+		    && sel->pad == SMIAPP_PAD_SINK
+		    && sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+		    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP)
+			return 0;
+		return -EINVAL;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		if (sel->pad == ssd->source_pad)
+			return -EINVAL;
+		if (ssd == sensor->binner)
+			return 0;
+		if (ssd == sensor->scaler
+		    && sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    != SMIAPP_SCALING_CAPABILITY_NONE)
+			return 0;
+		/* Fall through */
+	default:
+		return -EINVAL;
+	}
+}
+
+static int smiapp_set_crop(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *src_size, *crops[SMIAPP_PADS];
+	struct v4l2_rect _r;
+
+	smiapp_get_crop_compose(subdev, fh, crops, NULL, sel->which);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		if (sel->pad == ssd->sink_pad)
+			src_size = &ssd->sink_fmt;
+		else
+			src_size = &ssd->compose;
+	} else {
+		if (sel->pad == ssd->sink_pad) {
+			_r.left = 0;
+			_r.top = 0;
+			_r.width = v4l2_subdev_get_try_format(fh, sel->pad)
+				->width;
+			_r.height = v4l2_subdev_get_try_format(fh, sel->pad)
+				->height;
+			src_size = &_r;
+		} else {
+			src_size =
+				v4l2_subdev_get_try_compose(
+					fh, ssd->sink_pad);
+		}
+	}
+
+	if (ssd == sensor->src && sel->pad == SMIAPP_PAD_SRC) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+
+	sel->r.width = min(sel->r.width, src_size->width);
+	sel->r.height = min(sel->r.height, src_size->height);
+
+	sel->r.left = min(sel->r.left, src_size->width - sel->r.width);
+	sel->r.top = min(sel->r.top, src_size->height - sel->r.height);
+
+	*crops[sel->pad] = sel->r;
+
+	if (ssd != sensor->pixel_array && sel->pad == SMIAPP_PAD_SINK)
+		smiapp_propagate(subdev, fh, sel->which,
+				 V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL);
+
+	return 0;
+}
+
+static int __smiapp_get_selection(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+	struct v4l2_rect sink_fmt;
+	int ret;
+
+	ret = __smiapp_sel_supported(subdev, sel);
+	if (ret)
+		return ret;
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, sel->which);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sink_fmt = ssd->sink_fmt;
+	} else {
+		struct v4l2_mbus_framefmt *fmt =
+			v4l2_subdev_get_try_format(fh, ssd->sink_pad);
+
+		sink_fmt.left = 0;
+		sink_fmt.top = 0;
+		sink_fmt.width = fmt->width;
+		sink_fmt.height = fmt->height;
+	}
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		if (ssd == sensor->pixel_array) {
+			sel->r.width =
+				sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+			sel->r.height =
+				sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		} else if (sel->pad == ssd->sink_pad) {
+			sel->r = sink_fmt;
+		} else {
+			sel->r = *comp;
+		}
+		break;
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r = *crops[sel->pad];
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		sel->r = *comp;
+		break;
+	}
+
+	return 0;
+}
+
+static int smiapp_get_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __smiapp_get_selection(subdev, fh, sel);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+static int smiapp_set_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int ret;
+
+	ret = __smiapp_sel_supported(subdev, sel);
+	if (ret)
+		return ret;
+
+	mutex_lock(&sensor->mutex);
+
+	sel->r.left = max(0, sel->r.left & ~1);
+	sel->r.top = max(0, sel->r.top & ~1);
+	sel->r.width = max(0, SMIAPP_ALIGN_DIM(sel->r.width, sel->flags));
+	sel->r.height = max(0, SMIAPP_ALIGN_DIM(sel->r.height, sel->flags));
+
+	sel->r.width = max_t(unsigned int,
+			     sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
+			     sel->r.width);
+	sel->r.height = max_t(unsigned int,
+			      sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
+			      sel->r.height);
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		ret = smiapp_set_crop(subdev, fh, sel);
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		ret = smiapp_set_compose(subdev, fh, sel);
+		break;
+	default:
+		BUG();
+	}
+
+	mutex_unlock(&sensor->mutex);
+	return ret;
+}
+
+static int smiapp_get_skip_frames(struct v4l2_subdev *subdev, u32 *frames)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+
+	*frames = sensor->frame_skip;
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * sysfs attributes
+ */
+
+static ssize_t
+smiapp_sysfs_nvm_read(struct device *dev, struct device_attribute *attr,
+		      char *buf)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(to_i2c_client(dev));
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int nbytes;
+
+	if (!sensor->dev_init_done)
+		return -EBUSY;
+
+	if (!sensor->nvm_size) {
+		/* NVM not read yet - read it now */
+		sensor->nvm_size = sensor->platform_data->nvm_size;
+		if (smiapp_set_power(subdev, 1) < 0)
+			return -ENODEV;
+		if (smiapp_read_nvm(sensor, sensor->nvm)) {
+			dev_err(&client->dev, "nvm read failed\n");
+			return -ENODEV;
+		}
+		smiapp_set_power(subdev, 0);
+	}
+	/*
+	 * NVM is still way below a PAGE_SIZE, so we can safely
+	 * assume this for now.
+	 */
+	nbytes = min_t(unsigned int, sensor->nvm_size, PAGE_SIZE);
+	memcpy(buf, sensor->nvm, nbytes);
+
+	return nbytes;
+}
+static DEVICE_ATTR(nvm, S_IRUGO, smiapp_sysfs_nvm_read, NULL);
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+static int smiapp_identify_module(struct v4l2_subdev *subdev)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_module_info *minfo = &sensor->minfo;
+	unsigned int i;
+	int rval = 0;
+
+	minfo->name = SMIAPP_NAME;
+
+	/* Module info */
+	rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MANUFACTURER_ID,
+				 &minfo->manufacturer_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U16_MODEL_ID,
+					 &minfo->model_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_REVISION_NUMBER_MAJOR,
+					 &minfo->revision_number_major);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_REVISION_NUMBER_MINOR,
+					 &minfo->revision_number_minor);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_MODULE_DATE_YEAR,
+					 &minfo->module_year);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_MODULE_DATE_MONTH,
+					 &minfo->module_month);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MODULE_DATE_DAY,
+					 &minfo->module_day);
+
+	/* Sensor info */
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID,
+					 &minfo->sensor_manufacturer_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U16_SENSOR_MODEL_ID,
+					 &minfo->sensor_model_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_REVISION_NUMBER,
+					 &minfo->sensor_revision_number);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION,
+					 &minfo->sensor_firmware_version);
+
+	/* SMIA */
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIA_VERSION,
+					 &minfo->smia_version);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIAPP_VERSION,
+					 &minfo->smiapp_version);
+
+	if (rval) {
+		dev_err(&client->dev, "sensor detection failed\n");
+		return -ENODEV;
+	}
+
+	dev_dbg(&client->dev, "module 0x%2.2x-0x%4.4x\n",
+		minfo->manufacturer_id, minfo->model_id);
+
+	dev_dbg(&client->dev,
+		"module revision 0x%2.2x-0x%2.2x date %2.2d-%2.2d-%2.2d\n",
+		minfo->revision_number_major, minfo->revision_number_minor,
+		minfo->module_year, minfo->module_month, minfo->module_day);
+
+	dev_dbg(&client->dev, "sensor 0x%2.2x-0x%4.4x\n",
+		minfo->sensor_manufacturer_id, minfo->sensor_model_id);
+
+	dev_dbg(&client->dev,
+		"sensor revision 0x%2.2x firmware version 0x%2.2x\n",
+		minfo->sensor_revision_number, minfo->sensor_firmware_version);
+
+	dev_dbg(&client->dev, "smia version %2.2d smiapp version %2.2d\n",
+		minfo->smia_version, minfo->smiapp_version);
+
+	/*
+	 * Some modules have bad data in the lvalues below. Hope the
+	 * rvalues have better stuff. The lvalues are module
+	 * parameters whereas the rvalues are sensor parameters.
+	 */
+	if (!minfo->manufacturer_id && !minfo->model_id) {
+		minfo->manufacturer_id = minfo->sensor_manufacturer_id;
+		minfo->model_id = minfo->sensor_model_id;
+		minfo->revision_number_major = minfo->sensor_revision_number;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_module_idents); i++) {
+		if (smiapp_module_idents[i].manufacturer_id
+		    != minfo->manufacturer_id)
+			continue;
+		if (smiapp_module_idents[i].model_id != minfo->model_id)
+			continue;
+		if (smiapp_module_idents[i].flags
+		    & SMIAPP_MODULE_IDENT_FLAG_REV_LE) {
+			if (smiapp_module_idents[i].revision_number_major
+			    < minfo->revision_number_major)
+				continue;
+		} else {
+			if (smiapp_module_idents[i].revision_number_major
+			    != minfo->revision_number_major)
+				continue;
+		}
+
+		minfo->name = smiapp_module_idents[i].name;
+		minfo->quirk = smiapp_module_idents[i].quirk;
+		break;
+	}
+
+	if (i >= ARRAY_SIZE(smiapp_module_idents))
+		dev_warn(&client->dev,
+			 "no quirks for this module; let's hope it's fully compliant\n");
+
+	dev_dbg(&client->dev, "the sensor is called %s, ident %2.2x%4.4x%2.2x\n",
+		minfo->name, minfo->manufacturer_id, minfo->model_id,
+		minfo->revision_number_major);
+
+	strlcpy(subdev->name, sensor->minfo.name, sizeof(subdev->name));
+
+	return 0;
+}
+
+static const struct v4l2_subdev_ops smiapp_ops;
+static const struct v4l2_subdev_internal_ops smiapp_internal_ops;
+static const struct media_entity_operations smiapp_entity_ops;
+
+static int smiapp_registered(struct v4l2_subdev *subdev)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_subdev *last = NULL;
+	u32 tmp;
+	unsigned int i;
+	int rval;
+
+	sensor->vana = regulator_get(&client->dev, "VANA");
+	if (IS_ERR(sensor->vana)) {
+		dev_err(&client->dev, "could not get regulator for vana\n");
+		return -ENODEV;
+	}
+
+	if (!sensor->platform_data->set_xclk) {
+		sensor->ext_clk = clk_get(&client->dev,
+					  sensor->platform_data->ext_clk_name);
+		if (IS_ERR(sensor->ext_clk)) {
+			dev_err(&client->dev, "could not get clock %s\n",
+				sensor->platform_data->ext_clk_name);
+			rval = -ENODEV;
+			goto out_clk_get;
+		}
+
+		rval = clk_set_rate(sensor->ext_clk,
+				    sensor->platform_data->ext_clk);
+		if (rval < 0) {
+			dev_err(&client->dev,
+				"unable to set clock %s freq to %u\n",
+				sensor->platform_data->ext_clk_name,
+				sensor->platform_data->ext_clk);
+			rval = -ENODEV;
+			goto out_clk_set_rate;
+		}
+	}
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN) {
+		if (gpio_request_one(sensor->platform_data->xshutdown, 0,
+				     "SMIA++ xshutdown") != 0) {
+			dev_err(&client->dev,
+				"unable to acquire reset gpio %d\n",
+				sensor->platform_data->xshutdown);
+			rval = -ENODEV;
+			goto out_clk_set_rate;
+		}
+	}
+
+	rval = smiapp_power_on(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_smiapp_power_on;
+	}
+
+	rval = smiapp_identify_module(subdev);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	rval = smiapp_get_all_limits(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	/*
+	 * Handle Sensor Module orientation on the board.
+	 *
+	 * The application of H-FLIP and V-FLIP on the sensor is modified by
+	 * the sensor orientation on the board.
+	 *
+	 * For SMIAPP_BOARD_SENSOR_ORIENT_180 the default behaviour is to set
+	 * both H-FLIP and V-FLIP for normal operation which also implies
+	 * that a set/unset operation for user space HFLIP and VFLIP v4l2
+	 * controls will need to be internally inverted.
+	 *
+	 * Rotation also changes the bayer pattern.
+	 */
+	if (sensor->platform_data->module_board_orient ==
+	    SMIAPP_MODULE_BOARD_ORIENT_180)
+		sensor->hvflip_inv_mask = SMIAPP_IMAGE_ORIENTATION_HFLIP |
+					  SMIAPP_IMAGE_ORIENTATION_VFLIP;
+
+	rval = smiapp_get_mbus_formats(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY]) {
+		u32 val;
+
+		rval = smiapp_read(sensor,
+				   SMIAPP_REG_U8_BINNING_SUBTYPES, &val);
+		if (rval < 0) {
+			rval = -ENODEV;
+			goto out_power_off;
+		}
+		sensor->nbinning_subtypes = min_t(u8, val,
+						  SMIAPP_BINNING_SUBTYPES);
+
+		for (i = 0; i < sensor->nbinning_subtypes; i++) {
+			rval = smiapp_read(
+				sensor, SMIAPP_REG_U8_BINNING_TYPE_n(i), &val);
+			if (rval < 0) {
+				rval = -ENODEV;
+				goto out_power_off;
+			}
+			sensor->binning_subtypes[i] =
+				*(struct smiapp_binning_subtype *)&val;
+
+			dev_dbg(&client->dev, "binning %xx%x\n",
+				sensor->binning_subtypes[i].horizontal,
+				sensor->binning_subtypes[i].vertical);
+		}
+	}
+	sensor->binning_horizontal = 1;
+	sensor->binning_vertical = 1;
+
+	/* SMIA++ NVM initialization - it will be read from the sensor
+	 * when it is first requested by userspace.
+	 */
+	if (sensor->minfo.smiapp_version && sensor->platform_data->nvm_size) {
+		sensor->nvm = kzalloc(sensor->platform_data->nvm_size,
+				      GFP_KERNEL);
+		if (sensor->nvm == NULL) {
+			dev_err(&client->dev, "nvm buf allocation failed\n");
+			rval = -ENOMEM;
+			goto out_power_off;
+		}
+
+		if (device_create_file(&client->dev, &dev_attr_nvm) != 0) {
+			dev_err(&client->dev, "sysfs nvm entry failed\n");
+			rval = -EBUSY;
+			goto out_power_off;
+		}
+	}
+
+	rval = smiapp_call_quirk(sensor, limits);
+	if (rval) {
+		dev_err(&client->dev, "limits quirks failed\n");
+		goto out_nvm_release;
+	}
+
+	/* We consider this as profile 0 sensor if any of these are zero. */
+	if (!sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV]) {
+		sensor->minfo.smiapp_profile = SMIAPP_PROFILE_0;
+	} else if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		   != SMIAPP_SCALING_CAPABILITY_NONE) {
+		if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
+			sensor->minfo.smiapp_profile = SMIAPP_PROFILE_1;
+		else
+			sensor->minfo.smiapp_profile = SMIAPP_PROFILE_2;
+		sensor->scaler = &sensor->ssds[sensor->ssds_used];
+		sensor->ssds_used++;
+	} else if (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+		   == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
+		sensor->scaler = &sensor->ssds[sensor->ssds_used];
+		sensor->ssds_used++;
+	}
+	sensor->binner = &sensor->ssds[sensor->ssds_used];
+	sensor->ssds_used++;
+	sensor->pixel_array = &sensor->ssds[sensor->ssds_used];
+	sensor->ssds_used++;
+
+	sensor->scale_m = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+
+	for (i = 0; i < SMIAPP_SUBDEVS; i++) {
+		struct {
+			struct smiapp_subdev *ssd;
+			char *name;
+		} const __this[] = {
+			{ sensor->scaler, "scaler", },
+			{ sensor->binner, "binner", },
+			{ sensor->pixel_array, "pixel array", },
+		}, *_this = &__this[i];
+		struct smiapp_subdev *this = _this->ssd;
+
+		if (!this)
+			continue;
+
+		if (this != sensor->src)
+			v4l2_subdev_init(&this->sd, &smiapp_ops);
+
+		this->sensor = sensor;
+
+		if (this == sensor->pixel_array) {
+			this->npads = 1;
+		} else {
+			this->npads = 2;
+			this->source_pad = 1;
+		}
+
+		snprintf(this->sd.name,
+			 sizeof(this->sd.name), "%s %s",
+			 sensor->minfo.name, _this->name);
+
+		this->sink_fmt.width =
+			sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+		this->sink_fmt.height =
+			sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		this->compose.width = this->sink_fmt.width;
+		this->compose.height = this->sink_fmt.height;
+		this->crop[this->source_pad] = this->compose;
+		this->pads[this->source_pad].flags = MEDIA_PAD_FL_SOURCE;
+		if (this != sensor->pixel_array) {
+			this->crop[this->sink_pad] = this->compose;
+			this->pads[this->sink_pad].flags = MEDIA_PAD_FL_SINK;
+		}
+
+		this->sd.entity.ops = &smiapp_entity_ops;
+
+		if (last == NULL) {
+			last = this;
+			continue;
+		}
+
+		this->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+		this->sd.internal_ops = &smiapp_internal_ops;
+		this->sd.owner = NULL;
+		v4l2_set_subdevdata(&this->sd, client);
+
+		rval = media_entity_init(&this->sd.entity,
+					 this->npads, this->pads, 0);
+		if (rval) {
+			dev_err(&client->dev,
+				"media_entity_init failed\n");
+			goto out_nvm_release;
+		}
+
+		rval = media_entity_create_link(&this->sd.entity,
+						this->source_pad,
+						&last->sd.entity,
+						last->sink_pad,
+						MEDIA_LNK_FL_ENABLED |
+						MEDIA_LNK_FL_IMMUTABLE);
+		if (rval) {
+			dev_err(&client->dev,
+				"media_entity_create_link failed\n");
+			goto out_nvm_release;
+		}
+
+		rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev,
+						   &this->sd);
+		if (rval) {
+			dev_err(&client->dev,
+				"v4l2_device_register_subdev failed\n");
+			goto out_nvm_release;
+		}
+
+		last = this;
+	}
+
+	dev_dbg(&client->dev, "profile %d\n", sensor->minfo.smiapp_profile);
+
+	sensor->pixel_array->sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+
+	/* final steps */
+	smiapp_read_frame_fmt(sensor);
+	rval = smiapp_init_controls(sensor);
+	if (rval < 0)
+		goto out_nvm_release;
+
+	rval = smiapp_update_mode(sensor);
+	if (rval) {
+		dev_err(&client->dev, "update mode failed\n");
+		goto out_nvm_release;
+	}
+
+	sensor->streaming = false;
+	sensor->dev_init_done = true;
+
+	/* check flash capability */
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FLASH_MODE_CAPABILITY, &tmp);
+	sensor->flash_capability = tmp;
+	if (rval)
+		goto out_nvm_release;
+
+	smiapp_power_off(sensor);
+
+	return 0;
+
+out_nvm_release:
+	device_remove_file(&client->dev, &dev_attr_nvm);
+
+out_power_off:
+	kfree(sensor->nvm);
+	sensor->nvm = NULL;
+	smiapp_power_off(sensor);
+
+out_smiapp_power_on:
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_free(sensor->platform_data->xshutdown);
+
+out_clk_set_rate:
+	clk_put(sensor->ext_clk);
+	sensor->ext_clk = NULL;
+
+out_clk_get:
+	regulator_put(sensor->vana);
+	sensor->vana = NULL;
+	return rval;
+}
+
+static int smiapp_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(sd);
+	struct smiapp_sensor *sensor = ssd->sensor;
+	u32 mbus_code =
+		smiapp_csi_data_formats[smiapp_pixel_order(sensor)].code;
+	unsigned int i;
+
+	mutex_lock(&sensor->mutex);
+
+	for (i = 0; i < ssd->npads; i++) {
+		struct v4l2_mbus_framefmt *try_fmt =
+			v4l2_subdev_get_try_format(fh, i);
+		struct v4l2_rect *try_crop = v4l2_subdev_get_try_crop(fh, i);
+		struct v4l2_rect *try_comp;
+
+		try_fmt->width = sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+		try_fmt->height = sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		try_fmt->code = mbus_code;
+
+		try_crop->top = 0;
+		try_crop->left = 0;
+		try_crop->width = try_fmt->width;
+		try_crop->height = try_fmt->height;
+
+		if (ssd != sensor->pixel_array)
+			continue;
+
+		try_comp = v4l2_subdev_get_try_compose(fh, i);
+		*try_comp = *try_crop;
+	}
+
+	mutex_unlock(&sensor->mutex);
+
+	return smiapp_set_power(sd, 1);
+}
+
+static int smiapp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return smiapp_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_video_ops smiapp_video_ops = {
+	.s_stream = smiapp_set_stream,
+};
+
+static const struct v4l2_subdev_core_ops smiapp_core_ops = {
+	.s_power = smiapp_set_power,
+};
+
+static const struct v4l2_subdev_pad_ops smiapp_pad_ops = {
+	.enum_mbus_code = smiapp_enum_mbus_code,
+	.get_fmt = smiapp_get_format,
+	.set_fmt = smiapp_set_format,
+	.get_selection = smiapp_get_selection,
+	.set_selection = smiapp_set_selection,
+};
+
+static const struct v4l2_subdev_sensor_ops smiapp_sensor_ops = {
+	.g_skip_frames = smiapp_get_skip_frames,
+};
+
+static const struct v4l2_subdev_ops smiapp_ops = {
+	.core = &smiapp_core_ops,
+	.video = &smiapp_video_ops,
+	.pad = &smiapp_pad_ops,
+	.sensor = &smiapp_sensor_ops,
+};
+
+static const struct media_entity_operations smiapp_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops smiapp_internal_src_ops = {
+	.registered = smiapp_registered,
+	.open = smiapp_open,
+	.close = smiapp_close,
+};
+
+static const struct v4l2_subdev_internal_ops smiapp_internal_ops = {
+	.open = smiapp_open,
+	.close = smiapp_close,
+};
+
+/* -----------------------------------------------------------------------------
+ * I2C Driver
+ */
+
+#ifdef CONFIG_PM
+
+static int smiapp_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	bool streaming;
+
+	BUG_ON(mutex_is_locked(&sensor->mutex));
+
+	if (sensor->power_count == 0)
+		return 0;
+
+	if (sensor->streaming)
+		smiapp_stop_streaming(sensor);
+
+	streaming = sensor->streaming;
+
+	smiapp_power_off(sensor);
+
+	/* save state for resume */
+	sensor->streaming = streaming;
+
+	return 0;
+}
+
+static int smiapp_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	if (sensor->power_count == 0)
+		return 0;
+
+	rval = smiapp_power_on(sensor);
+	if (rval)
+		return rval;
+
+	if (sensor->streaming)
+		rval = smiapp_start_streaming(sensor);
+
+	return rval;
+}
+
+#else
+
+#define smiapp_suspend	NULL
+#define smiapp_resume	NULL
+
+#endif /* CONFIG_PM */
+
+static int smiapp_probe(struct i2c_client *client,
+			const struct i2c_device_id *devid)
+{
+	struct smiapp_sensor *sensor;
+	int rval;
+
+	if (client->dev.platform_data == NULL)
+		return -ENODEV;
+
+	sensor = kzalloc(sizeof(*sensor), GFP_KERNEL);
+	if (sensor == NULL)
+		return -ENOMEM;
+
+	sensor->platform_data = client->dev.platform_data;
+	mutex_init(&sensor->mutex);
+	mutex_init(&sensor->power_mutex);
+	sensor->src = &sensor->ssds[sensor->ssds_used];
+
+	v4l2_i2c_subdev_init(&sensor->src->sd, client, &smiapp_ops);
+	sensor->src->sd.internal_ops = &smiapp_internal_src_ops;
+	sensor->src->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->src->sensor = sensor;
+
+	sensor->src->pads[0].flags = MEDIA_PAD_FL_SOURCE;
+	rval = media_entity_init(&sensor->src->sd.entity, 2,
+				 sensor->src->pads, 0);
+	if (rval < 0)
+		kfree(sensor);
+
+	return rval;
+}
+
+static int __exit smiapp_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+
+	if (sensor->power_count) {
+		if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+			gpio_set_value(sensor->platform_data->xshutdown, 0);
+		if (sensor->platform_data->set_xclk)
+			sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+		else
+			clk_disable(sensor->ext_clk);
+		sensor->power_count = 0;
+	}
+
+	if (sensor->nvm) {
+		device_remove_file(&client->dev, &dev_attr_nvm);
+		kfree(sensor->nvm);
+	}
+
+	for (i = 0; i < sensor->ssds_used; i++) {
+		media_entity_cleanup(&sensor->ssds[i].sd.entity);
+		v4l2_device_unregister_subdev(&sensor->ssds[i].sd);
+	}
+	smiapp_free_controls(sensor);
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_free(sensor->platform_data->xshutdown);
+	if (sensor->ext_clk)
+		clk_put(sensor->ext_clk);
+	if (sensor->vana)
+		regulator_put(sensor->vana);
+
+	kfree(sensor);
+
+	return 0;
+}
+
+static const struct i2c_device_id smiapp_id_table[] = {
+	{ SMIAPP_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, smiapp_id_table);
+
+static const struct dev_pm_ops smiapp_pm_ops = {
+	.suspend	= smiapp_suspend,
+	.resume		= smiapp_resume,
+};
+
+static struct i2c_driver smiapp_i2c_driver = {
+	.driver	= {
+		.name = SMIAPP_NAME,
+		.pm = &smiapp_pm_ops,
+	},
+	.probe	= smiapp_probe,
+	.remove	= __exit_p(smiapp_remove),
+	.id_table = smiapp_id_table,
+};
+
+module_i2c_driver(smiapp_i2c_driver);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>");
+MODULE_DESCRIPTION("Generic SMIA/SMIA++ camera module driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/smiapp/smiapp-limits.c b/drivers/media/video/smiapp/smiapp-limits.c
new file mode 100644
index 000000000000..0800e095724e
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-limits.c
@@ -0,0 +1,132 @@
+/*
+ * drivers/media/video/smiapp/smiapp-limits.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include "smiapp.h"
+
+struct smiapp_reg_limits smiapp_reg_limits[] = {
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CAPABILITY, "analogue_gain_capability" }, /* 0 */
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MIN, "analogue_gain_code_min" },
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MAX, "analogue_gain_code_max" },
+	{ SMIAPP_REG_U8_THS_ZERO_MIN, "ths_zero_min" },
+	{ SMIAPP_REG_U8_TCLK_TRAIL_MIN, "tclk_trail_min" },
+	{ SMIAPP_REG_U16_INTEGRATION_TIME_CAPABILITY, "integration_time_capability" }, /* 5 */
+	{ SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MIN, "coarse_integration_time_min" },
+	{ SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MAX_MARGIN, "coarse_integration_time_max_margin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN, "fine_integration_time_min" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN, "fine_integration_time_max_margin" },
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_CAPABILITY, "digital_gain_capability" }, /* 10 */
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_MIN, "digital_gain_min" },
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_MAX, "digital_gain_max" },
+	{ SMIAPP_REG_F32_MIN_EXT_CLK_FREQ_HZ, "min_ext_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_EXT_CLK_FREQ_HZ, "max_ext_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_PRE_PLL_CLK_DIV, "min_pre_pll_clk_div" }, /* 15 */
+	{ SMIAPP_REG_U16_MAX_PRE_PLL_CLK_DIV, "max_pre_pll_clk_div" },
+	{ SMIAPP_REG_F32_MIN_PLL_IP_FREQ_HZ, "min_pll_ip_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_PLL_IP_FREQ_HZ, "max_pll_ip_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_PLL_MULTIPLIER, "min_pll_multiplier" },
+	{ SMIAPP_REG_U16_MAX_PLL_MULTIPLIER, "max_pll_multiplier" }, /* 20 */
+	{ SMIAPP_REG_F32_MIN_PLL_OP_FREQ_HZ, "min_pll_op_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_PLL_OP_FREQ_HZ, "max_pll_op_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_VT_SYS_CLK_DIV, "min_vt_sys_clk_div" },
+	{ SMIAPP_REG_U16_MAX_VT_SYS_CLK_DIV, "max_vt_sys_clk_div" },
+	{ SMIAPP_REG_F32_MIN_VT_SYS_CLK_FREQ_HZ, "min_vt_sys_clk_freq_hz" }, /* 25 */
+	{ SMIAPP_REG_F32_MAX_VT_SYS_CLK_FREQ_HZ, "max_vt_sys_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MIN_VT_PIX_CLK_FREQ_HZ, "min_vt_pix_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_VT_PIX_CLK_FREQ_HZ, "max_vt_pix_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_VT_PIX_CLK_DIV, "min_vt_pix_clk_div" },
+	{ SMIAPP_REG_U16_MAX_VT_PIX_CLK_DIV, "max_vt_pix_clk_div" }, /* 30 */
+	{ SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES, "min_frame_length_lines" },
+	{ SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES, "max_frame_length_lines" },
+	{ SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK, "min_line_length_pck" },
+	{ SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK, "max_line_length_pck" },
+	{ SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK, "min_line_blanking_pck" }, /* 35 */
+	{ SMIAPP_REG_U16_MIN_FRAME_BLANKING_LINES, "min_frame_blanking_lines" },
+	{ SMIAPP_REG_U8_MIN_LINE_LENGTH_PCK_STEP_SIZE, "min_line_length_pck_step_size" },
+	{ SMIAPP_REG_U16_MIN_OP_SYS_CLK_DIV, "min_op_sys_clk_div" },
+	{ SMIAPP_REG_U16_MAX_OP_SYS_CLK_DIV, "max_op_sys_clk_div" },
+	{ SMIAPP_REG_F32_MIN_OP_SYS_CLK_FREQ_HZ, "min_op_sys_clk_freq_hz" }, /* 40 */
+	{ SMIAPP_REG_F32_MAX_OP_SYS_CLK_FREQ_HZ, "max_op_sys_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_OP_PIX_CLK_DIV, "min_op_pix_clk_div" },
+	{ SMIAPP_REG_U16_MAX_OP_PIX_CLK_DIV, "max_op_pix_clk_div" },
+	{ SMIAPP_REG_F32_MIN_OP_PIX_CLK_FREQ_HZ, "min_op_pix_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_OP_PIX_CLK_FREQ_HZ, "max_op_pix_clk_freq_hz" }, /* 45 */
+	{ SMIAPP_REG_U16_X_ADDR_MIN, "x_addr_min" },
+	{ SMIAPP_REG_U16_Y_ADDR_MIN, "y_addr_min" },
+	{ SMIAPP_REG_U16_X_ADDR_MAX, "x_addr_max" },
+	{ SMIAPP_REG_U16_Y_ADDR_MAX, "y_addr_max" },
+	{ SMIAPP_REG_U16_MIN_X_OUTPUT_SIZE, "min_x_output_size" }, /* 50 */
+	{ SMIAPP_REG_U16_MIN_Y_OUTPUT_SIZE, "min_y_output_size" },
+	{ SMIAPP_REG_U16_MAX_X_OUTPUT_SIZE, "max_x_output_size" },
+	{ SMIAPP_REG_U16_MAX_Y_OUTPUT_SIZE, "max_y_output_size" },
+	{ SMIAPP_REG_U16_MIN_EVEN_INC, "min_even_inc" },
+	{ SMIAPP_REG_U16_MAX_EVEN_INC, "max_even_inc" }, /* 55 */
+	{ SMIAPP_REG_U16_MIN_ODD_INC, "min_odd_inc" },
+	{ SMIAPP_REG_U16_MAX_ODD_INC, "max_odd_inc" },
+	{ SMIAPP_REG_U16_SCALING_CAPABILITY, "scaling_capability" },
+	{ SMIAPP_REG_U16_SCALER_M_MIN, "scaler_m_min" },
+	{ SMIAPP_REG_U16_SCALER_M_MAX, "scaler_m_max" }, /* 60 */
+	{ SMIAPP_REG_U16_SCALER_N_MIN, "scaler_n_min" },
+	{ SMIAPP_REG_U16_SCALER_N_MAX, "scaler_n_max" },
+	{ SMIAPP_REG_U16_SPATIAL_SAMPLING_CAPABILITY, "spatial_sampling_capability" },
+	{ SMIAPP_REG_U8_DIGITAL_CROP_CAPABILITY, "digital_crop_capability" },
+	{ SMIAPP_REG_U16_COMPRESSION_CAPABILITY, "compression_capability" }, /* 65 */
+	{ SMIAPP_REG_U8_FIFO_SUPPORT_CAPABILITY, "fifo_support_capability" },
+	{ SMIAPP_REG_U8_DPHY_CTRL_CAPABILITY, "dphy_ctrl_capability" },
+	{ SMIAPP_REG_U8_CSI_LANE_MODE_CAPABILITY, "csi_lane_mode_capability" },
+	{ SMIAPP_REG_U8_CSI_SIGNALLING_MODE_CAPABILITY, "csi_signalling_mode_capability" },
+	{ SMIAPP_REG_U8_FAST_STANDBY_CAPABILITY, "fast_standby_capability" }, /* 70 */
+	{ SMIAPP_REG_U8_CCI_ADDRESS_CONTROL_CAPABILITY, "cci_address_control_capability" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS, "max_per_lane_bitrate_1_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS, "max_per_lane_bitrate_2_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS, "max_per_lane_bitrate_3_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS, "max_per_lane_bitrate_4_lane_mode_mbps" }, /* 75 */
+	{ SMIAPP_REG_U8_TEMP_SENSOR_CAPABILITY, "temp_sensor_capability" },
+	{ SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES_BIN, "min_frame_length_lines_bin" },
+	{ SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES_BIN, "max_frame_length_lines_bin" },
+	{ SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK_BIN, "min_line_length_pck_bin" },
+	{ SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK_BIN, "max_line_length_pck_bin" }, /* 80 */
+	{ SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK_BIN, "min_line_blanking_pck_bin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN_BIN, "fine_integration_time_min_bin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN, "fine_integration_time_max_margin_bin" },
+	{ SMIAPP_REG_U8_BINNING_CAPABILITY, "binning_capability" },
+	{ SMIAPP_REG_U8_BINNING_WEIGHTING_CAPABILITY, "binning_weighting_capability" }, /* 85 */
+	{ SMIAPP_REG_U8_DATA_TRANSFER_IF_CAPABILITY, "data_transfer_if_capability" },
+	{ SMIAPP_REG_U8_SHADING_CORRECTION_CAPABILITY, "shading_correction_capability" },
+	{ SMIAPP_REG_U8_GREEN_IMBALANCE_CAPABILITY, "green_imbalance_capability" },
+	{ SMIAPP_REG_U8_BLACK_LEVEL_CAPABILITY, "black_level_capability" },
+	{ SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_CAPABILITY, "module_specific_correction_capability" }, /* 90 */
+	{ SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY, "defect_correction_capability" },
+	{ SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY_2, "defect_correction_capability_2" },
+	{ SMIAPP_REG_U8_EDOF_CAPABILITY, "edof_capability" },
+	{ SMIAPP_REG_U8_COLOUR_FEEDBACK_CAPABILITY, "colour_feedback_capability" },
+	{ SMIAPP_REG_U8_ESTIMATION_MODE_CAPABILITY, "estimation_mode_capability" }, /* 95 */
+	{ SMIAPP_REG_U8_ESTIMATION_ZONE_CAPABILITY, "estimation_zone_capability" },
+	{ SMIAPP_REG_U16_CAPABILITY_TRDY_MIN, "capability_trdy_min" },
+	{ SMIAPP_REG_U8_FLASH_MODE_CAPABILITY, "flash_mode_capability" },
+	{ SMIAPP_REG_U8_ACTUATOR_CAPABILITY, "actuator_capability" },
+	{ SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_1, "bracketing_lut_capability_1" }, /* 100 */
+	{ SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_2, "bracketing_lut_capability_2" },
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_STEP, "analogue_gain_code_step" },
+	{ 0, NULL },
+};
diff --git a/drivers/media/video/smiapp/smiapp-limits.h b/drivers/media/video/smiapp/smiapp-limits.h
new file mode 100644
index 000000000000..7f4836bb78db
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-limits.h
@@ -0,0 +1,128 @@
+/*
+ * drivers/media/video/smiapp/smiapp-limits.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CAPABILITY			0
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN			1
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX			2
+#define SMIAPP_LIMIT_THS_ZERO_MIN				3
+#define SMIAPP_LIMIT_TCLK_TRAIL_MIN				4
+#define SMIAPP_LIMIT_INTEGRATION_TIME_CAPABILITY		5
+#define SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MIN		6
+#define SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN		7
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN			8
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN		9
+#define SMIAPP_LIMIT_DIGITAL_GAIN_CAPABILITY			10
+#define SMIAPP_LIMIT_DIGITAL_GAIN_MIN				11
+#define SMIAPP_LIMIT_DIGITAL_GAIN_MAX				12
+#define SMIAPP_LIMIT_MIN_EXT_CLK_FREQ_HZ			13
+#define SMIAPP_LIMIT_MAX_EXT_CLK_FREQ_HZ			14
+#define SMIAPP_LIMIT_MIN_PRE_PLL_CLK_DIV			15
+#define SMIAPP_LIMIT_MAX_PRE_PLL_CLK_DIV			16
+#define SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ				17
+#define SMIAPP_LIMIT_MAX_PLL_IP_FREQ_HZ				18
+#define SMIAPP_LIMIT_MIN_PLL_MULTIPLIER				19
+#define SMIAPP_LIMIT_MAX_PLL_MULTIPLIER				20
+#define SMIAPP_LIMIT_MIN_PLL_OP_FREQ_HZ				21
+#define SMIAPP_LIMIT_MAX_PLL_OP_FREQ_HZ				22
+#define SMIAPP_LIMIT_MIN_VT_SYS_CLK_DIV				23
+#define SMIAPP_LIMIT_MAX_VT_SYS_CLK_DIV				24
+#define SMIAPP_LIMIT_MIN_VT_SYS_CLK_FREQ_HZ			25
+#define SMIAPP_LIMIT_MAX_VT_SYS_CLK_FREQ_HZ			26
+#define SMIAPP_LIMIT_MIN_VT_PIX_CLK_FREQ_HZ			27
+#define SMIAPP_LIMIT_MAX_VT_PIX_CLK_FREQ_HZ			28
+#define SMIAPP_LIMIT_MIN_VT_PIX_CLK_DIV				29
+#define SMIAPP_LIMIT_MAX_VT_PIX_CLK_DIV				30
+#define SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES			31
+#define SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES			32
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK			33
+#define SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK			34
+#define SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK			35
+#define SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES			36
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_STEP_SIZE		37
+#define SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV				38
+#define SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV				39
+#define SMIAPP_LIMIT_MIN_OP_SYS_CLK_FREQ_HZ			40
+#define SMIAPP_LIMIT_MAX_OP_SYS_CLK_FREQ_HZ			41
+#define SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV				42
+#define SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV				43
+#define SMIAPP_LIMIT_MIN_OP_PIX_CLK_FREQ_HZ			44
+#define SMIAPP_LIMIT_MAX_OP_PIX_CLK_FREQ_HZ			45
+#define SMIAPP_LIMIT_X_ADDR_MIN					46
+#define SMIAPP_LIMIT_Y_ADDR_MIN					47
+#define SMIAPP_LIMIT_X_ADDR_MAX					48
+#define SMIAPP_LIMIT_Y_ADDR_MAX					49
+#define SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE				50
+#define SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE				51
+#define SMIAPP_LIMIT_MAX_X_OUTPUT_SIZE				52
+#define SMIAPP_LIMIT_MAX_Y_OUTPUT_SIZE				53
+#define SMIAPP_LIMIT_MIN_EVEN_INC				54
+#define SMIAPP_LIMIT_MAX_EVEN_INC				55
+#define SMIAPP_LIMIT_MIN_ODD_INC				56
+#define SMIAPP_LIMIT_MAX_ODD_INC				57
+#define SMIAPP_LIMIT_SCALING_CAPABILITY				58
+#define SMIAPP_LIMIT_SCALER_M_MIN				59
+#define SMIAPP_LIMIT_SCALER_M_MAX				60
+#define SMIAPP_LIMIT_SCALER_N_MIN				61
+#define SMIAPP_LIMIT_SCALER_N_MAX				62
+#define SMIAPP_LIMIT_SPATIAL_SAMPLING_CAPABILITY		63
+#define SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY			64
+#define SMIAPP_LIMIT_COMPRESSION_CAPABILITY			65
+#define SMIAPP_LIMIT_FIFO_SUPPORT_CAPABILITY			66
+#define SMIAPP_LIMIT_DPHY_CTRL_CAPABILITY			67
+#define SMIAPP_LIMIT_CSI_LANE_MODE_CAPABILITY			68
+#define SMIAPP_LIMIT_CSI_SIGNALLING_MODE_CAPABILITY		69
+#define SMIAPP_LIMIT_FAST_STANDBY_CAPABILITY			70
+#define SMIAPP_LIMIT_CCI_ADDRESS_CONTROL_CAPABILITY		71
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS	72
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS	73
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS	74
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS	75
+#define SMIAPP_LIMIT_TEMP_SENSOR_CAPABILITY			76
+#define SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN			77
+#define SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN			78
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN			79
+#define SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN			80
+#define SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN			81
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN_BIN		82
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN	83
+#define SMIAPP_LIMIT_BINNING_CAPABILITY				84
+#define SMIAPP_LIMIT_BINNING_WEIGHTING_CAPABILITY		85
+#define SMIAPP_LIMIT_DATA_TRANSFER_IF_CAPABILITY		86
+#define SMIAPP_LIMIT_SHADING_CORRECTION_CAPABILITY		87
+#define SMIAPP_LIMIT_GREEN_IMBALANCE_CAPABILITY			88
+#define SMIAPP_LIMIT_BLACK_LEVEL_CAPABILITY			89
+#define SMIAPP_LIMIT_MODULE_SPECIFIC_CORRECTION_CAPABILITY	90
+#define SMIAPP_LIMIT_DEFECT_CORRECTION_CAPABILITY		91
+#define SMIAPP_LIMIT_DEFECT_CORRECTION_CAPABILITY_2		92
+#define SMIAPP_LIMIT_EDOF_CAPABILITY				93
+#define SMIAPP_LIMIT_COLOUR_FEEDBACK_CAPABILITY			94
+#define SMIAPP_LIMIT_ESTIMATION_MODE_CAPABILITY			95
+#define SMIAPP_LIMIT_ESTIMATION_ZONE_CAPABILITY			96
+#define SMIAPP_LIMIT_CAPABILITY_TRDY_MIN			97
+#define SMIAPP_LIMIT_FLASH_MODE_CAPABILITY			98
+#define SMIAPP_LIMIT_ACTUATOR_CAPABILITY			99
+#define SMIAPP_LIMIT_BRACKETING_LUT_CAPABILITY_1		100
+#define SMIAPP_LIMIT_BRACKETING_LUT_CAPABILITY_2		101
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_STEP			102
+#define SMIAPP_LIMIT_LAST					103
diff --git a/drivers/media/video/smiapp/smiapp-quirk.c b/drivers/media/video/smiapp/smiapp-quirk.c
new file mode 100644
index 000000000000..55e87950dcea
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-quirk.c
@@ -0,0 +1,306 @@
+/*
+ * drivers/media/video/smiapp/smiapp-quirk.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/delay.h>
+
+#include "smiapp.h"
+
+static int smiapp_write_8(struct smiapp_sensor *sensor, u16 reg, u8 val)
+{
+	return smiapp_write(sensor, (SMIA_REG_8BIT << 16) | reg, val);
+}
+
+static int smiapp_write_8s(struct smiapp_sensor *sensor,
+			   struct smiapp_reg_8 *regs, int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	for (; len > 0; len--, regs++) {
+		rval = smiapp_write_8(sensor, regs->reg, regs->val);
+		if (rval < 0) {
+			dev_err(&client->dev,
+				"error %d writing reg 0x%4.4x, val 0x%2.2x",
+				rval, regs->reg, regs->val);
+			return rval;
+		}
+	}
+
+	return 0;
+}
+
+void smiapp_replace_limit(struct smiapp_sensor *sensor,
+			  u32 limit, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+
+	dev_dbg(&client->dev, "quirk: 0x%8.8x \"%s\" = %d, 0x%x\n",
+		smiapp_reg_limits[limit].addr,
+		smiapp_reg_limits[limit].what, val, val);
+	sensor->limits[limit] = val;
+}
+
+int smiapp_replace_limit_at(struct smiapp_sensor *sensor,
+			    u32 reg, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int i;
+
+	for (i = 0; smiapp_reg_limits[i].addr; i++) {
+		if ((smiapp_reg_limits[i].addr & 0xffff) != reg)
+			continue;
+
+		smiapp_replace_limit(sensor, i, val);
+
+		return 0;
+	}
+
+	dev_dbg(&client->dev, "quirk: bad register 0x%4.4x\n", reg);
+
+	return -EINVAL;
+}
+
+bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
+		      u32 reg, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	const struct smia_reg *sreg;
+
+	if (!sensor->minfo.quirk)
+		return false;
+
+	sreg = sensor->minfo.quirk->regs;
+
+	if (!sreg)
+		return false;
+
+	while (sreg->type) {
+		u16 type = reg >> 16;
+		u16 reg16 = reg;
+
+		if (sreg->type != type || sreg->reg != reg16) {
+			sreg++;
+			continue;
+		}
+
+		switch ((u8)type) {
+		case SMIA_REG_8BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%2.2x\n",
+				reg, sreg->val);
+			break;
+		case SMIA_REG_16BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%4.4x\n",
+				reg, sreg->val);
+			break;
+		case SMIA_REG_32BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%8.8x\n",
+				reg, sreg->val);
+			break;
+		}
+
+		*val = sreg->val;
+
+		return true;
+	}
+
+	return false;
+}
+
+static int jt8ew9_limits(struct smiapp_sensor *sensor)
+{
+	if (sensor->minfo.revision_number_major < 0x03)
+		sensor->frame_skip = 1;
+
+	/* Below 24 gain doesn't have effect at all, */
+	/* but ~59 is needed for full dynamic range */
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN, 59);
+	smiapp_replace_limit(
+		sensor, SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX, 6000);
+
+	return 0;
+}
+
+static int jt8ew9_post_poweron(struct smiapp_sensor *sensor)
+{
+	struct smiapp_reg_8 regs[] = {
+		{ 0x30a3, 0xd8 }, /* Output port control : LVDS ports only */
+		{ 0x30ae, 0x00 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
+		{ 0x30af, 0xd0 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
+		{ 0x322d, 0x04 }, /* Adjusting Processing Image Size to Scaler Toshiba Recommendation Setting */
+		{ 0x3255, 0x0f }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		{ 0x3256, 0x15 }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		{ 0x3258, 0x70 }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x3259, 0x70 }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x325f, 0x7c }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x3302, 0x06 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3304, 0x00 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3307, 0x22 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3308, 0x8d }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x331e, 0x0f }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3320, 0x30 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3321, 0x11 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3322, 0x98 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3323, 0x64 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3325, 0x83 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x3330, 0x18 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x333c, 0x01 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x3345, 0x2f }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x33de, 0x38 }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		/* Taken from v03. No idea what the rest are. */
+		{ 0x32e0, 0x05 },
+		{ 0x32e1, 0x05 },
+		{ 0x32e2, 0x04 },
+		{ 0x32e5, 0x04 },
+		{ 0x32e6, 0x04 },
+
+	};
+
+	return smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct smiapp_quirk smiapp_jt8ew9_quirk = {
+	.limits = jt8ew9_limits,
+	.post_poweron = jt8ew9_post_poweron,
+};
+
+static int imx125es_post_poweron(struct smiapp_sensor *sensor)
+{
+	/* Taken from v02. No idea what the other two are. */
+	struct smiapp_reg_8 regs[] = {
+		/*
+		 * 0x3302: clk during frame blanking:
+		 * 0x00 - HS mode, 0x01 - LP11
+		 */
+		{ 0x3302, 0x01 },
+		{ 0x302d, 0x00 },
+		{ 0x3b08, 0x8c },
+	};
+
+	return smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct smiapp_quirk smiapp_imx125es_quirk = {
+	.post_poweron = imx125es_post_poweron,
+};
+
+static int jt8ev1_limits(struct smiapp_sensor *sensor)
+{
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_X_ADDR_MAX, 4271);
+	smiapp_replace_limit(sensor,
+			     SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN, 184);
+
+	return 0;
+}
+
+static int jt8ev1_post_poweron(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	struct smiapp_reg_8 regs[] = {
+		{ 0x3031, 0xcd }, /* For digital binning (EQ_MONI) */
+		{ 0x30a3, 0xd0 }, /* FLASH STROBE enable */
+		{ 0x3237, 0x00 }, /* For control of pulse timing for ADC */
+		{ 0x3238, 0x43 },
+		{ 0x3301, 0x06 }, /* For analog bias for sensor */
+		{ 0x3302, 0x06 },
+		{ 0x3304, 0x00 },
+		{ 0x3305, 0x88 },
+		{ 0x332a, 0x14 },
+		{ 0x332c, 0x6b },
+		{ 0x3336, 0x01 },
+		{ 0x333f, 0x1f },
+		{ 0x3355, 0x00 },
+		{ 0x3356, 0x20 },
+		{ 0x33bf, 0x20 }, /* Adjust the FBC speed */
+		{ 0x33c9, 0x20 },
+		{ 0x33ce, 0x30 }, /* Adjust the parameter for logic function */
+		{ 0x33cf, 0xec }, /* For Black sun */
+		{ 0x3328, 0x80 }, /* Ugh. No idea what's this. */
+	};
+
+	struct smiapp_reg_8 regs_96[] = {
+		{ 0x30ae, 0x00 }, /* For control of ADC clock */
+		{ 0x30af, 0xd0 },
+		{ 0x30b0, 0x01 },
+	};
+
+	rval = smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+	if (rval < 0)
+		return rval;
+
+	switch (sensor->platform_data->ext_clk) {
+	case 9600000:
+		return smiapp_write_8s(sensor, regs_96,
+				       ARRAY_SIZE(regs_96));
+	default:
+		dev_warn(&client->dev, "no MSRs for %d Hz ext_clk\n",
+			 sensor->platform_data->ext_clk);
+		return 0;
+	}
+}
+
+static int jt8ev1_pre_streamon(struct smiapp_sensor *sensor)
+{
+	return smiapp_write_8(sensor, 0x3328, 0x00);
+}
+
+static int jt8ev1_post_streamoff(struct smiapp_sensor *sensor)
+{
+	int rval;
+
+	/* Workaround: allows fast standby to work properly */
+	rval = smiapp_write_8(sensor, 0x3205, 0x04);
+	if (rval < 0)
+		return rval;
+
+	/* Wait for 1 ms + one line => 2 ms is likely enough */
+	usleep_range(2000, 2000);
+
+	/* Restore it */
+	rval = smiapp_write_8(sensor, 0x3205, 0x00);
+	if (rval < 0)
+		return rval;
+
+	return smiapp_write_8(sensor, 0x3328, 0x80);
+}
+
+const struct smiapp_quirk smiapp_jt8ev1_quirk = {
+	.limits = jt8ev1_limits,
+	.post_poweron = jt8ev1_post_poweron,
+	.pre_streamon = jt8ev1_pre_streamon,
+	.post_streamoff = jt8ev1_post_streamoff,
+	.flags = SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE,
+};
+
+static int tcm8500md_limits(struct smiapp_sensor *sensor)
+{
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ, 2700000);
+
+	return 0;
+}
+
+const struct smiapp_quirk smiapp_tcm8500md_quirk = {
+	.limits = tcm8500md_limits,
+};
diff --git a/drivers/media/video/smiapp/smiapp-quirk.h b/drivers/media/video/smiapp/smiapp-quirk.h
new file mode 100644
index 000000000000..f4dcaabaefe7
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-quirk.h
@@ -0,0 +1,83 @@
+/*
+ * drivers/media/video/smiapp/smiapp-quirk.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_QUIRK__
+#define __SMIAPP_QUIRK__
+
+struct smiapp_sensor;
+
+/**
+ * struct smiapp_quirk - quirks for sensors that deviate from SMIA++ standard
+ *
+ * @limits: Replace sensor->limits with values which can't be read from
+ *	    sensor registers. Called the first time the sensor is powered up.
+ * @post_poweron: Called always after the sensor has been fully powered on.
+ * @pre_streamon: Called just before streaming is enabled.
+ * @post_streamon: Called right after stopping streaming.
+ */
+struct smiapp_quirk {
+	int (*limits)(struct smiapp_sensor *sensor);
+	int (*post_poweron)(struct smiapp_sensor *sensor);
+	int (*pre_streamon)(struct smiapp_sensor *sensor);
+	int (*post_streamoff)(struct smiapp_sensor *sensor);
+	const struct smia_reg *regs;
+	unsigned long flags;
+};
+
+/* op pix clock is for all lanes in total normally */
+#define SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE			(1 << 0)
+#define SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY			(1 << 1)
+
+struct smiapp_reg_8 {
+	u16 reg;
+	u8 val;
+};
+
+void smiapp_replace_limit(struct smiapp_sensor *sensor,
+			  u32 limit, u32 val);
+bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
+		      u32 reg, u32 *val);
+
+#define SMIAPP_MK_QUIRK_REG(_reg, _val) \
+	{				\
+		.type = (_reg >> 16),	\
+		.reg = (u16)_reg,	\
+		.val = _val,		\
+	}
+
+#define smiapp_call_quirk(_sensor, _quirk, ...)				\
+	(_sensor->minfo.quirk &&					\
+	 _sensor->minfo.quirk->_quirk ?					\
+	 _sensor->minfo.quirk->_quirk(_sensor, ##__VA_ARGS__) : 0)
+
+#define smiapp_needs_quirk(_sensor, _quirk)		\
+	(_sensor->minfo.quirk ?				\
+	 _sensor->minfo.quirk->flags & _quirk : 0)
+
+extern const struct smiapp_quirk smiapp_jt8ev1_quirk;
+extern const struct smiapp_quirk smiapp_imx125es_quirk;
+extern const struct smiapp_quirk smiapp_jt8ew9_quirk;
+extern const struct smiapp_quirk smiapp_tcm8500md_quirk;
+
+#endif /* __SMIAPP_QUIRK__ */
diff --git a/drivers/media/video/smiapp/smiapp-reg-defs.h b/drivers/media/video/smiapp/smiapp-reg-defs.h
new file mode 100644
index 000000000000..a089eb8161e1
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-reg-defs.h
@@ -0,0 +1,503 @@
+/*
+ * drivers/media/video/smiapp/smiapp-reg-defs.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+#define SMIAPP_REG_MK_U8(r) ((SMIA_REG_8BIT << 16) | (r))
+#define SMIAPP_REG_MK_U16(r) ((SMIA_REG_16BIT << 16) | (r))
+#define SMIAPP_REG_MK_U32(r) ((SMIA_REG_32BIT << 16) | (r))
+
+#define SMIAPP_REG_MK_F32(r) (SMIA_REG_FLAG_FLOAT | (SMIA_REG_32BIT << 16) | (r))
+
+#define SMIAPP_REG_U16_MODEL_ID					SMIAPP_REG_MK_U16(0x0000)
+#define SMIAPP_REG_U8_REVISION_NUMBER_MAJOR			SMIAPP_REG_MK_U8(0x0002)
+#define SMIAPP_REG_U8_MANUFACTURER_ID				SMIAPP_REG_MK_U8(0x0003)
+#define SMIAPP_REG_U8_SMIA_VERSION				SMIAPP_REG_MK_U8(0x0004)
+#define SMIAPP_REG_U8_FRAME_COUNT				SMIAPP_REG_MK_U8(0x0005)
+#define SMIAPP_REG_U8_PIXEL_ORDER				SMIAPP_REG_MK_U8(0x0006)
+#define SMIAPP_REG_U16_DATA_PEDESTAL				SMIAPP_REG_MK_U16(0x0008)
+#define SMIAPP_REG_U8_PIXEL_DEPTH				SMIAPP_REG_MK_U8(0x000c)
+#define SMIAPP_REG_U8_REVISION_NUMBER_MINOR			SMIAPP_REG_MK_U8(0x0010)
+#define SMIAPP_REG_U8_SMIAPP_VERSION				SMIAPP_REG_MK_U8(0x0011)
+#define SMIAPP_REG_U8_MODULE_DATE_YEAR				SMIAPP_REG_MK_U8(0x0012)
+#define SMIAPP_REG_U8_MODULE_DATE_MONTH				SMIAPP_REG_MK_U8(0x0013)
+#define SMIAPP_REG_U8_MODULE_DATE_DAY				SMIAPP_REG_MK_U8(0x0014)
+#define SMIAPP_REG_U8_MODULE_DATE_PHASE				SMIAPP_REG_MK_U8(0x0015)
+#define SMIAPP_REG_U16_SENSOR_MODEL_ID				SMIAPP_REG_MK_U16(0x0016)
+#define SMIAPP_REG_U8_SENSOR_REVISION_NUMBER			SMIAPP_REG_MK_U8(0x0018)
+#define SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID			SMIAPP_REG_MK_U8(0x0019)
+#define SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION			SMIAPP_REG_MK_U8(0x001a)
+#define SMIAPP_REG_U32_SERIAL_NUMBER				SMIAPP_REG_MK_U32(0x001c)
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE			SMIAPP_REG_MK_U8(0x0040)
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE		SMIAPP_REG_MK_U8(0x0041)
+#define SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(n)		SMIAPP_REG_MK_U16(0x0042 + ((n) << 1)) /* 0 <= n <= 14 */
+#define SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(n)		SMIAPP_REG_MK_U32(0x0060 + ((n) << 2)) /* 0 <= n <= 7 */
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CAPABILITY			SMIAPP_REG_MK_U16(0x0080)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MIN			SMIAPP_REG_MK_U16(0x0084)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MAX			SMIAPP_REG_MK_U16(0x0086)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_STEP			SMIAPP_REG_MK_U16(0x0088)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_TYPE			SMIAPP_REG_MK_U16(0x008a)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M0				SMIAPP_REG_MK_U16(0x008c)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C0				SMIAPP_REG_MK_U16(0x008e)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M1				SMIAPP_REG_MK_U16(0x0090)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C1				SMIAPP_REG_MK_U16(0x0092)
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE			SMIAPP_REG_MK_U8(0x00c0)
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_SUBTYPE			SMIAPP_REG_MK_U8(0x00c1)
+#define SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(n)		SMIAPP_REG_MK_U16(0x00c2 + ((n) << 1))
+#define SMIAPP_REG_U8_MODE_SELECT				SMIAPP_REG_MK_U8(0x0100)
+#define SMIAPP_REG_U8_IMAGE_ORIENTATION				SMIAPP_REG_MK_U8(0x0101)
+#define SMIAPP_REG_U8_SOFTWARE_RESET				SMIAPP_REG_MK_U8(0x0103)
+#define SMIAPP_REG_U8_GROUPED_PARAMETER_HOLD			SMIAPP_REG_MK_U8(0x0104)
+#define SMIAPP_REG_U8_MASK_CORRUPTED_FRAMES			SMIAPP_REG_MK_U8(0x0105)
+#define SMIAPP_REG_U8_FAST_STANDBY_CTRL				SMIAPP_REG_MK_U8(0x0106)
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL			SMIAPP_REG_MK_U8(0x0107)
+#define SMIAPP_REG_U8_2ND_CCI_IF_CONTROL			SMIAPP_REG_MK_U8(0x0108)
+#define SMIAPP_REG_U8_2ND_CCI_ADDRESS_CONTROL			SMIAPP_REG_MK_U8(0x0109)
+#define SMIAPP_REG_U8_CSI_CHANNEL_IDENTIFIER			SMIAPP_REG_MK_U8(0x0110)
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE			SMIAPP_REG_MK_U8(0x0111)
+#define SMIAPP_REG_U16_CSI_DATA_FORMAT				SMIAPP_REG_MK_U16(0x0112)
+#define SMIAPP_REG_U8_CSI_LANE_MODE				SMIAPP_REG_MK_U8(0x0114)
+#define SMIAPP_REG_U8_CSI2_10_TO_8_DT				SMIAPP_REG_MK_U8(0x0115)
+#define SMIAPP_REG_U8_CSI2_10_TO_7_DT				SMIAPP_REG_MK_U8(0x0116)
+#define SMIAPP_REG_U8_CSI2_10_TO_6_DT				SMIAPP_REG_MK_U8(0x0117)
+#define SMIAPP_REG_U8_CSI2_12_TO_8_DT				SMIAPP_REG_MK_U8(0x0118)
+#define SMIAPP_REG_U8_CSI2_12_TO_7_DT				SMIAPP_REG_MK_U8(0x0119)
+#define SMIAPP_REG_U8_CSI2_12_TO_6_DT				SMIAPP_REG_MK_U8(0x011a)
+#define SMIAPP_REG_U8_CSI2_14_TO_10_DT				SMIAPP_REG_MK_U8(0x011b)
+#define SMIAPP_REG_U8_CSI2_14_TO_8_DT				SMIAPP_REG_MK_U8(0x011c)
+#define SMIAPP_REG_U8_CSI2_16_TO_10_DT				SMIAPP_REG_MK_U8(0x011d)
+#define SMIAPP_REG_U8_CSI2_16_TO_8_DT				SMIAPP_REG_MK_U8(0x011e)
+#define SMIAPP_REG_U8_GAIN_MODE					SMIAPP_REG_MK_U8(0x0120)
+#define SMIAPP_REG_U16_VANA_VOLTAGE				SMIAPP_REG_MK_U16(0x0130)
+#define SMIAPP_REG_U16_VDIG_VOLTAGE				SMIAPP_REG_MK_U16(0x0132)
+#define SMIAPP_REG_U16_VIO_VOLTAGE				SMIAPP_REG_MK_U16(0x0134)
+#define SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ			SMIAPP_REG_MK_U16(0x0136)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CONTROL			SMIAPP_REG_MK_U8(0x0138)
+#define SMIAPP_REG_U8_TEMP_SENSOR_MODE				SMIAPP_REG_MK_U8(0x0139)
+#define SMIAPP_REG_U8_TEMP_SENSOR_OUTPUT			SMIAPP_REG_MK_U8(0x013a)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME			SMIAPP_REG_MK_U16(0x0200)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME			SMIAPP_REG_MK_U16(0x0202)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL		SMIAPP_REG_MK_U16(0x0204)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENR		SMIAPP_REG_MK_U16(0x0206)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_RED			SMIAPP_REG_MK_U16(0x0208)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_BLUE			SMIAPP_REG_MK_U16(0x020a)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENB		SMIAPP_REG_MK_U16(0x020c)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENR			SMIAPP_REG_MK_U16(0x020e)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_RED				SMIAPP_REG_MK_U16(0x0210)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_BLUE			SMIAPP_REG_MK_U16(0x0212)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENB			SMIAPP_REG_MK_U16(0x0214)
+#define SMIAPP_REG_U16_VT_PIX_CLK_DIV				SMIAPP_REG_MK_U16(0x0300)
+#define SMIAPP_REG_U16_VT_SYS_CLK_DIV				SMIAPP_REG_MK_U16(0x0302)
+#define SMIAPP_REG_U16_PRE_PLL_CLK_DIV				SMIAPP_REG_MK_U16(0x0304)
+#define SMIAPP_REG_U16_PLL_MULTIPLIER				SMIAPP_REG_MK_U16(0x0306)
+#define SMIAPP_REG_U16_OP_PIX_CLK_DIV				SMIAPP_REG_MK_U16(0x0308)
+#define SMIAPP_REG_U16_OP_SYS_CLK_DIV				SMIAPP_REG_MK_U16(0x030a)
+#define SMIAPP_REG_U16_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x0340)
+#define SMIAPP_REG_U16_LINE_LENGTH_PCK				SMIAPP_REG_MK_U16(0x0342)
+#define SMIAPP_REG_U16_X_ADDR_START				SMIAPP_REG_MK_U16(0x0344)
+#define SMIAPP_REG_U16_Y_ADDR_START				SMIAPP_REG_MK_U16(0x0346)
+#define SMIAPP_REG_U16_X_ADDR_END				SMIAPP_REG_MK_U16(0x0348)
+#define SMIAPP_REG_U16_Y_ADDR_END				SMIAPP_REG_MK_U16(0x034a)
+#define SMIAPP_REG_U16_X_OUTPUT_SIZE				SMIAPP_REG_MK_U16(0x034c)
+#define SMIAPP_REG_U16_Y_OUTPUT_SIZE				SMIAPP_REG_MK_U16(0x034e)
+#define SMIAPP_REG_U16_X_EVEN_INC				SMIAPP_REG_MK_U16(0x0380)
+#define SMIAPP_REG_U16_X_ODD_INC				SMIAPP_REG_MK_U16(0x0382)
+#define SMIAPP_REG_U16_Y_EVEN_INC				SMIAPP_REG_MK_U16(0x0384)
+#define SMIAPP_REG_U16_Y_ODD_INC				SMIAPP_REG_MK_U16(0x0386)
+#define SMIAPP_REG_U16_SCALING_MODE				SMIAPP_REG_MK_U16(0x0400)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING				SMIAPP_REG_MK_U16(0x0402)
+#define SMIAPP_REG_U16_SCALE_M					SMIAPP_REG_MK_U16(0x0404)
+#define SMIAPP_REG_U16_SCALE_N					SMIAPP_REG_MK_U16(0x0406)
+#define SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET			SMIAPP_REG_MK_U16(0x0408)
+#define SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET			SMIAPP_REG_MK_U16(0x040a)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH			SMIAPP_REG_MK_U16(0x040c)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT		SMIAPP_REG_MK_U16(0x040e)
+#define SMIAPP_REG_U16_COMPRESSION_MODE				SMIAPP_REG_MK_U16(0x0500)
+#define SMIAPP_REG_U16_TEST_PATTERN_MODE			SMIAPP_REG_MK_U16(0x0600)
+#define SMIAPP_REG_U16_TEST_DATA_RED				SMIAPP_REG_MK_U16(0x0602)
+#define SMIAPP_REG_U16_TEST_DATA_GREENR				SMIAPP_REG_MK_U16(0x0604)
+#define SMIAPP_REG_U16_TEST_DATA_BLUE				SMIAPP_REG_MK_U16(0x0606)
+#define SMIAPP_REG_U16_TEST_DATA_GREENB				SMIAPP_REG_MK_U16(0x0608)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_WIDTH			SMIAPP_REG_MK_U16(0x060a)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_POSITION		SMIAPP_REG_MK_U16(0x060c)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_WIDTH			SMIAPP_REG_MK_U16(0x060e)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_POSITION			SMIAPP_REG_MK_U16(0x0610)
+#define SMIAPP_REG_U16_FIFO_WATER_MARK_PIXELS			SMIAPP_REG_MK_U16(0x0700)
+#define SMIAPP_REG_U8_TCLK_POST					SMIAPP_REG_MK_U8(0x0800)
+#define SMIAPP_REG_U8_THS_PREPARE				SMIAPP_REG_MK_U8(0x0801)
+#define SMIAPP_REG_U8_THS_ZERO_MIN				SMIAPP_REG_MK_U8(0x0802)
+#define SMIAPP_REG_U8_THS_TRAIL					SMIAPP_REG_MK_U8(0x0803)
+#define SMIAPP_REG_U8_TCLK_TRAIL_MIN				SMIAPP_REG_MK_U8(0x0804)
+#define SMIAPP_REG_U8_TCLK_PREPARE				SMIAPP_REG_MK_U8(0x0805)
+#define SMIAPP_REG_U8_TCLK_ZERO					SMIAPP_REG_MK_U8(0x0806)
+#define SMIAPP_REG_U8_TLPX					SMIAPP_REG_MK_U8(0x0807)
+#define SMIAPP_REG_U8_DPHY_CTRL					SMIAPP_REG_MK_U8(0x0808)
+#define SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS		SMIAPP_REG_MK_U32(0x0820)
+#define SMIAPP_REG_U8_BINNING_MODE				SMIAPP_REG_MK_U8(0x0900)
+#define SMIAPP_REG_U8_BINNING_TYPE				SMIAPP_REG_MK_U8(0x0901)
+#define SMIAPP_REG_U8_BINNING_WEIGHTING				SMIAPP_REG_MK_U8(0x0902)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL			SMIAPP_REG_MK_U8(0x0a00)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS			SMIAPP_REG_MK_U8(0x0a01)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT		SMIAPP_REG_MK_U8(0x0a02)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0			SMIAPP_REG_MK_U8(0x0a04)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_1			SMIAPP_REG_MK_U8(0x0a05)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_2			SMIAPP_REG_MK_U8(0x0a06)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_3			SMIAPP_REG_MK_U8(0x0a07)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_4			SMIAPP_REG_MK_U8(0x0a08)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_5			SMIAPP_REG_MK_U8(0x0a09)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_12		SMIAPP_REG_MK_U8(0x0a10)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_13		SMIAPP_REG_MK_U8(0x0a11)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_14		SMIAPP_REG_MK_U8(0x0a12)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_15		SMIAPP_REG_MK_U8(0x0a13)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_16		SMIAPP_REG_MK_U8(0x0a14)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_17		SMIAPP_REG_MK_U8(0x0a15)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_18		SMIAPP_REG_MK_U8(0x0a16)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_19		SMIAPP_REG_MK_U8(0x0a17)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_20		SMIAPP_REG_MK_U8(0x0a18)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_21		SMIAPP_REG_MK_U8(0x0a19)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_22		SMIAPP_REG_MK_U8(0x0a1a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_23		SMIAPP_REG_MK_U8(0x0a1b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_24		SMIAPP_REG_MK_U8(0x0a1c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_25		SMIAPP_REG_MK_U8(0x0a1d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_26		SMIAPP_REG_MK_U8(0x0a1e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_27		SMIAPP_REG_MK_U8(0x0a1f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_28		SMIAPP_REG_MK_U8(0x0a20)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_29		SMIAPP_REG_MK_U8(0x0a21)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_30		SMIAPP_REG_MK_U8(0x0a22)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_31		SMIAPP_REG_MK_U8(0x0a23)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_32		SMIAPP_REG_MK_U8(0x0a24)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_33		SMIAPP_REG_MK_U8(0x0a25)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_34		SMIAPP_REG_MK_U8(0x0a26)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_35		SMIAPP_REG_MK_U8(0x0a27)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_36		SMIAPP_REG_MK_U8(0x0a28)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_37		SMIAPP_REG_MK_U8(0x0a29)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_38		SMIAPP_REG_MK_U8(0x0a2a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_39		SMIAPP_REG_MK_U8(0x0a2b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_40		SMIAPP_REG_MK_U8(0x0a2c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_41		SMIAPP_REG_MK_U8(0x0a2d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_42		SMIAPP_REG_MK_U8(0x0a2e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_43		SMIAPP_REG_MK_U8(0x0a2f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_44		SMIAPP_REG_MK_U8(0x0a30)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_45		SMIAPP_REG_MK_U8(0x0a31)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_46		SMIAPP_REG_MK_U8(0x0a32)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_47		SMIAPP_REG_MK_U8(0x0a33)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_48		SMIAPP_REG_MK_U8(0x0a34)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_49		SMIAPP_REG_MK_U8(0x0a35)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_50		SMIAPP_REG_MK_U8(0x0a36)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_51		SMIAPP_REG_MK_U8(0x0a37)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_52		SMIAPP_REG_MK_U8(0x0a38)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_53		SMIAPP_REG_MK_U8(0x0a39)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_54		SMIAPP_REG_MK_U8(0x0a3a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_55		SMIAPP_REG_MK_U8(0x0a3b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_56		SMIAPP_REG_MK_U8(0x0a3c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_57		SMIAPP_REG_MK_U8(0x0a3d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_58		SMIAPP_REG_MK_U8(0x0a3e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_59		SMIAPP_REG_MK_U8(0x0a3f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_60		SMIAPP_REG_MK_U8(0x0a40)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_61		SMIAPP_REG_MK_U8(0x0a41)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_62		SMIAPP_REG_MK_U8(0x0a42)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_63		SMIAPP_REG_MK_U8(0x0a43)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_CTRL			SMIAPP_REG_MK_U8(0x0a44)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_STATUS			SMIAPP_REG_MK_U8(0x0a45)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_PAGE_SELECT		SMIAPP_REG_MK_U8(0x0a46)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_0			SMIAPP_REG_MK_U8(0x0a48)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_1			SMIAPP_REG_MK_U8(0x0a49)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_2			SMIAPP_REG_MK_U8(0x0a4a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_3			SMIAPP_REG_MK_U8(0x0a4b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_4			SMIAPP_REG_MK_U8(0x0a4c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_5			SMIAPP_REG_MK_U8(0x0a4d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_6			SMIAPP_REG_MK_U8(0x0a4e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_7			SMIAPP_REG_MK_U8(0x0a4f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_8			SMIAPP_REG_MK_U8(0x0a50)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_9			SMIAPP_REG_MK_U8(0x0a51)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_10		SMIAPP_REG_MK_U8(0x0a52)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_11		SMIAPP_REG_MK_U8(0x0a53)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_12		SMIAPP_REG_MK_U8(0x0a54)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_13		SMIAPP_REG_MK_U8(0x0a55)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_14		SMIAPP_REG_MK_U8(0x0a56)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_15		SMIAPP_REG_MK_U8(0x0a57)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_16		SMIAPP_REG_MK_U8(0x0a58)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_17		SMIAPP_REG_MK_U8(0x0a59)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_18		SMIAPP_REG_MK_U8(0x0a5a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_19		SMIAPP_REG_MK_U8(0x0a5b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_20		SMIAPP_REG_MK_U8(0x0a5c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_21		SMIAPP_REG_MK_U8(0x0a5d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_22		SMIAPP_REG_MK_U8(0x0a5e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_23		SMIAPP_REG_MK_U8(0x0a5f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_24		SMIAPP_REG_MK_U8(0x0a60)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_25		SMIAPP_REG_MK_U8(0x0a61)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_26		SMIAPP_REG_MK_U8(0x0a62)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_27		SMIAPP_REG_MK_U8(0x0a63)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_28		SMIAPP_REG_MK_U8(0x0a64)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_29		SMIAPP_REG_MK_U8(0x0a65)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_30		SMIAPP_REG_MK_U8(0x0a66)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_31		SMIAPP_REG_MK_U8(0x0a67)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_32		SMIAPP_REG_MK_U8(0x0a68)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_33		SMIAPP_REG_MK_U8(0x0a69)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_34		SMIAPP_REG_MK_U8(0x0a6a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_35		SMIAPP_REG_MK_U8(0x0a6b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_36		SMIAPP_REG_MK_U8(0x0a6c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_37		SMIAPP_REG_MK_U8(0x0a6d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_38		SMIAPP_REG_MK_U8(0x0a6e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_39		SMIAPP_REG_MK_U8(0x0a6f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_40		SMIAPP_REG_MK_U8(0x0a70)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_41		SMIAPP_REG_MK_U8(0x0a71)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_42		SMIAPP_REG_MK_U8(0x0a72)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_43		SMIAPP_REG_MK_U8(0x0a73)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_44		SMIAPP_REG_MK_U8(0x0a74)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_45		SMIAPP_REG_MK_U8(0x0a75)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_46		SMIAPP_REG_MK_U8(0x0a76)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_47		SMIAPP_REG_MK_U8(0x0a77)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_48		SMIAPP_REG_MK_U8(0x0a78)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_49		SMIAPP_REG_MK_U8(0x0a79)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_50		SMIAPP_REG_MK_U8(0x0a7a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_51		SMIAPP_REG_MK_U8(0x0a7b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_52		SMIAPP_REG_MK_U8(0x0a7c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_53		SMIAPP_REG_MK_U8(0x0a7d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_54		SMIAPP_REG_MK_U8(0x0a7e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_55		SMIAPP_REG_MK_U8(0x0a7f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_56		SMIAPP_REG_MK_U8(0x0a80)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_57		SMIAPP_REG_MK_U8(0x0a81)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_58		SMIAPP_REG_MK_U8(0x0a82)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_59		SMIAPP_REG_MK_U8(0x0a83)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_60		SMIAPP_REG_MK_U8(0x0a84)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_61		SMIAPP_REG_MK_U8(0x0a85)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_62		SMIAPP_REG_MK_U8(0x0a86)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_63		SMIAPP_REG_MK_U8(0x0a87)
+#define SMIAPP_REG_U8_SHADING_CORRECTION_ENABLE			SMIAPP_REG_MK_U8(0x0b00)
+#define SMIAPP_REG_U8_LUMINANCE_CORRECTION_LEVEL		SMIAPP_REG_MK_U8(0x0b01)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_ENABLE		SMIAPP_REG_MK_U8(0x0b02)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_WEIGHT		SMIAPP_REG_MK_U8(0x0b03)
+#define SMIAPP_REG_U8_BLACK_LEVEL_CORRECTION_ENABLE		SMIAPP_REG_MK_U8(0x0b04)
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b05)
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b06)
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b07)
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b08)
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b09)
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b0a)
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b0b)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_ENABLE		SMIAPP_REG_MK_U8(0x0b0c)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_WEIGHT		SMIAPP_REG_MK_U8(0x0b0d)
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b0e)
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ADJUST		SMIAPP_REG_MK_U8(0x0b0f)
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ADJUST		SMIAPP_REG_MK_U8(0x0b10)
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b11)
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b12)
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b13)
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b14)
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b15)
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b16)
+#define SMIAPP_REG_U8_EDOF_MODE					SMIAPP_REG_MK_U8(0x0b80)
+#define SMIAPP_REG_U8_SHARPNESS					SMIAPP_REG_MK_U8(0x0b83)
+#define SMIAPP_REG_U8_DENOISING					SMIAPP_REG_MK_U8(0x0b84)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC				SMIAPP_REG_MK_U8(0x0b85)
+#define SMIAPP_REG_U16_DEPTH_OF_FIELD				SMIAPP_REG_MK_U16(0x0b86)
+#define SMIAPP_REG_U16_FOCUS_DISTANCE				SMIAPP_REG_MK_U16(0x0b88)
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CTRL			SMIAPP_REG_MK_U8(0x0b8a)
+#define SMIAPP_REG_U16_COLOUR_TEMPERATURE			SMIAPP_REG_MK_U16(0x0b8c)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENR			SMIAPP_REG_MK_U16(0x0b8e)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_RED			SMIAPP_REG_MK_U16(0x0b90)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_BLUE			SMIAPP_REG_MK_U16(0x0b92)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENB			SMIAPP_REG_MK_U16(0x0b94)
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_MODE			SMIAPP_REG_MK_U8(0x0bc0)
+#define SMIAPP_REG_U16_FIXED_ZONE_WEIGHTING			SMIAPP_REG_MK_U16(0x0bc2)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_X_START			SMIAPP_REG_MK_U16(0x0bc4)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_Y_START			SMIAPP_REG_MK_U16(0x0bc6)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_WIDTH			SMIAPP_REG_MK_U16(0x0bc8)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_HEIGHT			SMIAPP_REG_MK_U16(0x0bca)
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL1			SMIAPP_REG_MK_U8(0x0c00)
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL2			SMIAPP_REG_MK_U8(0x0c01)
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_1		SMIAPP_REG_MK_U8(0x0c02)
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_2		SMIAPP_REG_MK_U8(0x0c03)
+#define SMIAPP_REG_U16_TRDY_CTRL				SMIAPP_REG_MK_U16(0x0c04)
+#define SMIAPP_REG_U16_TRDOUT_CTRL				SMIAPP_REG_MK_U16(0x0c06)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_DELAY_CTRL		SMIAPP_REG_MK_U16(0x0c08)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_WIDTH_CTRL		SMIAPP_REG_MK_U16(0x0c0a)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_CTRL			SMIAPP_REG_MK_U16(0x0c0c)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_CTRL		SMIAPP_REG_MK_U16(0x0c0e)
+#define SMIAPP_REG_U16_TGRST_INTERVAL_CTRL			SMIAPP_REG_MK_U16(0x0c10)
+#define SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT			SMIAPP_REG_MK_U8(0x0c12)
+#define SMIAPP_REG_U16_FLASH_STROBE_START_POINT			SMIAPP_REG_MK_U16(0x0c14)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL		SMIAPP_REG_MK_U16(0x0c16)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL		SMIAPP_REG_MK_U16(0x0c18)
+#define SMIAPP_REG_U8_FLASH_MODE_RS				SMIAPP_REG_MK_U8(0x0c1a)
+#define SMIAPP_REG_U8_FLASH_TRIGGER_RS				SMIAPP_REG_MK_U8(0x0c1b)
+#define SMIAPP_REG_U8_FLASH_STATUS				SMIAPP_REG_MK_U8(0x0c1c)
+#define SMIAPP_REG_U8_SA_STROBE_MODE				SMIAPP_REG_MK_U8(0x0c1d)
+#define SMIAPP_REG_U16_SA_STROBE_START_POINT			SMIAPP_REG_MK_U16(0x0c1e)
+#define SMIAPP_REG_U16_TSA_STROBE_DELAY_CTRL			SMIAPP_REG_MK_U16(0x0c20)
+#define SMIAPP_REG_U16_TSA_STROBE_WIDTH_CTRL			SMIAPP_REG_MK_U16(0x0c22)
+#define SMIAPP_REG_U8_SA_STROBE_TRIGGER				SMIAPP_REG_MK_U8(0x0c24)
+#define SMIAPP_REG_U8_SPECIAL_ACTUATOR_STATUS			SMIAPP_REG_MK_U8(0x0c25)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_RS_CTRL	SMIAPP_REG_MK_U16(0x0c26)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_RS_CTRL		SMIAPP_REG_MK_U16(0x0c28)
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_RS_CTRL		SMIAPP_REG_MK_U8(0x0c2a)
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_CTRL			SMIAPP_REG_MK_U8(0x0c2b)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_CTRL		SMIAPP_REG_MK_U16(0x0c2c)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_CTRL		SMIAPP_REG_MK_U16(0x0c2e)
+#define SMIAPP_REG_U8_LOW_LEVEL_CTRL				SMIAPP_REG_MK_U8(0x0c80)
+#define SMIAPP_REG_U16_MAIN_TRIGGER_REF_POINT			SMIAPP_REG_MK_U16(0x0c82)
+#define SMIAPP_REG_U16_MAIN_TRIGGER_T3				SMIAPP_REG_MK_U16(0x0c84)
+#define SMIAPP_REG_U8_MAIN_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c86)
+#define SMIAPP_REG_U16_PHASE1_TRIGGER_T3			SMIAPP_REG_MK_U16(0x0c88)
+#define SMIAPP_REG_U8_PHASE1_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c8a)
+#define SMIAPP_REG_U16_PHASE2_TRIGGER_T3			SMIAPP_REG_MK_U16(0x0c8c)
+#define SMIAPP_REG_U8_PHASE2_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c8e)
+#define SMIAPP_REG_U8_MECH_SHUTTER_CTRL				SMIAPP_REG_MK_U8(0x0d00)
+#define SMIAPP_REG_U8_OPERATION_MODE				SMIAPP_REG_MK_U8(0x0d01)
+#define SMIAPP_REG_U8_ACT_STATE1				SMIAPP_REG_MK_U8(0x0d02)
+#define SMIAPP_REG_U8_ACT_STATE2				SMIAPP_REG_MK_U8(0x0d03)
+#define SMIAPP_REG_U16_FOCUS_CHANGE				SMIAPP_REG_MK_U16(0x0d80)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_CONTROL			SMIAPP_REG_MK_U16(0x0d82)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE1		SMIAPP_REG_MK_U16(0x0d84)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE2		SMIAPP_REG_MK_U16(0x0d86)
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE1			SMIAPP_REG_MK_U8(0x0d88)
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE2			SMIAPP_REG_MK_U8(0x0d89)
+#define SMIAPP_REG_U8_POSITION					SMIAPP_REG_MK_U8(0x0d8a)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CONTROL			SMIAPP_REG_MK_U8(0x0e00)
+#define SMIAPP_REG_U8_BRACKETING_LUT_MODE			SMIAPP_REG_MK_U8(0x0e01)
+#define SMIAPP_REG_U8_BRACKETING_LUT_ENTRY_CONTROL		SMIAPP_REG_MK_U8(0x0e02)
+#define SMIAPP_REG_U8_LUT_PARAMETERS_START			SMIAPP_REG_MK_U8(0x0e10)
+#define SMIAPP_REG_U8_LUT_PARAMETERS_END			SMIAPP_REG_MK_U8(0x0eff)
+#define SMIAPP_REG_U16_INTEGRATION_TIME_CAPABILITY		SMIAPP_REG_MK_U16(0x1000)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MIN		SMIAPP_REG_MK_U16(0x1004)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MAX_MARGIN	SMIAPP_REG_MK_U16(0x1006)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN		SMIAPP_REG_MK_U16(0x1008)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN		SMIAPP_REG_MK_U16(0x100a)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_CAPABILITY			SMIAPP_REG_MK_U16(0x1080)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MIN				SMIAPP_REG_MK_U16(0x1084)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MAX				SMIAPP_REG_MK_U16(0x1086)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_STEP_SIZE			SMIAPP_REG_MK_U16(0x1088)
+#define SMIAPP_REG_F32_MIN_EXT_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1100)
+#define SMIAPP_REG_F32_MAX_EXT_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1104)
+#define SMIAPP_REG_U16_MIN_PRE_PLL_CLK_DIV			SMIAPP_REG_MK_U16(0x1108)
+#define SMIAPP_REG_U16_MAX_PRE_PLL_CLK_DIV			SMIAPP_REG_MK_U16(0x110a)
+#define SMIAPP_REG_F32_MIN_PLL_IP_FREQ_HZ			SMIAPP_REG_MK_F32(0x110c)
+#define SMIAPP_REG_F32_MAX_PLL_IP_FREQ_HZ			SMIAPP_REG_MK_F32(0x1110)
+#define SMIAPP_REG_U16_MIN_PLL_MULTIPLIER			SMIAPP_REG_MK_U16(0x1114)
+#define SMIAPP_REG_U16_MAX_PLL_MULTIPLIER			SMIAPP_REG_MK_U16(0x1116)
+#define SMIAPP_REG_F32_MIN_PLL_OP_FREQ_HZ			SMIAPP_REG_MK_F32(0x1118)
+#define SMIAPP_REG_F32_MAX_PLL_OP_FREQ_HZ			SMIAPP_REG_MK_F32(0x111c)
+#define SMIAPP_REG_U16_MIN_VT_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1120)
+#define SMIAPP_REG_U16_MAX_VT_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1122)
+#define SMIAPP_REG_F32_MIN_VT_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1124)
+#define SMIAPP_REG_F32_MAX_VT_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1128)
+#define SMIAPP_REG_F32_MIN_VT_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x112c)
+#define SMIAPP_REG_F32_MAX_VT_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1130)
+#define SMIAPP_REG_U16_MIN_VT_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x1134)
+#define SMIAPP_REG_U16_MAX_VT_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x1136)
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x1140)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x1142)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK			SMIAPP_REG_MK_U16(0x1144)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK			SMIAPP_REG_MK_U16(0x1146)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK			SMIAPP_REG_MK_U16(0x1148)
+#define SMIAPP_REG_U16_MIN_FRAME_BLANKING_LINES			SMIAPP_REG_MK_U16(0x114a)
+#define SMIAPP_REG_U8_MIN_LINE_LENGTH_PCK_STEP_SIZE		SMIAPP_REG_MK_U8(0x114c)
+#define SMIAPP_REG_U16_MIN_OP_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1160)
+#define SMIAPP_REG_U16_MAX_OP_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1162)
+#define SMIAPP_REG_F32_MIN_OP_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1164)
+#define SMIAPP_REG_F32_MAX_OP_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1168)
+#define SMIAPP_REG_U16_MIN_OP_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x116c)
+#define SMIAPP_REG_U16_MAX_OP_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x116e)
+#define SMIAPP_REG_F32_MIN_OP_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1170)
+#define SMIAPP_REG_F32_MAX_OP_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1174)
+#define SMIAPP_REG_U16_X_ADDR_MIN				SMIAPP_REG_MK_U16(0x1180)
+#define SMIAPP_REG_U16_Y_ADDR_MIN				SMIAPP_REG_MK_U16(0x1182)
+#define SMIAPP_REG_U16_X_ADDR_MAX				SMIAPP_REG_MK_U16(0x1184)
+#define SMIAPP_REG_U16_Y_ADDR_MAX				SMIAPP_REG_MK_U16(0x1186)
+#define SMIAPP_REG_U16_MIN_X_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x1188)
+#define SMIAPP_REG_U16_MIN_Y_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118a)
+#define SMIAPP_REG_U16_MAX_X_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118c)
+#define SMIAPP_REG_U16_MAX_Y_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118e)
+#define SMIAPP_REG_U16_MIN_EVEN_INC				SMIAPP_REG_MK_U16(0x11c0)
+#define SMIAPP_REG_U16_MAX_EVEN_INC				SMIAPP_REG_MK_U16(0x11c2)
+#define SMIAPP_REG_U16_MIN_ODD_INC				SMIAPP_REG_MK_U16(0x11c4)
+#define SMIAPP_REG_U16_MAX_ODD_INC				SMIAPP_REG_MK_U16(0x11c6)
+#define SMIAPP_REG_U16_SCALING_CAPABILITY			SMIAPP_REG_MK_U16(0x1200)
+#define SMIAPP_REG_U16_SCALER_M_MIN				SMIAPP_REG_MK_U16(0x1204)
+#define SMIAPP_REG_U16_SCALER_M_MAX				SMIAPP_REG_MK_U16(0x1206)
+#define SMIAPP_REG_U16_SCALER_N_MIN				SMIAPP_REG_MK_U16(0x1208)
+#define SMIAPP_REG_U16_SCALER_N_MAX				SMIAPP_REG_MK_U16(0x120a)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING_CAPABILITY		SMIAPP_REG_MK_U16(0x120c)
+#define SMIAPP_REG_U8_DIGITAL_CROP_CAPABILITY			SMIAPP_REG_MK_U8(0x120e)
+#define SMIAPP_REG_U16_COMPRESSION_CAPABILITY			SMIAPP_REG_MK_U16(0x1300)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINRED			SMIAPP_REG_MK_U16(0x1400)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINRED		SMIAPP_REG_MK_U16(0x1402)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINRED			SMIAPP_REG_MK_U16(0x1404)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINGREEN		SMIAPP_REG_MK_U16(0x1406)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINGREEN		SMIAPP_REG_MK_U16(0x1408)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINGREEN		SMIAPP_REG_MK_U16(0x140a)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINBLUE			SMIAPP_REG_MK_U16(0x140c)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINBLUE		SMIAPP_REG_MK_U16(0x140e)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINBLUE		SMIAPP_REG_MK_U16(0x1410)
+#define SMIAPP_REG_U16_FIFO_SIZE_PIXELS				SMIAPP_REG_MK_U16(0x1500)
+#define SMIAPP_REG_U8_FIFO_SUPPORT_CAPABILITY			SMIAPP_REG_MK_U8(0x1502)
+#define SMIAPP_REG_U8_DPHY_CTRL_CAPABILITY			SMIAPP_REG_MK_U8(0x1600)
+#define SMIAPP_REG_U8_CSI_LANE_MODE_CAPABILITY			SMIAPP_REG_MK_U8(0x1601)
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE_CAPABILITY		SMIAPP_REG_MK_U8(0x1602)
+#define SMIAPP_REG_U8_FAST_STANDBY_CAPABILITY			SMIAPP_REG_MK_U8(0x1603)
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL_CAPABILITY		SMIAPP_REG_MK_U8(0x1604)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1608)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x160c)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1610)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1614)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CAPABILITY			SMIAPP_REG_MK_U8(0x1618)
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES_BIN		SMIAPP_REG_MK_U16(0x1700)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES_BIN		SMIAPP_REG_MK_U16(0x1702)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK_BIN			SMIAPP_REG_MK_U16(0x1704)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK_BIN			SMIAPP_REG_MK_U16(0x1706)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK_BIN		SMIAPP_REG_MK_U16(0x1708)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN_BIN		SMIAPP_REG_MK_U16(0x170a)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN	SMIAPP_REG_MK_U16(0x170c)
+#define SMIAPP_REG_U8_BINNING_CAPABILITY			SMIAPP_REG_MK_U8(0x1710)
+#define SMIAPP_REG_U8_BINNING_WEIGHTING_CAPABILITY		SMIAPP_REG_MK_U8(0x1711)
+#define SMIAPP_REG_U8_BINNING_SUBTYPES				SMIAPP_REG_MK_U8(0x1712)
+#define SMIAPP_REG_U8_BINNING_TYPE_n(n)				SMIAPP_REG_MK_U8(0x1713 + (n)) /* 1 <= n <= 237 */
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_CAPABILITY		SMIAPP_REG_MK_U8(0x1800)
+#define SMIAPP_REG_U8_SHADING_CORRECTION_CAPABILITY		SMIAPP_REG_MK_U8(0x1900)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_CAPABILITY		SMIAPP_REG_MK_U8(0x1901)
+#define SMIAPP_REG_U8_BLACK_LEVEL_CAPABILITY			SMIAPP_REG_MK_U8(0x1902)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_CAPABILITY	SMIAPP_REG_MK_U8(0x1903)
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY		SMIAPP_REG_MK_U16(0x1904)
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY_2		SMIAPP_REG_MK_U16(0x1906)
+#define SMIAPP_REG_U8_EDOF_CAPABILITY				SMIAPP_REG_MK_U8(0x1980)
+#define SMIAPP_REG_U8_ESTIMATION_FRAMES				SMIAPP_REG_MK_U8(0x1981)
+#define SMIAPP_REG_U8_SUPPORTS_SHARPNESS_ADJ			SMIAPP_REG_MK_U8(0x1982)
+#define SMIAPP_REG_U8_SUPPORTS_DENOISING_ADJ			SMIAPP_REG_MK_U8(0x1983)
+#define SMIAPP_REG_U8_SUPPORTS_MODULE_SPECIFIC_ADJ		SMIAPP_REG_MK_U8(0x1984)
+#define SMIAPP_REG_U8_SUPPORTS_DEPTH_OF_FIELD_ADJ		SMIAPP_REG_MK_U8(0x1985)
+#define SMIAPP_REG_U8_SUPPORTS_FOCUS_DISTANCE_ADJ		SMIAPP_REG_MK_U8(0x1986)
+#define SMIAPP_REG_U8_COLOUR_FEEDBACK_CAPABILITY		SMIAPP_REG_MK_U8(0x1987)
+#define SMIAPP_REG_U8_EDOF_SUPPORT_AB_NXM			SMIAPP_REG_MK_U8(0x1988)
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CAPABILITY		SMIAPP_REG_MK_U8(0x19c0)
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_CAPABILITY		SMIAPP_REG_MK_U8(0x19c1)
+#define SMIAPP_REG_U16_EST_DEPTH_OF_FIELD			SMIAPP_REG_MK_U16(0x19c2)
+#define SMIAPP_REG_U16_EST_FOCUS_DISTANCE			SMIAPP_REG_MK_U16(0x19c4)
+#define SMIAPP_REG_U16_CAPABILITY_TRDY_MIN			SMIAPP_REG_MK_U16(0x1a00)
+#define SMIAPP_REG_U8_FLASH_MODE_CAPABILITY			SMIAPP_REG_MK_U8(0x1a02)
+#define SMIAPP_REG_U16_MECH_SHUT_AND_ACT_START_ADDR		SMIAPP_REG_MK_U16(0x1b02)
+#define SMIAPP_REG_U8_ACTUATOR_CAPABILITY			SMIAPP_REG_MK_U8(0x1b04)
+#define SMIAPP_REG_U16_ACTUATOR_TYPE				SMIAPP_REG_MK_U16(0x1b40)
+#define SMIAPP_REG_U8_AF_DEVICE_ADDRESS				SMIAPP_REG_MK_U8(0x1b42)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_ADDRESS			SMIAPP_REG_MK_U16(0x1b44)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_1		SMIAPP_REG_MK_U8(0x1c00)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_2		SMIAPP_REG_MK_U8(0x1c01)
+#define SMIAPP_REG_U8_BRACKETING_LUT_SIZE			SMIAPP_REG_MK_U8(0x1c02)
diff --git a/drivers/media/video/smiapp/smiapp-reg.h b/drivers/media/video/smiapp/smiapp-reg.h
new file mode 100644
index 000000000000..d0167aa17534
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-reg.h
@@ -0,0 +1,122 @@
+/*
+ * drivers/media/video/smiapp/smiapp-reg.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_REG_H_
+#define __SMIAPP_REG_H_
+
+#include "smiapp-reg-defs.h"
+
+/* Bits for above register */
+#define SMIAPP_IMAGE_ORIENTATION_HFLIP		(1 << 0)
+#define SMIAPP_IMAGE_ORIENTATION_VFLIP		(1 << 1)
+
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN		(1 << 0)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_RD_EN		(0 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_WR_EN		(1 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_ERR_CLEAR	(1 << 2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY	(1 << 0)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_WR_READY	(1 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EDATA		(1 << 2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EUSAGE		(1 << 3)
+
+#define SMIAPP_SOFTWARE_RESET				(1 << 0)
+
+#define SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE	(1 << 0)
+#define SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE	(1 << 1)
+
+#define SMIAPP_DPHY_CTRL_AUTOMATIC			0
+/* DPHY control based on REQUESTED_LINK_BIT_RATE_MBPS */
+#define SMIAPP_DPHY_CTRL_UI				1
+#define SMIAPP_DPHY_CTRL_REGISTER			2
+
+#define SMIAPP_COMPRESSION_MODE_SIMPLE_PREDICTOR	1
+#define SMIAPP_COMPRESSION_MODE_ADVANCED_PREDICTOR	2
+
+#define SMIAPP_MODE_SELECT_SOFTWARE_STANDBY		0
+#define SMIAPP_MODE_SELECT_STREAMING			1
+
+#define SMIAPP_SCALING_MODE_NONE			0
+#define SMIAPP_SCALING_MODE_HORIZONTAL			1
+#define SMIAPP_SCALING_MODE_BOTH			2
+
+#define SMIAPP_SCALING_CAPABILITY_NONE			0
+#define SMIAPP_SCALING_CAPABILITY_HORIZONTAL		1
+#define SMIAPP_SCALING_CAPABILITY_BOTH			2 /* horizontal/both */
+
+/* digital crop right before scaler */
+#define SMIAPP_DIGITAL_CROP_CAPABILITY_NONE		0
+#define SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP	1
+
+#define SMIAPP_BINNING_CAPABILITY_NO			0
+#define SMIAPP_BINNING_CAPABILITY_YES			1
+
+/* Maximum number of binning subtypes */
+#define SMIAPP_BINNING_SUBTYPES				253
+
+#define SMIAPP_PIXEL_ORDER_GRBG				0
+#define SMIAPP_PIXEL_ORDER_RGGB				1
+#define SMIAPP_PIXEL_ORDER_BGGR				2
+#define SMIAPP_PIXEL_ORDER_GBRG				3
+
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL		1
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED		2
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N		8
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED_N	16
+
+#define SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE		0x01
+#define SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE		0x02
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NROWS_MASK	0x0f
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_MASK	0xf0
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_SHIFT	4
+
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_MASK	0xf000
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_SHIFT	12
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELS_MASK		0x0fff
+
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_MASK	0xf0000000
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_SHIFT	28
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELS_MASK		0x0000ffff
+
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED	1
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DUMMY	2
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_BLACK	3
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DARK		4
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE	5
+
+#define SMIAPP_FAST_STANDBY_CTRL_COMPLETE_FRAMES	0
+#define SMIAPP_FAST_STANDBY_CTRL_IMMEDIATE		1
+
+/* Scaling N factor */
+#define SMIAPP_SCALE_N					16
+
+/* Image statistics registers */
+/* Registers 0x2000 to 0x2fff are reserved for future
+ * use for statistics features.
+ */
+
+/* Manufacturer Specific Registers: 0x3000 to 0x3fff
+ * The manufacturer specifies these as a black box.
+ */
+
+#endif /* __SMIAPP_REG_H_ */
diff --git a/drivers/media/video/smiapp/smiapp-regs.c b/drivers/media/video/smiapp/smiapp-regs.c
new file mode 100644
index 000000000000..b1812b17a407
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-regs.c
@@ -0,0 +1,273 @@
+/*
+ * drivers/media/video/smiapp/smiapp-regs.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+
+#include "smiapp.h"
+#include "smiapp-regs.h"
+
+static uint32_t float_to_u32_mul_1000000(struct i2c_client *client,
+					 uint32_t phloat)
+{
+	int32_t exp;
+	uint64_t man;
+
+	if (phloat >= 0x80000000) {
+		dev_err(&client->dev, "this is a negative number\n");
+		return 0;
+	}
+
+	if (phloat == 0x7f800000)
+		return ~0; /* Inf. */
+
+	if ((phloat & 0x7f800000) == 0x7f800000) {
+		dev_err(&client->dev, "NaN or other special number\n");
+		return 0;
+	}
+
+	/* Valid cases begin here */
+	if (phloat == 0)
+		return 0; /* Valid zero */
+
+	if (phloat > 0x4f800000)
+		return ~0; /* larger than 4294967295 */
+
+	/*
+	 * Unbias exponent (note how phloat is now guaranteed to
+	 * have 0 in the high bit)
+	 */
+	exp = ((int32_t)phloat >> 23) - 127;
+
+	/* Extract mantissa, add missing '1' bit and it's in MHz */
+	man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
+
+	if (exp < 0)
+		man >>= -exp;
+	else
+		man <<= exp;
+
+	man >>= 23; /* Remove mantissa bias */
+
+	return man & 0xffffffff;
+}
+
+
+/*
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
+			   u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct i2c_msg msg;
+	unsigned char data[4];
+	u16 offset = reg;
+	int r;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (offset >> 8);
+	data[1] = (u8) offset;
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		if (r >= 0)
+			r = -EBUSY;
+		goto err;
+	}
+
+	msg.len = len;
+	msg.flags = I2C_M_RD;
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		if (r >= 0)
+			r = -EBUSY;
+		goto err;
+	}
+
+	*val = 0;
+	/* high byte comes first */
+	switch (len) {
+	case SMIA_REG_32BIT:
+		*val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
+			data[3];
+		break;
+	case SMIA_REG_16BIT:
+		*val = (data[0] << 8) + data[1];
+		break;
+	case SMIA_REG_8BIT:
+		*val = data[0];
+		break;
+	default:
+		BUG();
+	}
+
+	return 0;
+
+err:
+	dev_err(&client->dev, "read from offset 0x%x error %d\n", offset, r);
+
+	return r;
+}
+
+/* Read a register using 8-bit access only. */
+static int ____smiapp_read_8only(struct smiapp_sensor *sensor, u16 reg,
+				 u16 len, u32 *val)
+{
+	unsigned int i;
+	int rval;
+
+	*val = 0;
+
+	for (i = 0; i < len; i++) {
+		u32 val8;
+
+		rval = ____smiapp_read(sensor, reg + i, 1, &val8);
+		if (rval < 0)
+			return rval;
+		*val |= val8 << ((len - i - 1) << 3);
+	}
+
+	return 0;
+}
+
+/*
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
+			 bool only8)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int len = (u8)(reg >> 16);
+	int rval;
+
+	if (len != SMIA_REG_8BIT && len != SMIA_REG_16BIT
+	    && len != SMIA_REG_32BIT)
+		return -EINVAL;
+
+	if (smiapp_quirk_reg(sensor, reg, val))
+		goto found_quirk;
+
+	if (len == SMIA_REG_8BIT && !only8)
+		rval = ____smiapp_read(sensor, (u16)reg, len, val);
+	else
+		rval = ____smiapp_read_8only(sensor, (u16)reg, len, val);
+	if (rval < 0)
+		return rval;
+
+found_quirk:
+	if (reg & SMIA_REG_FLAG_FLOAT)
+		*val = float_to_u32_mul_1000000(client, *val);
+
+	return 0;
+}
+
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	return __smiapp_read(
+		sensor, reg, val,
+		smiapp_needs_quirk(sensor,
+				   SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
+}
+
+int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	return __smiapp_read(sensor, reg, val, true);
+}
+
+/*
+ * Write to a 8/16-bit register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct i2c_msg msg;
+	unsigned char data[6];
+	unsigned int retries;
+	unsigned int flags = reg >> 24;
+	unsigned int len = (u8)(reg >> 16);
+	u16 offset = reg;
+	int r;
+
+	if ((len != SMIA_REG_8BIT && len != SMIA_REG_16BIT &&
+	     len != SMIA_REG_32BIT) || flags)
+		return -EINVAL;
+
+	msg.addr = client->addr;
+	msg.flags = 0; /* Write */
+	msg.len = 2 + len;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (reg >> 8);
+	data[1] = (u8) (reg & 0xff);
+
+	switch (len) {
+	case SMIA_REG_8BIT:
+		data[2] = val;
+		break;
+	case SMIA_REG_16BIT:
+		data[2] = val >> 8;
+		data[3] = val;
+		break;
+	case SMIA_REG_32BIT:
+		data[2] = val >> 24;
+		data[3] = val >> 16;
+		data[4] = val >> 8;
+		data[5] = val;
+		break;
+	default:
+		BUG();
+	}
+
+	for (retries = 0; retries < 5; retries++) {
+		/*
+		 * Due to unknown reason sensor stops responding. This
+		 * loop is a temporaty solution until the root cause
+		 * is found.
+		 */
+		r = i2c_transfer(client->adapter, &msg, 1);
+		if (r == 1) {
+			if (retries)
+				dev_err(&client->dev,
+					"sensor i2c stall encountered. "
+					"retries: %d\n", retries);
+			return 0;
+		}
+
+		usleep_range(2000, 2000);
+	}
+
+	dev_err(&client->dev,
+		"wrote 0x%x to offset 0x%x error %d\n", val, offset, r);
+
+	return r;
+}
diff --git a/drivers/media/video/smiapp/smiapp-regs.h b/drivers/media/video/smiapp/smiapp-regs.h
new file mode 100644
index 000000000000..7f9013b47971
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-regs.h
@@ -0,0 +1,49 @@
+/*
+ * include/media/smiapp/smiapp-regs.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef SMIAPP_REGS_H
+#define SMIAPP_REGS_H
+
+#include <linux/i2c.h>
+#include <linux/types.h>
+
+/* Use upper 8 bits of the type field for flags */
+#define SMIA_REG_FLAG_FLOAT		(1 << 24)
+
+#define SMIA_REG_8BIT			1
+#define SMIA_REG_16BIT			2
+#define SMIA_REG_32BIT			4
+struct smia_reg {
+	u16 type;
+	u16 reg;			/* 16-bit offset */
+	u32 val;			/* 8/16/32-bit value */
+};
+
+struct smiapp_sensor;
+
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val);
+int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val);
+int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val);
+
+#endif
diff --git a/drivers/media/video/smiapp/smiapp.h b/drivers/media/video/smiapp/smiapp.h
new file mode 100644
index 000000000000..587f7f11238d
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp.h
@@ -0,0 +1,252 @@
+/*
+ * drivers/media/video/smiapp/smiapp.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_PRIV_H_
+#define __SMIAPP_PRIV_H_
+
+#include <linux/mutex.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <media/smiapp.h>
+
+#include "smiapp-pll.h"
+#include "smiapp-reg.h"
+#include "smiapp-regs.h"
+#include "smiapp-quirk.h"
+
+/*
+ * Standard SMIA++ constants
+ */
+#define SMIA_VERSION_1			10
+#define SMIAPP_VERSION_0_8		8 /* Draft 0.8 */
+#define SMIAPP_VERSION_0_9		9 /* Draft 0.9 */
+#define SMIAPP_VERSION_1		10
+
+#define SMIAPP_PROFILE_0		0
+#define SMIAPP_PROFILE_1		1
+#define SMIAPP_PROFILE_2		2
+
+#define SMIAPP_NVM_PAGE_SIZE		64	/* bytes */
+
+#define SMIAPP_RESET_DELAY_CLOCKS	2400
+#define SMIAPP_RESET_DELAY(clk)				\
+	(1000 +	(SMIAPP_RESET_DELAY_CLOCKS * 1000	\
+		 + (clk) / 1000 - 1) / ((clk) / 1000))
+
+#include "smiapp-limits.h"
+
+struct smiapp_quirk;
+
+#define SMIAPP_MODULE_IDENT_FLAG_REV_LE		(1 << 0)
+
+struct smiapp_module_ident {
+	u8 manufacturer_id;
+	u16 model_id;
+	u8 revision_number_major;
+
+	u8 flags;
+
+	char *name;
+	const struct smiapp_quirk *quirk;
+};
+
+struct smiapp_module_info {
+	u32 manufacturer_id;
+	u32 model_id;
+	u32 revision_number_major;
+	u32 revision_number_minor;
+
+	u32 module_year;
+	u32 module_month;
+	u32 module_day;
+
+	u32 sensor_manufacturer_id;
+	u32 sensor_model_id;
+	u32 sensor_revision_number;
+	u32 sensor_firmware_version;
+
+	u32 smia_version;
+	u32 smiapp_version;
+
+	u32 smiapp_profile;
+
+	char *name;
+	const struct smiapp_quirk *quirk;
+};
+
+#define SMIAPP_IDENT_FQ(manufacturer, model, rev, fl, _name, _quirk)	\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = fl,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT_LQ(manufacturer, model, rev, _name, _quirk)	\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = SMIAPP_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT_L(manufacturer, model, rev, _name)			\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = SMIAPP_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name, }
+
+#define SMIAPP_IDENT_Q(manufacturer, model, rev, _name, _quirk)		\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT(manufacturer, model, rev, _name)			\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name, }
+
+struct smiapp_reg_limits {
+	u32 addr;
+	char *what;
+};
+
+extern struct smiapp_reg_limits smiapp_reg_limits[];
+
+struct smiapp_csi_data_format {
+	u32 code;
+	u8 width;
+	u8 compressed;
+	u8 pixel_order;
+};
+
+#define SMIAPP_SUBDEVS			3
+
+#define SMIAPP_PA_PAD_SRC		0
+#define SMIAPP_PAD_SINK			0
+#define SMIAPP_PAD_SRC			1
+#define SMIAPP_PADS			2
+
+struct smiapp_binning_subtype {
+	u8 horizontal:4;
+	u8 vertical:4;
+} __packed;
+
+struct smiapp_subdev {
+	struct v4l2_subdev sd;
+	struct media_pad pads[2];
+	struct v4l2_rect sink_fmt;
+	struct v4l2_rect crop[2];
+	struct v4l2_rect compose; /* compose on sink */
+	unsigned short sink_pad;
+	unsigned short source_pad;
+	int npads;
+	struct smiapp_sensor *sensor;
+	struct v4l2_ctrl_handler ctrl_handler;
+};
+
+/*
+ * struct smiapp_sensor - Main device structure
+ */
+struct smiapp_sensor {
+	/*
+	 * "mutex" is used to serialise access to all fields here
+	 * except v4l2_ctrls at the end of the struct. "mutex" is also
+	 * used to serialise access to file handle specific
+	 * information. The exception to this rule is the power_mutex
+	 * below.
+	 */
+	struct mutex mutex;
+	/*
+	 * power_mutex is used to serialise power management related
+	 * activities. Acquiring "mutex" at that time isn't necessary
+	 * since there are no other users anyway.
+	 */
+	struct mutex power_mutex;
+	struct smiapp_subdev ssds[SMIAPP_SUBDEVS];
+	u32 ssds_used;
+	struct smiapp_subdev *src;
+	struct smiapp_subdev *binner;
+	struct smiapp_subdev *scaler;
+	struct smiapp_subdev *pixel_array;
+	struct smiapp_platform_data *platform_data;
+	struct regulator *vana;
+	struct clk *ext_clk;
+	u32 limits[SMIAPP_LIMIT_LAST];
+	u8 nbinning_subtypes;
+	struct smiapp_binning_subtype binning_subtypes[SMIAPP_BINNING_SUBTYPES];
+	u32 mbus_frame_fmts;
+	const struct smiapp_csi_data_format *csi_format;
+	const struct smiapp_csi_data_format *internal_csi_format;
+	u32 default_mbus_frame_fmts;
+	int default_pixel_order;
+
+	u8 binning_horizontal;
+	u8 binning_vertical;
+
+	u8 scale_m;
+	u8 scaling_mode;
+
+	u8 hvflip_inv_mask; /* H/VFLIP inversion due to sensor orientation */
+	u8 flash_capability;
+	u8 frame_skip;
+
+	int power_count;
+
+	bool streaming;
+	bool dev_init_done;
+
+	u8 *nvm;		/* nvm memory buffer */
+	unsigned int nvm_size;	/* bytes */
+
+	struct smiapp_module_info minfo;
+
+	struct smiapp_pll pll;
+
+	/* Pixel array controls */
+	struct v4l2_ctrl *analog_gain;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *pixel_rate_parray;
+	/* src controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate_csi;
+};
+
+#define to_smiapp_subdev(_sd)				\
+	container_of(_sd, struct smiapp_subdev, sd)
+
+#define to_smiapp_sensor(_sd)	\
+	(to_smiapp_subdev(_sd)->sensor)
+
+#endif /* __SMIAPP_PRIV_H_ */
diff --git a/drivers/media/video/sn9c102/sn9c102_core.c b/drivers/media/video/sn9c102/sn9c102_core.c
index c2882fa5be85..19ea780b16ff 100644
--- a/drivers/media/video/sn9c102/sn9c102_core.c
+++ b/drivers/media/video/sn9c102/sn9c102_core.c
@@ -995,10 +995,8 @@ static int sn9c102_stop_transfer(struct sn9c102_device* cam)
 
 static int sn9c102_stream_interrupt(struct sn9c102_device* cam)
 {
-	long timeout;
-
 	cam->stream = STREAM_INTERRUPT;
-	timeout = wait_event_timeout(cam->wait_stream,
+	wait_event_timeout(cam->wait_stream,
 				     (cam->stream == STREAM_OFF) ||
 				     (cam->state & DEV_DISCONNECTED),
 				     SN9C102_URB_TIMEOUT);
diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index aedb970d13f6..0421bf9453b4 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -164,35 +164,38 @@ static int soc_camera_try_fmt(struct soc_camera_device *icd,
 			      struct v4l2_format *f)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	int ret;
 
 	dev_dbg(icd->pdev, "TRY_FMT(%c%c%c%c, %ux%u)\n",
 		pixfmtstr(pix->pixelformat), pix->width, pix->height);
 
-	pix->bytesperline = 0;
-	pix->sizeimage = 0;
+	if (!(ici->capabilities & SOCAM_HOST_CAP_STRIDE)) {
+		pix->bytesperline = 0;
+		pix->sizeimage = 0;
+	}
 
 	ret = ici->ops->try_fmt(icd, f);
 	if (ret < 0)
 		return ret;
 
-	if (!pix->sizeimage) {
-		if (!pix->bytesperline) {
-			const struct soc_camera_format_xlate *xlate;
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate)
+		return -EINVAL;
+
+	ret = soc_mbus_bytes_per_line(pix->width, xlate->host_fmt);
+	if (ret < 0)
+		return ret;
+
+	pix->bytesperline = max_t(u32, pix->bytesperline, ret);
 
-			xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
-			if (!xlate)
-				return -EINVAL;
+	ret = soc_mbus_image_size(xlate->host_fmt, pix->bytesperline,
+				  pix->height);
+	if (ret < 0)
+		return ret;
 
-			ret = soc_mbus_bytes_per_line(pix->width,
-						      xlate->host_fmt);
-			if (ret > 0)
-				pix->bytesperline = ret;
-		}
-		if (pix->bytesperline)
-			pix->sizeimage = pix->bytesperline * pix->height;
-	}
+	pix->sizeimage = max_t(u32, pix->sizeimage, ret);
 
 	return 0;
 }
@@ -257,13 +260,13 @@ static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
 	return v4l2_subdev_call(sd, core, g_std, a);
 }
 
-static int soc_camera_enum_fsizes(struct file *file, void *fh,
+static int soc_camera_enum_framesizes(struct file *file, void *fh,
 					 struct v4l2_frmsizeenum *fsize)
 {
 	struct soc_camera_device *icd = file->private_data;
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
-	return ici->ops->enum_fsizes(icd, fsize);
+	return ici->ops->enum_framesizes(icd, fsize);
 }
 
 static int soc_camera_reqbufs(struct file *file, void *priv,
@@ -1244,8 +1247,8 @@ static int default_s_parm(struct soc_camera_device *icd,
 	return v4l2_subdev_call(sd, video, s_parm, parm);
 }
 
-static int default_enum_fsizes(struct soc_camera_device *icd,
-			  struct v4l2_frmsizeenum *fsize)
+static int default_enum_framesizes(struct soc_camera_device *icd,
+				   struct v4l2_frmsizeenum *fsize)
 {
 	int ret;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
@@ -1259,7 +1262,7 @@ static int default_enum_fsizes(struct soc_camera_device *icd,
 	/* map xlate-code to pixel_format, sensor only handle xlate-code*/
 	fsize_mbus.pixel_format = xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fsizes, &fsize_mbus);
+	ret = v4l2_subdev_call(sd, video, enum_framesizes, &fsize_mbus);
 	if (ret < 0)
 		return ret;
 
@@ -1298,8 +1301,8 @@ int soc_camera_host_register(struct soc_camera_host *ici)
 		ici->ops->set_parm = default_s_parm;
 	if (!ici->ops->get_parm)
 		ici->ops->get_parm = default_g_parm;
-	if (!ici->ops->enum_fsizes)
-		ici->ops->enum_fsizes = default_enum_fsizes;
+	if (!ici->ops->enum_framesizes)
+		ici->ops->enum_framesizes = default_enum_framesizes;
 
 	mutex_lock(&list_lock);
 	list_for_each_entry(ix, &hosts, list) {
@@ -1390,7 +1393,7 @@ static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
 	.vidioc_s_input		 = soc_camera_s_input,
 	.vidioc_s_std		 = soc_camera_s_std,
 	.vidioc_g_std		 = soc_camera_g_std,
-	.vidioc_enum_framesizes  = soc_camera_enum_fsizes,
+	.vidioc_enum_framesizes  = soc_camera_enum_framesizes,
 	.vidioc_reqbufs		 = soc_camera_reqbufs,
 	.vidioc_querybuf	 = soc_camera_querybuf,
 	.vidioc_qbuf		 = soc_camera_qbuf,
@@ -1429,6 +1432,10 @@ static int video_dev_create(struct soc_camera_device *icd)
 	vdev->tvnorms		= V4L2_STD_UNKNOWN;
 	vdev->ctrl_handler	= &icd->ctrl_handler;
 	vdev->lock		= &icd->video_lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags);
 
 	icd->vdev = vdev;
 
diff --git a/drivers/media/video/soc_mediabus.c b/drivers/media/video/soc_mediabus.c
index cf7f2194ded4..89dce097a827 100644
--- a/drivers/media/video/soc_mediabus.c
+++ b/drivers/media/video/soc_mediabus.c
@@ -24,6 +24,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_2X8,
@@ -33,6 +34,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_UYVY8_2X8,
@@ -42,6 +44,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_VYUY8_2X8,
@@ -51,6 +54,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
@@ -60,6 +64,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
@@ -69,6 +74,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_LE,
@@ -78,6 +84,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_BE,
@@ -87,6 +94,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR8_1X8,
@@ -96,6 +104,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_1X10,
@@ -105,6 +114,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_Y8_1X8,
@@ -114,6 +124,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_Y10_1X10,
@@ -123,6 +134,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE,
@@ -132,6 +144,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_LE,
@@ -141,6 +154,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADLO,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_BE,
@@ -150,6 +164,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_BE,
@@ -159,6 +174,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADLO,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_JPEG_1X8,
@@ -168,6 +184,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample        = 8,
 		.packing                = SOC_MBUS_PACKING_VARIABLE,
 		.order                  = SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB444_2X8_PADHI_BE,
@@ -177,6 +194,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YUYV8_1_5X8,
@@ -186,6 +204,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_1_5X8,
@@ -195,6 +214,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_UYVY8_1X16,
@@ -204,6 +224,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_VYUY8_1X16,
@@ -213,6 +234,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YUYV8_1X16,
@@ -222,6 +244,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_1X16,
@@ -231,6 +254,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG8_1X8,
@@ -240,6 +264,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8,
@@ -249,6 +274,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGBRG10_1X10,
@@ -258,6 +284,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG10_1X10,
@@ -267,6 +294,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SRGGB10_1X10,
@@ -276,6 +304,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR12_1X12,
@@ -285,6 +314,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGBRG12_1X12,
@@ -294,6 +324,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG12_1X12,
@@ -303,6 +334,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SRGGB12_1X12,
@@ -312,6 +344,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 },
 };
@@ -345,6 +378,9 @@ EXPORT_SYMBOL(soc_mbus_samples_per_pixel);
 
 s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 {
+	if (mf->layout != SOC_MBUS_LAYOUT_PACKED)
+		return width * mf->bits_per_sample / 8;
+
 	switch (mf->packing) {
 	case SOC_MBUS_PACKING_NONE:
 		return width * mf->bits_per_sample / 8;
@@ -361,6 +397,24 @@ s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 }
 EXPORT_SYMBOL(soc_mbus_bytes_per_line);
 
+s32 soc_mbus_image_size(const struct soc_mbus_pixelfmt *mf,
+			u32 bytes_per_line, u32 height)
+{
+	if (mf->layout == SOC_MBUS_LAYOUT_PACKED)
+		return bytes_per_line * height;
+
+	switch (mf->packing) {
+	case SOC_MBUS_PACKING_2X8_PADHI:
+	case SOC_MBUS_PACKING_2X8_PADLO:
+		return bytes_per_line * height * 2;
+	case SOC_MBUS_PACKING_1_5X8:
+		return bytes_per_line * height * 3 / 2;
+	default:
+		return -EINVAL;
+	}
+}
+EXPORT_SYMBOL(soc_mbus_image_size);
+
 const struct soc_mbus_pixelfmt *soc_mbus_find_fmtdesc(
 	enum v4l2_mbus_pixelcode code,
 	const struct soc_mbus_lookup *lookup,
diff --git a/drivers/media/video/sta2x11_vip.c b/drivers/media/video/sta2x11_vip.c
new file mode 100644
index 000000000000..4c10205264d4
--- /dev/null
+++ b/drivers/media/video/sta2x11_vip.c
@@ -0,0 +1,1550 @@
+/*
+ * This is the driver for the STA2x11 Video Input Port.
+ *
+ * Copyright (C) 2010       WindRiver Systems, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Author: Andreas Kies <andreas.kies@windriver.com>
+ *		Vlad Lungu <vlad.lungu@windriver.com>
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/vmalloc.h>
+
+#include <linux/videodev2.h>
+
+#include <linux/kmod.h>
+
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf-dma-contig.h>
+
+#include "sta2x11_vip.h"
+
+#define DRV_NAME "sta2x11_vip"
+#define DRV_VERSION "1.3"
+
+#ifndef PCI_DEVICE_ID_STMICRO_VIP
+#define PCI_DEVICE_ID_STMICRO_VIP 0xCC0D
+#endif
+
+#define MAX_FRAMES 4
+
+/*Register offsets*/
+#define DVP_CTL		0x00
+#define DVP_TFO		0x04
+#define DVP_TFS		0x08
+#define DVP_BFO		0x0C
+#define DVP_BFS		0x10
+#define DVP_VTP         0x14
+#define DVP_VBP         0x18
+#define DVP_VMP		0x1C
+#define DVP_ITM		0x98
+#define DVP_ITS		0x9C
+#define DVP_STA		0xA0
+#define DVP_HLFLN	0xA8
+#define DVP_RGB		0xC0
+#define DVP_PKZ		0xF0
+
+/*Register fields*/
+#define DVP_CTL_ENA	0x00000001
+#define DVP_CTL_RST	0x80000000
+#define DVP_CTL_DIS	(~0x00040001)
+
+#define DVP_IT_VSB	0x00000008
+#define DVP_IT_VST	0x00000010
+#define DVP_IT_FIFO	0x00000020
+
+#define DVP_HLFLN_SD	0x00000001
+
+#define REG_WRITE(vip, reg, value) iowrite32((value), (vip->iomem)+(reg))
+#define REG_READ(vip, reg) ioread32((vip->iomem)+(reg))
+
+#define SAVE_COUNT 8
+#define AUX_COUNT 3
+#define IRQ_COUNT 1
+
+/**
+ * struct sta2x11_vip - All internal data for one instance of device
+ * @v4l2_dev: device registered in v4l layer
+ * @video_dev: properties of our device
+ * @pdev: PCI device
+ * @adapter: contains I2C adapter information
+ * @register_save_area: All relevant register are saved here during suspend
+ * @decoder: contains information about video DAC
+ * @format: pixel format, fixed UYVY
+ * @std: video standard (e.g. PAL/NTSC)
+ * @input: input line for video signal ( 0 or 1 )
+ * @users: Number of open of device ( max. 1 )
+ * @disabled: Device is in power down state
+ * @mutex: ensures exclusive opening of device
+ * @slock: for excluse acces of registers
+ * @vb_vidq: queue maintained by videobuf layer
+ * @capture: linked list of capture buffer
+ * @active: struct videobuf_buffer currently beingg filled
+ * @started: device is ready to capture frame
+ * @closing: device will be shut down
+ * @tcount: Number of top frames
+ * @bcount: Number of bottom frames
+ * @overflow: Number of FIFO overflows
+ * @mem_spare: small buffer of unused frame
+ * @dma_spare: dma addres of mem_spare
+ * @iomem: hardware base address
+ * @config: I2C and gpio config from platform
+ *
+ * All non-local data is accessed via this structure.
+ */
+
+struct sta2x11_vip {
+	struct v4l2_device v4l2_dev;
+	struct video_device *video_dev;
+	struct pci_dev *pdev;
+	struct i2c_adapter *adapter;
+	unsigned int register_save_area[IRQ_COUNT + SAVE_COUNT + AUX_COUNT];
+	struct v4l2_subdev *decoder;
+	struct v4l2_pix_format format;
+	v4l2_std_id std;
+	unsigned int input;
+	int users;
+	int disabled;
+	struct mutex mutex;	/* exclusive access during open */
+	spinlock_t slock;	/* spin lock for hardware and queue access */
+	struct videobuf_queue vb_vidq;
+	struct list_head capture;
+	struct videobuf_buffer *active;
+	int started, closing, tcount, bcount;
+	int overflow;
+	void *mem_spare;
+	dma_addr_t dma_spare;
+	void *iomem;
+	struct vip_config *config;
+};
+
+static const unsigned int registers_to_save[AUX_COUNT] = {
+	DVP_HLFLN, DVP_RGB, DVP_PKZ
+};
+
+static struct v4l2_pix_format formats_50[] = {
+	{			/*PAL interlaced */
+	 .width = 720,
+	 .height = 576,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_INTERLACED,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 576,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*PAL top */
+	 .width = 720,
+	 .height = 288,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_TOP,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 288,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*PAL bottom */
+	 .width = 720,
+	 .height = 288,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_BOTTOM,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 288,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+
+};
+
+static struct v4l2_pix_format formats_60[] = {
+	{			/*NTSC interlaced */
+	 .width = 720,
+	 .height = 480,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_INTERLACED,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 480,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*NTSC top */
+	 .width = 720,
+	 .height = 240,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_TOP,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 240,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*NTSC bottom */
+	 .width = 720,
+	 .height = 240,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_BOTTOM,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 240,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+};
+
+/**
+ * buf_setup - Get size and number of video buffer
+ * @vq: queue in videobuf
+ * @count: Number of buffers (1..MAX_FRAMES).
+ *		0 use default value.
+ * @size:  size of buffer in bytes
+ *
+ * returns size and number of buffers
+ * a preset value of 0 returns the default number.
+ * return value: 0, always succesfull.
+ */
+static int buf_setup(struct videobuf_queue *vq, unsigned int *count,
+		     unsigned int *size)
+{
+	struct sta2x11_vip *vip = vq->priv_data;
+
+	*size = vip->format.width * vip->format.height * 2;
+	if (0 == *count || MAX_FRAMES < *count)
+		*count = MAX_FRAMES;
+	return 0;
+};
+
+/**
+ * buf_prepare - prepare buffer for usage
+ * @vq: queue in videobuf layer
+ * @vb: buffer to be prepared
+ * @field: type of video data (interlaced/non-interlaced)
+ *
+ * Allocate or realloc buffer
+ * return value: 0, successful.
+ *
+ * -EINVAL, supplied buffer is too small.
+ *
+ *  other, buffer could not be locked.
+ */
+static int buf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
+		       enum v4l2_field field)
+{
+	struct sta2x11_vip *vip = vq->priv_data;
+	int ret;
+
+	vb->size = vip->format.width * vip->format.height * 2;
+	if ((0 != vb->baddr) && (vb->bsize < vb->size))
+		return -EINVAL;
+	vb->width = vip->format.width;
+	vb->height = vip->format.height;
+	vb->field = field;
+
+	if (VIDEOBUF_NEEDS_INIT == vb->state) {
+		ret = videobuf_iolock(vq, vb, NULL);
+		if (ret)
+			goto fail;
+	}
+	vb->state = VIDEOBUF_PREPARED;
+	return 0;
+fail:
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+	return ret;
+}
+
+/**
+ * buf_queu - queue buffer for filling
+ * @vq: queue in videobuf layer
+ * @vb: buffer to be queued
+ *
+ * if capturing is already running, the buffer will be queued. Otherwise
+ * capture is started and the buffer is used directly.
+ */
+static void buf_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct sta2x11_vip *vip = vq->priv_data;
+	u32 dma;
+
+	vb->state = VIDEOBUF_QUEUED;
+
+	if (vip->active) {
+		list_add_tail(&vb->queue, &vip->capture);
+		return;
+	}
+
+	vip->started = 1;
+	vip->tcount = 0;
+	vip->bcount = 0;
+	vip->active = vb;
+	vb->state = VIDEOBUF_ACTIVE;
+
+	dma = videobuf_to_dma_contig(vb);
+
+	REG_WRITE(vip, DVP_TFO, (0 << 16) | (0));
+	/* despite of interlace mode, upper and lower frames start at zero */
+	REG_WRITE(vip, DVP_BFO, (0 << 16) | (0));
+
+	switch (vip->format.field) {
+	case V4L2_FIELD_INTERLACED:
+		REG_WRITE(vip, DVP_TFS,
+			  ((vip->format.height / 2 - 1) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_BFS, ((vip->format.height / 2 - 1) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_VTP, dma);
+		REG_WRITE(vip, DVP_VBP, dma + vip->format.width * 2);
+		REG_WRITE(vip, DVP_VMP, 4 * vip->format.width);
+		break;
+	case V4L2_FIELD_TOP:
+		REG_WRITE(vip, DVP_TFS,
+			  ((vip->format.height - 1) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_BFS, ((0) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_VTP, dma);
+		REG_WRITE(vip, DVP_VBP, dma);
+		REG_WRITE(vip, DVP_VMP, 2 * vip->format.width);
+		break;
+	case V4L2_FIELD_BOTTOM:
+		REG_WRITE(vip, DVP_TFS, ((0) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_BFS,
+			  ((vip->format.height) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_VTP, dma);
+		REG_WRITE(vip, DVP_VBP, dma);
+		REG_WRITE(vip, DVP_VMP, 2 * vip->format.width);
+		break;
+
+	default:
+		pr_warning("VIP: unknown field format\n");
+		return;
+	}
+
+	REG_WRITE(vip, DVP_CTL, DVP_CTL_ENA);
+}
+
+/**
+ * buff_release - release buffer
+ * @vq: queue in videobuf layer
+ * @vb: buffer to be released
+ *
+ * release buffer in videobuf layer
+ */
+static void buf_release(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static struct videobuf_queue_ops vip_qops = {
+	.buf_setup = buf_setup,
+	.buf_prepare = buf_prepare,
+	.buf_queue = buf_queue,
+	.buf_release = buf_release,
+};
+
+/**
+ * vip_open - open video device
+ * @file: descriptor of device
+ *
+ * open device, make sure it is only opened once.
+ * return value: 0, no error.
+ *
+ * -EBUSY, device is already opened
+ *
+ * -ENOMEM, no memory for auxiliary DMA buffer
+ */
+static int vip_open(struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	mutex_lock(&vip->mutex);
+	vip->users++;
+
+	if (vip->users > 1) {
+		vip->users--;
+		mutex_unlock(&vip->mutex);
+		return -EBUSY;
+	}
+
+	file->private_data = dev;
+	vip->overflow = 0;
+	vip->started = 0;
+	vip->closing = 0;
+	vip->active = NULL;
+
+	INIT_LIST_HEAD(&vip->capture);
+	vip->mem_spare = dma_alloc_coherent(&vip->pdev->dev, 64,
+					    &vip->dma_spare, GFP_KERNEL);
+	if (!vip->mem_spare) {
+		vip->users--;
+		mutex_unlock(&vip->mutex);
+		return -ENOMEM;
+	}
+
+	mutex_unlock(&vip->mutex);
+	videobuf_queue_dma_contig_init_cached(&vip->vb_vidq,
+					      &vip_qops,
+					      &vip->pdev->dev,
+					      &vip->slock,
+					      V4L2_BUF_TYPE_VIDEO_CAPTURE,
+					      V4L2_FIELD_INTERLACED,
+					      sizeof(struct videobuf_buffer),
+					      vip, NULL);
+	REG_READ(vip, DVP_ITS);
+	REG_WRITE(vip, DVP_HLFLN, DVP_HLFLN_SD);
+	REG_WRITE(vip, DVP_ITM, DVP_IT_VSB | DVP_IT_VST);
+	REG_WRITE(vip, DVP_CTL, DVP_CTL_RST);
+	REG_WRITE(vip, DVP_CTL, 0);
+	REG_READ(vip, DVP_ITS);
+	return 0;
+}
+
+/**
+ * vip_close - close video device
+ * @file: descriptor of device
+ *
+ * close video device, wait until all pending operations are finished
+ * ( maximum FRAME_MAX buffers pending )
+ * Turn off interrupts.
+ *
+ * return value: 0, always succesful.
+ */
+static int vip_close(struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	vip->closing = 1;
+	if (vip->active)
+		videobuf_waiton(&vip->vb_vidq, vip->active, 0, 0);
+	spin_lock_irq(&vip->slock);
+
+	REG_WRITE(vip, DVP_ITM, 0);
+	REG_WRITE(vip, DVP_CTL, DVP_CTL_RST);
+	REG_WRITE(vip, DVP_CTL, 0);
+	REG_READ(vip, DVP_ITS);
+
+	vip->started = 0;
+	vip->active = NULL;
+
+	spin_unlock_irq(&vip->slock);
+
+	videobuf_stop(&vip->vb_vidq);
+	videobuf_mmap_free(&vip->vb_vidq);
+
+	dma_free_coherent(&vip->pdev->dev, 64, vip->mem_spare, vip->dma_spare);
+	file->private_data = NULL;
+	mutex_lock(&vip->mutex);
+	vip->users--;
+	mutex_unlock(&vip->mutex);
+	return 0;
+}
+
+/**
+ * vip_read - read from video input
+ * @file: descriptor of device
+ * @data: user buffer
+ * @count: number of bytes to be read
+ * @ppos: position within stream
+ *
+ * read video data from video device.
+ * handling is done in generic videobuf layer
+ * return value: provided by videobuf layer
+ */
+static ssize_t vip_read(struct file *file, char __user *data,
+			size_t count, loff_t *ppos)
+{
+	struct video_device *dev = file->private_data;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_read_stream(&vip->vb_vidq, data, count, ppos, 0,
+				    file->f_flags & O_NONBLOCK);
+}
+
+/**
+ * vip_mmap - map user buffer
+ * @file: descriptor of device
+ * @vma: user buffer
+ *
+ * map user space buffer into kernel mode, including DMA address.
+ * handling is done in generic videobuf layer.
+ * return value: provided by videobuf layer
+ */
+static int vip_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = file->private_data;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_mmap_mapper(&vip->vb_vidq, vma);
+}
+
+/**
+ * vip_poll - poll for event
+ * @file: descriptor of device
+ * @wait: contains events to be waited for
+ *
+ * wait for event related to video device.
+ * handling is done in generic videobuf layer.
+ * return value: provided by videobuf layer
+ */
+static unsigned int vip_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct video_device *dev = file->private_data;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_poll_stream(file, &vip->vb_vidq, wait);
+}
+
+/**
+ * vidioc_querycap - return capabilities of device
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @cap: contains return values
+ *
+ * the capabilities of the device are returned
+ *
+ * return value: 0, no error.
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	memset(cap, 0, sizeof(struct v4l2_capability));
+	strcpy(cap->driver, DRV_NAME);
+	strcpy(cap->card, DRV_NAME);
+	cap->version = 0;
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
+		 pci_name(vip->pdev));
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+	    V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+/**
+ * vidioc_s_std - set video standard
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @std: contains standard to be set
+ *
+ * the video standard is set
+ *
+ * return value: 0, no error.
+ *
+ * -EIO, no input signal detected
+ *
+ * other, returned from video DAC.
+ */
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	v4l2_std_id oldstd = vip->std, newstd;
+	int status;
+
+	if (V4L2_STD_ALL == *std) {
+		v4l2_subdev_call(vip->decoder, core, s_std, *std);
+		ssleep(2);
+		v4l2_subdev_call(vip->decoder, video, querystd, &newstd);
+		v4l2_subdev_call(vip->decoder, video, g_input_status, &status);
+		if (status & V4L2_IN_ST_NO_SIGNAL)
+			return -EIO;
+		*std = vip->std = newstd;
+		if (oldstd != *std) {
+			if (V4L2_STD_525_60 & (*std))
+				vip->format = formats_60[0];
+			else
+				vip->format = formats_50[0];
+		}
+		return 0;
+	}
+
+	if (oldstd != *std) {
+		if (V4L2_STD_525_60 & (*std))
+			vip->format = formats_60[0];
+		else
+			vip->format = formats_50[0];
+	}
+
+	return v4l2_subdev_call(vip->decoder, core, s_std, *std);
+}
+
+/**
+ * vidioc_g_std - get video standard
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @std: contains return values
+ *
+ * the current video standard is returned
+ *
+ * return value: 0, no error.
+ */
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	*std = vip->std;
+	return 0;
+}
+
+/**
+ * vidioc_querystd - get possible video standards
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @std: contains return values
+ *
+ * all possible video standards are returned
+ *
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, video, querystd, std);
+
+}
+
+/**
+ * vidioc_queryctl - get possible control settings
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @ctrl: contains return values
+ *
+ * return possible values for a control
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_queryctrl(struct file *file, void *priv,
+			    struct v4l2_queryctrl *ctrl)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, core, queryctrl, ctrl);
+}
+
+/**
+ * vidioc_g_ctl - get control value
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @ctrl: contains return values
+ *
+ * return setting for a control value
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_g_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *ctrl)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, core, g_ctrl, ctrl);
+}
+
+/**
+ * vidioc_s_ctl - set control value
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @ctrl: contains value to be set
+ *
+ * set value for a specific control
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_s_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *ctrl)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, core, s_ctrl, ctrl);
+}
+
+/**
+ * vidioc_enum_input - return name of input line
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @inp: contains return values
+ *
+ * the user friendly name of the input line is returned
+ *
+ * return value: 0, no error.
+ *
+ * -EINVAL, input line number out of range
+ */
+static int vidioc_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *inp)
+{
+	if (inp->index > 1)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->std = V4L2_STD_ALL;
+	sprintf(inp->name, "Camera %u", inp->index);
+
+	return 0;
+}
+
+/**
+ * vidioc_s_input - set input line
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @i: new input line number
+ *
+ * the current active input line is set
+ *
+ * return value: 0, no error.
+ *
+ * -EINVAL, line number out of range
+ */
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	int ret;
+
+	if (i > 1)
+		return -EINVAL;
+	ret = v4l2_subdev_call(vip->decoder, video, s_routing, i, 0, 0);
+
+	if (!ret)
+		vip->input = i;
+
+	return 0;
+}
+
+/**
+ * vidioc_g_input - return input line
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @i: returned input line number
+ *
+ * the current active input line is returned
+ *
+ * return value: always 0.
+ */
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	*i = vip->input;
+	return 0;
+}
+
+/**
+ * vidioc_enum_fmt_vid_cap - return video capture format
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: returned format information
+ *
+ * returns name and format of video capture
+ * Only UYVY is supported by hardware.
+ *
+ * return value: always 0.
+ */
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+
+	if (f->index != 0)
+		return -EINVAL;
+
+	strcpy(f->description, "4:2:2, packed, UYVY");
+	f->pixelformat = V4L2_PIX_FMT_UYVY;
+	f->flags = 0;
+	return 0;
+}
+
+/**
+ * vidioc_try_fmt_vid_cap - set video capture format
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: new format
+ *
+ * new video format is set which includes width and
+ * field type. width is fixed to 720, no scaling.
+ * Only UYVY is supported by this hardware.
+ * the minimum height is 200, the maximum is 576 (PAL)
+ *
+ * return value: 0, no error
+ *
+ * -EINVAL, pixel or field format not supported
+ *
+ */
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	int interlace_lim;
+
+	if (V4L2_PIX_FMT_UYVY != f->fmt.pix.pixelformat)
+		return -EINVAL;
+
+	if (V4L2_STD_525_60 & vip->std)
+		interlace_lim = 240;
+	else
+		interlace_lim = 288;
+
+	switch (f->fmt.pix.field) {
+	case V4L2_FIELD_ANY:
+		if (interlace_lim < f->fmt.pix.height)
+			f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+		else
+			f->fmt.pix.field = V4L2_FIELD_BOTTOM;
+		break;
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+		if (interlace_lim < f->fmt.pix.height)
+			f->fmt.pix.height = interlace_lim;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	f->fmt.pix.height &= ~1;
+	if (2 * interlace_lim < f->fmt.pix.height)
+		f->fmt.pix.height = 2 * interlace_lim;
+	if (200 > f->fmt.pix.height)
+		f->fmt.pix.height = 200;
+	f->fmt.pix.width = 720;
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.width * 2 * f->fmt.pix.height;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+	return 0;
+}
+
+/**
+ * vidioc_s_fmt_vid_cap - set current video format parameters
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: returned format information
+ *
+ * set new capture format
+ * return value: 0, no error
+ *
+ * other, delivered by video DAC routine.
+ */
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	int ret;
+
+	ret = vidioc_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	memcpy(&vip->format, &f->fmt.pix, sizeof(struct v4l2_pix_format));
+	return 0;
+}
+
+/**
+ * vidioc_g_fmt_vid_cap - get current video format parameters
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: contains format information
+ *
+ * returns current video format parameters
+ *
+ * return value: 0, always successful
+ */
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	memcpy(&f->fmt.pix, &vip->format, sizeof(struct v4l2_pix_format));
+	return 0;
+}
+
+/**
+ * vidioc_reqfs - request buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_reqbufs(&vip->vb_vidq, p);
+}
+
+/**
+ * vidioc_querybuf - query buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * query buffer state.
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_querybuf(&vip->vb_vidq, p);
+}
+
+/**
+ * vidioc_qbuf - queue a buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_qbuf(&vip->vb_vidq, p);
+}
+
+/**
+ * vidioc_dqbuf - dequeue a buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_dqbuf(&vip->vb_vidq, p, file->f_flags & O_NONBLOCK);
+}
+
+/**
+ * vidioc_streamon - turn on streaming
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @type: type of capture
+ *
+ * turn on streaming.
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type type)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_streamon(&vip->vb_vidq);
+}
+
+/**
+ * vidioc_streamoff - turn off streaming
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @type: type of capture
+ *
+ * turn off streaming.
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_streamoff(&vip->vb_vidq);
+}
+
+static const struct v4l2_file_operations vip_fops = {
+	.owner = THIS_MODULE,
+	.open = vip_open,
+	.release = vip_close,
+	.ioctl = video_ioctl2,
+	.read = vip_read,
+	.mmap = vip_mmap,
+	.poll = vip_poll
+};
+
+static const struct v4l2_ioctl_ops vip_ioctl_ops = {
+	.vidioc_querycap = vidioc_querycap,
+	.vidioc_s_std = vidioc_s_std,
+	.vidioc_g_std = vidioc_g_std,
+	.vidioc_querystd = vidioc_querystd,
+	.vidioc_queryctrl = vidioc_queryctrl,
+	.vidioc_g_ctrl = vidioc_g_ctrl,
+	.vidioc_s_ctrl = vidioc_s_ctrl,
+	.vidioc_enum_input = vidioc_enum_input,
+	.vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
+	.vidioc_s_input = vidioc_s_input,
+	.vidioc_g_input = vidioc_g_input,
+	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
+	.vidioc_reqbufs = vidioc_reqbufs,
+	.vidioc_querybuf = vidioc_querybuf,
+	.vidioc_qbuf = vidioc_qbuf,
+	.vidioc_dqbuf = vidioc_dqbuf,
+	.vidioc_streamon = vidioc_streamon,
+	.vidioc_streamoff = vidioc_streamoff,
+};
+
+static struct video_device video_dev_template = {
+	.name = DRV_NAME,
+	.release = video_device_release,
+	.fops = &vip_fops,
+	.ioctl_ops = &vip_ioctl_ops,
+	.tvnorms = V4L2_STD_ALL,
+};
+
+/**
+ * vip_irq - interrupt routine
+ * @irq: Number of interrupt ( not used, correct number is assumed )
+ * @vip: local data structure containing all information
+ *
+ * check for both frame interrupts set ( top and bottom ).
+ * check FIFO overflow, but limit number of log messages after open.
+ * signal a complete buffer if done.
+ * dequeue a new buffer if available.
+ * disable VIP if no buffer available.
+ *
+ * return value: IRQ_NONE, interrupt was not generated by VIP
+ *
+ * IRQ_HANDLED, interrupt done.
+ */
+static irqreturn_t vip_irq(int irq, struct sta2x11_vip *vip)
+{
+	u32 status, dma;
+	unsigned long flags;
+	struct videobuf_buffer *vb;
+
+	status = REG_READ(vip, DVP_ITS);
+
+	if (!status) {
+		pr_debug("VIP: irq ignored\n");
+		return IRQ_NONE;
+	}
+
+	if (!vip->started)
+		return IRQ_HANDLED;
+
+	if (status & DVP_IT_VSB)
+		vip->bcount++;
+
+	if (status & DVP_IT_VST)
+		vip->tcount++;
+
+	if ((DVP_IT_VSB | DVP_IT_VST) == (status & (DVP_IT_VST | DVP_IT_VSB))) {
+		/* this is bad, we are too slow, hope the condition is gone
+		 * on the next frame */
+		pr_info("VIP: both irqs\n");
+		return IRQ_HANDLED;
+	}
+
+	if (status & DVP_IT_FIFO) {
+		if (5 > vip->overflow++)
+			pr_info("VIP: fifo overflow\n");
+	}
+
+	if (2 > vip->tcount)
+		return IRQ_HANDLED;
+
+	if (status & DVP_IT_VSB)
+		return IRQ_HANDLED;
+
+	spin_lock_irqsave(&vip->slock, flags);
+
+	REG_WRITE(vip, DVP_CTL, REG_READ(vip, DVP_CTL) & ~DVP_CTL_ENA);
+	if (vip->active) {
+		do_gettimeofday(&vip->active->ts);
+		vip->active->field_count++;
+		vip->active->state = VIDEOBUF_DONE;
+		wake_up(&vip->active->done);
+		vip->active = NULL;
+	}
+	if (!vip->closing) {
+		if (list_empty(&vip->capture))
+			goto done;
+
+		vb = list_first_entry(&vip->capture, struct videobuf_buffer,
+				      queue);
+		if (NULL == vb) {
+			pr_info("VIP: no buffer\n");
+			goto done;
+		}
+		vb->state = VIDEOBUF_ACTIVE;
+		list_del(&vb->queue);
+		vip->active = vb;
+		dma = videobuf_to_dma_contig(vb);
+		switch (vip->format.field) {
+		case V4L2_FIELD_INTERLACED:
+			REG_WRITE(vip, DVP_VTP, dma);
+			REG_WRITE(vip, DVP_VBP, dma + vip->format.width * 2);
+			break;
+		case V4L2_FIELD_TOP:
+		case V4L2_FIELD_BOTTOM:
+			REG_WRITE(vip, DVP_VTP, dma);
+			REG_WRITE(vip, DVP_VBP, dma);
+			break;
+		default:
+			pr_warning("VIP: unknown field format\n");
+			goto done;
+			break;
+		}
+		REG_WRITE(vip, DVP_CTL, REG_READ(vip, DVP_CTL) | DVP_CTL_ENA);
+	}
+done:
+	spin_unlock_irqrestore(&vip->slock, flags);
+	return IRQ_HANDLED;
+}
+
+/**
+ * vip_gpio_reserve - reserve gpio pin
+ * @dev: device
+ * @pin: GPIO pin number
+ * @dir: direction, input or output
+ * @name: GPIO pin name
+ *
+ */
+static int vip_gpio_reserve(struct device *dev, int pin, int dir,
+			    const char *name)
+{
+	int ret;
+
+	if (pin == -1)
+		return 0;
+
+	ret = gpio_request(pin, name);
+	if (ret) {
+		dev_err(dev, "Failed to allocate pin %d (%s)\n", pin, name);
+		return ret;
+	}
+
+	ret = gpio_direction_output(pin, dir);
+	if (ret) {
+		dev_err(dev, "Failed to set direction for pin %d (%s)\n",
+			pin, name);
+		gpio_free(pin);
+		return ret;
+	}
+
+	ret = gpio_export(pin, false);
+	if (ret) {
+		dev_err(dev, "Failed to export pin %d (%s)\n", pin, name);
+		gpio_free(pin);
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * vip_gpio_release - release gpio pin
+ * @dev: device
+ * @pin: GPIO pin number
+ * @name: GPIO pin name
+ *
+ */
+static void vip_gpio_release(struct device *dev, int pin, const char *name)
+{
+	if (pin != -1) {
+		dev_dbg(dev, "releasing pin %d (%s)\n",	pin, name);
+		gpio_unexport(pin);
+		gpio_free(pin);
+	}
+}
+
+/**
+ * sta2x11_vip_init_one - init one instance of video device
+ * @pdev: PCI device
+ * @ent: (not used)
+ *
+ * allocate reset pins for DAC.
+ * Reset video DAC, this is done via reset line.
+ * allocate memory for managing device
+ * request interrupt
+ * map IO region
+ * register device
+ * find and initialize video DAC
+ *
+ * return value: 0, no error
+ *
+ * -ENOMEM, no memory
+ *
+ * -ENODEV, device could not be detected or registered
+ */
+static int __devinit sta2x11_vip_init_one(struct pci_dev *pdev,
+					  const struct pci_device_id *ent)
+{
+	int ret;
+	struct sta2x11_vip *vip;
+	struct vip_config *config;
+
+	ret = pci_enable_device(pdev);
+	if (ret)
+		return ret;
+
+	config = dev_get_platdata(&pdev->dev);
+	if (!config) {
+		dev_info(&pdev->dev, "VIP slot disabled\n");
+		ret = -EINVAL;
+		goto disable;
+	}
+
+	ret = vip_gpio_reserve(&pdev->dev, config->pwr_pin, 0,
+			       config->pwr_name);
+	if (ret)
+		goto disable;
+
+	if (config->reset_pin >= 0) {
+		ret = vip_gpio_reserve(&pdev->dev, config->reset_pin, 0,
+				       config->reset_name);
+		if (ret) {
+			vip_gpio_release(&pdev->dev, config->pwr_pin,
+					 config->pwr_name);
+			goto disable;
+		}
+	}
+
+	if (config->pwr_pin != -1) {
+		/* Datasheet says 5ms between PWR and RST */
+		usleep_range(5000, 25000);
+		ret = gpio_direction_output(config->pwr_pin, 1);
+	}
+
+	if (config->reset_pin != -1) {
+		/* Datasheet says 5ms between PWR and RST */
+		usleep_range(5000, 25000);
+		ret = gpio_direction_output(config->reset_pin, 1);
+	}
+	usleep_range(5000, 25000);
+
+	vip = kzalloc(sizeof(struct sta2x11_vip), GFP_KERNEL);
+	if (!vip) {
+		ret = -ENOMEM;
+		goto release_gpios;
+	}
+
+	vip->pdev = pdev;
+	vip->std = V4L2_STD_PAL;
+	vip->format = formats_50[0];
+	vip->config = config;
+
+	if (v4l2_device_register(&pdev->dev, &vip->v4l2_dev))
+		goto free_mem;
+
+	dev_dbg(&pdev->dev, "BAR #0 at 0x%lx 0x%lx irq %d\n",
+		(unsigned long)pci_resource_start(pdev, 0),
+		(unsigned long)pci_resource_len(pdev, 0), pdev->irq);
+
+	pci_set_master(pdev);
+
+	ret = pci_request_regions(pdev, DRV_NAME);
+	if (ret)
+		goto unreg;
+
+	vip->iomem = pci_iomap(pdev, 0, 0x100);
+	if (!vip->iomem) {
+		ret = -ENOMEM; /* FIXME */
+		goto release;
+	}
+
+	pci_enable_msi(pdev);
+
+	INIT_LIST_HEAD(&vip->capture);
+	spin_lock_init(&vip->slock);
+	mutex_init(&vip->mutex);
+	vip->started = 0;
+	vip->disabled = 0;
+
+	ret = request_irq(pdev->irq,
+			  (irq_handler_t) vip_irq,
+			  IRQF_SHARED, DRV_NAME, vip);
+	if (ret) {
+		dev_err(&pdev->dev, "request_irq failed\n");
+		ret = -ENODEV;
+		goto unmap;
+	}
+
+	vip->video_dev = video_device_alloc();
+	if (!vip->video_dev) {
+		ret = -ENOMEM;
+		goto release_irq;
+	}
+
+	*(vip->video_dev) = video_dev_template;
+	video_set_drvdata(vip->video_dev, vip);
+
+	ret = video_register_device(vip->video_dev, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto vrelease;
+
+	vip->adapter = i2c_get_adapter(vip->config->i2c_id);
+	if (!vip->adapter) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "no I2C adapter found\n");
+		goto vunreg;
+	}
+
+	vip->decoder = v4l2_i2c_new_subdev(&vip->v4l2_dev, vip->adapter,
+					   "adv7180", vip->config->i2c_addr,
+					   NULL);
+	if (!vip->decoder) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "no decoder found\n");
+		goto vunreg;
+	}
+
+	i2c_put_adapter(vip->adapter);
+
+	v4l2_subdev_call(vip->decoder, core, init, 0);
+
+	pr_info("STA2X11 Video Input Port (VIP) loaded\n");
+	return 0;
+
+vunreg:
+	video_set_drvdata(vip->video_dev, NULL);
+vrelease:
+	if (video_is_registered(vip->video_dev))
+		video_unregister_device(vip->video_dev);
+	else
+		video_device_release(vip->video_dev);
+release_irq:
+	free_irq(pdev->irq, vip);
+	pci_disable_msi(pdev);
+unmap:
+	pci_iounmap(pdev, vip->iomem);
+	mutex_destroy(&vip->mutex);
+release:
+	pci_release_regions(pdev);
+unreg:
+	v4l2_device_unregister(&vip->v4l2_dev);
+free_mem:
+	kfree(vip);
+release_gpios:
+	vip_gpio_release(&pdev->dev, config->reset_pin, config->reset_name);
+	vip_gpio_release(&pdev->dev, config->pwr_pin, config->pwr_name);
+disable:
+	/*
+	 * do not call pci_disable_device on sta2x11 because it break all
+	 * other Bus masters on this EP
+	 */
+	return ret;
+}
+
+/**
+ * sta2x11_vip_remove_one - release device
+ * @pdev: PCI device
+ *
+ * Undo everything done in .._init_one
+ *
+ * unregister video device
+ * free interrupt
+ * unmap ioadresses
+ * free memory
+ * free GPIO pins
+ */
+static void __devexit sta2x11_vip_remove_one(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct sta2x11_vip *vip =
+	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
+
+	video_set_drvdata(vip->video_dev, NULL);
+	video_unregister_device(vip->video_dev);
+	/*do not call video_device_release() here, is already done */
+	free_irq(pdev->irq, vip);
+	pci_disable_msi(pdev);
+	pci_iounmap(pdev, vip->iomem);
+	pci_release_regions(pdev);
+
+	v4l2_device_unregister(&vip->v4l2_dev);
+	mutex_destroy(&vip->mutex);
+
+	vip_gpio_release(&pdev->dev, vip->config->pwr_pin,
+			 vip->config->pwr_name);
+	vip_gpio_release(&pdev->dev, vip->config->reset_pin,
+			 vip->config->reset_name);
+
+	kfree(vip);
+	/*
+	 * do not call pci_disable_device on sta2x11 because it break all
+	 * other Bus masters on this EP
+	 */
+}
+
+#ifdef CONFIG_PM
+
+/**
+ * sta2x11_vip_suspend - set device into power save mode
+ * @pdev: PCI device
+ * @state: new state of device
+ *
+ * all relevant registers are saved and an attempt to set a new state is made.
+ *
+ * return value: 0 always indicate success,
+ * even if device could not be disabled. (workaround for hardware problem)
+ *
+ * reurn value : 0, always succesful, even if hardware does not not support
+ * power down mode.
+ */
+static int sta2x11_vip_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct sta2x11_vip *vip =
+	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&vip->slock, flags);
+	vip->register_save_area[0] = REG_READ(vip, DVP_CTL);
+	REG_WRITE(vip, DVP_CTL, vip->register_save_area[0] & DVP_CTL_DIS);
+	vip->register_save_area[SAVE_COUNT] = REG_READ(vip, DVP_ITM);
+	REG_WRITE(vip, DVP_ITM, 0);
+	for (i = 1; i < SAVE_COUNT; i++)
+		vip->register_save_area[i] = REG_READ(vip, 4 * i);
+	for (i = 0; i < AUX_COUNT; i++)
+		vip->register_save_area[SAVE_COUNT + IRQ_COUNT + i] =
+		    REG_READ(vip, registers_to_save[i]);
+	spin_unlock_irqrestore(&vip->slock, flags);
+	/* save pci state */
+	pci_save_state(pdev);
+	if (pci_set_power_state(pdev, pci_choose_state(pdev, state))) {
+		/*
+		 * do not call pci_disable_device on sta2x11 because it
+		 * break all other Bus masters on this EP
+		 */
+		vip->disabled = 1;
+	}
+
+	pr_info("VIP: suspend\n");
+	return 0;
+}
+
+/**
+ * sta2x11_vip_resume - resume device operation
+ * @pdev : PCI device
+ *
+ * re-enable device, set PCI state to powered and restore registers.
+ * resume normal device operation afterwards.
+ *
+ * return value: 0, no error.
+ *
+ * other, could not set device to power on state.
+ */
+static int sta2x11_vip_resume(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct sta2x11_vip *vip =
+	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
+	unsigned long flags;
+	int ret, i;
+
+	pr_info("VIP: resume\n");
+	/* restore pci state */
+	if (vip->disabled) {
+		ret = pci_enable_device(pdev);
+		if (ret) {
+			pr_warning("VIP: Can't enable device.\n");
+			return ret;
+		}
+		vip->disabled = 0;
+	}
+	ret = pci_set_power_state(pdev, PCI_D0);
+	if (ret) {
+		/*
+		 * do not call pci_disable_device on sta2x11 because it
+		 * break all other Bus masters on this EP
+		 */
+		pr_warning("VIP: Can't enable device.\n");
+		vip->disabled = 1;
+		return ret;
+	}
+
+	pci_restore_state(pdev);
+
+	spin_lock_irqsave(&vip->slock, flags);
+	for (i = 1; i < SAVE_COUNT; i++)
+		REG_WRITE(vip, 4 * i, vip->register_save_area[i]);
+	for (i = 0; i < AUX_COUNT; i++)
+		REG_WRITE(vip, registers_to_save[i],
+			  vip->register_save_area[SAVE_COUNT + IRQ_COUNT + i]);
+	REG_WRITE(vip, DVP_CTL, vip->register_save_area[0]);
+	REG_WRITE(vip, DVP_ITM, vip->register_save_area[SAVE_COUNT]);
+	spin_unlock_irqrestore(&vip->slock, flags);
+	return 0;
+}
+
+#endif
+
+static DEFINE_PCI_DEVICE_TABLE(sta2x11_vip_pci_tbl) = {
+	{PCI_DEVICE(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_VIP)},
+	{0,}
+};
+
+static struct pci_driver sta2x11_vip_driver = {
+	.name = DRV_NAME,
+	.probe = sta2x11_vip_init_one,
+	.remove = __devexit_p(sta2x11_vip_remove_one),
+	.id_table = sta2x11_vip_pci_tbl,
+#ifdef CONFIG_PM
+	.suspend = sta2x11_vip_suspend,
+	.resume = sta2x11_vip_resume,
+#endif
+};
+
+static int __init sta2x11_vip_init_module(void)
+{
+	return pci_register_driver(&sta2x11_vip_driver);
+}
+
+static void __exit sta2x11_vip_exit_module(void)
+{
+	pci_unregister_driver(&sta2x11_vip_driver);
+}
+
+#ifdef MODULE
+module_init(sta2x11_vip_init_module);
+module_exit(sta2x11_vip_exit_module);
+#else
+late_initcall_sync(sta2x11_vip_init_module);
+#endif
+
+MODULE_DESCRIPTION("STA2X11 Video Input Port driver");
+MODULE_AUTHOR("Wind River");
+MODULE_LICENSE("GPL v2");
+MODULE_SUPPORTED_DEVICE("sta2x11 video input");
+MODULE_VERSION(DRV_VERSION);
+MODULE_DEVICE_TABLE(pci, sta2x11_vip_pci_tbl);
diff --git a/drivers/media/video/sta2x11_vip.h b/drivers/media/video/sta2x11_vip.h
new file mode 100644
index 000000000000..4f81a13666eb
--- /dev/null
+++ b/drivers/media/video/sta2x11_vip.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2011 Wind River Systems, Inc.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Author:  Anders Wallin <anders.wallin@windriver.com>
+ *
+ */
+
+#ifndef __STA2X11_VIP_H
+#define __STA2X11_VIP_H
+
+/**
+ * struct vip_config - video input configuration data
+ * @pwr_name: ADV powerdown name
+ * @pwr_pin: ADV powerdown pin
+ * @reset_name: ADV reset name
+ * @reset_pin: ADV reset pin
+ */
+struct vip_config {
+	const char *pwr_name;
+	int pwr_pin;
+	const char *reset_name;
+	int reset_pin;
+	int i2c_id;
+	int i2c_addr;
+};
+
+#endif /* __STA2X11_VIP_H */
diff --git a/drivers/media/video/stk-webcam.c b/drivers/media/video/stk-webcam.c
index d427f8436c70..86a0fc56c330 100644
--- a/drivers/media/video/stk-webcam.c
+++ b/drivers/media/video/stk-webcam.c
@@ -38,13 +38,13 @@
 #include "stk-webcam.h"
 
 
-static bool hflip = 1;
+static bool hflip;
 module_param(hflip, bool, 0444);
-MODULE_PARM_DESC(hflip, "Horizontal image flip (mirror). Defaults to 1");
+MODULE_PARM_DESC(hflip, "Horizontal image flip (mirror). Defaults to 0");
 
-static bool vflip = 1;
+static bool vflip;
 module_param(vflip, bool, 0444);
-MODULE_PARM_DESC(vflip, "Vertical image flip. Defaults to 1");
+MODULE_PARM_DESC(vflip, "Vertical image flip. Defaults to 0");
 
 static int debug;
 module_param(debug, int, 0444);
diff --git a/drivers/media/video/tda9840.c b/drivers/media/video/tda9840.c
index 465d7086babf..3d7ddd93282d 100644
--- a/drivers/media/video/tda9840.c
+++ b/drivers/media/video/tda9840.c
@@ -66,29 +66,53 @@ static void tda9840_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 				val, reg);
 }
 
+static int tda9840_status(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 byte;
+
+	if (1 != i2c_master_recv(client, &byte, 1)) {
+		v4l2_dbg(1, debug, sd,
+			"i2c_master_recv() failed\n");
+		return -EIO;
+	}
+
+	if (byte & 0x80) {
+		v4l2_dbg(1, debug, sd,
+			"TDA9840_DETECT: register contents invalid\n");
+		return -EINVAL;
+	}
+
+	v4l2_dbg(1, debug, sd, "TDA9840_DETECT: byte: 0x%02x\n", byte);
+	return byte & 0x60;
+}
+
 static int tda9840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
 {
+	int stat = tda9840_status(sd);
 	int byte;
 
 	if (t->index)
 		return -EINVAL;
 
-	switch (t->audmode) {
-	case V4L2_TUNER_MODE_STEREO:
-		byte = TDA9840_SET_STEREO;
-		break;
-	case V4L2_TUNER_MODE_LANG1_LANG2:
-		byte = TDA9840_SET_BOTH;
-		break;
-	case V4L2_TUNER_MODE_LANG1:
-		byte = TDA9840_SET_LANG1;
-		break;
-	case V4L2_TUNER_MODE_LANG2:
-		byte = TDA9840_SET_LANG2;
-		break;
-	default:
+	stat = stat < 0 ? 0 : stat;
+	if (stat == 0 || stat == 0x60) /* mono input */
 		byte = TDA9840_SET_MONO;
-		break;
+	else if (stat == 0x40) /* stereo input */
+		byte = (t->audmode == V4L2_TUNER_MODE_MONO) ?
+			TDA9840_SET_MONO : TDA9840_SET_STEREO;
+	else { /* bilingual */
+		switch (t->audmode) {
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			byte = TDA9840_SET_BOTH;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			byte = TDA9840_SET_LANG2;
+			break;
+		default:
+			byte = TDA9840_SET_LANG1;
+			break;
+		}
 	}
 	v4l2_dbg(1, debug, sd, "TDA9840_SWITCH: 0x%02x\n", byte);
 	tda9840_write(sd, SWITCH, byte);
@@ -97,25 +121,14 @@ static int tda9840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
 
 static int tda9840_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	u8 byte;
-
-	t->rxsubchans = V4L2_TUNER_SUB_MONO;
-	if (1 != i2c_master_recv(client, &byte, 1)) {
-		v4l2_dbg(1, debug, sd,
-			"i2c_master_recv() failed\n");
-		return -EIO;
-	}
+	int stat = tda9840_status(sd);
 
-	if (byte & 0x80) {
-		v4l2_dbg(1, debug, sd,
-			"TDA9840_DETECT: register contents invalid\n");
-		return -EINVAL;
-	}
+	if (stat < 0)
+		return stat;
 
-	v4l2_dbg(1, debug, sd, "TDA9840_DETECT: byte: 0x%02x\n", byte);
+	t->rxsubchans = V4L2_TUNER_SUB_MONO;
 
-	switch (byte & 0x60) {
+	switch (stat & 0x60) {
 	case 0x00:
 		t->rxsubchans = V4L2_TUNER_SUB_MONO;
 		break;
diff --git a/drivers/media/video/tlg2300/pd-video.c b/drivers/media/video/tlg2300/pd-video.c
index a794ae62aebf..bfbf9e56b0a4 100644
--- a/drivers/media/video/tlg2300/pd-video.c
+++ b/drivers/media/video/tlg2300/pd-video.c
@@ -150,7 +150,6 @@ static int vidioc_querycap(struct file *file, void *fh,
 	strcpy(cap->driver, "tele-video");
 	strcpy(cap->card, "Telegent Poseidon");
 	usb_make_path(p->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->version = KERNEL_VERSION(0, 0, 1);
 	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TUNER |
 				V4L2_CAP_AUDIO | V4L2_CAP_STREAMING |
 				V4L2_CAP_READWRITE | V4L2_CAP_VBI_CAPTURE;
diff --git a/drivers/media/video/tm6000/tm6000-input.c b/drivers/media/video/tm6000/tm6000-input.c
index 859eb90e4d56..e80b7e190471 100644
--- a/drivers/media/video/tm6000/tm6000-input.c
+++ b/drivers/media/video/tm6000/tm6000-input.c
@@ -168,7 +168,6 @@ static void tm6000_ir_urb_received(struct urb *urb)
 	struct tm6000_IR *ir = dev->ir;
 	struct tm6000_ir_poll_result poll_result;
 	char *buf;
-	int rc;
 
 	dprintk(2, "%s\n",__func__);
 	if (urb->status < 0 || urb->actual_length <= 0) {
@@ -192,7 +191,7 @@ static void tm6000_ir_urb_received(struct urb *urb)
 	dprintk(1, "%s, scancode: 0x%04x\n",__func__, poll_result.rc_data);
 	rc_keydown(ir->rc, poll_result.rc_data, 0);
 
-	rc = usb_submit_urb(urb, GFP_ATOMIC);
+	usb_submit_urb(urb, GFP_ATOMIC);
 	/*
 	 * Flash the led. We can't do it here, as it is running on IRQ context.
 	 * So, use the scheduler to do it, in a few ms.
diff --git a/drivers/media/video/tm6000/tm6000-stds.c b/drivers/media/video/tm6000/tm6000-stds.c
index 9dc0831d813f..5e28d6a2412f 100644
--- a/drivers/media/video/tm6000/tm6000-stds.c
+++ b/drivers/media/video/tm6000/tm6000-stds.c
@@ -338,7 +338,6 @@ static int tm6000_set_audio_std(struct tm6000_core *dev)
 	uint8_t areg_02 = 0x04; /* GC1 Fixed gain 0dB */
 	uint8_t areg_05 = 0x01; /* Auto 4.5 = M Japan, Auto 6.5 = DK */
 	uint8_t areg_06 = 0x02; /* Auto de-emphasis, mannual channel mode */
-	uint8_t nicam_flag = 0; /* No NICAM */
 
 	if (dev->radio) {
 		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x00);
@@ -398,7 +397,6 @@ static int tm6000_set_audio_std(struct tm6000_core *dev)
 		} else {
 			areg_05 = 0x07;
 		}
-		nicam_flag = 1;
 		break;
 	/* other */
 	case 3:
diff --git a/drivers/media/video/tm6000/tm6000-video.c b/drivers/media/video/tm6000/tm6000-video.c
index bc13db736e24..f7034df94e0a 100644
--- a/drivers/media/video/tm6000/tm6000-video.c
+++ b/drivers/media/video/tm6000/tm6000-video.c
@@ -169,7 +169,6 @@ static inline void get_next_buf(struct tm6000_dmaqueue *dma_q,
 			       struct tm6000_buffer   **buf)
 {
 	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
-	char *outp;
 
 	if (list_empty(&dma_q->active)) {
 		dprintk(dev, V4L2_DEBUG_QUEUE, "No active queue to serve\n");
@@ -179,11 +178,6 @@ static inline void get_next_buf(struct tm6000_dmaqueue *dma_q,
 
 	*buf = list_entry(dma_q->active.next,
 			struct tm6000_buffer, vb.queue);
-
-	/* Cleans up buffer - Useful for testing for frame/URB loss */
-	outp = videobuf_to_vmalloc(&(*buf)->vb);
-
-	return;
 }
 
 /*
@@ -211,7 +205,7 @@ static int copy_streams(u8 *data, unsigned long len,
 {
 	struct tm6000_dmaqueue  *dma_q = urb->context;
 	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
-	u8 *ptr = data, *endp = data+len, c;
+	u8 *ptr = data, *endp = data+len;
 	unsigned long header = 0;
 	int rc = 0;
 	unsigned int cmd, cpysize, pktsize, size, field, block, line, pos = 0;
@@ -264,7 +258,6 @@ static int copy_streams(u8 *data, unsigned long len,
 			}
 
 			/* split the header fields */
-			c = (header >> 24) & 0xff;
 			size = ((header & 0x7e) << 1);
 			if (size > 0)
 				size -= 4;
@@ -889,7 +882,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
 
 	strlcpy(cap->driver, "tm6000", sizeof(cap->driver));
 	strlcpy(cap->card, "Trident TVMaster TM5600/6000/6010", sizeof(cap->card));
-	cap->version = TM6000_VERSION;
 	cap->capabilities =	V4L2_CAP_VIDEO_CAPTURE |
 				V4L2_CAP_STREAMING     |
 				V4L2_CAP_AUDIO         |
@@ -1732,6 +1724,10 @@ static struct video_device *vdev_init(struct tm6000_core *dev,
 	vfd->release = video_device_release;
 	vfd->debug = tm6000_debug;
 	vfd->lock = &dev->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s", dev->name, type_name);
 
diff --git a/drivers/media/video/tm6000/tm6000.h b/drivers/media/video/tm6000/tm6000.h
index 27ba659cfa85..6df418658c9c 100644
--- a/drivers/media/video/tm6000/tm6000.h
+++ b/drivers/media/video/tm6000/tm6000.h
@@ -33,8 +33,6 @@
 #include "dvb_frontend.h"
 #include "dmxdev.h"
 
-#define TM6000_VERSION KERNEL_VERSION(0, 0, 2)
-
 /* Inputs */
 enum tm6000_itype {
 	TM6000_INPUT_TV	= 1,
diff --git a/drivers/media/video/tuner-core.c b/drivers/media/video/tuner-core.c
index a5c6397ad591..3e050e12153b 100644
--- a/drivers/media/video/tuner-core.c
+++ b/drivers/media/video/tuner-core.c
@@ -1241,8 +1241,10 @@ static int tuner_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int tuner_suspend(struct i2c_client *c, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int tuner_suspend(struct device *dev)
 {
+	struct i2c_client *c = to_i2c_client(dev);
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
 	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
 
@@ -1254,8 +1256,9 @@ static int tuner_suspend(struct i2c_client *c, pm_message_t state)
 	return 0;
 }
 
-static int tuner_resume(struct i2c_client *c)
+static int tuner_resume(struct device *dev)
 {
+	struct i2c_client *c = to_i2c_client(dev);
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
 
 	tuner_dbg("resume\n");
@@ -1266,6 +1269,7 @@ static int tuner_resume(struct i2c_client *c)
 
 	return 0;
 }
+#endif
 
 static int tuner_command(struct i2c_client *client, unsigned cmd, void *arg)
 {
@@ -1310,6 +1314,10 @@ static const struct v4l2_subdev_ops tuner_ops = {
  * I2C structs and module init functions
  */
 
+static const struct dev_pm_ops tuner_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(tuner_suspend, tuner_resume)
+};
+
 static const struct i2c_device_id tuner_id[] = {
 	{ "tuner", }, /* autodetect */
 	{ }
@@ -1320,12 +1328,11 @@ static struct i2c_driver tuner_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= "tuner",
+		.pm	= &tuner_pm_ops,
 	},
 	.probe		= tuner_probe,
 	.remove		= tuner_remove,
 	.command	= tuner_command,
-	.suspend	= tuner_suspend,
-	.resume		= tuner_resume,
 	.id_table	= tuner_id,
 };
 
diff --git a/drivers/media/video/tvp5150.c b/drivers/media/video/tvp5150.c
index 1326e11cf4a9..b7867427e5c4 100644
--- a/drivers/media/video/tvp5150.c
+++ b/drivers/media/video/tvp5150.c
@@ -822,7 +822,7 @@ static int tvp5150_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
 	if (index)
 		return -EINVAL;
 
-	*code = V4L2_MBUS_FMT_YUYV8_2X8;
+	*code = V4L2_MBUS_FMT_UYVY8_2X8;
 	return 0;
 }
 
@@ -830,23 +830,16 @@ static int tvp5150_mbus_fmt(struct v4l2_subdev *sd,
 			    struct v4l2_mbus_framefmt *f)
 {
 	struct tvp5150 *decoder = to_tvp5150(sd);
-	v4l2_std_id std;
 
 	if (f == NULL)
 		return -EINVAL;
 
 	tvp5150_reset(sd, 0);
 
-	/* Calculate height and width based on current standard */
-	if (decoder->norm == V4L2_STD_ALL)
-		std = tvp5150_read_std(sd);
-	else
-		std = decoder->norm;
-
 	f->width = decoder->rect.width;
 	f->height = decoder->rect.height;
 
-	f->code = V4L2_MBUS_FMT_YUYV8_2X8;
+	f->code = V4L2_MBUS_FMT_UYVY8_2X8;
 	f->field = V4L2_FIELD_SEQ_TB;
 	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
 
diff --git a/drivers/media/video/tvp7002.c b/drivers/media/video/tvp7002.c
index d7676d85c4df..fb6a5b57eb83 100644
--- a/drivers/media/video/tvp7002.c
+++ b/drivers/media/video/tvp7002.c
@@ -29,6 +29,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/module.h>
+#include <linux/v4l2-dv-timings.h>
 #include <media/tvp7002.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
@@ -328,6 +329,7 @@ static const struct i2c_reg_value tvp7002_parms_720P50[] = {
 /* Preset definition for handling device operation */
 struct tvp7002_preset_definition {
 	u32 preset;
+	struct v4l2_dv_timings timings;
 	const struct i2c_reg_value *p_settings;
 	enum v4l2_colorspace color_space;
 	enum v4l2_field scanmode;
@@ -341,6 +343,7 @@ struct tvp7002_preset_definition {
 static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	{
 		V4L2_DV_720P60,
+		V4L2_DV_BT_CEA_1280X720P60,
 		tvp7002_parms_720P60,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_NONE,
@@ -351,6 +354,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_1080I60,
+		V4L2_DV_BT_CEA_1920X1080I60,
 		tvp7002_parms_1080I60,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_INTERLACED,
@@ -361,6 +365,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_1080I50,
+		V4L2_DV_BT_CEA_1920X1080I50,
 		tvp7002_parms_1080I50,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_INTERLACED,
@@ -371,6 +376,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_720P50,
+		V4L2_DV_BT_CEA_1280X720P50,
 		tvp7002_parms_720P50,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_NONE,
@@ -381,6 +387,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_1080P60,
+		V4L2_DV_BT_CEA_1920X1080P60,
 		tvp7002_parms_1080P60,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_NONE,
@@ -391,6 +398,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_480P59_94,
+		V4L2_DV_BT_CEA_720X480P59_94,
 		tvp7002_parms_480P,
 		V4L2_COLORSPACE_SMPTE170M,
 		V4L2_FIELD_NONE,
@@ -401,6 +409,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_576P50,
+		V4L2_DV_BT_CEA_720X576P50,
 		tvp7002_parms_576P,
 		V4L2_COLORSPACE_SMPTE170M,
 		V4L2_FIELD_NONE,
@@ -605,6 +614,35 @@ static int tvp7002_s_dv_preset(struct v4l2_subdev *sd,
 	return -EINVAL;
 }
 
+static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *dv_timings)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+	const struct v4l2_bt_timings *bt = &dv_timings->bt;
+	int i;
+
+	if (dv_timings->type != V4L2_DV_BT_656_1120)
+		return -EINVAL;
+	for (i = 0; i < NUM_PRESETS; i++) {
+		const struct v4l2_bt_timings *t = &tvp7002_presets[i].timings.bt;
+
+		if (!memcmp(bt, t, &bt->standards - &bt->width)) {
+			device->current_preset = &tvp7002_presets[i];
+			return tvp7002_write_inittab(sd, tvp7002_presets[i].p_settings);
+		}
+	}
+	return -EINVAL;
+}
+
+static int tvp7002_g_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *dv_timings)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+
+	*dv_timings = device->current_preset->timings;
+	return 0;
+}
+
 /*
  * tvp7002_s_ctrl() - Set a control
  * @ctrl: ptr to v4l2_ctrl struct
@@ -666,11 +704,9 @@ static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f
  * Returns the current DV preset by TVP7002. If no active input is
  * detected, returns -EINVAL
  */
-static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
-						struct v4l2_dv_preset *qpreset)
+static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 {
 	const struct tvp7002_preset_definition *presets = tvp7002_presets;
-	struct tvp7002 *device;
 	u8 progressive;
 	u32 lpfr;
 	u32 cpln;
@@ -679,12 +715,9 @@ static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
 	u8 lpf_msb;
 	u8 cpl_lsb;
 	u8 cpl_msb;
-	int index;
-
-	/* Return invalid preset if no active input is detected */
-	qpreset->preset = V4L2_DV_INVALID;
 
-	device = to_tvp7002(sd);
+	/* Return invalid index if no active input is detected */
+	*index = NUM_PRESETS;
 
 	/* Read standards from device registers */
 	tvp7002_read_err(sd, TVP7002_L_FRAME_STAT_LSBS, &lpf_lsb, &error);
@@ -705,8 +738,8 @@ static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
 	progressive = (lpf_msb & TVP7002_INPR_MASK) >> TVP7002_IP_SHIFT;
 
 	/* Do checking of video modes */
-	for (index = 0; index < NUM_PRESETS; index++, presets++)
-		if (lpfr  == presets->lines_per_frame &&
+	for (*index = 0; *index < NUM_PRESETS; (*index)++, presets++)
+		if (lpfr == presets->lines_per_frame &&
 			progressive == presets->progressive) {
 			if (presets->cpl_min == 0xffff)
 				break;
@@ -714,17 +747,42 @@ static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
 				break;
 		}
 
-	if (index == NUM_PRESETS) {
+	if (*index == NUM_PRESETS) {
 		v4l2_dbg(1, debug, sd, "detection failed: lpf = %x, cpl = %x\n",
 								lpfr, cpln);
-		return 0;
+		return -ENOLINK;
 	}
 
-	/* Set values in found preset */
-	qpreset->preset = presets->preset;
-
 	/* Update lines per frame and clocks per line info */
-	v4l2_dbg(1, debug, sd, "detected preset: %d\n", presets->preset);
+	v4l2_dbg(1, debug, sd, "detected preset: %d\n", *index);
+	return 0;
+}
+
+static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
+					struct v4l2_dv_preset *qpreset)
+{
+	int index;
+	int err = tvp7002_query_dv(sd, &index);
+
+	if (err || index == NUM_PRESETS) {
+		qpreset->preset = V4L2_DV_INVALID;
+		if (err == -ENOLINK)
+			err = 0;
+		return err;
+	}
+	qpreset->preset = tvp7002_presets[index].preset;
+	return 0;
+}
+
+static int tvp7002_query_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *timings)
+{
+	int index;
+	int err = tvp7002_query_dv(sd, &index);
+
+	if (err)
+		return err;
+	*timings = tvp7002_presets[index].timings;
 	return 0;
 }
 
@@ -894,6 +952,17 @@ static int tvp7002_enum_dv_presets(struct v4l2_subdev *sd,
 	return v4l_fill_dv_preset_info(tvp7002_presets[preset->index].preset, preset);
 }
 
+static int tvp7002_enum_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_enum_dv_timings *timings)
+{
+	/* Check requested format index is within range */
+	if (timings->index >= NUM_PRESETS)
+		return -EINVAL;
+
+	timings->timings = tvp7002_presets[timings->index].timings;
+	return 0;
+}
+
 static const struct v4l2_ctrl_ops tvp7002_ctrl_ops = {
 	.s_ctrl = tvp7002_s_ctrl,
 };
@@ -920,6 +989,10 @@ static const struct v4l2_subdev_video_ops tvp7002_video_ops = {
 	.enum_dv_presets = tvp7002_enum_dv_presets,
 	.s_dv_preset = tvp7002_s_dv_preset,
 	.query_dv_preset = tvp7002_query_dv_preset,
+	.g_dv_timings = tvp7002_g_dv_timings,
+	.s_dv_timings = tvp7002_s_dv_timings,
+	.enum_dv_timings = tvp7002_enum_dv_timings,
+	.query_dv_timings = tvp7002_query_dv_timings,
 	.s_stream = tvp7002_s_stream,
 	.g_mbus_fmt = tvp7002_mbus_fmt,
 	.try_mbus_fmt = tvp7002_mbus_fmt,
diff --git a/drivers/media/video/usbvision/usbvision-core.c b/drivers/media/video/usbvision/usbvision-core.c
index f344411a4578..c9b2042f8bdf 100644
--- a/drivers/media/video/usbvision/usbvision-core.c
+++ b/drivers/media/video/usbvision/usbvision-core.c
@@ -601,13 +601,12 @@ static int usbvision_decompress(struct usb_usbvision *usbvision, unsigned char *
 								unsigned char *decompressed, int *start_pos,
 								int *block_typestart_pos, int len)
 {
-	int rest_pixel, idx, max_pos, pos, extra_pos, block_len, block_type_pos, block_type_len;
+	int rest_pixel, idx, pos, extra_pos, block_len, block_type_pos, block_type_len;
 	unsigned char block_byte, block_code, block_type, block_type_byte, integrator;
 
 	integrator = 0;
 	pos = *start_pos;
 	block_type_pos = *block_typestart_pos;
-	max_pos = 396; /* pos + len; */
 	extra_pos = pos;
 	block_len = 0;
 	block_byte = 0;
@@ -702,7 +701,7 @@ static enum parse_state usbvision_parse_compress(struct usb_usbvision *usbvision
 	unsigned char strip_data[USBVISION_STRIP_LEN_MAX];
 	unsigned char strip_header[USBVISION_STRIP_HEADER_LEN];
 	int idx, idx_end, strip_len, strip_ptr, startblock_pos, block_pos, block_type_pos;
-	int clipmask_index, bytes_per_pixel, rc;
+	int clipmask_index;
 	int image_size;
 	unsigned char rv, gv, bv;
 	static unsigned char *Y, *U, *V;
@@ -769,7 +768,6 @@ static enum parse_state usbvision_parse_compress(struct usb_usbvision *usbvision
 		return parse_state_next_frame;
 	}
 
-	bytes_per_pixel = frame->v4l2_format.bytes_per_pixel;
 	clipmask_index = frame->curline * MAX_FRAME_WIDTH;
 
 	scratch_get(usbvision, strip_data, strip_len);
@@ -781,14 +779,14 @@ static enum parse_state usbvision_parse_compress(struct usb_usbvision *usbvision
 
 	usbvision->block_pos = block_pos;
 
-	rc = usbvision_decompress(usbvision, strip_data, Y, &block_pos, &block_type_pos, idx_end);
+	usbvision_decompress(usbvision, strip_data, Y, &block_pos, &block_type_pos, idx_end);
 	if (strip_len > usbvision->max_strip_len)
 		usbvision->max_strip_len = strip_len;
 
 	if (frame->curline % 2)
-		rc = usbvision_decompress(usbvision, strip_data, V, &block_pos, &block_type_pos, idx_end / 2);
+		usbvision_decompress(usbvision, strip_data, V, &block_pos, &block_type_pos, idx_end / 2);
 	else
-		rc = usbvision_decompress(usbvision, strip_data, U, &block_pos, &block_type_pos, idx_end / 2);
+		usbvision_decompress(usbvision, strip_data, U, &block_pos, &block_type_pos, idx_end / 2);
 
 	if (block_pos > usbvision->comprblock_pos)
 		usbvision->comprblock_pos = block_pos;
diff --git a/drivers/media/video/usbvision/usbvision-video.c b/drivers/media/video/usbvision/usbvision-video.c
index 5a74f5e07d7d..9bd8f084f348 100644
--- a/drivers/media/video/usbvision/usbvision-video.c
+++ b/drivers/media/video/usbvision/usbvision-video.c
@@ -1296,6 +1296,10 @@ static struct video_device *usbvision_vdev_init(struct usb_usbvision *usbvision,
 	if (NULL == vdev)
 		return NULL;
 	*vdev = *vdev_template;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags);
 	vdev->lock = &usbvision->v4l2_lock;
 	vdev->v4l2_dev = &usbvision->v4l2_dev;
 	snprintf(vdev->name, sizeof(vdev->name), "%s", name);
diff --git a/drivers/media/video/uvc/uvc_ctrl.c b/drivers/media/video/uvc/uvc_ctrl.c
index 0efd3b10b353..af26bbe6f76e 100644
--- a/drivers/media/video/uvc/uvc_ctrl.c
+++ b/drivers/media/video/uvc/uvc_ctrl.c
@@ -21,6 +21,7 @@
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
 #include <linux/atomic.h>
+#include <media/v4l2-ctrls.h>
 
 #include "uvcvideo.h"
 
@@ -420,6 +421,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.master_id	= V4L2_CID_HUE_AUTO,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_SATURATION,
@@ -492,6 +495,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_HUE, },
 	},
 	{
 		.id		= V4L2_CID_EXPOSURE_AUTO,
@@ -504,6 +508,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.data_type	= UVC_CTRL_DATA_TYPE_BITMASK,
 		.menu_info	= exposure_auto_controls,
 		.menu_count	= ARRAY_SIZE(exposure_auto_controls),
+		.slave_ids	= { V4L2_CID_EXPOSURE_ABSOLUTE, },
 	},
 	{
 		.id		= V4L2_CID_EXPOSURE_AUTO_PRIORITY,
@@ -524,6 +529,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_UNSIGNED,
+		.master_id	= V4L2_CID_EXPOSURE_AUTO,
+		.master_manual	= V4L2_EXPOSURE_MANUAL,
 	},
 	{
 		.id		= V4L2_CID_AUTO_WHITE_BALANCE,
@@ -534,6 +541,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_WHITE_BALANCE_TEMPERATURE, },
 	},
 	{
 		.id		= V4L2_CID_WHITE_BALANCE_TEMPERATURE,
@@ -544,6 +552,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_UNSIGNED,
+		.master_id	= V4L2_CID_AUTO_WHITE_BALANCE,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_AUTO_WHITE_BALANCE,
@@ -554,6 +564,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_BLUE_BALANCE,
+				    V4L2_CID_RED_BALANCE },
 	},
 	{
 		.id		= V4L2_CID_BLUE_BALANCE,
@@ -564,6 +576,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.master_id	= V4L2_CID_AUTO_WHITE_BALANCE,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_RED_BALANCE,
@@ -574,6 +588,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 16,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.master_id	= V4L2_CID_AUTO_WHITE_BALANCE,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_FOCUS_ABSOLUTE,
@@ -584,6 +600,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_UNSIGNED,
+		.master_id	= V4L2_CID_FOCUS_AUTO,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_FOCUS_AUTO,
@@ -594,6 +612,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_FOCUS_ABSOLUTE, },
 	},
 	{
 		.id		= V4L2_CID_IRIS_ABSOLUTE,
@@ -899,25 +918,54 @@ static int uvc_ctrl_populate_cache(struct uvc_video_chain *chain,
 	return 0;
 }
 
-int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
-	struct v4l2_queryctrl *v4l2_ctrl)
+static int __uvc_ctrl_get(struct uvc_video_chain *chain,
+	struct uvc_control *ctrl, struct uvc_control_mapping *mapping,
+	s32 *value)
 {
-	struct uvc_control *ctrl;
-	struct uvc_control_mapping *mapping;
 	struct uvc_menu_info *menu;
 	unsigned int i;
 	int ret;
 
-	ret = mutex_lock_interruptible(&chain->ctrl_mutex);
-	if (ret < 0)
-		return -ERESTARTSYS;
+	if ((ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR) == 0)
+		return -EINVAL;
 
-	ctrl = uvc_find_control(chain, v4l2_ctrl->id, &mapping);
-	if (ctrl == NULL) {
-		ret = -EINVAL;
-		goto done;
+	if (!ctrl->loaded) {
+		ret = uvc_query_ctrl(chain->dev, UVC_GET_CUR, ctrl->entity->id,
+				chain->dev->intfnum, ctrl->info.selector,
+				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
+				ctrl->info.size);
+		if (ret < 0)
+			return ret;
+
+		ctrl->loaded = 1;
 	}
 
+	*value = mapping->get(mapping, UVC_GET_CUR,
+		uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
+
+	if (mapping->v4l2_type == V4L2_CTRL_TYPE_MENU) {
+		menu = mapping->menu_info;
+		for (i = 0; i < mapping->menu_count; ++i, ++menu) {
+			if (menu->value == *value) {
+				*value = i;
+				break;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int __uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
+	struct uvc_control *ctrl,
+	struct uvc_control_mapping *mapping,
+	struct v4l2_queryctrl *v4l2_ctrl)
+{
+	struct uvc_control_mapping *master_map = NULL;
+	struct uvc_control *master_ctrl = NULL;
+	struct uvc_menu_info *menu;
+	unsigned int i;
+
 	memset(v4l2_ctrl, 0, sizeof *v4l2_ctrl);
 	v4l2_ctrl->id = mapping->id;
 	v4l2_ctrl->type = mapping->v4l2_type;
@@ -929,10 +977,23 @@ int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR))
 		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
+	if (mapping->master_id)
+		__uvc_find_control(ctrl->entity, mapping->master_id,
+				   &master_map, &master_ctrl, 0);
+	if (master_ctrl && (master_ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR)) {
+		s32 val;
+		int ret = __uvc_ctrl_get(chain, master_ctrl, master_map, &val);
+		if (ret < 0)
+			return ret;
+
+		if (val != mapping->master_manual)
+				v4l2_ctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+	}
+
 	if (!ctrl->cached) {
-		ret = uvc_ctrl_populate_cache(chain, ctrl);
+		int ret = uvc_ctrl_populate_cache(chain, ctrl);
 		if (ret < 0)
-			goto done;
+			return ret;
 	}
 
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_DEF) {
@@ -954,19 +1015,19 @@ int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 			}
 		}
 
-		goto done;
+		return 0;
 
 	case V4L2_CTRL_TYPE_BOOLEAN:
 		v4l2_ctrl->minimum = 0;
 		v4l2_ctrl->maximum = 1;
 		v4l2_ctrl->step = 1;
-		goto done;
+		return 0;
 
 	case V4L2_CTRL_TYPE_BUTTON:
 		v4l2_ctrl->minimum = 0;
 		v4l2_ctrl->maximum = 0;
 		v4l2_ctrl->step = 0;
-		goto done;
+		return 0;
 
 	default:
 		break;
@@ -984,6 +1045,27 @@ int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 		v4l2_ctrl->step = mapping->get(mapping, UVC_GET_RES,
 				  uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
 
+	return 0;
+}
+
+int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
+	struct v4l2_queryctrl *v4l2_ctrl)
+{
+	struct uvc_control *ctrl;
+	struct uvc_control_mapping *mapping;
+	int ret;
+
+	ret = mutex_lock_interruptible(&chain->ctrl_mutex);
+	if (ret < 0)
+		return -ERESTARTSYS;
+
+	ctrl = uvc_find_control(chain, v4l2_ctrl->id, &mapping);
+	if (ctrl == NULL) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = __uvc_query_v4l2_ctrl(chain, ctrl, mapping, v4l2_ctrl);
 done:
 	mutex_unlock(&chain->ctrl_mutex);
 	return ret;
@@ -1054,6 +1136,174 @@ done:
 	return ret;
 }
 
+/* --------------------------------------------------------------------------
+ * Ctrl event handling
+ */
+
+static void uvc_ctrl_fill_event(struct uvc_video_chain *chain,
+	struct v4l2_event *ev,
+	struct uvc_control *ctrl,
+	struct uvc_control_mapping *mapping,
+	s32 value, u32 changes)
+{
+	struct v4l2_queryctrl v4l2_ctrl;
+
+	__uvc_query_v4l2_ctrl(chain, ctrl, mapping, &v4l2_ctrl);
+
+	memset(ev->reserved, 0, sizeof(ev->reserved));
+	ev->type = V4L2_EVENT_CTRL;
+	ev->id = v4l2_ctrl.id;
+	ev->u.ctrl.value = value;
+	ev->u.ctrl.changes = changes;
+	ev->u.ctrl.type = v4l2_ctrl.type;
+	ev->u.ctrl.flags = v4l2_ctrl.flags;
+	ev->u.ctrl.minimum = v4l2_ctrl.minimum;
+	ev->u.ctrl.maximum = v4l2_ctrl.maximum;
+	ev->u.ctrl.step = v4l2_ctrl.step;
+	ev->u.ctrl.default_value = v4l2_ctrl.default_value;
+}
+
+static void uvc_ctrl_send_event(struct uvc_fh *handle,
+	struct uvc_control *ctrl, struct uvc_control_mapping *mapping,
+	s32 value, u32 changes)
+{
+	struct v4l2_subscribed_event *sev;
+	struct v4l2_event ev;
+
+	if (list_empty(&mapping->ev_subs))
+		return;
+
+	uvc_ctrl_fill_event(handle->chain, &ev, ctrl, mapping, value, changes);
+
+	list_for_each_entry(sev, &mapping->ev_subs, node) {
+		if (sev->fh && (sev->fh != &handle->vfh ||
+		    (sev->flags & V4L2_EVENT_SUB_FL_ALLOW_FEEDBACK) ||
+		    (changes & V4L2_EVENT_CTRL_CH_FLAGS)))
+			v4l2_event_queue_fh(sev->fh, &ev);
+	}
+}
+
+static void uvc_ctrl_send_slave_event(struct uvc_fh *handle,
+	struct uvc_control *master, u32 slave_id,
+	const struct v4l2_ext_control *xctrls, unsigned int xctrls_count)
+{
+	struct uvc_control_mapping *mapping = NULL;
+	struct uvc_control *ctrl = NULL;
+	u32 changes = V4L2_EVENT_CTRL_CH_FLAGS;
+	unsigned int i;
+	s32 val = 0;
+
+	/*
+	 * We can skip sending an event for the slave if the slave
+	 * is being modified in the same transaction.
+	 */
+	for (i = 0; i < xctrls_count; i++) {
+		if (xctrls[i].id == slave_id)
+			return;
+	}
+
+	__uvc_find_control(master->entity, slave_id, &mapping, &ctrl, 0);
+	if (ctrl == NULL)
+		return;
+
+	if (__uvc_ctrl_get(handle->chain, ctrl, mapping, &val) == 0)
+		changes |= V4L2_EVENT_CTRL_CH_VALUE;
+
+	uvc_ctrl_send_event(handle, ctrl, mapping, val, changes);
+}
+
+static void uvc_ctrl_send_events(struct uvc_fh *handle,
+	const struct v4l2_ext_control *xctrls, unsigned int xctrls_count)
+{
+	struct uvc_control_mapping *mapping;
+	struct uvc_control *ctrl;
+	u32 changes = V4L2_EVENT_CTRL_CH_VALUE;
+	unsigned int i;
+	unsigned int j;
+
+	for (i = 0; i < xctrls_count; ++i) {
+		ctrl = uvc_find_control(handle->chain, xctrls[i].id, &mapping);
+
+		for (j = 0; j < ARRAY_SIZE(mapping->slave_ids); ++j) {
+			if (!mapping->slave_ids[j])
+				break;
+			uvc_ctrl_send_slave_event(handle, ctrl,
+						  mapping->slave_ids[j],
+						  xctrls, xctrls_count);
+		}
+
+		/*
+		 * If the master is being modified in the same transaction
+		 * flags may change too.
+		 */
+		if (mapping->master_id) {
+			for (j = 0; j < xctrls_count; j++) {
+				if (xctrls[j].id == mapping->master_id) {
+					changes |= V4L2_EVENT_CTRL_CH_FLAGS;
+					break;
+				}
+			}
+		}
+
+		uvc_ctrl_send_event(handle, ctrl, mapping, xctrls[i].value,
+				    changes);
+	}
+}
+
+static int uvc_ctrl_add_event(struct v4l2_subscribed_event *sev, unsigned elems)
+{
+	struct uvc_fh *handle = container_of(sev->fh, struct uvc_fh, vfh);
+	struct uvc_control_mapping *mapping;
+	struct uvc_control *ctrl;
+	int ret;
+
+	ret = mutex_lock_interruptible(&handle->chain->ctrl_mutex);
+	if (ret < 0)
+		return -ERESTARTSYS;
+
+	ctrl = uvc_find_control(handle->chain, sev->id, &mapping);
+	if (ctrl == NULL) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	list_add_tail(&sev->node, &mapping->ev_subs);
+	if (sev->flags & V4L2_EVENT_SUB_FL_SEND_INITIAL) {
+		struct v4l2_event ev;
+		u32 changes = V4L2_EVENT_CTRL_CH_FLAGS;
+		s32 val = 0;
+
+		if (__uvc_ctrl_get(handle->chain, ctrl, mapping, &val) == 0)
+			changes |= V4L2_EVENT_CTRL_CH_VALUE;
+
+		uvc_ctrl_fill_event(handle->chain, &ev, ctrl, mapping, val,
+				    changes);
+		/* Mark the queue as active, allowing this initial
+		   event to be accepted. */
+		sev->elems = elems;
+		v4l2_event_queue_fh(sev->fh, &ev);
+	}
+
+done:
+	mutex_unlock(&handle->chain->ctrl_mutex);
+	return ret;
+}
+
+static void uvc_ctrl_del_event(struct v4l2_subscribed_event *sev)
+{
+	struct uvc_fh *handle = container_of(sev->fh, struct uvc_fh, vfh);
+
+	mutex_lock(&handle->chain->ctrl_mutex);
+	list_del(&sev->node);
+	mutex_unlock(&handle->chain->ctrl_mutex);
+}
+
+const struct v4l2_subscribed_event_ops uvc_ctrl_sub_ev_ops = {
+	.add = uvc_ctrl_add_event,
+	.del = uvc_ctrl_del_event,
+	.replace = v4l2_ctrl_replace,
+	.merge = v4l2_ctrl_merge,
+};
 
 /* --------------------------------------------------------------------------
  * Control transactions
@@ -1101,9 +1351,12 @@ static int uvc_ctrl_commit_entity(struct uvc_device *dev,
 
 		/* Reset the loaded flag for auto-update controls that were
 		 * marked as loaded in uvc_ctrl_get/uvc_ctrl_set to prevent
-		 * uvc_ctrl_get from using the cached value.
+		 * uvc_ctrl_get from using the cached value, and for write-only
+		 * controls to prevent uvc_ctrl_set from setting bits not
+		 * explicitly set by the user.
 		 */
-		if (ctrl->info.flags & UVC_CTRL_FLAG_AUTO_UPDATE)
+		if (ctrl->info.flags & UVC_CTRL_FLAG_AUTO_UPDATE ||
+		    !(ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR))
 			ctrl->loaded = 0;
 
 		if (!ctrl->dirty)
@@ -1131,8 +1384,11 @@ static int uvc_ctrl_commit_entity(struct uvc_device *dev,
 	return 0;
 }
 
-int __uvc_ctrl_commit(struct uvc_video_chain *chain, int rollback)
+int __uvc_ctrl_commit(struct uvc_fh *handle, int rollback,
+		      const struct v4l2_ext_control *xctrls,
+		      unsigned int xctrls_count)
 {
+	struct uvc_video_chain *chain = handle->chain;
 	struct uvc_entity *entity;
 	int ret = 0;
 
@@ -1143,6 +1399,8 @@ int __uvc_ctrl_commit(struct uvc_video_chain *chain, int rollback)
 			goto done;
 	}
 
+	if (!rollback)
+		uvc_ctrl_send_events(handle, xctrls, xctrls_count);
 done:
 	mutex_unlock(&chain->ctrl_mutex);
 	return ret;
@@ -1153,39 +1411,12 @@ int uvc_ctrl_get(struct uvc_video_chain *chain,
 {
 	struct uvc_control *ctrl;
 	struct uvc_control_mapping *mapping;
-	struct uvc_menu_info *menu;
-	unsigned int i;
-	int ret;
 
 	ctrl = uvc_find_control(chain, xctrl->id, &mapping);
-	if (ctrl == NULL || (ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR) == 0)
+	if (ctrl == NULL)
 		return -EINVAL;
 
-	if (!ctrl->loaded) {
-		ret = uvc_query_ctrl(chain->dev, UVC_GET_CUR, ctrl->entity->id,
-				chain->dev->intfnum, ctrl->info.selector,
-				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
-				ctrl->info.size);
-		if (ret < 0)
-			return ret;
-
-		ctrl->loaded = 1;
-	}
-
-	xctrl->value = mapping->get(mapping, UVC_GET_CUR,
-		uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
-
-	if (mapping->v4l2_type == V4L2_CTRL_TYPE_MENU) {
-		menu = mapping->menu_info;
-		for (i = 0; i < mapping->menu_count; ++i, ++menu) {
-			if (menu->value == xctrl->value) {
-				xctrl->value = i;
-				break;
-			}
-		}
-	}
-
-	return 0;
+	return __uvc_ctrl_get(chain, ctrl, mapping, &xctrl->value);
 }
 
 int uvc_ctrl_set(struct uvc_video_chain *chain,
@@ -1641,6 +1872,8 @@ static int __uvc_ctrl_add_mapping(struct uvc_device *dev,
 	if (map == NULL)
 		return -ENOMEM;
 
+	INIT_LIST_HEAD(&map->ev_subs);
+
 	size = sizeof(*mapping->menu_info) * mapping->menu_count;
 	map->menu_info = kmemdup(mapping->menu_info, size, GFP_KERNEL);
 	if (map->menu_info == NULL) {
@@ -1653,7 +1886,6 @@ static int __uvc_ctrl_add_mapping(struct uvc_device *dev,
 	if (map->set == NULL)
 		map->set = uvc_set_le_value;
 
-	map->ctrl = &ctrl->info;
 	list_add_tail(&map->list, &ctrl->info.mappings);
 	uvc_trace(UVC_TRACE_CONTROL,
 		"Adding mapping '%s' to control %pUl/%u.\n",
diff --git a/drivers/media/video/uvc/uvc_queue.c b/drivers/media/video/uvc/uvc_queue.c
index 8f54e24e3f35..9288fbd5001b 100644
--- a/drivers/media/video/uvc/uvc_queue.c
+++ b/drivers/media/video/uvc/uvc_queue.c
@@ -207,6 +207,19 @@ int uvc_queue_mmap(struct uvc_video_queue *queue, struct vm_area_struct *vma)
 	return ret;
 }
 
+#ifndef CONFIG_MMU
+unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
+		unsigned long pgoff)
+{
+	unsigned long ret;
+
+	mutex_lock(&queue->mutex);
+	ret = vb2_get_unmapped_area(&queue->queue, 0, 0, pgoff, 0);
+	mutex_unlock(&queue->mutex);
+	return ret;
+}
+#endif
+
 unsigned int uvc_queue_poll(struct uvc_video_queue *queue, struct file *file,
 			    poll_table *wait)
 {
@@ -237,36 +250,6 @@ int uvc_queue_allocated(struct uvc_video_queue *queue)
 	return allocated;
 }
 
-#ifndef CONFIG_MMU
-/*
- * Get unmapped area.
- *
- * NO-MMU arch need this function to make mmap() work correctly.
- */
-unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
-		unsigned long pgoff)
-{
-	struct uvc_buffer *buffer;
-	unsigned int i;
-	unsigned long ret;
-
-	mutex_lock(&queue->mutex);
-	for (i = 0; i < queue->count; ++i) {
-		buffer = &queue->buffer[i];
-		if ((buffer->buf.m.offset >> PAGE_SHIFT) == pgoff)
-			break;
-	}
-	if (i == queue->count) {
-		ret = -EINVAL;
-		goto done;
-	}
-	ret = (unsigned long)buf->mem;
-done:
-	mutex_unlock(&queue->mutex);
-	return ret;
-}
-#endif
-
 /*
  * Enable or disable the video buffers queue.
  *
diff --git a/drivers/media/video/uvc/uvc_v4l2.c b/drivers/media/video/uvc/uvc_v4l2.c
index ff2cdddf9bc6..759bef8897e9 100644
--- a/drivers/media/video/uvc/uvc_v4l2.c
+++ b/drivers/media/video/uvc/uvc_v4l2.c
@@ -25,6 +25,8 @@
 #include <linux/atomic.h>
 
 #include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-ioctl.h>
 
 #include "uvcvideo.h"
@@ -505,6 +507,8 @@ static int uvc_v4l2_open(struct file *file)
 		}
 	}
 
+	v4l2_fh_init(&handle->vfh, stream->vdev);
+	v4l2_fh_add(&handle->vfh);
 	handle->chain = stream->chain;
 	handle->stream = stream;
 	handle->state = UVC_HANDLE_PASSIVE;
@@ -528,6 +532,8 @@ static int uvc_v4l2_release(struct file *file)
 
 	/* Release the file handle. */
 	uvc_dismiss_privileges(handle);
+	v4l2_fh_del(&handle->vfh);
+	v4l2_fh_exit(&handle->vfh);
 	kfree(handle);
 	file->private_data = NULL;
 
@@ -584,7 +590,7 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 			return ret;
 
 		ret = uvc_ctrl_get(chain, &xctrl);
-		uvc_ctrl_rollback(chain);
+		uvc_ctrl_rollback(handle);
 		if (ret >= 0)
 			ctrl->value = xctrl.value;
 		break;
@@ -605,10 +611,10 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 
 		ret = uvc_ctrl_set(chain, &xctrl);
 		if (ret < 0) {
-			uvc_ctrl_rollback(chain);
+			uvc_ctrl_rollback(handle);
 			return ret;
 		}
-		ret = uvc_ctrl_commit(chain);
+		ret = uvc_ctrl_commit(handle, &xctrl, 1);
 		if (ret == 0)
 			ctrl->value = xctrl.value;
 		break;
@@ -630,13 +636,13 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		for (i = 0; i < ctrls->count; ++ctrl, ++i) {
 			ret = uvc_ctrl_get(chain, ctrl);
 			if (ret < 0) {
-				uvc_ctrl_rollback(chain);
+				uvc_ctrl_rollback(handle);
 				ctrls->error_idx = i;
 				return ret;
 			}
 		}
 		ctrls->error_idx = 0;
-		ret = uvc_ctrl_rollback(chain);
+		ret = uvc_ctrl_rollback(handle);
 		break;
 	}
 
@@ -654,7 +660,7 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		for (i = 0; i < ctrls->count; ++ctrl, ++i) {
 			ret = uvc_ctrl_set(chain, ctrl);
 			if (ret < 0) {
-				uvc_ctrl_rollback(chain);
+				uvc_ctrl_rollback(handle);
 				ctrls->error_idx = i;
 				return ret;
 			}
@@ -663,9 +669,10 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		ctrls->error_idx = 0;
 
 		if (cmd == VIDIOC_S_EXT_CTRLS)
-			ret = uvc_ctrl_commit(chain);
+			ret = uvc_ctrl_commit(handle,
+					      ctrls->controls, ctrls->count);
 		else
-			ret = uvc_ctrl_rollback(chain);
+			ret = uvc_ctrl_rollback(handle);
 		break;
 	}
 
@@ -687,7 +694,7 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 					break;
 			}
 			pin = iterm->id;
-		} else if (pin < selector->bNrInPins) {
+		} else if (index < selector->bNrInPins) {
 			pin = selector->baSourceID[index];
 			list_for_each_entry(iterm, &chain->entities, chain) {
 				if (!UVC_ENTITY_IS_ITERM(iterm))
@@ -990,6 +997,26 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		return uvc_video_enable(stream, 0);
 	}
 
+	case VIDIOC_SUBSCRIBE_EVENT:
+	{
+		struct v4l2_event_subscription *sub = arg;
+
+		switch (sub->type) {
+		case V4L2_EVENT_CTRL:
+			return v4l2_event_subscribe(&handle->vfh, sub, 0,
+						    &uvc_ctrl_sub_ev_ops);
+		default:
+			return -EINVAL;
+		}
+	}
+
+	case VIDIOC_UNSUBSCRIBE_EVENT:
+		return v4l2_event_unsubscribe(&handle->vfh, arg);
+
+	case VIDIOC_DQEVENT:
+		return v4l2_event_dequeue(&handle->vfh, arg,
+					  file->f_flags & O_NONBLOCK);
+
 	/* Analog video standards make no sense for digital cameras. */
 	case VIDIOC_ENUMSTD:
 	case VIDIOC_QUERYSTD:
@@ -1097,7 +1124,8 @@ static int uvc_v4l2_put_xu_mapping(const struct uvc_xu_control_mapping *kp,
 	    __put_user(kp->menu_count, &up->menu_count))
 		return -EFAULT;
 
-	__clear_user(up->reserved, sizeof(up->reserved));
+	if (__clear_user(up->reserved, sizeof(up->reserved)))
+		return -EFAULT;
 
 	if (kp->menu_count == 0)
 		return 0;
@@ -1105,8 +1133,6 @@ static int uvc_v4l2_put_xu_mapping(const struct uvc_xu_control_mapping *kp,
 	if (get_user(p, &up->menu_info))
 		return -EFAULT;
 	umenus = compat_ptr(p);
-	if (!access_ok(VERIFY_WRITE, umenus, kp->menu_count * sizeof(*umenus)))
-		return -EFAULT;
 
 	if (copy_in_user(umenus, kmenus, kp->menu_count * sizeof(*umenus)))
 		return -EFAULT;
diff --git a/drivers/media/video/uvc/uvcvideo.h b/drivers/media/video/uvc/uvcvideo.h
index 67f88d85bb16..7c3d082505b7 100644
--- a/drivers/media/video/uvc/uvcvideo.h
+++ b/drivers/media/video/uvc/uvcvideo.h
@@ -13,6 +13,8 @@
 #include <linux/videodev2.h>
 #include <media/media-device.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
 #include <media/videobuf2-core.h>
 
 /* --------------------------------------------------------------------------
@@ -153,8 +155,7 @@ struct uvc_control_info {
 
 struct uvc_control_mapping {
 	struct list_head list;
-
-	struct uvc_control_info *ctrl;
+	struct list_head ev_subs;
 
 	__u32 id;
 	__u8 name[32];
@@ -169,6 +170,10 @@ struct uvc_control_mapping {
 	struct uvc_menu_info *menu_info;
 	__u32 menu_count;
 
+	__u32 master_id;
+	__s32 master_manual;
+	__u32 slave_ids[2];
+
 	__s32 (*get) (struct uvc_control_mapping *mapping, __u8 query,
 		      const __u8 *data);
 	void (*set) (struct uvc_control_mapping *mapping, __s32 value,
@@ -524,6 +529,7 @@ enum uvc_handle_state {
 };
 
 struct uvc_fh {
+	struct v4l2_fh vfh;
 	struct uvc_video_chain *chain;
 	struct uvc_streaming *stream;
 	enum uvc_handle_state state;
@@ -643,6 +649,8 @@ extern int uvc_status_suspend(struct uvc_device *dev);
 extern int uvc_status_resume(struct uvc_device *dev);
 
 /* Controls */
+extern const struct v4l2_subscribed_event_ops uvc_ctrl_sub_ev_ops;
+
 extern int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 		struct v4l2_queryctrl *v4l2_ctrl);
 extern int uvc_query_v4l2_menu(struct uvc_video_chain *chain,
@@ -655,14 +663,18 @@ extern void uvc_ctrl_cleanup_device(struct uvc_device *dev);
 extern int uvc_ctrl_resume_device(struct uvc_device *dev);
 
 extern int uvc_ctrl_begin(struct uvc_video_chain *chain);
-extern int __uvc_ctrl_commit(struct uvc_video_chain *chain, int rollback);
-static inline int uvc_ctrl_commit(struct uvc_video_chain *chain)
+extern int __uvc_ctrl_commit(struct uvc_fh *handle, int rollback,
+			const struct v4l2_ext_control *xctrls,
+			unsigned int xctrls_count);
+static inline int uvc_ctrl_commit(struct uvc_fh *handle,
+			const struct v4l2_ext_control *xctrls,
+			unsigned int xctrls_count)
 {
-	return __uvc_ctrl_commit(chain, 0);
+	return __uvc_ctrl_commit(handle, 0, xctrls, xctrls_count);
 }
-static inline int uvc_ctrl_rollback(struct uvc_video_chain *chain)
+static inline int uvc_ctrl_rollback(struct uvc_fh *handle)
 {
-	return __uvc_ctrl_commit(chain, 1);
+	return __uvc_ctrl_commit(handle, 1, NULL, 0);
 }
 
 extern int uvc_ctrl_get(struct uvc_video_chain *chain,
diff --git a/drivers/media/video/v4l2-compat-ioctl32.c b/drivers/media/video/v4l2-compat-ioctl32.c
index 2829d256e4b7..5327ad3a6390 100644
--- a/drivers/media/video/v4l2-compat-ioctl32.c
+++ b/drivers/media/video/v4l2-compat-ioctl32.c
@@ -37,7 +37,7 @@ struct v4l2_clip32 {
 
 struct v4l2_window32 {
 	struct v4l2_rect        w;
-	enum v4l2_field  	field;
+	__u32		  	field;	/* enum v4l2_field */
 	__u32			chromakey;
 	compat_caddr_t		clips; /* actually struct v4l2_clip32 * */
 	__u32			clipcount;
@@ -147,7 +147,7 @@ static inline int put_v4l2_sliced_vbi_format(struct v4l2_sliced_vbi_format *kp,
 }
 
 struct v4l2_format32 {
-	enum v4l2_buf_type type;
+	__u32	type;	/* enum v4l2_buf_type */
 	union {
 		struct v4l2_pix_format	pix;
 		struct v4l2_pix_format_mplane	pix_mp;
@@ -170,7 +170,7 @@ struct v4l2_format32 {
 struct v4l2_create_buffers32 {
 	__u32			index;
 	__u32			count;
-	enum v4l2_memory        memory;
+	__u32			memory;	/* enum v4l2_memory */
 	struct v4l2_format32	format;
 	__u32			reserved[8];
 };
@@ -311,16 +311,16 @@ struct v4l2_plane32 {
 
 struct v4l2_buffer32 {
 	__u32			index;
-	enum v4l2_buf_type      type;
+	__u32			type;	/* enum v4l2_buf_type */
 	__u32			bytesused;
 	__u32			flags;
-	enum v4l2_field		field;
+	__u32			field;	/* enum v4l2_field */
 	struct compat_timeval	timestamp;
 	struct v4l2_timecode	timecode;
 	__u32			sequence;
 
 	/* memory location */
-	enum v4l2_memory        memory;
+	__u32			memory;	/* enum v4l2_memory */
 	union {
 		__u32           offset;
 		compat_long_t   userptr;
@@ -1023,6 +1023,9 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
 	case VIDIOC_UNSUBSCRIBE_EVENT:
 	case VIDIOC_CREATE_BUFS32:
 	case VIDIOC_PREPARE_BUF32:
+	case VIDIOC_ENUM_DV_TIMINGS:
+	case VIDIOC_QUERY_DV_TIMINGS:
+	case VIDIOC_DV_TIMINGS_CAP:
 		ret = do_video_ioctl(file, cmd, arg);
 		break;
 
diff --git a/drivers/media/video/v4l2-ctrls.c b/drivers/media/video/v4l2-ctrls.c
index 18015c0a8d31..9abd9abd4502 100644
--- a/drivers/media/video/v4l2-ctrls.c
+++ b/drivers/media/video/v4l2-ctrls.c
@@ -230,6 +230,19 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Aperture Priority Mode",
 		NULL
 	};
+	static const char * const camera_exposure_metering[] = {
+		"Average",
+		"Center Weighted",
+		"Spot",
+		NULL
+	};
+	static const char * const camera_auto_focus_range[] = {
+		"Auto",
+		"Normal",
+		"Macro",
+		"Infinity",
+		NULL
+	};
 	static const char * const colorfx[] = {
 		"None",
 		"Black & White",
@@ -241,6 +254,47 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Grass Green",
 		"Skin Whiten",
 		"Vivid",
+		"Aqua",
+		"Art Freeze",
+		"Silhouette",
+		"Solarization",
+		"Antique",
+		"Set Cb/Cr",
+		NULL
+	};
+	static const char * const auto_n_preset_white_balance[] = {
+		"Manual",
+		"Auto",
+		"Incandescent",
+		"Fluorescent",
+		"Fluorescent H",
+		"Horizon",
+		"Daylight",
+		"Flash",
+		"Cloudy",
+		"Shade",
+		NULL,
+	};
+	static const char * const camera_iso_sensitivity_auto[] = {
+		"Manual",
+		"Auto",
+		NULL
+	};
+	static const char * const scene_mode[] = {
+		"None",
+		"Backlight",
+		"Beach/Snow",
+		"Candle Light",
+		"Dusk/Dawn",
+		"Fall Colors",
+		"Fireworks",
+		"Landscape",
+		"Night",
+		"Party/Indoor",
+		"Portrait",
+		"Sports",
+		"Sunset",
+		"Text",
 		NULL
 	};
 	static const char * const tune_preemphasis[] = {
@@ -410,8 +464,18 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		return camera_power_line_frequency;
 	case V4L2_CID_EXPOSURE_AUTO:
 		return camera_exposure_auto;
+	case V4L2_CID_EXPOSURE_METERING:
+		return camera_exposure_metering;
+	case V4L2_CID_AUTO_FOCUS_RANGE:
+		return camera_auto_focus_range;
 	case V4L2_CID_COLORFX:
 		return colorfx;
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		return auto_n_preset_white_balance;
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		return camera_iso_sensitivity_auto;
+	case V4L2_CID_SCENE_MODE:
+		return scene_mode;
 	case V4L2_CID_TUNE_PREEMPHASIS:
 		return tune_preemphasis;
 	case V4L2_CID_FLASH_LED_MODE:
@@ -493,6 +557,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:	return "Min Number of Capture Buffers";
 	case V4L2_CID_MIN_BUFFERS_FOR_OUTPUT:	return "Min Number of Output Buffers";
 	case V4L2_CID_ALPHA_COMPONENT:		return "Alpha Component";
+	case V4L2_CID_COLORFX_CBCR:		return "Color Effects, CbCr";
 
 	/* MPEG controls */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
@@ -590,13 +655,26 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_TILT_ABSOLUTE:		return "Tilt, Absolute";
 	case V4L2_CID_FOCUS_ABSOLUTE:		return "Focus, Absolute";
 	case V4L2_CID_FOCUS_RELATIVE:		return "Focus, Relative";
-	case V4L2_CID_FOCUS_AUTO:		return "Focus, Automatic";
+	case V4L2_CID_FOCUS_AUTO:		return "Focus, Automatic Continuous";
 	case V4L2_CID_ZOOM_ABSOLUTE:		return "Zoom, Absolute";
 	case V4L2_CID_ZOOM_RELATIVE:		return "Zoom, Relative";
 	case V4L2_CID_ZOOM_CONTINUOUS:		return "Zoom, Continuous";
 	case V4L2_CID_PRIVACY:			return "Privacy";
 	case V4L2_CID_IRIS_ABSOLUTE:		return "Iris, Absolute";
 	case V4L2_CID_IRIS_RELATIVE:		return "Iris, Relative";
+	case V4L2_CID_AUTO_EXPOSURE_BIAS:	return "Auto Exposure, Bias";
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE: return "White Balance, Auto & Preset";
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:	return "Wide Dynamic Range";
+	case V4L2_CID_IMAGE_STABILIZATION:	return "Image Stabilization";
+	case V4L2_CID_ISO_SENSITIVITY:		return "ISO Sensitivity";
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:	return "ISO Sensitivity, Auto";
+	case V4L2_CID_EXPOSURE_METERING:	return "Exposure, Metering Mode";
+	case V4L2_CID_SCENE_MODE:		return "Scene Mode";
+	case V4L2_CID_3A_LOCK:			return "3A Lock";
+	case V4L2_CID_AUTO_FOCUS_START:		return "Auto Focus, Start";
+	case V4L2_CID_AUTO_FOCUS_STOP:		return "Auto Focus, Stop";
+	case V4L2_CID_AUTO_FOCUS_STATUS:	return "Auto Focus, Status";
+	case V4L2_CID_AUTO_FOCUS_RANGE:		return "Auto Focus, Range";
 
 	/* FM Radio Modulator control */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
@@ -644,6 +722,17 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_JPEG_COMPRESSION_QUALITY:	return "Compression Quality";
 	case V4L2_CID_JPEG_ACTIVE_MARKER:	return "Active Markers";
 
+	/* Image source controls */
+	case V4L2_CID_IMAGE_SOURCE_CLASS:	return "Image Source Controls";
+	case V4L2_CID_VBLANK:			return "Vertical Blanking";
+	case V4L2_CID_HBLANK:			return "Horizontal Blanking";
+	case V4L2_CID_ANALOGUE_GAIN:		return "Analogue Gain";
+
+	/* Image processing controls */
+	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";
+
 	default:
 		return NULL;
 	}
@@ -688,6 +777,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM:
 	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:
 	case V4L2_CID_MPEG_VIDEO_MPEG4_QPEL:
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:
+	case V4L2_CID_IMAGE_STABILIZATION:
 		*type = V4L2_CTRL_TYPE_BOOLEAN;
 		*min = 0;
 		*max = *step = 1;
@@ -696,6 +787,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_TILT_RESET:
 	case V4L2_CID_FLASH_STROBE:
 	case V4L2_CID_FLASH_STROBE_STOP:
+	case V4L2_CID_AUTO_FOCUS_START:
+	case V4L2_CID_AUTO_FOCUS_STOP:
 		*type = V4L2_CTRL_TYPE_BUTTON;
 		*flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
 		*min = *max = *step = *def = 0;
@@ -719,7 +812,9 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_MPEG_STREAM_TYPE:
 	case V4L2_CID_MPEG_STREAM_VBI_FMT:
 	case V4L2_CID_EXPOSURE_AUTO:
+	case V4L2_CID_AUTO_FOCUS_RANGE:
 	case V4L2_CID_COLORFX:
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
 	case V4L2_CID_TUNE_PREEMPHASIS:
 	case V4L2_CID_FLASH_LED_MODE:
 	case V4L2_CID_FLASH_STROBE_SOURCE:
@@ -733,18 +828,30 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
 	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
 	case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+	case V4L2_CID_EXPOSURE_METERING:
+	case V4L2_CID_SCENE_MODE:
 		*type = V4L2_CTRL_TYPE_MENU;
 		break;
+	case V4L2_CID_LINK_FREQ:
+		*type = V4L2_CTRL_TYPE_INTEGER_MENU;
+		break;
 	case V4L2_CID_RDS_TX_PS_NAME:
 	case V4L2_CID_RDS_TX_RADIO_TEXT:
 		*type = V4L2_CTRL_TYPE_STRING;
 		break;
+	case V4L2_CID_ISO_SENSITIVITY:
+	case V4L2_CID_AUTO_EXPOSURE_BIAS:
+		*type = V4L2_CTRL_TYPE_INTEGER_MENU;
+		break;
 	case V4L2_CID_USER_CLASS:
 	case V4L2_CID_CAMERA_CLASS:
 	case V4L2_CID_MPEG_CLASS:
 	case V4L2_CID_FM_TX_CLASS:
 	case V4L2_CID_FLASH_CLASS:
 	case V4L2_CID_JPEG_CLASS:
+	case V4L2_CID_IMAGE_SOURCE_CLASS:
+	case V4L2_CID_IMAGE_PROC_CLASS:
 		*type = V4L2_CTRL_TYPE_CTRL_CLASS;
 		/* You can neither read not write these */
 		*flags |= V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY;
@@ -759,6 +866,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		break;
 	case V4L2_CID_FLASH_FAULT:
 	case V4L2_CID_JPEG_ACTIVE_MARKER:
+	case V4L2_CID_3A_LOCK:
+	case V4L2_CID_AUTO_FOCUS_STATUS:
 		*type = V4L2_CTRL_TYPE_BITMASK;
 		break;
 	case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
@@ -768,8 +877,12 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		break;
 	case V4L2_CID_MPEG_VIDEO_DEC_FRAME:
 	case V4L2_CID_MPEG_VIDEO_DEC_PTS:
+		*flags |= V4L2_CTRL_FLAG_VOLATILE;
+		/* Fall through */
+	case V4L2_CID_PIXEL_RATE:
 		*type = V4L2_CTRL_TYPE_INTEGER64;
-		*flags |= V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE;
+		*flags |= V4L2_CTRL_FLAG_READ_ONLY;
+		*min = *max = *step = *def = 0;
 		break;
 	default:
 		*type = V4L2_CTRL_TYPE_INTEGER;
@@ -817,6 +930,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		*flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
 		break;
 	case V4L2_CID_FLASH_STROBE_STATUS:
+	case V4L2_CID_AUTO_FOCUS_STATUS:
 	case V4L2_CID_FLASH_READY:
 		*flags |= V4L2_CTRL_FLAG_READ_ONLY;
 		break;
@@ -852,7 +966,8 @@ static void fill_event(struct v4l2_event *ev, struct v4l2_ctrl *ctrl, u32 change
 		ev->u.ctrl.value64 = ctrl->cur.val64;
 	ev->u.ctrl.minimum = ctrl->minimum;
 	ev->u.ctrl.maximum = ctrl->maximum;
-	if (ctrl->type == V4L2_CTRL_TYPE_MENU)
+	if (ctrl->type == V4L2_CTRL_TYPE_MENU
+	    || ctrl->type == V4L2_CTRL_TYPE_INTEGER_MENU)
 		ev->u.ctrl.step = 1;
 	else
 		ev->u.ctrl.step = ctrl->step;
@@ -1083,10 +1198,13 @@ static int validate_new_int(const struct v4l2_ctrl *ctrl, s32 *pval)
 		return 0;
 
 	case V4L2_CTRL_TYPE_MENU:
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
 		if (val < ctrl->minimum || val > ctrl->maximum)
 			return -ERANGE;
-		if (ctrl->qmenu[val][0] == '\0' ||
-		    (ctrl->menu_skip_mask & (1 << val)))
+		if (ctrl->menu_skip_mask & (1 << val))
+			return -EINVAL;
+		if (ctrl->type == V4L2_CTRL_TYPE_MENU &&
+		    ctrl->qmenu[val][0] == '\0')
 			return -EINVAL;
 		return 0;
 
@@ -1114,6 +1232,7 @@ static int validate_new(const struct v4l2_ctrl *ctrl, struct v4l2_ext_control *c
 	case V4L2_CTRL_TYPE_INTEGER:
 	case V4L2_CTRL_TYPE_BOOLEAN:
 	case V4L2_CTRL_TYPE_MENU:
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
 	case V4L2_CTRL_TYPE_BITMASK:
 	case V4L2_CTRL_TYPE_BUTTON:
 	case V4L2_CTRL_TYPE_CTRL_CLASS:
@@ -1152,7 +1271,8 @@ static inline int handler_set_err(struct v4l2_ctrl_handler *hdl, int err)
 int v4l2_ctrl_handler_init(struct v4l2_ctrl_handler *hdl,
 			   unsigned nr_of_controls_hint)
 {
-	mutex_init(&hdl->lock);
+	hdl->lock = &hdl->_lock;
+	mutex_init(hdl->lock);
 	INIT_LIST_HEAD(&hdl->ctrls);
 	INIT_LIST_HEAD(&hdl->ctrl_refs);
 	hdl->nr_of_buckets = 1 + nr_of_controls_hint / 8;
@@ -1173,7 +1293,7 @@ void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
 	if (hdl == NULL || hdl->buckets == NULL)
 		return;
 
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	/* Free all nodes */
 	list_for_each_entry_safe(ref, next_ref, &hdl->ctrl_refs, node) {
 		list_del(&ref->node);
@@ -1190,7 +1310,7 @@ void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
 	hdl->buckets = NULL;
 	hdl->cached = NULL;
 	hdl->error = 0;
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_free);
 
@@ -1255,9 +1375,9 @@ static struct v4l2_ctrl_ref *find_ref_lock(
 	struct v4l2_ctrl_ref *ref = NULL;
 
 	if (hdl) {
-		mutex_lock(&hdl->lock);
+		mutex_lock(hdl->lock);
 		ref = find_ref(hdl, id);
-		mutex_unlock(&hdl->lock);
+		mutex_unlock(hdl->lock);
 	}
 	return ref;
 }
@@ -1304,7 +1424,7 @@ static int handler_new_ref(struct v4l2_ctrl_handler *hdl,
 
 	INIT_LIST_HEAD(&new_ref->node);
 
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 
 	/* Add immediately at the end of the list if the list is empty, or if
 	   the last element in the list has a lower ID.
@@ -1334,7 +1454,7 @@ insert_in_hash:
 	hdl->buckets[bucket] = new_ref;
 
 unlock:
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return 0;
 }
 
@@ -1343,7 +1463,8 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 			const struct v4l2_ctrl_ops *ops,
 			u32 id, const char *name, enum v4l2_ctrl_type type,
 			s32 min, s32 max, u32 step, s32 def,
-			u32 flags, const char * const *qmenu, void *priv)
+			u32 flags, const char * const *qmenu,
+			const s64 *qmenu_int, void *priv)
 {
 	struct v4l2_ctrl *ctrl;
 	unsigned sz_extra = 0;
@@ -1356,6 +1477,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	    (type == V4L2_CTRL_TYPE_INTEGER && step == 0) ||
 	    (type == V4L2_CTRL_TYPE_BITMASK && max == 0) ||
 	    (type == V4L2_CTRL_TYPE_MENU && qmenu == NULL) ||
+	    (type == V4L2_CTRL_TYPE_INTEGER_MENU && qmenu_int == NULL) ||
 	    (type == V4L2_CTRL_TYPE_STRING && max == 0)) {
 		handler_set_err(hdl, -ERANGE);
 		return NULL;
@@ -1366,6 +1488,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	}
 	if ((type == V4L2_CTRL_TYPE_INTEGER ||
 	     type == V4L2_CTRL_TYPE_MENU ||
+	     type == V4L2_CTRL_TYPE_INTEGER_MENU ||
 	     type == V4L2_CTRL_TYPE_BOOLEAN) &&
 	    (def < min || def > max)) {
 		handler_set_err(hdl, -ERANGE);
@@ -1400,7 +1523,10 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	ctrl->minimum = min;
 	ctrl->maximum = max;
 	ctrl->step = step;
-	ctrl->qmenu = qmenu;
+	if (type == V4L2_CTRL_TYPE_MENU)
+		ctrl->qmenu = qmenu;
+	else if (type == V4L2_CTRL_TYPE_INTEGER_MENU)
+		ctrl->qmenu_int = qmenu_int;
 	ctrl->priv = priv;
 	ctrl->cur.val = ctrl->val = ctrl->default_value = def;
 
@@ -1414,9 +1540,9 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 		kfree(ctrl);
 		return NULL;
 	}
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	list_add_tail(&ctrl->node, &hdl->ctrls);
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return ctrl;
 }
 
@@ -1427,6 +1553,7 @@ struct v4l2_ctrl *v4l2_ctrl_new_custom(struct v4l2_ctrl_handler *hdl,
 	struct v4l2_ctrl *ctrl;
 	const char *name = cfg->name;
 	const char * const *qmenu = cfg->qmenu;
+	const s64 *qmenu_int = cfg->qmenu_int;
 	enum v4l2_ctrl_type type = cfg->type;
 	u32 flags = cfg->flags;
 	s32 min = cfg->min;
@@ -1438,18 +1565,24 @@ struct v4l2_ctrl *v4l2_ctrl_new_custom(struct v4l2_ctrl_handler *hdl,
 		v4l2_ctrl_fill(cfg->id, &name, &type, &min, &max, &step,
 								&def, &flags);
 
-	is_menu = (cfg->type == V4L2_CTRL_TYPE_MENU);
+	is_menu = (cfg->type == V4L2_CTRL_TYPE_MENU ||
+		   cfg->type == V4L2_CTRL_TYPE_INTEGER_MENU);
 	if (is_menu)
 		WARN_ON(step);
 	else
 		WARN_ON(cfg->menu_skip_mask);
-	if (is_menu && qmenu == NULL)
+	if (cfg->type == V4L2_CTRL_TYPE_MENU && qmenu == NULL)
 		qmenu = v4l2_ctrl_get_menu(cfg->id);
+	else if (cfg->type == V4L2_CTRL_TYPE_INTEGER_MENU &&
+		 qmenu_int == NULL) {
+		handler_set_err(hdl, -EINVAL);
+		return NULL;
+	}
 
 	ctrl = v4l2_ctrl_new(hdl, cfg->ops, cfg->id, name,
 			type, min, max,
 			is_menu ? cfg->menu_skip_mask : step,
-			def, flags, qmenu, priv);
+			def, flags, qmenu, qmenu_int, priv);
 	if (ctrl)
 		ctrl->is_private = cfg->is_private;
 	return ctrl;
@@ -1466,12 +1599,13 @@ struct v4l2_ctrl *v4l2_ctrl_new_std(struct v4l2_ctrl_handler *hdl,
 	u32 flags;
 
 	v4l2_ctrl_fill(id, &name, &type, &min, &max, &step, &def, &flags);
-	if (type == V4L2_CTRL_TYPE_MENU) {
+	if (type == V4L2_CTRL_TYPE_MENU
+	    || type == V4L2_CTRL_TYPE_INTEGER_MENU) {
 		handler_set_err(hdl, -EINVAL);
 		return NULL;
 	}
 	return v4l2_ctrl_new(hdl, ops, id, name, type,
-				    min, max, step, def, flags, NULL, NULL);
+			     min, max, step, def, flags, NULL, NULL, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std);
 
@@ -1493,10 +1627,31 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu(struct v4l2_ctrl_handler *hdl,
 		return NULL;
 	}
 	return v4l2_ctrl_new(hdl, ops, id, name, type,
-				    0, max, mask, def, flags, qmenu, NULL);
+			     0, max, mask, def, flags, qmenu, NULL, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_menu);
 
+/* 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,
+			u32 id, s32 max, s32 def, const s64 *qmenu_int)
+{
+	const char *name;
+	enum v4l2_ctrl_type type;
+	s32 min;
+	s32 step;
+	u32 flags;
+
+	v4l2_ctrl_fill(id, &name, &type, &min, &max, &step, &def, &flags);
+	if (type != V4L2_CTRL_TYPE_INTEGER_MENU) {
+		handler_set_err(hdl, -EINVAL);
+		return NULL;
+	}
+	return v4l2_ctrl_new(hdl, ops, id, name, type,
+			     0, max, 0, def, flags, NULL, qmenu_int, NULL);
+}
+EXPORT_SYMBOL(v4l2_ctrl_new_int_menu);
+
 /* Add a control from another handler to this handler */
 struct v4l2_ctrl *v4l2_ctrl_add_ctrl(struct v4l2_ctrl_handler *hdl,
 					  struct v4l2_ctrl *ctrl)
@@ -1525,7 +1680,7 @@ int v4l2_ctrl_add_handler(struct v4l2_ctrl_handler *hdl,
 		return 0;
 	if (hdl->error)
 		return hdl->error;
-	mutex_lock(&add->lock);
+	mutex_lock(add->lock);
 	list_for_each_entry(ref, &add->ctrl_refs, node) {
 		struct v4l2_ctrl *ctrl = ref->ctrl;
 
@@ -1539,7 +1694,7 @@ int v4l2_ctrl_add_handler(struct v4l2_ctrl_handler *hdl,
 		if (ret)
 			break;
 	}
-	mutex_unlock(&add->lock);
+	mutex_unlock(add->lock);
 	return ret;
 }
 EXPORT_SYMBOL(v4l2_ctrl_add_handler);
@@ -1659,6 +1814,9 @@ static void log_ctrl(const struct v4l2_ctrl *ctrl,
 	case V4L2_CTRL_TYPE_MENU:
 		printk(KERN_CONT "%s", ctrl->qmenu[ctrl->cur.val]);
 		break;
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
+		printk(KERN_CONT "%lld", ctrl->qmenu_int[ctrl->cur.val]);
+		break;
 	case V4L2_CTRL_TYPE_BITMASK:
 		printk(KERN_CONT "0x%08x", ctrl->cur.val);
 		break;
@@ -1700,11 +1858,11 @@ void v4l2_ctrl_handler_log_status(struct v4l2_ctrl_handler *hdl,
 	len = strlen(prefix);
 	if (len && prefix[len - 1] != ' ')
 		colon = ": ";
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	list_for_each_entry(ctrl, &hdl->ctrls, node)
 		if (!(ctrl->flags & V4L2_CTRL_FLAG_DISABLED))
 			log_ctrl(ctrl, prefix, colon);
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_log_status);
 
@@ -1716,7 +1874,7 @@ int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
 
 	if (hdl == NULL)
 		return 0;
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	list_for_each_entry(ctrl, &hdl->ctrls, node)
 		ctrl->done = false;
 
@@ -1741,7 +1899,7 @@ int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
 		if (ret)
 			break;
 	}
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return ret;
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_setup);
@@ -1756,7 +1914,7 @@ int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
 	if (hdl == NULL)
 		return -EINVAL;
 
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 
 	/* Try to find it */
 	ref = find_ref(hdl, id);
@@ -1781,7 +1939,7 @@ int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
 					break;
 		}
 	}
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	if (!ref)
 		return -EINVAL;
 
@@ -1795,7 +1953,8 @@ int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
 	qc->minimum = ctrl->minimum;
 	qc->maximum = ctrl->maximum;
 	qc->default_value = ctrl->default_value;
-	if (ctrl->type == V4L2_CTRL_TYPE_MENU)
+	if (ctrl->type == V4L2_CTRL_TYPE_MENU
+	    || ctrl->type == V4L2_CTRL_TYPE_INTEGER_MENU)
 		qc->step = 1;
 	else
 		qc->step = ctrl->step;
@@ -1825,16 +1984,33 @@ int v4l2_querymenu(struct v4l2_ctrl_handler *hdl, struct v4l2_querymenu *qm)
 
 	qm->reserved = 0;
 	/* Sanity checks */
-	if (ctrl->qmenu == NULL ||
-	    i < ctrl->minimum || i > ctrl->maximum)
+	switch (ctrl->type) {
+	case V4L2_CTRL_TYPE_MENU:
+		if (ctrl->qmenu == NULL)
+			return -EINVAL;
+		break;
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
+		if (ctrl->qmenu_int == NULL)
+			return -EINVAL;
+		break;
+	default:
 		return -EINVAL;
+	}
+
+	if (i < ctrl->minimum || i > ctrl->maximum)
+		return -EINVAL;
+
 	/* Use mask to see if this menu item should be skipped */
 	if (ctrl->menu_skip_mask & (1 << i))
 		return -EINVAL;
 	/* Empty menu items should also be skipped */
-	if (ctrl->qmenu[i] == NULL || ctrl->qmenu[i][0] == '\0')
-		return -EINVAL;
-	strlcpy(qm->name, ctrl->qmenu[i], sizeof(qm->name));
+	if (ctrl->type == V4L2_CTRL_TYPE_MENU) {
+		if (ctrl->qmenu[i] == NULL || ctrl->qmenu[i][0] == '\0')
+			return -EINVAL;
+		strlcpy(qm->name, ctrl->qmenu[i], sizeof(qm->name));
+	} else {
+		qm->value = ctrl->qmenu_int[i];
+	}
 	return 0;
 }
 EXPORT_SYMBOL(v4l2_querymenu);
@@ -1940,7 +2116,7 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 	   belong to the same cluster. */
 
 	/* This has to be done with the handler lock taken. */
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 
 	/* First zero the helper field in the master control references */
 	for (i = 0; i < cs->count; i++)
@@ -1962,7 +2138,7 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 		/* Point the mref helper to the current helper struct. */
 		mref->helper = h;
 	}
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return 0;
 }
 
@@ -1996,7 +2172,8 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
 		return class_check(hdl, cs->ctrl_class);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
-		helpers = kmalloc(sizeof(helper[0]) * cs->count, GFP_KERNEL);
+		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
+					GFP_KERNEL);
 		if (helpers == NULL)
 			return -ENOMEM;
 	}
@@ -2218,7 +2395,8 @@ static int try_set_ext_ctrls(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
 		return class_check(hdl, cs->ctrl_class);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
-		helpers = kmalloc(sizeof(helper[0]) * cs->count, GFP_KERNEL);
+		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
+					GFP_KERNEL);
 		if (!helpers)
 			return -ENOMEM;
 	}
@@ -2381,9 +2559,13 @@ int v4l2_ctrl_s_ctrl(struct v4l2_ctrl *ctrl, s32 val)
 }
 EXPORT_SYMBOL(v4l2_ctrl_s_ctrl);
 
-void v4l2_ctrl_add_event(struct v4l2_ctrl *ctrl,
-				struct v4l2_subscribed_event *sev)
+static int v4l2_ctrl_add_event(struct v4l2_subscribed_event *sev, unsigned elems)
 {
+	struct v4l2_ctrl *ctrl = v4l2_ctrl_find(sev->fh->ctrl_handler, sev->id);
+
+	if (ctrl == NULL)
+		return -EINVAL;
+
 	v4l2_ctrl_lock(ctrl);
 	list_add_tail(&sev->node, &ctrl->ev_subs);
 	if (ctrl->type != V4L2_CTRL_TYPE_CTRL_CLASS &&
@@ -2394,20 +2576,46 @@ void v4l2_ctrl_add_event(struct v4l2_ctrl *ctrl,
 		if (!(ctrl->flags & V4L2_CTRL_FLAG_WRITE_ONLY))
 			changes |= V4L2_EVENT_CTRL_CH_VALUE;
 		fill_event(&ev, ctrl, changes);
+		/* Mark the queue as active, allowing this initial
+		   event to be accepted. */
+		sev->elems = elems;
 		v4l2_event_queue_fh(sev->fh, &ev);
 	}
 	v4l2_ctrl_unlock(ctrl);
+	return 0;
 }
-EXPORT_SYMBOL(v4l2_ctrl_add_event);
 
-void v4l2_ctrl_del_event(struct v4l2_ctrl *ctrl,
-				struct v4l2_subscribed_event *sev)
+static void v4l2_ctrl_del_event(struct v4l2_subscribed_event *sev)
 {
+	struct v4l2_ctrl *ctrl = v4l2_ctrl_find(sev->fh->ctrl_handler, sev->id);
+
 	v4l2_ctrl_lock(ctrl);
 	list_del(&sev->node);
 	v4l2_ctrl_unlock(ctrl);
 }
-EXPORT_SYMBOL(v4l2_ctrl_del_event);
+
+void v4l2_ctrl_replace(struct v4l2_event *old, const struct v4l2_event *new)
+{
+	u32 old_changes = old->u.ctrl.changes;
+
+	old->u.ctrl = new->u.ctrl;
+	old->u.ctrl.changes |= old_changes;
+}
+EXPORT_SYMBOL(v4l2_ctrl_replace);
+
+void v4l2_ctrl_merge(const struct v4l2_event *old, struct v4l2_event *new)
+{
+	new->u.ctrl.changes |= old->u.ctrl.changes;
+}
+EXPORT_SYMBOL(v4l2_ctrl_merge);
+
+const struct v4l2_subscribed_event_ops v4l2_ctrl_sub_ev_ops = {
+	.add = v4l2_ctrl_add_event,
+	.del = v4l2_ctrl_del_event,
+	.replace = v4l2_ctrl_replace,
+	.merge = v4l2_ctrl_merge,
+};
+EXPORT_SYMBOL(v4l2_ctrl_sub_ev_ops);
 
 int v4l2_ctrl_log_status(struct file *file, void *fh)
 {
@@ -2425,7 +2633,7 @@ int v4l2_ctrl_subscribe_event(struct v4l2_fh *fh,
 				struct v4l2_event_subscription *sub)
 {
 	if (sub->type == V4L2_EVENT_CTRL)
-		return v4l2_event_subscribe(fh, sub, 0);
+		return v4l2_event_subscribe(fh, sub, 0, &v4l2_ctrl_sub_ev_ops);
 	return -EINVAL;
 }
 EXPORT_SYMBOL(v4l2_ctrl_subscribe_event);
diff --git a/drivers/media/video/v4l2-dev.c b/drivers/media/video/v4l2-dev.c
index 70bec548d904..5ccbd4629f9c 100644
--- a/drivers/media/video/v4l2-dev.c
+++ b/drivers/media/video/v4l2-dev.c
@@ -274,11 +274,12 @@ static ssize_t v4l2_read(struct file *filp, char __user *buf,
 
 	if (!vdev->fops->read)
 		return -EINVAL;
-	if (vdev->lock && mutex_lock_interruptible(vdev->lock))
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+	    mutex_lock_interruptible(vdev->lock))
 		return -ERESTARTSYS;
 	if (video_is_registered(vdev))
 		ret = vdev->fops->read(filp, buf, sz, off);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -291,11 +292,12 @@ static ssize_t v4l2_write(struct file *filp, const char __user *buf,
 
 	if (!vdev->fops->write)
 		return -EINVAL;
-	if (vdev->lock && mutex_lock_interruptible(vdev->lock))
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+	    mutex_lock_interruptible(vdev->lock))
 		return -ERESTARTSYS;
 	if (video_is_registered(vdev))
 		ret = vdev->fops->write(filp, buf, sz, off);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -307,11 +309,11 @@ static unsigned int v4l2_poll(struct file *filp, struct poll_table_struct *poll)
 
 	if (!vdev->fops->poll)
 		return DEFAULT_POLLMASK;
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_lock(vdev->lock);
 	if (video_is_registered(vdev))
 		ret = vdev->fops->poll(filp, poll);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -322,11 +324,19 @@ static long v4l2_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	int ret = -ENODEV;
 
 	if (vdev->fops->unlocked_ioctl) {
-		if (vdev->lock && mutex_lock_interruptible(vdev->lock))
-			return -ERESTARTSYS;
+		bool locked = false;
+
+		if (vdev->lock) {
+			/* always lock unless the cmd is marked as "don't use lock" */
+			locked = !v4l2_is_known_ioctl(cmd) ||
+				 !test_bit(_IOC_NR(cmd), vdev->disable_locking);
+
+			if (locked && mutex_lock_interruptible(vdev->lock))
+				return -ERESTARTSYS;
+		}
 		if (video_is_registered(vdev))
 			ret = vdev->fops->unlocked_ioctl(filp, cmd, arg);
-		if (vdev->lock)
+		if (locked)
 			mutex_unlock(vdev->lock);
 	} else if (vdev->fops->ioctl) {
 		/* This code path is a replacement for the BKL. It is a major
@@ -391,11 +401,12 @@ static int v4l2_mmap(struct file *filp, struct vm_area_struct *vm)
 
 	if (!vdev->fops->mmap)
 		return ret;
-	if (vdev->lock && mutex_lock_interruptible(vdev->lock))
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+	    mutex_lock_interruptible(vdev->lock))
 		return -ERESTARTSYS;
 	if (video_is_registered(vdev))
 		ret = vdev->fops->mmap(filp, vm);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -418,7 +429,8 @@ static int v4l2_open(struct inode *inode, struct file *filp)
 	video_get(vdev);
 	mutex_unlock(&videodev_lock);
 	if (vdev->fops->open) {
-		if (vdev->lock && mutex_lock_interruptible(vdev->lock)) {
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+		    mutex_lock_interruptible(vdev->lock)) {
 			ret = -ERESTARTSYS;
 			goto err;
 		}
@@ -426,7 +438,7 @@ static int v4l2_open(struct inode *inode, struct file *filp)
 			ret = vdev->fops->open(filp);
 		else
 			ret = -ENODEV;
-		if (vdev->lock)
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 			mutex_unlock(vdev->lock);
 	}
 
@@ -444,10 +456,10 @@ static int v4l2_release(struct inode *inode, struct file *filp)
 	int ret = 0;
 
 	if (vdev->fops->release) {
-		if (vdev->lock)
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 			mutex_lock(vdev->lock);
 		vdev->fops->release(filp);
-		if (vdev->lock)
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 			mutex_unlock(vdev->lock);
 	}
 	/* decrease the refcount unconditionally since the release()
@@ -508,6 +520,175 @@ static int get_index(struct video_device *vdev)
 	return find_first_zero_bit(used, VIDEO_NUM_DEVICES);
 }
 
+#define SET_VALID_IOCTL(ops, cmd, op)			\
+	if (ops->op)					\
+		set_bit(_IOC_NR(cmd), valid_ioctls)
+
+/* This determines which ioctls are actually implemented in the driver.
+   It's a one-time thing which simplifies video_ioctl2 as it can just do
+   a bit test.
+
+   Note that drivers can override this by setting bits to 1 in
+   vdev->valid_ioctls. If an ioctl is marked as 1 when this function is
+   called, then that ioctl will actually be marked as unimplemented.
+
+   It does that by first setting up the local valid_ioctls bitmap, and
+   at the end do a:
+
+   vdev->valid_ioctls = valid_ioctls & ~(vdev->valid_ioctls)
+ */
+static void determine_valid_ioctls(struct video_device *vdev)
+{
+	DECLARE_BITMAP(valid_ioctls, BASE_VIDIOC_PRIVATE);
+	const struct v4l2_ioctl_ops *ops = vdev->ioctl_ops;
+
+	bitmap_zero(valid_ioctls, BASE_VIDIOC_PRIVATE);
+
+	SET_VALID_IOCTL(ops, VIDIOC_QUERYCAP, vidioc_querycap);
+	if (ops->vidioc_g_priority ||
+			test_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags))
+		set_bit(_IOC_NR(VIDIOC_G_PRIORITY), valid_ioctls);
+	if (ops->vidioc_s_priority ||
+			test_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags))
+		set_bit(_IOC_NR(VIDIOC_S_PRIORITY), valid_ioctls);
+	if (ops->vidioc_enum_fmt_vid_cap ||
+	    ops->vidioc_enum_fmt_vid_out ||
+	    ops->vidioc_enum_fmt_vid_cap_mplane ||
+	    ops->vidioc_enum_fmt_vid_out_mplane ||
+	    ops->vidioc_enum_fmt_vid_overlay ||
+	    ops->vidioc_enum_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_ENUM_FMT), valid_ioctls);
+	if (ops->vidioc_g_fmt_vid_cap ||
+	    ops->vidioc_g_fmt_vid_out ||
+	    ops->vidioc_g_fmt_vid_cap_mplane ||
+	    ops->vidioc_g_fmt_vid_out_mplane ||
+	    ops->vidioc_g_fmt_vid_overlay ||
+	    ops->vidioc_g_fmt_vbi_cap ||
+	    ops->vidioc_g_fmt_vid_out_overlay ||
+	    ops->vidioc_g_fmt_vbi_out ||
+	    ops->vidioc_g_fmt_sliced_vbi_cap ||
+	    ops->vidioc_g_fmt_sliced_vbi_out ||
+	    ops->vidioc_g_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_G_FMT), valid_ioctls);
+	if (ops->vidioc_s_fmt_vid_cap ||
+	    ops->vidioc_s_fmt_vid_out ||
+	    ops->vidioc_s_fmt_vid_cap_mplane ||
+	    ops->vidioc_s_fmt_vid_out_mplane ||
+	    ops->vidioc_s_fmt_vid_overlay ||
+	    ops->vidioc_s_fmt_vbi_cap ||
+	    ops->vidioc_s_fmt_vid_out_overlay ||
+	    ops->vidioc_s_fmt_vbi_out ||
+	    ops->vidioc_s_fmt_sliced_vbi_cap ||
+	    ops->vidioc_s_fmt_sliced_vbi_out ||
+	    ops->vidioc_s_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_S_FMT), valid_ioctls);
+	if (ops->vidioc_try_fmt_vid_cap ||
+	    ops->vidioc_try_fmt_vid_out ||
+	    ops->vidioc_try_fmt_vid_cap_mplane ||
+	    ops->vidioc_try_fmt_vid_out_mplane ||
+	    ops->vidioc_try_fmt_vid_overlay ||
+	    ops->vidioc_try_fmt_vbi_cap ||
+	    ops->vidioc_try_fmt_vid_out_overlay ||
+	    ops->vidioc_try_fmt_vbi_out ||
+	    ops->vidioc_try_fmt_sliced_vbi_cap ||
+	    ops->vidioc_try_fmt_sliced_vbi_out ||
+	    ops->vidioc_try_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_TRY_FMT), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_REQBUFS, vidioc_reqbufs);
+	SET_VALID_IOCTL(ops, VIDIOC_QUERYBUF, vidioc_querybuf);
+	SET_VALID_IOCTL(ops, VIDIOC_QBUF, vidioc_qbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_DQBUF, vidioc_dqbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_OVERLAY, vidioc_overlay);
+	SET_VALID_IOCTL(ops, VIDIOC_G_FBUF, vidioc_g_fbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_S_FBUF, vidioc_s_fbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_STREAMON, vidioc_streamon);
+	SET_VALID_IOCTL(ops, VIDIOC_STREAMOFF, vidioc_streamoff);
+	if (vdev->tvnorms)
+		set_bit(_IOC_NR(VIDIOC_ENUMSTD), valid_ioctls);
+	if (ops->vidioc_g_std || vdev->current_norm)
+		set_bit(_IOC_NR(VIDIOC_G_STD), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_S_STD, vidioc_s_std);
+	SET_VALID_IOCTL(ops, VIDIOC_QUERYSTD, vidioc_querystd);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUMINPUT, vidioc_enum_input);
+	SET_VALID_IOCTL(ops, VIDIOC_G_INPUT, vidioc_g_input);
+	SET_VALID_IOCTL(ops, VIDIOC_S_INPUT, vidioc_s_input);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUMOUTPUT, vidioc_enum_output);
+	SET_VALID_IOCTL(ops, VIDIOC_G_OUTPUT, vidioc_g_output);
+	SET_VALID_IOCTL(ops, VIDIOC_S_OUTPUT, vidioc_s_output);
+	/* Note: the control handler can also be passed through the filehandle,
+	   and that can't be tested here. If the bit for these control ioctls
+	   is set, then the ioctl is valid. But if it is 0, then it can still
+	   be valid if the filehandle passed the control handler. */
+	if (vdev->ctrl_handler || ops->vidioc_queryctrl)
+		set_bit(_IOC_NR(VIDIOC_QUERYCTRL), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_g_ctrl || ops->vidioc_g_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_G_CTRL), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_s_ctrl || ops->vidioc_s_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_S_CTRL), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_g_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_G_EXT_CTRLS), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_s_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_S_EXT_CTRLS), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_try_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_TRY_EXT_CTRLS), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_querymenu)
+		set_bit(_IOC_NR(VIDIOC_QUERYMENU), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUMAUDIO, vidioc_enumaudio);
+	SET_VALID_IOCTL(ops, VIDIOC_G_AUDIO, vidioc_g_audio);
+	SET_VALID_IOCTL(ops, VIDIOC_S_AUDIO, vidioc_s_audio);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUMAUDOUT, vidioc_enumaudout);
+	SET_VALID_IOCTL(ops, VIDIOC_G_AUDOUT, vidioc_g_audout);
+	SET_VALID_IOCTL(ops, VIDIOC_S_AUDOUT, vidioc_s_audout);
+	SET_VALID_IOCTL(ops, VIDIOC_G_MODULATOR, vidioc_g_modulator);
+	SET_VALID_IOCTL(ops, VIDIOC_S_MODULATOR, vidioc_s_modulator);
+	if (ops->vidioc_g_crop || ops->vidioc_g_selection)
+		set_bit(_IOC_NR(VIDIOC_G_CROP), valid_ioctls);
+	if (ops->vidioc_s_crop || ops->vidioc_s_selection)
+		set_bit(_IOC_NR(VIDIOC_S_CROP), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_G_SELECTION, vidioc_g_selection);
+	SET_VALID_IOCTL(ops, VIDIOC_S_SELECTION, vidioc_s_selection);
+	if (ops->vidioc_cropcap || ops->vidioc_g_selection)
+		set_bit(_IOC_NR(VIDIOC_CROPCAP), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_G_JPEGCOMP, vidioc_g_jpegcomp);
+	SET_VALID_IOCTL(ops, VIDIOC_S_JPEGCOMP, vidioc_s_jpegcomp);
+	SET_VALID_IOCTL(ops, VIDIOC_G_ENC_INDEX, vidioc_g_enc_index);
+	SET_VALID_IOCTL(ops, VIDIOC_ENCODER_CMD, vidioc_encoder_cmd);
+	SET_VALID_IOCTL(ops, VIDIOC_TRY_ENCODER_CMD, vidioc_try_encoder_cmd);
+	SET_VALID_IOCTL(ops, VIDIOC_DECODER_CMD, vidioc_decoder_cmd);
+	SET_VALID_IOCTL(ops, VIDIOC_TRY_DECODER_CMD, vidioc_try_decoder_cmd);
+	if (ops->vidioc_g_parm || vdev->current_norm)
+		set_bit(_IOC_NR(VIDIOC_G_PARM), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_S_PARM, vidioc_s_parm);
+	SET_VALID_IOCTL(ops, VIDIOC_G_TUNER, vidioc_g_tuner);
+	SET_VALID_IOCTL(ops, VIDIOC_S_TUNER, vidioc_s_tuner);
+	SET_VALID_IOCTL(ops, VIDIOC_G_FREQUENCY, vidioc_g_frequency);
+	SET_VALID_IOCTL(ops, VIDIOC_S_FREQUENCY, vidioc_s_frequency);
+	SET_VALID_IOCTL(ops, VIDIOC_G_SLICED_VBI_CAP, vidioc_g_sliced_vbi_cap);
+	SET_VALID_IOCTL(ops, VIDIOC_LOG_STATUS, vidioc_log_status);
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	SET_VALID_IOCTL(ops, VIDIOC_DBG_G_REGISTER, vidioc_g_register);
+	SET_VALID_IOCTL(ops, VIDIOC_DBG_S_REGISTER, vidioc_s_register);
+#endif
+	SET_VALID_IOCTL(ops, VIDIOC_DBG_G_CHIP_IDENT, vidioc_g_chip_ident);
+	SET_VALID_IOCTL(ops, VIDIOC_S_HW_FREQ_SEEK, vidioc_s_hw_freq_seek);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUM_FRAMESIZES, vidioc_enum_framesizes);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUM_FRAMEINTERVALS, vidioc_enum_frameintervals);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUM_DV_PRESETS, vidioc_enum_dv_presets);
+	SET_VALID_IOCTL(ops, VIDIOC_S_DV_PRESET, vidioc_s_dv_preset);
+	SET_VALID_IOCTL(ops, VIDIOC_G_DV_PRESET, vidioc_g_dv_preset);
+	SET_VALID_IOCTL(ops, VIDIOC_QUERY_DV_PRESET, vidioc_query_dv_preset);
+	SET_VALID_IOCTL(ops, VIDIOC_S_DV_TIMINGS, vidioc_s_dv_timings);
+	SET_VALID_IOCTL(ops, VIDIOC_G_DV_TIMINGS, vidioc_g_dv_timings);
+	/* yes, really vidioc_subscribe_event */
+	SET_VALID_IOCTL(ops, VIDIOC_DQEVENT, vidioc_subscribe_event);
+	SET_VALID_IOCTL(ops, VIDIOC_SUBSCRIBE_EVENT, vidioc_subscribe_event);
+	SET_VALID_IOCTL(ops, VIDIOC_UNSUBSCRIBE_EVENT, vidioc_unsubscribe_event);
+	SET_VALID_IOCTL(ops, VIDIOC_CREATE_BUFS, vidioc_create_bufs);
+	SET_VALID_IOCTL(ops, VIDIOC_PREPARE_BUF, vidioc_prepare_buf);
+	bitmap_andnot(vdev->valid_ioctls, valid_ioctls, vdev->valid_ioctls,
+			BASE_VIDIOC_PRIVATE);
+}
+
 /**
  *	__video_register_device - register video4linux devices
  *	@vdev: video device structure we want to register
@@ -654,6 +835,13 @@ int __video_register_device(struct video_device *vdev, int type, int nr,
 	WARN_ON(video_device[vdev->minor] != NULL);
 	vdev->index = get_index(vdev);
 	mutex_unlock(&videodev_lock);
+	/* if no lock was passed, then make sure the LOCK_ALL_FOPS bit is
+	   clear and warn if it wasn't. */
+	if (vdev->lock == NULL)
+		WARN_ON(test_and_clear_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags));
+
+	if (vdev->ioctl_ops)
+		determine_valid_ioctls(vdev);
 
 	/* Part 3: Initialize the character device */
 	vdev->cdev = cdev_alloc();
diff --git a/drivers/media/video/v4l2-event.c b/drivers/media/video/v4l2-event.c
index c26ad9637143..ef2a33c94045 100644
--- a/drivers/media/video/v4l2-event.c
+++ b/drivers/media/video/v4l2-event.c
@@ -25,7 +25,6 @@
 #include <media/v4l2-dev.h>
 #include <media/v4l2-fh.h>
 #include <media/v4l2-event.h>
-#include <media/v4l2-ctrls.h>
 
 #include <linux/sched.h>
 #include <linux/slab.h>
@@ -120,6 +119,14 @@ static void __v4l2_event_queue_fh(struct v4l2_fh *fh, const struct v4l2_event *e
 	if (sev == NULL)
 		return;
 
+	/*
+	 * If the event has been added to the fh->subscribed list, but its
+	 * add op has not completed yet elems will be 0, treat this as
+	 * not being subscribed.
+	 */
+	if (!sev->elems)
+		return;
+
 	/* Increase event sequence number on fh. */
 	fh->sequence++;
 
@@ -132,14 +139,14 @@ static void __v4l2_event_queue_fh(struct v4l2_fh *fh, const struct v4l2_event *e
 		sev->first = sev_pos(sev, 1);
 		fh->navailable--;
 		if (sev->elems == 1) {
-			if (sev->replace) {
-				sev->replace(&kev->event, ev);
+			if (sev->ops && sev->ops->replace) {
+				sev->ops->replace(&kev->event, ev);
 				copy_payload = false;
 			}
-		} else if (sev->merge) {
+		} else if (sev->ops && sev->ops->merge) {
 			struct v4l2_kevent *second_oldest =
 				sev->events + sev_pos(sev, 0);
-			sev->merge(&kev->event, &second_oldest->event);
+			sev->ops->merge(&kev->event, &second_oldest->event);
 		}
 	}
 
@@ -195,24 +202,11 @@ int v4l2_event_pending(struct v4l2_fh *fh)
 }
 EXPORT_SYMBOL_GPL(v4l2_event_pending);
 
-static void ctrls_replace(struct v4l2_event *old, const struct v4l2_event *new)
-{
-	u32 old_changes = old->u.ctrl.changes;
-
-	old->u.ctrl = new->u.ctrl;
-	old->u.ctrl.changes |= old_changes;
-}
-
-static void ctrls_merge(const struct v4l2_event *old, struct v4l2_event *new)
-{
-	new->u.ctrl.changes |= old->u.ctrl.changes;
-}
-
 int v4l2_event_subscribe(struct v4l2_fh *fh,
-			 struct v4l2_event_subscription *sub, unsigned elems)
+			 struct v4l2_event_subscription *sub, unsigned elems,
+			 const struct v4l2_subscribed_event_ops *ops)
 {
 	struct v4l2_subscribed_event *sev, *found_ev;
-	struct v4l2_ctrl *ctrl = NULL;
 	unsigned long flags;
 	unsigned i;
 
@@ -221,11 +215,6 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 
 	if (elems < 1)
 		elems = 1;
-	if (sub->type == V4L2_EVENT_CTRL) {
-		ctrl = v4l2_ctrl_find(fh->ctrl_handler, sub->id);
-		if (ctrl == NULL)
-			return -EINVAL;
-	}
 
 	sev = kzalloc(sizeof(*sev) + sizeof(struct v4l2_kevent) * elems, GFP_KERNEL);
 	if (!sev)
@@ -236,11 +225,7 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 	sev->id = sub->id;
 	sev->flags = sub->flags;
 	sev->fh = fh;
-	sev->elems = elems;
-	if (ctrl) {
-		sev->replace = ctrls_replace;
-		sev->merge = ctrls_merge;
-	}
+	sev->ops = ops;
 
 	spin_lock_irqsave(&fh->vdev->fh_lock, flags);
 	found_ev = v4l2_event_subscribed(fh, sub->type, sub->id);
@@ -248,11 +233,22 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 		list_add(&sev->list, &fh->subscribed);
 	spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
 
-	/* v4l2_ctrl_add_event uses a mutex, so do this outside the spin lock */
-	if (found_ev)
+	if (found_ev) {
 		kfree(sev);
-	else if (ctrl)
-		v4l2_ctrl_add_event(ctrl, sev);
+		return 0; /* Already listening */
+	}
+
+	if (sev->ops && sev->ops->add) {
+		int ret = sev->ops->add(sev, elems);
+		if (ret) {
+			sev->ops = NULL;
+			v4l2_event_unsubscribe(fh, sub);
+			return ret;
+		}
+	}
+
+	/* Mark as ready for use */
+	sev->elems = elems;
 
 	return 0;
 }
@@ -306,12 +302,9 @@ int v4l2_event_unsubscribe(struct v4l2_fh *fh,
 	}
 
 	spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
-	if (sev && sev->type == V4L2_EVENT_CTRL) {
-		struct v4l2_ctrl *ctrl = v4l2_ctrl_find(fh->ctrl_handler, sev->id);
 
-		if (ctrl)
-			v4l2_ctrl_del_event(ctrl, sev);
-	}
+	if (sev && sev->ops && sev->ops->del)
+		sev->ops->del(sev);
 
 	kfree(sev);
 
diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
index 5b2ec1fd2d0a..91be4e871f43 100644
--- a/drivers/media/video/v4l2-ioctl.c
+++ b/drivers/media/video/v4l2-ioctl.c
@@ -55,19 +55,6 @@
 	memset((u8 *)(p) + offsetof(typeof(*(p)), field) + sizeof((p)->field), \
 	0, sizeof(*(p)) - offsetof(typeof(*(p)), field) - sizeof((p)->field))
 
-#define have_fmt_ops(foo) (						\
-	ops->vidioc_##foo##_fmt_vid_cap ||				\
-	ops->vidioc_##foo##_fmt_vid_out ||				\
-	ops->vidioc_##foo##_fmt_vid_cap_mplane ||			\
-	ops->vidioc_##foo##_fmt_vid_out_mplane ||			\
-	ops->vidioc_##foo##_fmt_vid_overlay ||				\
-	ops->vidioc_##foo##_fmt_vbi_cap ||				\
-	ops->vidioc_##foo##_fmt_vid_out_overlay ||			\
-	ops->vidioc_##foo##_fmt_vbi_out ||				\
-	ops->vidioc_##foo##_fmt_sliced_vbi_cap ||			\
-	ops->vidioc_##foo##_fmt_sliced_vbi_out ||			\
-	ops->vidioc_##foo##_fmt_type_private)
-
 struct std_descr {
 	v4l2_std_id std;
 	const char *descr;
@@ -195,93 +182,118 @@ static const char *v4l2_memory_names[] = {
 
 /* ------------------------------------------------------------------ */
 /* debug help functions                                               */
-static const char *v4l2_ioctls[] = {
-	[_IOC_NR(VIDIOC_QUERYCAP)]         = "VIDIOC_QUERYCAP",
-	[_IOC_NR(VIDIOC_RESERVED)]         = "VIDIOC_RESERVED",
-	[_IOC_NR(VIDIOC_ENUM_FMT)]         = "VIDIOC_ENUM_FMT",
-	[_IOC_NR(VIDIOC_G_FMT)]            = "VIDIOC_G_FMT",
-	[_IOC_NR(VIDIOC_S_FMT)]            = "VIDIOC_S_FMT",
-	[_IOC_NR(VIDIOC_REQBUFS)]          = "VIDIOC_REQBUFS",
-	[_IOC_NR(VIDIOC_QUERYBUF)]         = "VIDIOC_QUERYBUF",
-	[_IOC_NR(VIDIOC_G_FBUF)]           = "VIDIOC_G_FBUF",
-	[_IOC_NR(VIDIOC_S_FBUF)]           = "VIDIOC_S_FBUF",
-	[_IOC_NR(VIDIOC_OVERLAY)]          = "VIDIOC_OVERLAY",
-	[_IOC_NR(VIDIOC_QBUF)]             = "VIDIOC_QBUF",
-	[_IOC_NR(VIDIOC_DQBUF)]            = "VIDIOC_DQBUF",
-	[_IOC_NR(VIDIOC_STREAMON)]         = "VIDIOC_STREAMON",
-	[_IOC_NR(VIDIOC_STREAMOFF)]        = "VIDIOC_STREAMOFF",
-	[_IOC_NR(VIDIOC_G_PARM)]           = "VIDIOC_G_PARM",
-	[_IOC_NR(VIDIOC_S_PARM)]           = "VIDIOC_S_PARM",
-	[_IOC_NR(VIDIOC_G_STD)]            = "VIDIOC_G_STD",
-	[_IOC_NR(VIDIOC_S_STD)]            = "VIDIOC_S_STD",
-	[_IOC_NR(VIDIOC_ENUMSTD)]          = "VIDIOC_ENUMSTD",
-	[_IOC_NR(VIDIOC_ENUMINPUT)]        = "VIDIOC_ENUMINPUT",
-	[_IOC_NR(VIDIOC_G_CTRL)]           = "VIDIOC_G_CTRL",
-	[_IOC_NR(VIDIOC_S_CTRL)]           = "VIDIOC_S_CTRL",
-	[_IOC_NR(VIDIOC_G_TUNER)]          = "VIDIOC_G_TUNER",
-	[_IOC_NR(VIDIOC_S_TUNER)]          = "VIDIOC_S_TUNER",
-	[_IOC_NR(VIDIOC_G_AUDIO)]          = "VIDIOC_G_AUDIO",
-	[_IOC_NR(VIDIOC_S_AUDIO)]          = "VIDIOC_S_AUDIO",
-	[_IOC_NR(VIDIOC_QUERYCTRL)]        = "VIDIOC_QUERYCTRL",
-	[_IOC_NR(VIDIOC_QUERYMENU)]        = "VIDIOC_QUERYMENU",
-	[_IOC_NR(VIDIOC_G_INPUT)]          = "VIDIOC_G_INPUT",
-	[_IOC_NR(VIDIOC_S_INPUT)]          = "VIDIOC_S_INPUT",
-	[_IOC_NR(VIDIOC_G_OUTPUT)]         = "VIDIOC_G_OUTPUT",
-	[_IOC_NR(VIDIOC_S_OUTPUT)]         = "VIDIOC_S_OUTPUT",
-	[_IOC_NR(VIDIOC_ENUMOUTPUT)]       = "VIDIOC_ENUMOUTPUT",
-	[_IOC_NR(VIDIOC_G_AUDOUT)]         = "VIDIOC_G_AUDOUT",
-	[_IOC_NR(VIDIOC_S_AUDOUT)]         = "VIDIOC_S_AUDOUT",
-	[_IOC_NR(VIDIOC_G_MODULATOR)]      = "VIDIOC_G_MODULATOR",
-	[_IOC_NR(VIDIOC_S_MODULATOR)]      = "VIDIOC_S_MODULATOR",
-	[_IOC_NR(VIDIOC_G_FREQUENCY)]      = "VIDIOC_G_FREQUENCY",
-	[_IOC_NR(VIDIOC_S_FREQUENCY)]      = "VIDIOC_S_FREQUENCY",
-	[_IOC_NR(VIDIOC_CROPCAP)]          = "VIDIOC_CROPCAP",
-	[_IOC_NR(VIDIOC_G_CROP)]           = "VIDIOC_G_CROP",
-	[_IOC_NR(VIDIOC_S_CROP)]           = "VIDIOC_S_CROP",
-	[_IOC_NR(VIDIOC_G_SELECTION)]      = "VIDIOC_G_SELECTION",
-	[_IOC_NR(VIDIOC_S_SELECTION)]      = "VIDIOC_S_SELECTION",
-	[_IOC_NR(VIDIOC_G_JPEGCOMP)]       = "VIDIOC_G_JPEGCOMP",
-	[_IOC_NR(VIDIOC_S_JPEGCOMP)]       = "VIDIOC_S_JPEGCOMP",
-	[_IOC_NR(VIDIOC_QUERYSTD)]         = "VIDIOC_QUERYSTD",
-	[_IOC_NR(VIDIOC_TRY_FMT)]          = "VIDIOC_TRY_FMT",
-	[_IOC_NR(VIDIOC_ENUMAUDIO)]        = "VIDIOC_ENUMAUDIO",
-	[_IOC_NR(VIDIOC_ENUMAUDOUT)]       = "VIDIOC_ENUMAUDOUT",
-	[_IOC_NR(VIDIOC_G_PRIORITY)]       = "VIDIOC_G_PRIORITY",
-	[_IOC_NR(VIDIOC_S_PRIORITY)]       = "VIDIOC_S_PRIORITY",
-	[_IOC_NR(VIDIOC_G_SLICED_VBI_CAP)] = "VIDIOC_G_SLICED_VBI_CAP",
-	[_IOC_NR(VIDIOC_LOG_STATUS)]       = "VIDIOC_LOG_STATUS",
-	[_IOC_NR(VIDIOC_G_EXT_CTRLS)]      = "VIDIOC_G_EXT_CTRLS",
-	[_IOC_NR(VIDIOC_S_EXT_CTRLS)]      = "VIDIOC_S_EXT_CTRLS",
-	[_IOC_NR(VIDIOC_TRY_EXT_CTRLS)]    = "VIDIOC_TRY_EXT_CTRLS",
-#if 1
-	[_IOC_NR(VIDIOC_ENUM_FRAMESIZES)]  = "VIDIOC_ENUM_FRAMESIZES",
-	[_IOC_NR(VIDIOC_ENUM_FRAMEINTERVALS)] = "VIDIOC_ENUM_FRAMEINTERVALS",
-	[_IOC_NR(VIDIOC_G_ENC_INDEX)] 	   = "VIDIOC_G_ENC_INDEX",
-	[_IOC_NR(VIDIOC_ENCODER_CMD)] 	   = "VIDIOC_ENCODER_CMD",
-	[_IOC_NR(VIDIOC_TRY_ENCODER_CMD)]  = "VIDIOC_TRY_ENCODER_CMD",
-
-	[_IOC_NR(VIDIOC_DECODER_CMD)]	   = "VIDIOC_DECODER_CMD",
-	[_IOC_NR(VIDIOC_TRY_DECODER_CMD)]  = "VIDIOC_TRY_DECODER_CMD",
-	[_IOC_NR(VIDIOC_DBG_S_REGISTER)]   = "VIDIOC_DBG_S_REGISTER",
-	[_IOC_NR(VIDIOC_DBG_G_REGISTER)]   = "VIDIOC_DBG_G_REGISTER",
-
-	[_IOC_NR(VIDIOC_DBG_G_CHIP_IDENT)] = "VIDIOC_DBG_G_CHIP_IDENT",
-	[_IOC_NR(VIDIOC_S_HW_FREQ_SEEK)]   = "VIDIOC_S_HW_FREQ_SEEK",
+
+struct v4l2_ioctl_info {
+	unsigned int ioctl;
+	u16 flags;
+	const char * const name;
+};
+
+/* This control needs a priority check */
+#define INFO_FL_PRIO	(1 << 0)
+/* This control can be valid if the filehandle passes a control handler. */
+#define INFO_FL_CTRL	(1 << 1)
+
+#define IOCTL_INFO(_ioctl, _flags) [_IOC_NR(_ioctl)] = {	\
+	.ioctl = _ioctl,					\
+	.flags = _flags,					\
+	.name = #_ioctl,					\
+}
+
+static struct v4l2_ioctl_info v4l2_ioctls[] = {
+	IOCTL_INFO(VIDIOC_QUERYCAP, 0),
+	IOCTL_INFO(VIDIOC_ENUM_FMT, 0),
+	IOCTL_INFO(VIDIOC_G_FMT, 0),
+	IOCTL_INFO(VIDIOC_S_FMT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_REQBUFS, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QUERYBUF, 0),
+	IOCTL_INFO(VIDIOC_G_FBUF, 0),
+	IOCTL_INFO(VIDIOC_S_FBUF, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_OVERLAY, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QBUF, 0),
+	IOCTL_INFO(VIDIOC_DQBUF, 0),
+	IOCTL_INFO(VIDIOC_STREAMON, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_STREAMOFF, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_PARM, 0),
+	IOCTL_INFO(VIDIOC_S_PARM, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_STD, 0),
+	IOCTL_INFO(VIDIOC_S_STD, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_ENUMSTD, 0),
+	IOCTL_INFO(VIDIOC_ENUMINPUT, 0),
+	IOCTL_INFO(VIDIOC_G_CTRL, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_S_CTRL, INFO_FL_PRIO | INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_G_TUNER, 0),
+	IOCTL_INFO(VIDIOC_S_TUNER, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_AUDIO, 0),
+	IOCTL_INFO(VIDIOC_S_AUDIO, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QUERYCTRL, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_QUERYMENU, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_G_INPUT, 0),
+	IOCTL_INFO(VIDIOC_S_INPUT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_OUTPUT, 0),
+	IOCTL_INFO(VIDIOC_S_OUTPUT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_ENUMOUTPUT, 0),
+	IOCTL_INFO(VIDIOC_G_AUDOUT, 0),
+	IOCTL_INFO(VIDIOC_S_AUDOUT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_MODULATOR, 0),
+	IOCTL_INFO(VIDIOC_S_MODULATOR, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_FREQUENCY, 0),
+	IOCTL_INFO(VIDIOC_S_FREQUENCY, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_CROPCAP, 0),
+	IOCTL_INFO(VIDIOC_G_CROP, 0),
+	IOCTL_INFO(VIDIOC_S_CROP, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_SELECTION, 0),
+	IOCTL_INFO(VIDIOC_S_SELECTION, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_JPEGCOMP, 0),
+	IOCTL_INFO(VIDIOC_S_JPEGCOMP, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QUERYSTD, 0),
+	IOCTL_INFO(VIDIOC_TRY_FMT, 0),
+	IOCTL_INFO(VIDIOC_ENUMAUDIO, 0),
+	IOCTL_INFO(VIDIOC_ENUMAUDOUT, 0),
+	IOCTL_INFO(VIDIOC_G_PRIORITY, 0),
+	IOCTL_INFO(VIDIOC_S_PRIORITY, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_SLICED_VBI_CAP, 0),
+	IOCTL_INFO(VIDIOC_LOG_STATUS, 0),
+	IOCTL_INFO(VIDIOC_G_EXT_CTRLS, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_S_EXT_CTRLS, INFO_FL_PRIO | INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_TRY_EXT_CTRLS, 0),
+	IOCTL_INFO(VIDIOC_ENUM_FRAMESIZES, 0),
+	IOCTL_INFO(VIDIOC_ENUM_FRAMEINTERVALS, 0),
+	IOCTL_INFO(VIDIOC_G_ENC_INDEX, 0),
+	IOCTL_INFO(VIDIOC_ENCODER_CMD, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_TRY_ENCODER_CMD, 0),
+	IOCTL_INFO(VIDIOC_DECODER_CMD, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_TRY_DECODER_CMD, 0),
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	IOCTL_INFO(VIDIOC_DBG_S_REGISTER, 0),
+	IOCTL_INFO(VIDIOC_DBG_G_REGISTER, 0),
 #endif
-	[_IOC_NR(VIDIOC_ENUM_DV_PRESETS)]  = "VIDIOC_ENUM_DV_PRESETS",
-	[_IOC_NR(VIDIOC_S_DV_PRESET)]	   = "VIDIOC_S_DV_PRESET",
-	[_IOC_NR(VIDIOC_G_DV_PRESET)]	   = "VIDIOC_G_DV_PRESET",
-	[_IOC_NR(VIDIOC_QUERY_DV_PRESET)]  = "VIDIOC_QUERY_DV_PRESET",
-	[_IOC_NR(VIDIOC_S_DV_TIMINGS)]     = "VIDIOC_S_DV_TIMINGS",
-	[_IOC_NR(VIDIOC_G_DV_TIMINGS)]     = "VIDIOC_G_DV_TIMINGS",
-	[_IOC_NR(VIDIOC_DQEVENT)]	   = "VIDIOC_DQEVENT",
-	[_IOC_NR(VIDIOC_SUBSCRIBE_EVENT)]  = "VIDIOC_SUBSCRIBE_EVENT",
-	[_IOC_NR(VIDIOC_UNSUBSCRIBE_EVENT)] = "VIDIOC_UNSUBSCRIBE_EVENT",
-	[_IOC_NR(VIDIOC_CREATE_BUFS)]      = "VIDIOC_CREATE_BUFS",
-	[_IOC_NR(VIDIOC_PREPARE_BUF)]      = "VIDIOC_PREPARE_BUF",
+	IOCTL_INFO(VIDIOC_DBG_G_CHIP_IDENT, 0),
+	IOCTL_INFO(VIDIOC_S_HW_FREQ_SEEK, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_ENUM_DV_PRESETS, 0),
+	IOCTL_INFO(VIDIOC_S_DV_PRESET, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_DV_PRESET, 0),
+	IOCTL_INFO(VIDIOC_QUERY_DV_PRESET, 0),
+	IOCTL_INFO(VIDIOC_S_DV_TIMINGS, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_DV_TIMINGS, 0),
+	IOCTL_INFO(VIDIOC_DQEVENT, 0),
+	IOCTL_INFO(VIDIOC_SUBSCRIBE_EVENT, 0),
+	IOCTL_INFO(VIDIOC_UNSUBSCRIBE_EVENT, 0),
+	IOCTL_INFO(VIDIOC_CREATE_BUFS, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_PREPARE_BUF, 0),
+	IOCTL_INFO(VIDIOC_ENUM_DV_TIMINGS, 0),
+	IOCTL_INFO(VIDIOC_QUERY_DV_TIMINGS, 0),
+	IOCTL_INFO(VIDIOC_DV_TIMINGS_CAP, 0),
 };
 #define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
 
+bool v4l2_is_known_ioctl(unsigned int cmd)
+{
+	if (_IOC_NR(cmd) >= V4L2_IOCTLS)
+		return false;
+	return v4l2_ioctls[_IOC_NR(cmd)].ioctl == cmd;
+}
+
 /* Common ioctl debug function. This function can be used by
    external ioctl messages as well as internal V4L ioctl */
 void v4l_printk_ioctl(unsigned int cmd)
@@ -297,7 +309,7 @@ void v4l_printk_ioctl(unsigned int cmd)
 			type = "v4l2";
 			break;
 		}
-		printk("%s", v4l2_ioctls[_IOC_NR(cmd)]);
+		printk("%s", v4l2_ioctls[_IOC_NR(cmd)].name);
 		return;
 	default:
 		type = "unknown";
@@ -359,6 +371,34 @@ static inline void dbgrect(struct video_device *vfd, char *s,
 						r->width, r->height);
 };
 
+static void dbgtimings(struct video_device *vfd,
+			const struct v4l2_dv_timings *p)
+{
+	switch (p->type) {
+	case V4L2_DV_BT_656_1120:
+		dbgarg2("bt-656/1120:interlaced=%d,"
+				" pixelclock=%lld,"
+				" width=%d, height=%d, polarities=%x,"
+				" hfrontporch=%d, hsync=%d,"
+				" hbackporch=%d, vfrontporch=%d,"
+				" vsync=%d, vbackporch=%d,"
+				" il_vfrontporch=%d, il_vsync=%d,"
+				" il_vbackporch=%d, standards=%x, flags=%x\n",
+				p->bt.interlaced, p->bt.pixelclock,
+				p->bt.width, p->bt.height,
+				p->bt.polarities, p->bt.hfrontporch,
+				p->bt.hsync, p->bt.hbackporch,
+				p->bt.vfrontporch, p->bt.vsync,
+				p->bt.vbackporch, p->bt.il_vfrontporch,
+				p->bt.il_vsync, p->bt.il_vbackporch,
+				p->bt.standards, p->bt.flags);
+		break;
+	default:
+		dbgarg2("Unknown type %d!\n", p->type);
+		break;
+	}
+}
+
 static inline void v4l_print_pix_fmt(struct video_device *vfd,
 						struct v4l2_pix_format *fmt)
 {
@@ -504,7 +544,6 @@ static long __video_do_ioctl(struct file *file,
 	void *fh = file->private_data;
 	struct v4l2_fh *vfh = NULL;
 	int use_fh_prio = 0;
-	long ret_prio = 0;
 	long ret = -ENOTTY;
 
 	if (ops == NULL) {
@@ -513,19 +552,30 @@ static long __video_do_ioctl(struct file *file,
 		return ret;
 	}
 
-	if ((vfd->debug & V4L2_DEBUG_IOCTL) &&
-				!(vfd->debug & V4L2_DEBUG_IOCTL_ARG)) {
-		v4l_print_ioctl(vfd->name, cmd);
-		printk(KERN_CONT "\n");
-	}
-
 	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
 		vfh = file->private_data;
 		use_fh_prio = test_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	}
 
-	if (use_fh_prio)
-		ret_prio = v4l2_prio_check(vfd->prio, vfh->prio);
+	if (v4l2_is_known_ioctl(cmd)) {
+		struct v4l2_ioctl_info *info = &v4l2_ioctls[_IOC_NR(cmd)];
+
+	        if (!test_bit(_IOC_NR(cmd), vfd->valid_ioctls) &&
+		    !((info->flags & INFO_FL_CTRL) && vfh && vfh->ctrl_handler))
+			return -ENOTTY;
+
+		if (use_fh_prio && (info->flags & INFO_FL_PRIO)) {
+			ret = v4l2_prio_check(vfd->prio, vfh->prio);
+			if (ret)
+				return ret;
+		}
+	}
+
+	if ((vfd->debug & V4L2_DEBUG_IOCTL) &&
+				!(vfd->debug & V4L2_DEBUG_IOCTL_ARG)) {
+		v4l_print_ioctl(vfd->name, cmd);
+		printk(KERN_CONT "\n");
+	}
 
 	switch (cmd) {
 
@@ -534,9 +584,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_capability *cap = (struct v4l2_capability *)arg;
 
-		if (!ops->vidioc_querycap)
-			break;
-
 		cap->version = LINUX_VERSION_CODE;
 		ret = ops->vidioc_querycap(file, fh, cap);
 		if (!ret)
@@ -570,14 +617,11 @@ static long __video_do_ioctl(struct file *file,
 	{
 		enum v4l2_priority *p = arg;
 
-		if (!ops->vidioc_s_priority && !use_fh_prio)
-			break;
 		dbgarg(cmd, "setting priority to %d\n", *p);
 		if (ops->vidioc_s_priority)
 			ret = ops->vidioc_s_priority(file, fh, *p);
 		else
-			ret = ret_prio ? ret_prio :
-				v4l2_prio_change(&vfd->v4l2_dev->prio,
+			ret = v4l2_prio_change(&vfd->v4l2_dev->prio,
 							&vfh->prio, *p);
 		break;
 	}
@@ -587,6 +631,7 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_fmtdesc *f = arg;
 
+		ret = -EINVAL;
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			if (likely(ops->vidioc_enum_fmt_vid_cap))
@@ -619,7 +664,7 @@ static long __video_do_ioctl(struct file *file,
 		default:
 			break;
 		}
-		if (likely (!ret))
+		if (likely(!ret))
 			dbgarg(cmd, "index=%d, type=%d, flags=%d, "
 				"pixelformat=%c%c%c%c, description='%s'\n",
 				f->index, f->type, f->flags,
@@ -628,14 +673,6 @@ static long __video_do_ioctl(struct file *file,
 				(f->pixelformat >> 16) & 0xff,
 				(f->pixelformat >> 24) & 0xff,
 				f->description);
-		else if (ret == -ENOTTY &&
-			 (ops->vidioc_enum_fmt_vid_cap ||
-			  ops->vidioc_enum_fmt_vid_out ||
-			  ops->vidioc_enum_fmt_vid_cap_mplane ||
-			  ops->vidioc_enum_fmt_vid_out_mplane ||
-			  ops->vidioc_enum_fmt_vid_overlay ||
-			  ops->vidioc_enum_fmt_type_private))
-			ret = -EINVAL;
 		break;
 	}
 	case VIDIOC_G_FMT:
@@ -645,6 +682,7 @@ static long __video_do_ioctl(struct file *file,
 		/* FIXME: Should be one dump per type */
 		dbgarg(cmd, "type=%s\n", prt_names(f->type, v4l2_type_names));
 
+		ret = -EINVAL;
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			if (ops->vidioc_g_fmt_vid_cap)
@@ -706,21 +744,12 @@ static long __video_do_ioctl(struct file *file,
 								fh, f);
 			break;
 		}
-		if (unlikely(ret == -ENOTTY && have_fmt_ops(g)))
-			ret = -EINVAL;
-
 		break;
 	}
 	case VIDIOC_S_FMT:
 	{
 		struct v4l2_format *f = (struct v4l2_format *)arg;
 
-		if (!have_fmt_ops(s))
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = -EINVAL;
 
 		/* FIXME: Should be one dump per type */
@@ -804,6 +833,7 @@ static long __video_do_ioctl(struct file *file,
 		/* FIXME: Should be one dump per type */
 		dbgarg(cmd, "type=%s\n", prt_names(f->type,
 						v4l2_type_names));
+		ret = -EINVAL;
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			CLEAR_AFTER_FIELD(f, fmt.pix);
@@ -876,8 +906,6 @@ static long __video_do_ioctl(struct file *file,
 								fh, f);
 			break;
 		}
-		if (unlikely(ret == -ENOTTY && have_fmt_ops(try)))
-			ret = -EINVAL;
 		break;
 	}
 	/* FIXME: Those buf reqs could be handled here,
@@ -888,12 +916,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_requestbuffers *p = arg;
 
-		if (!ops->vidioc_reqbufs)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -912,8 +934,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *p = arg;
 
-		if (!ops->vidioc_querybuf)
-			break;
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -927,8 +947,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *p = arg;
 
-		if (!ops->vidioc_qbuf)
-			break;
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -942,8 +960,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *p = arg;
 
-		if (!ops->vidioc_dqbuf)
-			break;
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -957,12 +973,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		int *i = arg;
 
-		if (!ops->vidioc_overlay)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "value=%d\n", *i);
 		ret = ops->vidioc_overlay(file, fh, *i);
 		break;
@@ -971,8 +981,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_framebuffer *p = arg;
 
-		if (!ops->vidioc_g_fbuf)
-			break;
 		ret = ops->vidioc_g_fbuf(file, fh, arg);
 		if (!ret) {
 			dbgarg(cmd, "capability=0x%x, flags=%d, base=0x%08lx\n",
@@ -986,12 +994,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_framebuffer *p = arg;
 
-		if (!ops->vidioc_s_fbuf)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "capability=0x%x, flags=%d, base=0x%08lx\n",
 			p->capability, p->flags, (unsigned long)p->base);
 		v4l_print_pix_fmt(vfd, &p->fmt);
@@ -1002,12 +1004,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		enum v4l2_buf_type i = *(int *)arg;
 
-		if (!ops->vidioc_streamon)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "type=%s\n", prt_names(i, v4l2_type_names));
 		ret = ops->vidioc_streamon(file, fh, i);
 		break;
@@ -1016,12 +1012,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		enum v4l2_buf_type i = *(int *)arg;
 
-		if (!ops->vidioc_streamoff)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "type=%s\n", prt_names(i, v4l2_type_names));
 		ret = ops->vidioc_streamoff(file, fh, i);
 		break;
@@ -1091,13 +1081,6 @@ static long __video_do_ioctl(struct file *file,
 
 		dbgarg(cmd, "std=%08Lx\n", (long long unsigned)*id);
 
-		if (!ops->vidioc_s_std)
-			break;
-
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = -EINVAL;
 		norm = (*id) & vfd->tvnorms;
 		if (vfd->tvnorms && !norm)	/* Check if std is supported */
@@ -1115,8 +1098,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		v4l2_std_id *p = arg;
 
-		if (!ops->vidioc_querystd)
-			break;
 		/*
 		 * If nothing detected, it should return all supported
 		 * Drivers just need to mask the std argument, in order
@@ -1150,9 +1131,6 @@ static long __video_do_ioctl(struct file *file,
 		if (ops->vidioc_s_dv_timings)
 			p->capabilities |= V4L2_IN_CAP_CUSTOM_TIMINGS;
 
-		if (!ops->vidioc_enum_input)
-			break;
-
 		ret = ops->vidioc_enum_input(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, type=%d, "
@@ -1168,8 +1146,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_g_input)
-			break;
 		ret = ops->vidioc_g_input(file, fh, i);
 		if (!ret)
 			dbgarg(cmd, "value=%d\n", *i);
@@ -1179,12 +1155,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_s_input)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "value=%d\n", *i);
 		ret = ops->vidioc_s_input(file, fh, *i);
 		break;
@@ -1195,9 +1165,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_output *p = arg;
 
-		if (!ops->vidioc_enum_output)
-			break;
-
 		/*
 		 * We set the flags for CAP_PRESETS, CAP_CUSTOM_TIMINGS &
 		 * CAP_STD here based on ioctl handler provided by the
@@ -1224,8 +1191,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_g_output)
-			break;
 		ret = ops->vidioc_g_output(file, fh, i);
 		if (!ret)
 			dbgarg(cmd, "value=%d\n", *i);
@@ -1235,12 +1200,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_s_output)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "value=%d\n", *i);
 		ret = ops->vidioc_s_output(file, fh, *i);
 		break;
@@ -1310,10 +1269,6 @@ static long __video_do_ioctl(struct file *file,
 		if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
 			!ops->vidioc_s_ctrl && !ops->vidioc_s_ext_ctrls)
 			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 
 		dbgarg(cmd, "id=0x%x, value=%d\n", p->id, p->value);
 
@@ -1369,10 +1324,6 @@ static long __video_do_ioctl(struct file *file,
 		if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
 				!ops->vidioc_s_ext_ctrls)
 			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		v4l_print_ext_ctrls(cmd, vfd, p, 1);
 		if (vfh && vfh->ctrl_handler)
 			ret = v4l2_s_ext_ctrls(vfh, vfh->ctrl_handler, p);
@@ -1428,8 +1379,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audio *p = arg;
 
-		if (!ops->vidioc_enumaudio)
-			break;
 		ret = ops->vidioc_enumaudio(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
@@ -1443,9 +1392,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audio *p = arg;
 
-		if (!ops->vidioc_g_audio)
-			break;
-
 		ret = ops->vidioc_g_audio(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
@@ -1459,12 +1405,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audio *p = arg;
 
-		if (!ops->vidioc_s_audio)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
 					"mode=0x%x\n", p->index, p->name,
 					p->capability, p->mode);
@@ -1475,8 +1415,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audioout *p = arg;
 
-		if (!ops->vidioc_enumaudout)
-			break;
 		dbgarg(cmd, "Enum for index=%d\n", p->index);
 		ret = ops->vidioc_enumaudout(file, fh, p);
 		if (!ret)
@@ -1489,9 +1427,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audioout *p = arg;
 
-		if (!ops->vidioc_g_audout)
-			break;
-
 		ret = ops->vidioc_g_audout(file, fh, p);
 		if (!ret)
 			dbgarg2("index=%d, name=%s, capability=%d, "
@@ -1503,12 +1438,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audioout *p = arg;
 
-		if (!ops->vidioc_s_audout)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "index=%d, name=%s, capability=%d, "
 					"mode=%d\n", p->index, p->name,
 					p->capability, p->mode);
@@ -1520,8 +1449,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_modulator *p = arg;
 
-		if (!ops->vidioc_g_modulator)
-			break;
 		ret = ops->vidioc_g_modulator(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, "
@@ -1536,12 +1463,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_modulator *p = arg;
 
-		if (!ops->vidioc_s_modulator)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "index=%d, name=%s, capability=%d, "
 				"rangelow=%d, rangehigh=%d, txsubchans=%d\n",
 				p->index, p->name, p->capability, p->rangelow,
@@ -1553,9 +1474,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_crop *p = arg;
 
-		if (!ops->vidioc_g_crop && !ops->vidioc_g_selection)
-			break;
-
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 
 		if (ops->vidioc_g_crop) {
@@ -1587,13 +1505,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_crop *p = arg;
 
-		if (!ops->vidioc_s_crop && !ops->vidioc_s_selection)
-			break;
-
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 		dbgrect(vfd, "", &p->c);
 
@@ -1620,9 +1531,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_selection *p = arg;
 
-		if (!ops->vidioc_g_selection)
-			break;
-
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 
 		ret = ops->vidioc_g_selection(file, fh, p);
@@ -1634,13 +1542,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_selection *p = arg;
 
-		if (!ops->vidioc_s_selection)
-			break;
-
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 		dbgrect(vfd, "", &p->r);
@@ -1653,9 +1554,6 @@ static long __video_do_ioctl(struct file *file,
 		struct v4l2_cropcap *p = arg;
 
 		/*FIXME: Should also show v4l2_fract pixelaspect */
-		if (!ops->vidioc_cropcap && !ops->vidioc_g_selection)
-			break;
-
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 		if (ops->vidioc_cropcap) {
 			ret = ops->vidioc_cropcap(file, fh, p);
@@ -1699,9 +1597,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_jpegcompression *p = arg;
 
-		if (!ops->vidioc_g_jpegcomp)
-			break;
-
 		ret = ops->vidioc_g_jpegcomp(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "quality=%d, APPn=%d, "
@@ -1715,12 +1610,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_jpegcompression *p = arg;
 
-		if (!ops->vidioc_g_jpegcomp)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "quality=%d, APPn=%d, APP_len=%d, "
 					"COM_len=%d, jpeg_markers=%d\n",
 					p->quality, p->APPn, p->APP_len,
@@ -1732,8 +1621,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_enc_idx *p = arg;
 
-		if (!ops->vidioc_g_enc_index)
-			break;
 		ret = ops->vidioc_g_enc_index(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "entries=%d, entries_cap=%d\n",
@@ -1744,12 +1631,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_encoder_cmd *p = arg;
 
-		if (!ops->vidioc_encoder_cmd)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = ops->vidioc_encoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1759,8 +1640,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_encoder_cmd *p = arg;
 
-		if (!ops->vidioc_try_encoder_cmd)
-			break;
 		ret = ops->vidioc_try_encoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1770,12 +1649,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_decoder_cmd *p = arg;
 
-		if (!ops->vidioc_decoder_cmd)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = ops->vidioc_decoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1785,8 +1658,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_decoder_cmd *p = arg;
 
-		if (!ops->vidioc_try_decoder_cmd)
-			break;
 		ret = ops->vidioc_try_decoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1796,8 +1667,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_streamparm *p = arg;
 
-		if (!ops->vidioc_g_parm && !vfd->current_norm)
-			break;
 		if (ops->vidioc_g_parm) {
 			ret = check_fmt(ops, p->type);
 			if (ret)
@@ -1825,12 +1694,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_streamparm *p = arg;
 
-		if (!ops->vidioc_s_parm)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -1843,9 +1706,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_tuner *p = arg;
 
-		if (!ops->vidioc_g_tuner)
-			break;
-
 		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		ret = ops->vidioc_g_tuner(file, fh, p);
@@ -1864,12 +1724,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_tuner *p = arg;
 
-		if (!ops->vidioc_s_tuner)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		dbgarg(cmd, "index=%d, name=%s, type=%d, "
@@ -1887,9 +1741,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_frequency *p = arg;
 
-		if (!ops->vidioc_g_frequency)
-			break;
-
 		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		ret = ops->vidioc_g_frequency(file, fh, p);
@@ -1903,12 +1754,6 @@ static long __video_do_ioctl(struct file *file,
 		struct v4l2_frequency *p = arg;
 		enum v4l2_tuner_type type;
 
-		if (!ops->vidioc_s_frequency)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		dbgarg(cmd, "tuner=%d, type=%d, frequency=%d\n",
@@ -1923,9 +1768,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_sliced_vbi_cap *p = arg;
 
-		if (!ops->vidioc_g_sliced_vbi_cap)
-			break;
-
 		/* Clear up to type, everything after type is zerod already */
 		memset(p, 0, offsetof(struct v4l2_sliced_vbi_cap, type));
 
@@ -1937,8 +1779,6 @@ static long __video_do_ioctl(struct file *file,
 	}
 	case VIDIOC_LOG_STATUS:
 	{
-		if (!ops->vidioc_log_status)
-			break;
 		if (vfd->v4l2_dev)
 			pr_info("%s: =================  START STATUS  =================\n",
 				vfd->v4l2_dev->name);
@@ -1948,38 +1788,34 @@ static long __video_do_ioctl(struct file *file,
 				vfd->v4l2_dev->name);
 		break;
 	}
-#ifdef CONFIG_VIDEO_ADV_DEBUG
 	case VIDIOC_DBG_G_REGISTER:
 	{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
 		struct v4l2_dbg_register *p = arg;
 
-		if (ops->vidioc_g_register) {
-			if (!capable(CAP_SYS_ADMIN))
-				ret = -EPERM;
-			else
-				ret = ops->vidioc_g_register(file, fh, p);
-		}
+		if (!capable(CAP_SYS_ADMIN))
+			ret = -EPERM;
+		else
+			ret = ops->vidioc_g_register(file, fh, p);
+#endif
 		break;
 	}
 	case VIDIOC_DBG_S_REGISTER:
 	{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
 		struct v4l2_dbg_register *p = arg;
 
-		if (ops->vidioc_s_register) {
-			if (!capable(CAP_SYS_ADMIN))
-				ret = -EPERM;
-			else
-				ret = ops->vidioc_s_register(file, fh, p);
-		}
+		if (!capable(CAP_SYS_ADMIN))
+			ret = -EPERM;
+		else
+			ret = ops->vidioc_s_register(file, fh, p);
+#endif
 		break;
 	}
-#endif
 	case VIDIOC_DBG_G_CHIP_IDENT:
 	{
 		struct v4l2_dbg_chip_ident *p = arg;
 
-		if (!ops->vidioc_g_chip_ident)
-			break;
 		p->ident = V4L2_IDENT_NONE;
 		p->revision = 0;
 		ret = ops->vidioc_g_chip_ident(file, fh, p);
@@ -1992,12 +1828,6 @@ static long __video_do_ioctl(struct file *file,
 		struct v4l2_hw_freq_seek *p = arg;
 		enum v4l2_tuner_type type;
 
-		if (!ops->vidioc_s_hw_freq_seek)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		dbgarg(cmd,
@@ -2013,9 +1843,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_frmsizeenum *p = arg;
 
-		if (!ops->vidioc_enum_framesizes)
-			break;
-
 		ret = ops->vidioc_enum_framesizes(file, fh, p);
 		dbgarg(cmd,
 			"index=%d, pixelformat=%c%c%c%c, type=%d ",
@@ -2049,9 +1876,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_frmivalenum *p = arg;
 
-		if (!ops->vidioc_enum_frameintervals)
-			break;
-
 		ret = ops->vidioc_enum_frameintervals(file, fh, p);
 		dbgarg(cmd,
 			"index=%d, pixelformat=%d, width=%d, height=%d, type=%d ",
@@ -2084,9 +1908,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_enum_preset *p = arg;
 
-		if (!ops->vidioc_enum_dv_presets)
-			break;
-
 		ret = ops->vidioc_enum_dv_presets(file, fh, p);
 		if (!ret)
 			dbgarg(cmd,
@@ -2100,13 +1921,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_preset *p = arg;
 
-		if (!ops->vidioc_s_dv_preset)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
-
 		dbgarg(cmd, "preset=%d\n", p->preset);
 		ret = ops->vidioc_s_dv_preset(file, fh, p);
 		break;
@@ -2115,9 +1929,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_preset *p = arg;
 
-		if (!ops->vidioc_g_dv_preset)
-			break;
-
 		ret = ops->vidioc_g_dv_preset(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "preset=%d\n", p->preset);
@@ -2127,9 +1938,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_preset *p = arg;
 
-		if (!ops->vidioc_query_dv_preset)
-			break;
-
 		ret = ops->vidioc_query_dv_preset(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "preset=%d\n", p->preset);
@@ -2139,32 +1947,13 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_timings *p = arg;
 
-		if (!ops->vidioc_s_dv_timings)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
-
+		dbgtimings(vfd, p);
 		switch (p->type) {
 		case V4L2_DV_BT_656_1120:
-			dbgarg2("bt-656/1120:interlaced=%d, pixelclock=%lld,"
-				" width=%d, height=%d, polarities=%x,"
-				" hfrontporch=%d, hsync=%d, hbackporch=%d,"
-				" vfrontporch=%d, vsync=%d, vbackporch=%d,"
-				" il_vfrontporch=%d, il_vsync=%d,"
-				" il_vbackporch=%d\n",
-				p->bt.interlaced, p->bt.pixelclock,
-				p->bt.width, p->bt.height, p->bt.polarities,
-				p->bt.hfrontporch, p->bt.hsync,
-				p->bt.hbackporch, p->bt.vfrontporch,
-				p->bt.vsync, p->bt.vbackporch,
-				p->bt.il_vfrontporch, p->bt.il_vsync,
-				p->bt.il_vbackporch);
 			ret = ops->vidioc_s_dv_timings(file, fh, p);
 			break;
 		default:
-			dbgarg2("Unknown type %d!\n", p->type);
+			ret = -EINVAL;
 			break;
 		}
 		break;
@@ -2173,43 +1962,68 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_timings *p = arg;
 
-		if (!ops->vidioc_g_dv_timings)
+		ret = ops->vidioc_g_dv_timings(file, fh, p);
+		if (!ret)
+			dbgtimings(vfd, p);
+		break;
+	}
+	case VIDIOC_ENUM_DV_TIMINGS:
+	{
+		struct v4l2_enum_dv_timings *p = arg;
+
+		if (!ops->vidioc_enum_dv_timings)
 			break;
 
-		ret = ops->vidioc_g_dv_timings(file, fh, p);
+		ret = ops->vidioc_enum_dv_timings(file, fh, p);
 		if (!ret) {
-			switch (p->type) {
-			case V4L2_DV_BT_656_1120:
-				dbgarg2("bt-656/1120:interlaced=%d,"
-					" pixelclock=%lld,"
-					" width=%d, height=%d, polarities=%x,"
-					" hfrontporch=%d, hsync=%d,"
-					" hbackporch=%d, vfrontporch=%d,"
-					" vsync=%d, vbackporch=%d,"
-					" il_vfrontporch=%d, il_vsync=%d,"
-					" il_vbackporch=%d\n",
-					p->bt.interlaced, p->bt.pixelclock,
-					p->bt.width, p->bt.height,
-					p->bt.polarities, p->bt.hfrontporch,
-					p->bt.hsync, p->bt.hbackporch,
-					p->bt.vfrontporch, p->bt.vsync,
-					p->bt.vbackporch, p->bt.il_vfrontporch,
-					p->bt.il_vsync, p->bt.il_vbackporch);
-				break;
-			default:
-				dbgarg2("Unknown type %d!\n", p->type);
-				break;
-			}
+			dbgarg(cmd, "index=%d: ", p->index);
+			dbgtimings(vfd, &p->timings);
 		}
 		break;
 	}
-	case VIDIOC_DQEVENT:
+	case VIDIOC_QUERY_DV_TIMINGS:
 	{
-		struct v4l2_event *ev = arg;
+		struct v4l2_dv_timings *p = arg;
+
+		if (!ops->vidioc_query_dv_timings)
+			break;
 
-		if (!ops->vidioc_subscribe_event)
+		ret = ops->vidioc_query_dv_timings(file, fh, p);
+		if (!ret)
+			dbgtimings(vfd, p);
+		break;
+	}
+	case VIDIOC_DV_TIMINGS_CAP:
+	{
+		struct v4l2_dv_timings_cap *p = arg;
+
+		if (!ops->vidioc_dv_timings_cap)
 			break;
 
+		ret = ops->vidioc_dv_timings_cap(file, fh, p);
+		if (ret)
+			break;
+		switch (p->type) {
+		case V4L2_DV_BT_656_1120:
+			dbgarg(cmd,
+			       "type=%d, width=%u-%u, height=%u-%u, "
+			       "pixelclock=%llu-%llu, standards=%x, capabilities=%x ",
+			       p->type,
+			       p->bt.min_width, p->bt.max_width,
+			       p->bt.min_height, p->bt.max_height,
+			       p->bt.min_pixelclock, p->bt.max_pixelclock,
+			       p->bt.standards, p->bt.capabilities);
+			break;
+		default:
+			dbgarg(cmd, "unknown type ");
+			break;
+		}
+		break;
+	}
+	case VIDIOC_DQEVENT:
+	{
+		struct v4l2_event *ev = arg;
+
 		ret = v4l2_event_dequeue(fh, ev, file->f_flags & O_NONBLOCK);
 		if (ret < 0) {
 			dbgarg(cmd, "no pending events?");
@@ -2226,9 +2040,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_event_subscription *sub = arg;
 
-		if (!ops->vidioc_subscribe_event)
-			break;
-
 		ret = ops->vidioc_subscribe_event(fh, sub);
 		if (ret < 0) {
 			dbgarg(cmd, "failed, ret=%ld", ret);
@@ -2241,9 +2052,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_event_subscription *sub = arg;
 
-		if (!ops->vidioc_unsubscribe_event)
-			break;
-
 		ret = ops->vidioc_unsubscribe_event(fh, sub);
 		if (ret < 0) {
 			dbgarg(cmd, "failed, ret=%ld", ret);
@@ -2256,12 +2064,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_create_buffers *create = arg;
 
-		if (!ops->vidioc_create_bufs)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = check_fmt(ops, create->format.type);
 		if (ret)
 			break;
@@ -2275,8 +2077,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *b = arg;
 
-		if (!ops->vidioc_prepare_buf)
-			break;
 		ret = check_fmt(ops, b->type);
 		if (ret)
 			break;
@@ -2289,7 +2089,9 @@ static long __video_do_ioctl(struct file *file,
 	default:
 		if (!ops->vidioc_default)
 			break;
-		ret = ops->vidioc_default(file, fh, ret_prio >= 0, cmd, arg);
+		ret = ops->vidioc_default(file, fh, use_fh_prio ?
+				v4l2_prio_check(vfd->prio, vfh->prio) >= 0 : 0,
+				cmd, arg);
 		break;
 	} /* switch */
 
@@ -2463,7 +2265,9 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 			err = -EFAULT;
 		goto out_array_args;
 	}
-	if (err < 0)
+	/* VIDIOC_QUERY_DV_TIMINGS can return an error, but still have valid
+	   results that must be returned. */
+	if (err < 0 && cmd != VIDIOC_QUERY_DV_TIMINGS)
 		goto out;
 
 out_array_args:
diff --git a/drivers/media/video/v4l2-subdev.c b/drivers/media/video/v4l2-subdev.c
index 6fe88e965a8c..db6e859b93d4 100644
--- a/drivers/media/video/v4l2-subdev.c
+++ b/drivers/media/video/v4l2-subdev.c
@@ -35,14 +35,9 @@
 static int subdev_fh_init(struct v4l2_subdev_fh *fh, struct v4l2_subdev *sd)
 {
 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
-	/* Allocate try format and crop in the same memory block */
-	fh->try_fmt = kzalloc((sizeof(*fh->try_fmt) + sizeof(*fh->try_crop))
-			      * sd->entity.num_pads, GFP_KERNEL);
-	if (fh->try_fmt == NULL)
+	fh->pad = kzalloc(sizeof(*fh->pad) * sd->entity.num_pads, GFP_KERNEL);
+	if (fh->pad == NULL)
 		return -ENOMEM;
-
-	fh->try_crop = (struct v4l2_rect *)
-		(fh->try_fmt + sd->entity.num_pads);
 #endif
 	return 0;
 }
@@ -50,9 +45,8 @@ static int subdev_fh_init(struct v4l2_subdev_fh *fh, struct v4l2_subdev *sd)
 static void subdev_fh_free(struct v4l2_subdev_fh *fh)
 {
 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
-	kfree(fh->try_fmt);
-	fh->try_fmt = NULL;
-	fh->try_crop = NULL;
+	kfree(fh->pad);
+	fh->pad = NULL;
 #endif
 }
 
@@ -234,6 +228,8 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 
 	case VIDIOC_SUBDEV_G_CROP: {
 		struct v4l2_subdev_crop *crop = arg;
+		struct v4l2_subdev_selection sel;
+		int rval;
 
 		if (crop->which != V4L2_SUBDEV_FORMAT_TRY &&
 		    crop->which != V4L2_SUBDEV_FORMAT_ACTIVE)
@@ -242,11 +238,27 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		if (crop->pad >= sd->entity.num_pads)
 			return -EINVAL;
 
-		return v4l2_subdev_call(sd, pad, get_crop, subdev_fh, crop);
+		rval = v4l2_subdev_call(sd, pad, get_crop, subdev_fh, crop);
+		if (rval != -ENOIOCTLCMD)
+			return rval;
+
+		memset(&sel, 0, sizeof(sel));
+		sel.which = crop->which;
+		sel.pad = crop->pad;
+		sel.target = V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL;
+
+		rval = v4l2_subdev_call(
+			sd, pad, get_selection, subdev_fh, &sel);
+
+		crop->rect = sel.r;
+
+		return rval;
 	}
 
 	case VIDIOC_SUBDEV_S_CROP: {
 		struct v4l2_subdev_crop *crop = arg;
+		struct v4l2_subdev_selection sel;
+		int rval;
 
 		if (crop->which != V4L2_SUBDEV_FORMAT_TRY &&
 		    crop->which != V4L2_SUBDEV_FORMAT_ACTIVE)
@@ -255,7 +267,22 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		if (crop->pad >= sd->entity.num_pads)
 			return -EINVAL;
 
-		return v4l2_subdev_call(sd, pad, set_crop, subdev_fh, crop);
+		rval = v4l2_subdev_call(sd, pad, set_crop, subdev_fh, crop);
+		if (rval != -ENOIOCTLCMD)
+			return rval;
+
+		memset(&sel, 0, sizeof(sel));
+		sel.which = crop->which;
+		sel.pad = crop->pad;
+		sel.target = V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL;
+		sel.r = crop->rect;
+
+		rval = v4l2_subdev_call(
+			sd, pad, set_selection, subdev_fh, &sel);
+
+		crop->rect = sel.r;
+
+		return rval;
 	}
 
 	case VIDIOC_SUBDEV_ENUM_MBUS_CODE: {
@@ -293,6 +320,34 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		return v4l2_subdev_call(sd, pad, enum_frame_interval, subdev_fh,
 					fie);
 	}
+
+	case VIDIOC_SUBDEV_G_SELECTION: {
+		struct v4l2_subdev_selection *sel = arg;
+
+		if (sel->which != V4L2_SUBDEV_FORMAT_TRY &&
+		    sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+			return -EINVAL;
+
+		if (sel->pad >= sd->entity.num_pads)
+			return -EINVAL;
+
+		return v4l2_subdev_call(
+			sd, pad, get_selection, subdev_fh, sel);
+	}
+
+	case VIDIOC_SUBDEV_S_SELECTION: {
+		struct v4l2_subdev_selection *sel = arg;
+
+		if (sel->which != V4L2_SUBDEV_FORMAT_TRY &&
+		    sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+			return -EINVAL;
+
+		if (sel->pad >= sd->entity.num_pads)
+			return -EINVAL;
+
+		return v4l2_subdev_call(
+			sd, pad, set_selection, subdev_fh, sel);
+	}
 #endif
 	default:
 		return v4l2_subdev_call(sd, core, ioctl, cmd, arg);
@@ -332,6 +387,70 @@ const struct v4l2_file_operations v4l2_subdev_fops = {
 	.poll = subdev_poll,
 };
 
+#ifdef CONFIG_MEDIA_CONTROLLER
+int v4l2_subdev_link_validate_default(struct v4l2_subdev *sd,
+				      struct media_link *link,
+				      struct v4l2_subdev_format *source_fmt,
+				      struct v4l2_subdev_format *sink_fmt)
+{
+	if (source_fmt->format.width != sink_fmt->format.width
+	    || source_fmt->format.height != sink_fmt->format.height
+	    || source_fmt->format.code != sink_fmt->format.code)
+		return -EINVAL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(v4l2_subdev_link_validate_default);
+
+static int
+v4l2_subdev_link_validate_get_format(struct media_pad *pad,
+				     struct v4l2_subdev_format *fmt)
+{
+	switch (media_entity_type(pad->entity)) {
+	case MEDIA_ENT_T_V4L2_SUBDEV:
+		fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		fmt->pad = pad->index;
+		return v4l2_subdev_call(media_entity_to_v4l2_subdev(
+						pad->entity),
+					pad, get_fmt, NULL, fmt);
+	default:
+		WARN(1, "Driver bug! Wrong media entity type %d, entity %s\n",
+		     media_entity_type(pad->entity), pad->entity->name);
+		/* Fall through */
+	case MEDIA_ENT_T_DEVNODE_V4L:
+		return -EINVAL;
+	}
+}
+
+int v4l2_subdev_link_validate(struct media_link *link)
+{
+	struct v4l2_subdev *sink;
+	struct v4l2_subdev_format sink_fmt, source_fmt;
+	int rval;
+
+	rval = v4l2_subdev_link_validate_get_format(
+		link->source, &source_fmt);
+	if (rval < 0)
+		return 0;
+
+	rval = v4l2_subdev_link_validate_get_format(
+		link->sink, &sink_fmt);
+	if (rval < 0)
+		return 0;
+
+	sink = media_entity_to_v4l2_subdev(link->sink->entity);
+
+	rval = v4l2_subdev_call(sink, pad, link_validate, link,
+				&source_fmt, &sink_fmt);
+	if (rval != -ENOIOCTLCMD)
+		return rval;
+
+	return v4l2_subdev_link_validate_default(
+		sink, link, &source_fmt, &sink_fmt);
+}
+EXPORT_SYMBOL_GPL(v4l2_subdev_link_validate);
+#endif /* CONFIG_MEDIA_CONTROLLER */
+
 void v4l2_subdev_init(struct v4l2_subdev *sd, const struct v4l2_subdev_ops *ops)
 {
 	INIT_LIST_HEAD(&sd->list);
diff --git a/drivers/media/video/via-camera.c b/drivers/media/video/via-camera.c
index 20f7237b8242..308e150a39bc 100644
--- a/drivers/media/video/via-camera.c
+++ b/drivers/media/video/via-camera.c
@@ -18,6 +18,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/ov7670.h>
 #include <media/videobuf-dma-sg.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
@@ -1347,11 +1348,21 @@ static __devinit bool viacam_serial_is_enabled(void)
 	return false;
 }
 
+static struct ov7670_config sensor_cfg = {
+	/* The XO-1.5 (only known user) clocks the camera at 90MHz. */
+	.clock_speed = 90,
+};
+
 static __devinit int viacam_probe(struct platform_device *pdev)
 {
 	int ret;
 	struct i2c_adapter *sensor_adapter;
 	struct viafb_dev *viadev = pdev->dev.platform_data;
+	struct i2c_board_info ov7670_info = {
+		.type = "ov7670",
+		.addr = 0x42 >> 1,
+		.platform_data = &sensor_cfg,
+	};
 
 	/*
 	 * Note that there are actually two capture channels on
@@ -1433,8 +1444,8 @@ static __devinit int viacam_probe(struct platform_device *pdev)
 	 * is OLPC-specific.  0x42 assumption is ov7670-specific.
 	 */
 	sensor_adapter = viafb_find_i2c_adapter(VIA_PORT_31);
-	cam->sensor = v4l2_i2c_new_subdev(&cam->v4l2_dev, sensor_adapter,
-			"ov7670", 0x42 >> 1, NULL);
+	cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev, sensor_adapter,
+			&ov7670_info, NULL);
 	if (cam->sensor == NULL) {
 		dev_err(&pdev->dev, "Unable to find the sensor!\n");
 		ret = -ENODEV;
diff --git a/drivers/media/video/videobuf-core.c b/drivers/media/video/videobuf-core.c
index de4fa4eb8844..ffdf59cfe405 100644
--- a/drivers/media/video/videobuf-core.c
+++ b/drivers/media/video/videobuf-core.c
@@ -1129,6 +1129,7 @@ unsigned int videobuf_poll_stream(struct file *file,
 				  struct videobuf_queue *q,
 				  poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct videobuf_buffer *buf = NULL;
 	unsigned int rc = 0;
 
@@ -1137,7 +1138,7 @@ unsigned int videobuf_poll_stream(struct file *file,
 		if (!list_empty(&q->stream))
 			buf = list_entry(q->stream.next,
 					 struct videobuf_buffer, stream);
-	} else {
+	} else if (req_events & (POLLIN | POLLRDNORM)) {
 		if (!q->reading)
 			__videobuf_read_start(q);
 		if (!q->reading) {
diff --git a/drivers/media/video/videobuf-dma-contig.c b/drivers/media/video/videobuf-dma-contig.c
index c9691115f2d2..b6b5cc1a43cb 100644
--- a/drivers/media/video/videobuf-dma-contig.c
+++ b/drivers/media/video/videobuf-dma-contig.c
@@ -27,6 +27,7 @@ struct videobuf_dma_contig_memory {
 	u32 magic;
 	void *vaddr;
 	dma_addr_t dma_handle;
+	bool cached;
 	unsigned long size;
 };
 
@@ -37,8 +38,58 @@ struct videobuf_dma_contig_memory {
 		BUG();							    \
 	}
 
-static void
-videobuf_vm_open(struct vm_area_struct *vma)
+static int __videobuf_dc_alloc(struct device *dev,
+			       struct videobuf_dma_contig_memory *mem,
+			       unsigned long size, unsigned long flags)
+{
+	mem->size = size;
+	if (mem->cached) {
+		mem->vaddr = alloc_pages_exact(mem->size, flags | GFP_DMA);
+		if (mem->vaddr) {
+			int err;
+
+			mem->dma_handle = dma_map_single(dev, mem->vaddr,
+							 mem->size,
+							 DMA_FROM_DEVICE);
+			err = dma_mapping_error(dev, mem->dma_handle);
+			if (err) {
+				dev_err(dev, "dma_map_single failed\n");
+
+				free_pages_exact(mem->vaddr, mem->size);
+				mem->vaddr = 0;
+				return err;
+			}
+		}
+	} else
+		mem->vaddr = dma_alloc_coherent(dev, mem->size,
+						&mem->dma_handle, flags);
+
+	if (!mem->vaddr) {
+		dev_err(dev, "memory alloc size %ld failed\n", mem->size);
+		return -ENOMEM;
+	}
+
+	dev_dbg(dev, "dma mapped data is at %p (%ld)\n", mem->vaddr, mem->size);
+
+	return 0;
+}
+
+static void __videobuf_dc_free(struct device *dev,
+			       struct videobuf_dma_contig_memory *mem)
+{
+	if (mem->cached) {
+		if (!mem->vaddr)
+			return;
+		dma_unmap_single(dev, mem->dma_handle, mem->size,
+				 DMA_FROM_DEVICE);
+		free_pages_exact(mem->vaddr, mem->size);
+	} else
+		dma_free_coherent(dev, mem->size, mem->vaddr, mem->dma_handle);
+
+	mem->vaddr = NULL;
+}
+
+static void videobuf_vm_open(struct vm_area_struct *vma)
 {
 	struct videobuf_mapping *map = vma->vm_private_data;
 
@@ -91,12 +142,11 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 				dev_dbg(q->dev, "buf[%d] freeing %p\n",
 					i, mem->vaddr);
 
-				dma_free_coherent(q->dev, mem->size,
-						  mem->vaddr, mem->dma_handle);
+				__videobuf_dc_free(q->dev, mem);
 				mem->vaddr = NULL;
 			}
 
-			q->bufs[i]->map   = NULL;
+			q->bufs[i]->map = NULL;
 			q->bufs[i]->baddr = 0;
 		}
 
@@ -107,8 +157,8 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 }
 
 static const struct vm_operations_struct videobuf_vm_ops = {
-	.open     = videobuf_vm_open,
-	.close    = videobuf_vm_close,
+	.open	= videobuf_vm_open,
+	.close	= videobuf_vm_close,
 };
 
 /**
@@ -178,26 +228,38 @@ static int videobuf_dma_contig_user_get(struct videobuf_dma_contig_memory *mem,
 		pages_done++;
 	}
 
- out_up:
+out_up:
 	up_read(&current->mm->mmap_sem);
 
 	return ret;
 }
 
-static struct videobuf_buffer *__videobuf_alloc_vb(size_t size)
+static struct videobuf_buffer *__videobuf_alloc_vb(size_t size, bool cached)
 {
 	struct videobuf_dma_contig_memory *mem;
 	struct videobuf_buffer *vb;
 
 	vb = kzalloc(size + sizeof(*mem), GFP_KERNEL);
 	if (vb) {
-		mem = vb->priv = ((char *)vb) + size;
+		vb->priv = ((char *)vb) + size;
+		mem = vb->priv;
 		mem->magic = MAGIC_DC_MEM;
+		mem->cached = cached;
 	}
 
 	return vb;
 }
 
+static struct videobuf_buffer *__videobuf_alloc_uncached(size_t size)
+{
+	return __videobuf_alloc_vb(size, false);
+}
+
+static struct videobuf_buffer *__videobuf_alloc_cached(size_t size)
+{
+	return __videobuf_alloc_vb(size, true);
+}
+
 static void *__videobuf_to_vaddr(struct videobuf_buffer *buf)
 {
 	struct videobuf_dma_contig_memory *mem = buf->priv;
@@ -235,28 +297,32 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 			return videobuf_dma_contig_user_get(mem, vb);
 
 		/* allocate memory for the read() method */
-		mem->size = PAGE_ALIGN(vb->size);
-		mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
-						&mem->dma_handle, GFP_KERNEL);
-		if (!mem->vaddr) {
-			dev_err(q->dev, "dma_alloc_coherent %ld failed\n",
-					 mem->size);
+		if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(vb->size),
+					GFP_KERNEL))
 			return -ENOMEM;
-		}
-
-		dev_dbg(q->dev, "dma_alloc_coherent data is at %p (%ld)\n",
-			mem->vaddr, mem->size);
 		break;
 	case V4L2_MEMORY_OVERLAY:
 	default:
-		dev_dbg(q->dev, "%s memory method OVERLAY/unknown\n",
-			__func__);
+		dev_dbg(q->dev, "%s memory method OVERLAY/unknown\n", __func__);
 		return -EINVAL;
 	}
 
 	return 0;
 }
 
+static int __videobuf_sync(struct videobuf_queue *q,
+			   struct videobuf_buffer *buf)
+{
+	struct videobuf_dma_contig_memory *mem = buf->priv;
+	BUG_ON(!mem);
+	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
+
+	dma_sync_single_for_cpu(q->dev, mem->dma_handle, mem->size,
+				DMA_FROM_DEVICE);
+
+	return 0;
+}
+
 static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 				  struct videobuf_buffer *buf,
 				  struct vm_area_struct *vma)
@@ -265,6 +331,8 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	struct videobuf_mapping *map;
 	int retval;
 	unsigned long size;
+	unsigned long pos, start = vma->vm_start;
+	struct page *page;
 
 	dev_dbg(q->dev, "%s\n", __func__);
 
@@ -282,41 +350,50 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	BUG_ON(!mem);
 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
 
-	mem->size = PAGE_ALIGN(buf->bsize);
-	mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
-					&mem->dma_handle, GFP_KERNEL);
-	if (!mem->vaddr) {
-		dev_err(q->dev, "dma_alloc_coherent size %ld failed\n",
-			mem->size);
+	if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(buf->bsize),
+				GFP_KERNEL | __GFP_COMP))
 		goto error;
-	}
-	dev_dbg(q->dev, "dma_alloc_coherent data is at addr %p (size %ld)\n",
-		mem->vaddr, mem->size);
 
 	/* Try to remap memory */
 
 	size = vma->vm_end - vma->vm_start;
 	size = (size < mem->size) ? size : mem->size;
 
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-	retval = remap_pfn_range(vma, vma->vm_start,
-				 mem->dma_handle >> PAGE_SHIFT,
-				 size, vma->vm_page_prot);
-	if (retval) {
-		dev_err(q->dev, "mmap: remap failed with error %d. ", retval);
-		dma_free_coherent(q->dev, mem->size,
-				  mem->vaddr, mem->dma_handle);
-		goto error;
+	if (!mem->cached)
+		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	pos = (unsigned long)mem->vaddr;
+
+	while (size > 0) {
+		page = virt_to_page((void *)pos);
+		if (NULL == page) {
+			dev_err(q->dev, "mmap: virt_to_page failed\n");
+			__videobuf_dc_free(q->dev, mem);
+			goto error;
+		}
+		retval = vm_insert_page(vma, start, page);
+		if (retval) {
+			dev_err(q->dev, "mmap: insert failed with error %d\n",
+				retval);
+			__videobuf_dc_free(q->dev, mem);
+			goto error;
+		}
+		start += PAGE_SIZE;
+		pos += PAGE_SIZE;
+
+		if (size > PAGE_SIZE)
+			size -= PAGE_SIZE;
+		else
+			size = 0;
 	}
 
-	vma->vm_ops          = &videobuf_vm_ops;
-	vma->vm_flags       |= VM_DONTEXPAND;
+	vma->vm_ops = &videobuf_vm_ops;
+	vma->vm_flags |= VM_DONTEXPAND;
 	vma->vm_private_data = map;
 
 	dev_dbg(q->dev, "mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
 		map, q, vma->vm_start, vma->vm_end,
-		(long int)buf->bsize,
-		vma->vm_pgoff, buf->i);
+		(long int)buf->bsize, vma->vm_pgoff, buf->i);
 
 	videobuf_vm_open(vma);
 
@@ -328,12 +405,20 @@ error:
 }
 
 static struct videobuf_qtype_ops qops = {
-	.magic        = MAGIC_QTYPE_OPS,
+	.magic		= MAGIC_QTYPE_OPS,
+	.alloc_vb	= __videobuf_alloc_uncached,
+	.iolock		= __videobuf_iolock,
+	.mmap_mapper	= __videobuf_mmap_mapper,
+	.vaddr		= __videobuf_to_vaddr,
+};
 
-	.alloc_vb     = __videobuf_alloc_vb,
-	.iolock       = __videobuf_iolock,
-	.mmap_mapper  = __videobuf_mmap_mapper,
-	.vaddr        = __videobuf_to_vaddr,
+static struct videobuf_qtype_ops qops_cached = {
+	.magic		= MAGIC_QTYPE_OPS,
+	.alloc_vb	= __videobuf_alloc_cached,
+	.iolock		= __videobuf_iolock,
+	.sync		= __videobuf_sync,
+	.mmap_mapper	= __videobuf_mmap_mapper,
+	.vaddr		= __videobuf_to_vaddr,
 };
 
 void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
@@ -351,6 +436,20 @@ void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
 }
 EXPORT_SYMBOL_GPL(videobuf_queue_dma_contig_init);
 
+void videobuf_queue_dma_contig_init_cached(struct videobuf_queue *q,
+					   const struct videobuf_queue_ops *ops,
+					   struct device *dev,
+					   spinlock_t *irqlock,
+					   enum v4l2_buf_type type,
+					   enum v4l2_field field,
+					   unsigned int msize,
+					   void *priv, struct mutex *ext_lock)
+{
+	videobuf_queue_core_init(q, ops, dev, irqlock, type, field, msize,
+				 priv, &qops_cached, ext_lock);
+}
+EXPORT_SYMBOL_GPL(videobuf_queue_dma_contig_init_cached);
+
 dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf)
 {
 	struct videobuf_dma_contig_memory *mem = buf->priv;
@@ -389,7 +488,7 @@ void videobuf_dma_contig_free(struct videobuf_queue *q,
 
 	/* read() method */
 	if (mem->vaddr) {
-		dma_free_coherent(q->dev, mem->size, mem->vaddr, mem->dma_handle);
+		__videobuf_dc_free(q->dev, mem);
 		mem->vaddr = NULL;
 	}
 }
diff --git a/drivers/media/video/videobuf-dvb.c b/drivers/media/video/videobuf-dvb.c
index 59cb54aa2946..94d83a41381b 100644
--- a/drivers/media/video/videobuf-dvb.c
+++ b/drivers/media/video/videobuf-dvb.c
@@ -45,7 +45,6 @@ static int videobuf_dvb_thread(void *data)
 	struct videobuf_dvb *dvb = data;
 	struct videobuf_buffer *buf;
 	unsigned long flags;
-	int err;
 	void *outp;
 
 	dprintk("dvb thread started\n");
@@ -57,7 +56,7 @@ static int videobuf_dvb_thread(void *data)
 		buf = list_entry(dvb->dvbq.stream.next,
 				 struct videobuf_buffer, stream);
 		list_del(&buf->stream);
-		err = videobuf_waiton(&dvb->dvbq, buf, 0, 1);
+		videobuf_waiton(&dvb->dvbq, buf, 0, 1);
 
 		/* no more feeds left or stop_feed() asked us to quit */
 		if (0 == dvb->nfeeds)
diff --git a/drivers/media/video/videobuf2-core.c b/drivers/media/video/videobuf2-core.c
index 2e8f1df775b6..9d4e9edbd2e7 100644
--- a/drivers/media/video/videobuf2-core.c
+++ b/drivers/media/video/videobuf2-core.c
@@ -19,6 +19,9 @@
 #include <linux/slab.h>
 #include <linux/sched.h>
 
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 #include <media/videobuf2-core.h>
 
 static int debug;
@@ -1642,32 +1645,46 @@ static int __vb2_cleanup_fileio(struct vb2_queue *q);
  * For OUTPUT queues, if a buffer is ready to be dequeued, the file descriptor
  * will be reported as available for writing.
  *
+ * If the driver uses struct v4l2_fh, then vb2_poll() will also check for any
+ * pending events.
+ *
  * The return values from this function are intended to be directly returned
  * from poll handler in driver.
  */
 unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 {
-	unsigned long flags;
-	unsigned int ret;
+	struct video_device *vfd = video_devdata(file);
+	unsigned long req_events = poll_requested_events(wait);
 	struct vb2_buffer *vb = NULL;
+	unsigned int res = 0;
+	unsigned long flags;
+
+	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
+		struct v4l2_fh *fh = file->private_data;
+
+		if (v4l2_event_pending(fh))
+			res = POLLPRI;
+		else if (req_events & POLLPRI)
+			poll_wait(file, &fh->wait, wait);
+	}
 
 	/*
 	 * Start file I/O emulator only if streaming API has not been used yet.
 	 */
 	if (q->num_buffers == 0 && q->fileio == NULL) {
-		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ)) {
-			ret = __vb2_init_fileio(q, 1);
-			if (ret)
-				return POLLERR;
+		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ) &&
+				(req_events & (POLLIN | POLLRDNORM))) {
+			if (__vb2_init_fileio(q, 1))
+				return res | POLLERR;
 		}
-		if (V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_WRITE)) {
-			ret = __vb2_init_fileio(q, 0);
-			if (ret)
-				return POLLERR;
+		if (V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_WRITE) &&
+				(req_events & (POLLOUT | POLLWRNORM))) {
+			if (__vb2_init_fileio(q, 0))
+				return res | POLLERR;
 			/*
 			 * Write to OUTPUT queue can be done immediately.
 			 */
-			return POLLOUT | POLLWRNORM;
+			return res | POLLOUT | POLLWRNORM;
 		}
 	}
 
@@ -1675,7 +1692,7 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 	 * There is nothing to wait for if no buffers have already been queued.
 	 */
 	if (list_empty(&q->queued_list))
-		return POLLERR;
+		return res | POLLERR;
 
 	poll_wait(file, &q->done_wq, wait);
 
@@ -1690,10 +1707,11 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 
 	if (vb && (vb->state == VB2_BUF_STATE_DONE
 			|| vb->state == VB2_BUF_STATE_ERROR)) {
-		return (V4L2_TYPE_IS_OUTPUT(q->type)) ? POLLOUT | POLLWRNORM :
-			POLLIN | POLLRDNORM;
+		return (V4L2_TYPE_IS_OUTPUT(q->type)) ?
+				res | POLLOUT | POLLWRNORM :
+				res | POLLIN | POLLRDNORM;
 	}
-	return 0;
+	return res;
 }
 EXPORT_SYMBOL_GPL(vb2_poll);
 
@@ -1839,7 +1857,6 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	 * (multiplane buffers are not supported).
 	 */
 	if (q->bufs[0]->num_planes != 1) {
-		fileio->req.count = 0;
 		ret = -EBUSY;
 		goto err_reqbufs;
 	}
@@ -1886,6 +1903,7 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	return ret;
 
 err_reqbufs:
+	fileio->req.count = 0;
 	vb2_reqbufs(q, &fileio->req);
 
 err_kfree:
diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index 5e8b0710105b..0960d7f0d394 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -95,6 +95,16 @@ static struct vivi_fmt formats[] = {
 		.depth    = 16,
 	},
 	{
+		.name     = "4:2:2, packed, YVYU",
+		.fourcc   = V4L2_PIX_FMT_YVYU,
+		.depth    = 16,
+	},
+	{
+		.name     = "4:2:2, packed, VYUY",
+		.fourcc   = V4L2_PIX_FMT_VYUY,
+		.depth    = 16,
+	},
+	{
 		.name     = "RGB565 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
 		.depth    = 16,
@@ -114,6 +124,26 @@ static struct vivi_fmt formats[] = {
 		.fourcc   = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
 		.depth    = 16,
 	},
+	{
+		.name     = "RGB24 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB24, /* rgb */
+		.depth    = 24,
+	},
+	{
+		.name     = "RGB24 (BE)",
+		.fourcc   = V4L2_PIX_FMT_BGR24, /* bgr */
+		.depth    = 24,
+	},
+	{
+		.name     = "RGB32 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB32, /* argb */
+		.depth    = 32,
+	},
+	{
+		.name     = "RGB32 (BE)",
+		.fourcc   = V4L2_PIX_FMT_BGR32, /* bgra */
+		.depth    = 32,
+	},
 };
 
 static struct vivi_fmt *get_format(struct v4l2_format *f)
@@ -170,6 +200,7 @@ struct vivi_dev {
 		struct v4l2_ctrl	   *gain;
 	};
 	struct v4l2_ctrl	   *volume;
+	struct v4l2_ctrl	   *alpha;
 	struct v4l2_ctrl	   *button;
 	struct v4l2_ctrl	   *boolean;
 	struct v4l2_ctrl	   *int32;
@@ -177,6 +208,7 @@ struct vivi_dev {
 	struct v4l2_ctrl	   *menu;
 	struct v4l2_ctrl	   *string;
 	struct v4l2_ctrl	   *bitmask;
+	struct v4l2_ctrl	   *int_menu;
 
 	spinlock_t                 slock;
 	struct mutex		   mutex;
@@ -203,8 +235,10 @@ struct vivi_dev {
 	enum v4l2_field		   field;
 	unsigned int		   field_count;
 
-	u8 			   bars[9][3];
-	u8 			   line[MAX_WIDTH * 4];
+	u8			   bars[9][3];
+	u8			   line[MAX_WIDTH * 8];
+	unsigned int		   pixelsize;
+	u8			   alpha_component;
 };
 
 /* ------------------------------------------------------------------
@@ -283,6 +317,8 @@ static void precalculate_bars(struct vivi_dev *dev)
 		switch (dev->fmt->fourcc) {
 		case V4L2_PIX_FMT_YUYV:
 		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YVYU:
+		case V4L2_PIX_FMT_VYUY:
 			is_yuv = 1;
 			break;
 		case V4L2_PIX_FMT_RGB565:
@@ -297,6 +333,11 @@ static void precalculate_bars(struct vivi_dev *dev)
 			g >>= 3;
 			b >>= 3;
 			break;
+		case V4L2_PIX_FMT_RGB24:
+		case V4L2_PIX_FMT_BGR24:
+		case V4L2_PIX_FMT_RGB32:
+		case V4L2_PIX_FMT_BGR32:
+			break;
 		}
 
 		if (is_yuv) {
@@ -316,9 +357,11 @@ static void precalculate_bars(struct vivi_dev *dev)
 #define TSTAMP_INPUT_X	10
 #define TSTAMP_MIN_X	(54 + TSTAMP_INPUT_X)
 
-static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
+/* 'odd' is true for pixels 1, 3, 5, etc. and false for pixels 0, 2, 4, etc. */
+static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos, bool odd)
 {
 	u8 r_y, g_u, b_v;
+	u8 alpha = dev->alpha_component;
 	int color;
 	u8 *p;
 
@@ -326,46 +369,56 @@ static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
 	g_u = dev->bars[colorpos][1]; /* G or precalculated U */
 	b_v = dev->bars[colorpos][2]; /* B or precalculated V */
 
-	for (color = 0; color < 4; color++) {
+	for (color = 0; color < dev->pixelsize; color++) {
 		p = buf + color;
 
 		switch (dev->fmt->fourcc) {
 		case V4L2_PIX_FMT_YUYV:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = r_y;
 				break;
 			case 1:
-				*p = g_u;
-				break;
-			case 3:
-				*p = b_v;
+				*p = odd ? b_v : g_u;
 				break;
 			}
 			break;
 		case V4L2_PIX_FMT_UYVY:
 			switch (color) {
+			case 0:
+				*p = odd ? b_v : g_u;
+				break;
 			case 1:
-			case 3:
 				*p = r_y;
 				break;
+			}
+			break;
+		case V4L2_PIX_FMT_YVYU:
+			switch (color) {
 			case 0:
-				*p = g_u;
+				*p = r_y;
 				break;
-			case 2:
-				*p = b_v;
+			case 1:
+				*p = odd ? g_u : b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_VYUY:
+			switch (color) {
+			case 0:
+				*p = odd ? g_u : b_v;
+				break;
+			case 1:
+				*p = r_y;
 				break;
 			}
 			break;
 		case V4L2_PIX_FMT_RGB565:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = (g_u << 5) | b_v;
 				break;
 			case 1:
-			case 3:
 				*p = (r_y << 3) | (g_u >> 3);
 				break;
 			}
@@ -373,11 +426,9 @@ static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
 		case V4L2_PIX_FMT_RGB565X:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = (r_y << 3) | (g_u >> 3);
 				break;
 			case 1:
-			case 3:
 				*p = (g_u << 5) | b_v;
 				break;
 			}
@@ -385,24 +436,78 @@ static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
 		case V4L2_PIX_FMT_RGB555:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = (g_u << 5) | b_v;
 				break;
 			case 1:
-			case 3:
-				*p = (r_y << 2) | (g_u >> 3);
+				*p = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
 				break;
 			}
 			break;
 		case V4L2_PIX_FMT_RGB555X:
 			switch (color) {
 			case 0:
+				*p = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
+				break;
+			case 1:
+				*p = (g_u << 5) | b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_RGB24:
+			switch (color) {
+			case 0:
+				*p = r_y;
+				break;
+			case 1:
+				*p = g_u;
+				break;
 			case 2:
-				*p = (r_y << 2) | (g_u >> 3);
+				*p = b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_BGR24:
+			switch (color) {
+			case 0:
+				*p = b_v;
 				break;
 			case 1:
+				*p = g_u;
+				break;
+			case 2:
+				*p = r_y;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_RGB32:
+			switch (color) {
+			case 0:
+				*p = alpha;
+				break;
+			case 1:
+				*p = r_y;
+				break;
+			case 2:
+				*p = g_u;
+				break;
 			case 3:
-				*p = (g_u << 5) | b_v;
+				*p = b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_BGR32:
+			switch (color) {
+			case 0:
+				*p = b_v;
+				break;
+			case 1:
+				*p = g_u;
+				break;
+			case 2:
+				*p = r_y;
+				break;
+			case 3:
+				*p = alpha;
 				break;
 			}
 			break;
@@ -414,10 +519,10 @@ static void precalculate_line(struct vivi_dev *dev)
 {
 	int w;
 
-	for (w = 0; w < dev->width * 2; w += 2) {
-		int colorpos = (w / (dev->width / 8) % 8);
+	for (w = 0; w < dev->width * 2; w++) {
+		int colorpos = w / (dev->width / 8) % 8;
 
-		gen_twopix(dev, dev->line + w * 2, colorpos);
+		gen_twopix(dev, dev->line + w * dev->pixelsize, colorpos, w & 1);
 	}
 }
 
@@ -433,7 +538,7 @@ static void gen_text(struct vivi_dev *dev, char *basep,
 	/* Print stream time */
 	for (line = y; line < y + 16; line++) {
 		int j = 0;
-		char *pos = basep + line * dev->width * 2 + x * 2;
+		char *pos = basep + line * dev->width * dev->pixelsize + x * dev->pixelsize;
 		char *s;
 
 		for (s = text; *s; s++) {
@@ -443,9 +548,9 @@ static void gen_text(struct vivi_dev *dev, char *basep,
 			for (i = 0; i < 7; i++, j++) {
 				/* Draw white font on black background */
 				if (chr & (1 << (7 - i)))
-					gen_twopix(dev, pos + j * 2, WHITE);
+					gen_twopix(dev, pos + j * dev->pixelsize, WHITE, (x+y) & 1);
 				else
-					gen_twopix(dev, pos + j * 2, TEXT_BLACK);
+					gen_twopix(dev, pos + j * dev->pixelsize, TEXT_BLACK, (x+y) & 1);
 			}
 		}
 	}
@@ -466,7 +571,9 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 		return;
 
 	for (h = 0; h < hmax; h++)
-		memcpy(vbuf + h * wmax * 2, dev->line + (dev->mv_count % wmax) * 2, wmax * 2);
+		memcpy(vbuf + h * wmax * dev->pixelsize,
+		       dev->line + (dev->mv_count % wmax) * dev->pixelsize,
+		       wmax * dev->pixelsize);
 
 	/* Updates stream time */
 
@@ -484,15 +591,16 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 	gen_text(dev, vbuf, line++ * 16, 16, str);
 
 	gain = v4l2_ctrl_g_ctrl(dev->gain);
-	mutex_lock(&dev->ctrl_handler.lock);
+	mutex_lock(dev->ctrl_handler.lock);
 	snprintf(str, sizeof(str), " brightness %3d, contrast %3d, saturation %3d, hue %d ",
 			dev->brightness->cur.val,
 			dev->contrast->cur.val,
 			dev->saturation->cur.val,
 			dev->hue->cur.val);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " autogain %d, gain %3d, volume %3d ",
-			dev->autogain->cur.val, gain, dev->volume->cur.val);
+	snprintf(str, sizeof(str), " autogain %d, gain %3d, volume %3d, alpha 0x%02x ",
+			dev->autogain->cur.val, gain, dev->volume->cur.val,
+			dev->alpha->cur.val);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
 	snprintf(str, sizeof(str), " int32 %d, int64 %lld, bitmask %08x ",
 			dev->int32->cur.val,
@@ -503,8 +611,12 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 			dev->boolean->cur.val,
 			dev->menu->qmenu[dev->menu->cur.val],
 			dev->string->cur.string);
-	mutex_unlock(&dev->ctrl_handler.lock);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
+	snprintf(str, sizeof(str), " integer_menu %lld, value %d ",
+			dev->int_menu->qmenu_int[dev->int_menu->cur.val],
+			dev->int_menu->cur.val);
+	gen_text(dev, vbuf, line++ * 16, 16, str);
+	mutex_unlock(dev->ctrl_handler.lock);
 	if (dev->button_pressed) {
 		dev->button_pressed--;
 		snprintf(str, sizeof(str), " button pressed!");
@@ -657,7 +769,7 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	struct vivi_dev *dev = vb2_get_drv_priv(vq);
 	unsigned long size;
 
-	size = dev->width * dev->height * 2;
+	size = dev->width * dev->height * dev->pixelsize;
 
 	if (0 == *nbuffers)
 		*nbuffers = 32;
@@ -721,7 +833,7 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	    dev->height < 32 || dev->height > MAX_HEIGHT)
 		return -EINVAL;
 
-	size = dev->width * dev->height * 2;
+	size = dev->width * dev->height * dev->pixelsize;
 	if (vb2_plane_size(vb, 0) < size) {
 		dprintk(dev, 1, "%s data will not fit into plane (%lu < %lu)\n",
 				__func__, vb2_plane_size(vb, 0), size);
@@ -915,6 +1027,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	}
 
 	dev->fmt = get_format(f);
+	dev->pixelsize = dev->fmt->depth / 8;
 	dev->width = f->fmt.pix.width;
 	dev->height = f->fmt.pix.height;
 	dev->field = f->fmt.pix.field;
@@ -1016,8 +1129,15 @@ static int vivi_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct vivi_dev *dev = container_of(ctrl->handler, struct vivi_dev, ctrl_handler);
 
-	if (ctrl == dev->button)
-		dev->button_pressed = 30;
+	switch (ctrl->id) {
+	case V4L2_CID_ALPHA_COMPONENT:
+		dev->alpha_component = ctrl->val;
+		break;
+	default:
+		if (ctrl == dev->button)
+			dev->button_pressed = 30;
+		break;
+	}
 	return 0;
 }
 
@@ -1039,17 +1159,10 @@ static unsigned int
 vivi_poll(struct file *file, struct poll_table_struct *wait)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-	struct v4l2_fh *fh = file->private_data;
 	struct vb2_queue *q = &dev->vb_vidq;
-	unsigned int res;
 
 	dprintk(dev, 1, "%s\n", __func__);
-	res = vb2_poll(q, file, wait);
-	if (v4l2_event_pending(fh))
-		res |= POLLPRI;
-	else
-		poll_wait(file, &fh->wait, wait);
-	return res;
+	return vb2_poll(q, file, wait);
 }
 
 static int vivi_close(struct file *file)
@@ -1165,6 +1278,22 @@ static const struct v4l2_ctrl_config vivi_ctrl_bitmask = {
 	.step = 0,
 };
 
+static const s64 vivi_ctrl_int_menu_values[] = {
+	1, 1, 2, 3, 5, 8, 13, 21, 42,
+};
+
+static const struct v4l2_ctrl_config vivi_ctrl_int_menu = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 7,
+	.name = "Integer menu",
+	.type = V4L2_CTRL_TYPE_INTEGER_MENU,
+	.min = 1,
+	.max = 8,
+	.def = 4,
+	.menu_skip_mask = 0x02,
+	.qmenu_int = vivi_ctrl_int_menu_values,
+};
+
 static const struct v4l2_file_operations vivi_fops = {
 	.owner		= THIS_MODULE,
 	.open           = v4l2_fh_open,
@@ -1252,6 +1381,7 @@ static int __init vivi_create_instance(int inst)
 	dev->fmt = &formats[0];
 	dev->width = 640;
 	dev->height = 480;
+	dev->pixelsize = dev->fmt->depth / 8;
 	hdl = &dev->ctrl_handler;
 	v4l2_ctrl_handler_init(hdl, 11);
 	dev->volume = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
@@ -1268,6 +1398,8 @@ static int __init vivi_create_instance(int inst)
 			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
 	dev->gain = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
 			V4L2_CID_GAIN, 0, 255, 1, 100);
+	dev->alpha = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+			V4L2_CID_ALPHA_COMPONENT, 0, 255, 1, 0);
 	dev->button = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_button, NULL);
 	dev->int32 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int32, NULL);
 	dev->int64 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int64, NULL);
@@ -1275,6 +1407,7 @@ static int __init vivi_create_instance(int inst)
 	dev->menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_menu, NULL);
 	dev->string = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_string, NULL);
 	dev->bitmask = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_bitmask, NULL);
+	dev->int_menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int_menu, NULL);
 	if (hdl->error) {
 		ret = hdl->error;
 		goto unreg_dev;
diff --git a/drivers/media/video/w9966.c b/drivers/media/video/w9966.c
index 7fd7ac567e1a..db2a6003a1c3 100644
--- a/drivers/media/video/w9966.c
+++ b/drivers/media/video/w9966.c
@@ -62,6 +62,9 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <linux/parport.h>
 
 /*#define DEBUG*/				/* Undef me for production */
@@ -104,6 +107,7 @@
 
 struct w9966 {
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 	unsigned char dev_state;
 	unsigned char i2c_state;
 	unsigned short ppmode;
@@ -567,7 +571,8 @@ static int cam_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, cam->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, W9966_DRIVERNAME, sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -595,67 +600,25 @@ static int cam_s_input(struct file *file, void *fh, unsigned int inp)
 	return (inp > 0) ? -EINVAL : 0;
 }
 
-static int cam_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
+static int cam_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, -64, 64, 1, 64);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, -64, 64, 1, 64);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-	}
-	return -EINVAL;
-}
-
-static int cam_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct w9966 *cam = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = cam->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = cam->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = cam->color;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = cam->hue;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
-}
-
-static int cam_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct w9966 *cam = video_drvdata(file);
+	struct w9966 *cam =
+		container_of(ctrl->handler, struct w9966, hdl);
 	int ret = 0;
 
 	mutex_lock(&cam->lock);
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		cam->brightness = ctrl->value;
+		cam->brightness = ctrl->val;
 		break;
 	case V4L2_CID_CONTRAST:
-		cam->contrast = ctrl->value;
+		cam->contrast = ctrl->val;
 		break;
 	case V4L2_CID_SATURATION:
-		cam->color = ctrl->value;
+		cam->color = ctrl->val;
 		break;
 	case V4L2_CID_HUE:
-		cam->hue = ctrl->value;
+		cam->hue = ctrl->val;
 		break;
 	default:
 		ret = -EINVAL;
@@ -813,6 +776,9 @@ out:
 
 static const struct v4l2_file_operations w9966_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl = video_ioctl2,
 	.read           = w9966_v4l_read,
 };
@@ -822,13 +788,17 @@ static const struct v4l2_ioctl_ops w9966_ioctl_ops = {
 	.vidioc_g_input      		    = cam_g_input,
 	.vidioc_s_input      		    = cam_s_input,
 	.vidioc_enum_input   		    = cam_enum_input,
-	.vidioc_queryctrl 		    = cam_queryctrl,
-	.vidioc_g_ctrl  		    = cam_g_ctrl,
-	.vidioc_s_ctrl 			    = cam_s_ctrl,
 	.vidioc_enum_fmt_vid_cap 	    = cam_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap 		    = cam_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap  		    = cam_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap  	    = cam_try_fmt_vid_cap,
+	.vidioc_log_status		    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ctrl_ops cam_ctrl_ops = {
+	.s_ctrl = cam_s_ctrl,
 };
 
 
@@ -849,6 +819,20 @@ static int w9966_init(struct w9966 *cam, struct parport *port)
 		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
 		return -1;
 	}
+
+	v4l2_ctrl_handler_init(&cam->hdl, 4);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_CONTRAST, -64, 64, 1, 64);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_SATURATION, -64, 64, 1, 64);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_HUE, -128, 127, 1, 0);
+	if (cam->hdl.error) {
+		v4l2_err(v4l2_dev, "couldn't register controls\n");
+		return -1;
+	}
 	cam->pport = port;
 	cam->brightness = 128;
 	cam->contrast = 64;
@@ -898,6 +882,8 @@ static int w9966_init(struct w9966 *cam, struct parport *port)
 	cam->vdev.fops = &w9966_fops;
 	cam->vdev.ioctl_ops = &w9966_ioctl_ops;
 	cam->vdev.release = video_device_release_empty;
+	cam->vdev.ctrl_handler = &cam->hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &cam->vdev.flags);
 	video_set_drvdata(&cam->vdev, cam);
 
 	mutex_init(&cam->lock);
@@ -923,6 +909,8 @@ static void w9966_term(struct w9966 *cam)
 		w9966_set_state(cam, W9966_STATE_VDEV, 0);
 	}
 
+	v4l2_ctrl_handler_free(&cam->hdl);
+
 	/* Terminate from IEEE1284 mode and release pdev block */
 	if (w9966_get_state(cam, W9966_STATE_PDEV, W9966_STATE_PDEV)) {
 		w9966_pdev_claim(cam);
diff --git a/drivers/media/video/zoran/zoran_device.c b/drivers/media/video/zoran/zoran_device.c
index e86173bd1327..a4cd504b8eee 100644
--- a/drivers/media/video/zoran/zoran_device.c
+++ b/drivers/media/video/zoran/zoran_device.c
@@ -542,11 +542,9 @@ void write_overlay_mask(struct zoran_fh *fh, struct v4l2_clip *vp, int count)
 	u32 *mask;
 	int x, y, width, height;
 	unsigned i, j, k;
-	u32 reg;
 
 	/* fill mask with one bits */
 	memset(fh->overlay_mask, ~0, mask_line_size * 4 * BUZ_MAX_HEIGHT);
-	reg = 0;
 
 	for (i = 0; i < count; ++i) {
 		/* pick up local copy of clip */
diff --git a/drivers/media/video/zoran/zoran_driver.c b/drivers/media/video/zoran/zoran_driver.c
index 4c09ab781ec3..c57310931810 100644
--- a/drivers/media/video/zoran/zoran_driver.c
+++ b/drivers/media/video/zoran/zoran_driver.c
@@ -1131,8 +1131,14 @@ static int setup_fbuffer(struct zoran_fh *fh,
 }
 
 
-static int setup_window(struct zoran_fh *fh, int x, int y, int width, int height,
-	struct v4l2_clip __user *clips, int clipcount, void __user *bitmap)
+static int setup_window(struct zoran_fh *fh,
+			int x,
+			int y,
+			int width,
+			int height,
+			struct v4l2_clip __user *clips,
+			unsigned int clipcount,
+			void __user *bitmap)
 {
 	struct zoran *zr = fh->zr;
 	struct v4l2_clip *vcp = NULL;
@@ -1155,6 +1161,14 @@ static int setup_window(struct zoran_fh *fh, int x, int y, int width, int height
 		return -EINVAL;
 	}
 
+	if (clipcount > 2048) {
+		dprintk(1,
+			KERN_ERR
+			"%s: %s - invalid clipcount\n",
+			 ZR_DEVNAME(zr), __func__);
+		return -EINVAL;
+	}
+
 	/*
 	 * The video front end needs 4-byte alinged line sizes, we correct that
 	 * silently here if necessary
@@ -1218,7 +1232,7 @@ static int setup_window(struct zoran_fh *fh, int x, int y, int width, int height
 				   (width * height + 7) / 8)) {
 			return -EFAULT;
 		}
-	} else if (clipcount > 0) {
+	} else if (clipcount) {
 		/* write our own bitmap from the clips */
 		vcp = vmalloc(sizeof(struct v4l2_clip) * (clipcount + 4));
 		if (vcp == NULL) {
diff --git a/drivers/media/video/zr364xx.c b/drivers/media/video/zr364xx.c
index cd2e39fc4bf0..e44cb330bbc8 100644
--- a/drivers/media/video/zr364xx.c
+++ b/drivers/media/video/zr364xx.c
@@ -507,14 +507,12 @@ static void zr364xx_fillbuff(struct zr364xx_camera *cam,
 	const char *tmpbuf;
 	char *vbuf = videobuf_to_vmalloc(&buf->vb);
 	unsigned long last_frame;
-	struct zr364xx_framei *frm;
 
 	if (!vbuf)
 		return;
 
 	last_frame = cam->last_frame;
 	if (last_frame != -1) {
-		frm = &cam->buffer.frame[last_frame];
 		tmpbuf = (const char *)cam->buffer.frame[last_frame].lpvbits;
 		switch (buf->fmt->fourcc) {
 		case V4L2_PIX_FMT_JPEG:
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 1adb0245b9dd..0741aded9eb0 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -1082,7 +1082,7 @@ static int gfar_probe(struct platform_device *ofdev)
 
 	if (dev->features & NETIF_F_IP_CSUM ||
 			priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER)
-		dev->hard_header_len += GMAC_FCB_LEN;
+		dev->needed_headroom = GMAC_FCB_LEN;
 
 	/* Program the isrg regs only if number of grps > 1 */
 	if (priv->num_grps > 1) {
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index 28a65d3a03d0..b869a358ce43 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -693,8 +693,8 @@ ath6kl_add_bss_if_needed(struct ath6kl_vif *vif,
 					  ie, 2 + vif->ssid_len + beacon_ie_len,
 					  0, GFP_KERNEL);
 		if (bss)
-			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "added bss %pM to "
-				   "cfg80211\n", bssid);
+			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+				   "added bss %pM to cfg80211\n", bssid);
 		kfree(ie);
 	} else
 		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "cfg80211 already has a bss\n");
@@ -882,6 +882,32 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
 	vif->sme_state = SME_DISCONNECTED;
 }
 
+static int ath6kl_set_probed_ssids(struct ath6kl *ar,
+				   struct ath6kl_vif *vif,
+				   struct cfg80211_ssid *ssids, int n_ssids)
+{
+	u8 i;
+
+	if (n_ssids > MAX_PROBED_SSID_INDEX)
+		return -EINVAL;
+
+	for (i = 0; i < n_ssids; i++) {
+		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
+					  ssids[i].ssid_len ?
+					  SPECIFIC_SSID_FLAG : ANY_SSID_FLAG,
+					  ssids[i].ssid_len,
+					  ssids[i].ssid);
+	}
+
+	/* Make sure no old entries are left behind */
+	for (i = n_ssids; i < MAX_PROBED_SSID_INDEX; i++) {
+		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
+					  DISABLE_SSID_FLAG, 0, NULL);
+	}
+
+	return 0;
+}
+
 static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 				struct cfg80211_scan_request *request)
 {
@@ -899,36 +925,25 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 
 	if (!ar->usr_bss_filter) {
 		clear_bit(CLEAR_BSSFILTER_ON_BEACON, &vif->flags);
-		ret = ath6kl_wmi_bssfilter_cmd(
-			ar->wmi, vif->fw_vif_idx,
-			(test_bit(CONNECTED, &vif->flags) ?
-			 ALL_BUT_BSS_FILTER : ALL_BSS_FILTER), 0);
+		ret = ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx,
+					       ALL_BSS_FILTER, 0);
 		if (ret) {
 			ath6kl_err("couldn't set bss filtering\n");
 			return ret;
 		}
 	}
 
-	if (request->n_ssids && request->ssids[0].ssid_len) {
-		u8 i;
-
-		if (request->n_ssids > (MAX_PROBED_SSID_INDEX - 1))
-			request->n_ssids = MAX_PROBED_SSID_INDEX - 1;
-
-		for (i = 0; i < request->n_ssids; i++)
-			ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx,
-						  i + 1, SPECIFIC_SSID_FLAG,
-						  request->ssids[i].ssid_len,
-						  request->ssids[i].ssid);
-	}
+	ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
+				      request->n_ssids);
+	if (ret < 0)
+		return ret;
 
 	/* this also clears IE in fw if it's not set */
 	ret = ath6kl_wmi_set_appie_cmd(ar->wmi, vif->fw_vif_idx,
 				       WMI_FRAME_PROBE_REQ,
 				       request->ie, request->ie_len);
 	if (ret) {
-		ath6kl_err("failed to set Probe Request appie for "
-			   "scan");
+		ath6kl_err("failed to set Probe Request appie for scan");
 		return ret;
 	}
 
@@ -945,8 +960,7 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 
 		channels = kzalloc(n_channels * sizeof(u16), GFP_KERNEL);
 		if (channels == NULL) {
-			ath6kl_warn("failed to set scan channels, "
-				    "scan all channels");
+			ath6kl_warn("failed to set scan channels, scan all channels");
 			n_channels = 0;
 		}
 
@@ -1018,6 +1032,20 @@ out:
 	vif->scan_req = NULL;
 }
 
+void ath6kl_cfg80211_ch_switch_notify(struct ath6kl_vif *vif, int freq,
+				      enum wmi_phy_mode mode)
+{
+	enum nl80211_channel_type type;
+
+	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+		   "channel switch notify nw_type %d freq %d mode %d\n",
+		   vif->nw_type, freq, mode);
+
+	type = (mode == WMI_11G_HT20) ? NL80211_CHAN_HT20 : NL80211_CHAN_NO_HT;
+
+	cfg80211_ch_switch_notify(vif->ndev, freq, type);
+}
+
 static int ath6kl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 				   u8 key_index, bool pairwise,
 				   const u8 *mac_addr,
@@ -1111,9 +1139,8 @@ static int ath6kl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		ar->ap_mode_bkey.key_len = key->key_len;
 		memcpy(ar->ap_mode_bkey.key, key->key, key->key_len);
 		if (!test_bit(CONNECTED, &vif->flags)) {
-			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delay initial group "
-				   "key configuration until AP mode has been "
-				   "started\n");
+			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+				   "Delay initial group key configuration until AP mode has been started\n");
 			/*
 			 * The key will be set in ath6kl_connect_ap_mode() once
 			 * the connected event is received from the target.
@@ -1129,8 +1156,8 @@ static int ath6kl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		 * the AP mode has properly started
 		 * (ath6kl_install_statioc_wep_keys).
 		 */
-		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delay WEP key configuration "
-			   "until AP mode has been started\n");
+		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+			   "Delay WEP key configuration until AP mode has been started\n");
 		vif->wep_key_list[key_index].key_len = key->key_len;
 		memcpy(vif->wep_key_list[key_index].key, key->key,
 		       key->key_len);
@@ -1962,8 +1989,7 @@ static int ath6kl_wow_sta(struct ath6kl *ar, struct ath6kl_vif *vif)
 				sizeof(discvr_pattern), discvr_offset,
 				discvr_pattern, discvr_mask);
 		if (ret) {
-			ath6kl_err("failed to add WOW mDNS/SSDP/LLMNR "
-				   "pattern\n");
+			ath6kl_err("failed to add WOW mDNS/SSDP/LLMNR pattern\n");
 			return ret;
 		}
 	}
@@ -2031,6 +2057,10 @@ static int ath6kl_wow_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
 	u8 index = 0;
 	__be32 ips[MAX_IP_ADDRS];
 
+	/* The FW currently can't support multi-vif WoW properly. */
+	if (ar->num_vif > 1)
+		return -EIO;
+
 	vif = ath6kl_vif_first(ar);
 	if (!vif)
 		return -EIO;
@@ -2044,6 +2074,13 @@ static int ath6kl_wow_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
 	if (wow && (wow->n_patterns > WOW_MAX_FILTERS_PER_LIST))
 		return -EINVAL;
 
+	if (!test_bit(NETDEV_MCAST_ALL_ON, &vif->flags)) {
+		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
+						vif->fw_vif_idx, false);
+		if (ret)
+			return ret;
+	}
+
 	/* Clear existing WOW patterns */
 	for (i = 0; i < WOW_MAX_FILTERS_PER_LIST; i++)
 		ath6kl_wmi_del_wow_pattern_cmd(ar->wmi, vif->fw_vif_idx,
@@ -2147,8 +2184,8 @@ static int ath6kl_wow_resume(struct ath6kl *ar)
 	ret = ath6kl_wmi_set_host_sleep_mode_cmd(ar->wmi, vif->fw_vif_idx,
 						 ATH6KL_HOST_MODE_AWAKE);
 	if (ret) {
-		ath6kl_warn("Failed to configure host sleep mode for "
-			    "wow resume: %d\n", ret);
+		ath6kl_warn("Failed to configure host sleep mode for wow resume: %d\n",
+			    ret);
 		ar->state = ATH6KL_STATE_WOW;
 		return ret;
 	}
@@ -2172,6 +2209,13 @@ static int ath6kl_wow_resume(struct ath6kl *ar)
 
 	ar->state = ATH6KL_STATE_ON;
 
+	if (!test_bit(NETDEV_MCAST_ALL_OFF, &vif->flags)) {
+		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
+					vif->fw_vif_idx, true);
+		if (ret)
+			return ret;
+	}
+
 	netif_wake_queue(vif->ndev);
 
 	return 0;
@@ -2186,8 +2230,10 @@ static int ath6kl_cfg80211_deepsleep_suspend(struct ath6kl *ar)
 	if (!vif)
 		return -EIO;
 
-	if (!ath6kl_cfg80211_ready(vif))
+	if (!test_bit(WMI_READY, &ar->flag)) {
+		ath6kl_err("deepsleep failed as wmi is not ready\n");
 		return -EIO;
+	}
 
 	ath6kl_cfg80211_stop_all(ar);
 
@@ -2447,6 +2493,24 @@ static int ath6kl_set_htcap(struct ath6kl_vif *vif, enum ieee80211_band band,
 					band, htcap);
 }
 
+static int ath6kl_restore_htcap(struct ath6kl_vif *vif)
+{
+	struct wiphy *wiphy = vif->ar->wiphy;
+	int band, ret = 0;
+
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		if (!wiphy->bands[band])
+			continue;
+
+		ret = ath6kl_set_htcap(vif, band,
+				wiphy->bands[band]->ht_cap.ht_supported);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
 static bool ath6kl_is_p2p_ie(const u8 *pos)
 {
 	return pos[0] == WLAN_EID_VENDOR_SPECIFIC && pos[1] >= 4 &&
@@ -2568,28 +2632,34 @@ static int ath6kl_get_rsn_capab(struct cfg80211_beacon_data *beacon,
 	/* skip element id and length */
 	rsn_ie += 2;
 
-	/* skip version, group cipher */
-	if (rsn_ie_len < 6)
+	/* skip version */
+	if (rsn_ie_len < 2)
 		return -EINVAL;
-	rsn_ie +=  6;
-	rsn_ie_len -= 6;
+	rsn_ie +=  2;
+	rsn_ie_len -= 2;
+
+	/* skip group cipher suite */
+	if (rsn_ie_len < 4)
+		return 0;
+	rsn_ie +=  4;
+	rsn_ie_len -= 4;
 
 	/* skip pairwise cipher suite */
 	if (rsn_ie_len < 2)
-		return -EINVAL;
-	cnt = *((u16 *) rsn_ie);
+		return 0;
+	cnt = get_unaligned_le16(rsn_ie);
 	rsn_ie += (2 + cnt * 4);
 	rsn_ie_len -= (2 + cnt * 4);
 
 	/* skip akm suite */
 	if (rsn_ie_len < 2)
-		return -EINVAL;
-	cnt = *((u16 *) rsn_ie);
+		return 0;
+	cnt = get_unaligned_le16(rsn_ie);
 	rsn_ie += (2 + cnt * 4);
 	rsn_ie_len -= (2 + cnt * 4);
 
 	if (rsn_ie_len < 2)
-		return -EINVAL;
+		return 0;
 
 	memcpy(rsn_capab, rsn_ie, 2);
 
@@ -2766,6 +2836,7 @@ static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 			return res;
 	}
 
+	memcpy(&vif->profile, &p, sizeof(p));
 	res = ath6kl_wmi_ap_profile_commit(ar->wmi, vif->fw_vif_idx, &p);
 	if (res < 0)
 		return res;
@@ -2801,13 +2872,7 @@ static int ath6kl_stop_ap(struct wiphy *wiphy, struct net_device *dev)
 	clear_bit(CONNECTED, &vif->flags);
 
 	/* Restore ht setting in firmware */
-	if (ath6kl_set_htcap(vif, IEEE80211_BAND_2GHZ, true))
-		return -EIO;
-
-	if (ath6kl_set_htcap(vif, IEEE80211_BAND_5GHZ, true))
-		return -EIO;
-
-	return 0;
+	return ath6kl_restore_htcap(vif);
 }
 
 static const u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
@@ -3081,7 +3146,6 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 	struct ath6kl_vif *vif = netdev_priv(dev);
 	u16 interval;
 	int ret;
-	u8 i;
 
 	if (ar->state != ATH6KL_STATE_ON)
 		return -EIO;
@@ -3089,29 +3153,23 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 	if (vif->sme_state != SME_DISCONNECTED)
 		return -EBUSY;
 
+	/* The FW currently can't support multi-vif WoW properly. */
+	if (ar->num_vif > 1)
+		return -EIO;
+
 	ath6kl_cfg80211_scan_complete_event(vif, true);
 
-	for (i = 0; i < ar->wiphy->max_sched_scan_ssids; i++) {
-		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx,
-					  i, DISABLE_SSID_FLAG,
-					  0, NULL);
-	}
+	ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
+				      request->n_ssids);
+	if (ret < 0)
+		return ret;
 
 	/* fw uses seconds, also make sure that it's >0 */
 	interval = max_t(u16, 1, request->interval / 1000);
 
 	ath6kl_wmi_scanparams_cmd(ar->wmi, vif->fw_vif_idx,
 				  interval, interval,
-				  10, 0, 0, 0, 3, 0, 0, 0);
-
-	if (request->n_ssids && request->ssids[0].ssid_len) {
-		for (i = 0; i < request->n_ssids; i++) {
-			ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx,
-						  i, SPECIFIC_SSID_FLAG,
-						  request->ssids[i].ssid_len,
-						  request->ssids[i].ssid);
-		}
-	}
+				  vif->bg_scan_period, 0, 0, 0, 3, 0, 0, 0);
 
 	ret = ath6kl_wmi_set_wow_mode_cmd(ar->wmi, vif->fw_vif_idx,
 					  ATH6KL_WOW_MODE_ENABLE,
@@ -3271,8 +3329,7 @@ void ath6kl_cfg80211_stop_all(struct ath6kl *ar)
 		ar->wmi->saved_pwr_mode = ar->wmi->pwr_mode;
 
 		if (ath6kl_wmi_powermode_cmd(ar->wmi, 0, REC_POWER) != 0)
-			ath6kl_warn("ath6kl_deep_sleep_enable: "
-				    "wmi_powermode_cmd failed\n");
+			ath6kl_warn("ath6kl_deep_sleep_enable: wmi_powermode_cmd failed\n");
 		return;
 	}
 
@@ -3352,6 +3409,7 @@ struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
 	vif->next_mode = nw_type;
 	vif->listen_intvl_t = ATH6KL_DEFAULT_LISTEN_INTVAL;
 	vif->bmiss_time_t = ATH6KL_DEFAULT_BMISS_TIME;
+	vif->bg_scan_period = 0;
 	vif->htcap.ht_enable = true;
 
 	memcpy(ndev->dev_addr, ar->mac_addr, ETH_ALEN);
@@ -3393,6 +3451,7 @@ err:
 int ath6kl_cfg80211_init(struct ath6kl *ar)
 {
 	struct wiphy *wiphy = ar->wiphy;
+	bool band_2gig = false, band_5gig = false, ht = false;
 	int ret;
 
 	wiphy->mgmt_stypes = ath6kl_mgmt_stypes;
@@ -3413,8 +3472,46 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	/* max num of ssids that can be probed during scanning */
 	wiphy->max_scan_ssids = MAX_PROBED_SSID_INDEX;
 	wiphy->max_scan_ie_len = 1000; /* FIX: what is correct limit? */
-	wiphy->bands[IEEE80211_BAND_2GHZ] = &ath6kl_band_2ghz;
-	wiphy->bands[IEEE80211_BAND_5GHZ] = &ath6kl_band_5ghz;
+	switch (ar->hw.cap) {
+	case WMI_11AN_CAP:
+		ht = true;
+	case WMI_11A_CAP:
+		band_5gig = true;
+		break;
+	case WMI_11GN_CAP:
+		ht = true;
+	case WMI_11G_CAP:
+		band_2gig = true;
+		break;
+	case WMI_11AGN_CAP:
+		ht = true;
+	case WMI_11AG_CAP:
+		band_2gig = true;
+		band_5gig = true;
+		break;
+	default:
+		ath6kl_err("invalid phy capability!\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Even if the fw has HT support, advertise HT cap only when
+	 * the firmware has support to override RSN capability, otherwise
+	 * 4-way handshake would fail.
+	 */
+	if (!(ht &&
+	      test_bit(ATH6KL_FW_CAPABILITY_RSN_CAP_OVERRIDE,
+		       ar->fw_capabilities))) {
+		ath6kl_band_2ghz.ht_cap.cap = 0;
+		ath6kl_band_2ghz.ht_cap.ht_supported = false;
+		ath6kl_band_5ghz.ht_cap.cap = 0;
+		ath6kl_band_5ghz.ht_cap.ht_supported = false;
+	}
+	if (band_2gig)
+		wiphy->bands[IEEE80211_BAND_2GHZ] = &ath6kl_band_2ghz;
+	if (band_5gig)
+		wiphy->bands[IEEE80211_BAND_5GHZ] = &ath6kl_band_5ghz;
+
 	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
 
 	wiphy->cipher_suites = cipher_suites;
@@ -3430,7 +3527,7 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	wiphy->wowlan.pattern_min_len = 1;
 	wiphy->wowlan.pattern_max_len = WOW_PATTERN_SIZE;
 
-	wiphy->max_sched_scan_ssids = 10;
+	wiphy->max_sched_scan_ssids = MAX_PROBED_SSID_INDEX;
 
 	ar->wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM |
 			    WIPHY_FLAG_HAVE_AP_SME |
@@ -3447,8 +3544,7 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	ar->wiphy->probe_resp_offload =
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS |
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2 |
-		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P |
-		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_80211U;
+		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P;
 
 	ret = wiphy_register(wiphy);
 	if (ret < 0) {
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.h b/drivers/net/wireless/ath/ath6kl/cfg80211.h
index c5def436417f..5ea8cbb79f43 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.h
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.h
@@ -28,6 +28,8 @@ enum ath6kl_cfg_suspend_mode {
 struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
 					enum nl80211_iftype type,
 					u8 fw_vif_idx, u8 nw_type);
+void ath6kl_cfg80211_ch_switch_notify(struct ath6kl_vif *vif, int freq,
+				      enum wmi_phy_mode mode);
 void ath6kl_cfg80211_scan_complete_event(struct ath6kl_vif *vif, bool aborted);
 
 void ath6kl_cfg80211_connect_event(struct ath6kl_vif *vif, u16 channel,
diff --git a/drivers/net/wireless/ath/ath6kl/core.h b/drivers/net/wireless/ath/ath6kl/core.h
index 9d67964a51dd..4d9c6f142698 100644
--- a/drivers/net/wireless/ath/ath6kl/core.h
+++ b/drivers/net/wireless/ath/ath6kl/core.h
@@ -126,9 +126,9 @@ struct ath6kl_fw_ie {
 #define AR6003_HW_2_0_FIRMWARE_FILE		"athwlan.bin.z77"
 #define AR6003_HW_2_0_TCMD_FIRMWARE_FILE	"athtcmd_ram.bin"
 #define AR6003_HW_2_0_PATCH_FILE		"data.patch.bin"
-#define AR6003_HW_2_0_BOARD_DATA_FILE "ath6k/AR6003/hw2.0/bdata.bin"
+#define AR6003_HW_2_0_BOARD_DATA_FILE AR6003_HW_2_0_FW_DIR "/bdata.bin"
 #define AR6003_HW_2_0_DEFAULT_BOARD_DATA_FILE \
-			"ath6k/AR6003/hw2.0/bdata.SD31.bin"
+			AR6003_HW_2_0_FW_DIR "/bdata.SD31.bin"
 
 /* AR6003 3.0 definitions */
 #define AR6003_HW_2_1_1_VERSION                 0x30000582
@@ -139,25 +139,33 @@ struct ath6kl_fw_ie {
 #define AR6003_HW_2_1_1_UTF_FIRMWARE_FILE	"utf.bin"
 #define AR6003_HW_2_1_1_TESTSCRIPT_FILE	"nullTestFlow.bin"
 #define AR6003_HW_2_1_1_PATCH_FILE		"data.patch.bin"
-#define AR6003_HW_2_1_1_BOARD_DATA_FILE "ath6k/AR6003/hw2.1.1/bdata.bin"
+#define AR6003_HW_2_1_1_BOARD_DATA_FILE AR6003_HW_2_1_1_FW_DIR "/bdata.bin"
 #define AR6003_HW_2_1_1_DEFAULT_BOARD_DATA_FILE	\
-			"ath6k/AR6003/hw2.1.1/bdata.SD31.bin"
+			AR6003_HW_2_1_1_FW_DIR "/bdata.SD31.bin"
 
 /* AR6004 1.0 definitions */
 #define AR6004_HW_1_0_VERSION                 0x30000623
 #define AR6004_HW_1_0_FW_DIR			"ath6k/AR6004/hw1.0"
 #define AR6004_HW_1_0_FIRMWARE_FILE		"fw.ram.bin"
-#define AR6004_HW_1_0_BOARD_DATA_FILE         "ath6k/AR6004/hw1.0/bdata.bin"
+#define AR6004_HW_1_0_BOARD_DATA_FILE         AR6004_HW_1_0_FW_DIR "/bdata.bin"
 #define AR6004_HW_1_0_DEFAULT_BOARD_DATA_FILE \
-	"ath6k/AR6004/hw1.0/bdata.DB132.bin"
+	AR6004_HW_1_0_FW_DIR "/bdata.DB132.bin"
 
 /* AR6004 1.1 definitions */
 #define AR6004_HW_1_1_VERSION                 0x30000001
 #define AR6004_HW_1_1_FW_DIR			"ath6k/AR6004/hw1.1"
 #define AR6004_HW_1_1_FIRMWARE_FILE		"fw.ram.bin"
-#define AR6004_HW_1_1_BOARD_DATA_FILE         "ath6k/AR6004/hw1.1/bdata.bin"
+#define AR6004_HW_1_1_BOARD_DATA_FILE         AR6004_HW_1_1_FW_DIR "/bdata.bin"
 #define AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE \
-	"ath6k/AR6004/hw1.1/bdata.DB132.bin"
+	AR6004_HW_1_1_FW_DIR "/bdata.DB132.bin"
+
+/* AR6004 1.2 definitions */
+#define AR6004_HW_1_2_VERSION                 0x300007e8
+#define AR6004_HW_1_2_FW_DIR			"ath6k/AR6004/hw1.2"
+#define AR6004_HW_1_2_FIRMWARE_FILE           "fw.ram.bin"
+#define AR6004_HW_1_2_BOARD_DATA_FILE         AR6004_HW_1_2_FW_DIR "/bdata.bin"
+#define AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE \
+	AR6004_HW_1_2_FW_DIR "/bdata.bin"
 
 /* Per STA data, used in AP mode */
 #define STA_PS_AWAKE		BIT(0)
@@ -502,6 +510,8 @@ enum ath6kl_vif_state {
 	WLAN_ENABLED,
 	STATS_UPDATE_PEND,
 	HOST_SLEEP_MODE_CMD_PROCESSED,
+	NETDEV_MCAST_ALL_ON,
+	NETDEV_MCAST_ALL_OFF,
 };
 
 struct ath6kl_vif {
@@ -549,9 +559,11 @@ struct ath6kl_vif {
 	u16 assoc_bss_beacon_int;
 	u16 listen_intvl_t;
 	u16 bmiss_time_t;
+	u16 bg_scan_period;
 	u8 assoc_bss_dtim_period;
 	struct net_device_stats net_stats;
 	struct target_stats target_stats;
+	struct wmi_connect_cmd profile;
 
 	struct list_head mc_filter;
 };
@@ -640,6 +652,7 @@ struct ath6kl {
 	u8 sta_list_index;
 	struct ath6kl_req_key ap_mode_bkey;
 	struct sk_buff_head mcastpsq;
+	u32 want_ch_switch;
 
 	/*
 	 * FIXME: protects access to mcastpsq but is actually useless as
@@ -672,6 +685,7 @@ struct ath6kl {
 		u32 refclk_hz;
 		u32 uarttx_pin;
 		u32 testscript_addr;
+		enum wmi_phy_cap cap;
 
 		struct ath6kl_hw_fw {
 			const char *dir;
@@ -805,7 +819,8 @@ void aggr_reset_state(struct aggr_info_conn *aggr_conn);
 struct ath6kl_sta *ath6kl_find_sta(struct ath6kl_vif *vif, u8 *node_addr);
 struct ath6kl_sta *ath6kl_find_sta_by_aid(struct ath6kl *ar, u8 aid);
 
-void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver);
+void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver,
+			enum wmi_phy_cap cap);
 int ath6kl_control_tx(void *devt, struct sk_buff *skb,
 		      enum htc_endpoint_id eid);
 void ath6kl_connect_event(struct ath6kl_vif *vif, u16 channel,
diff --git a/drivers/net/wireless/ath/ath6kl/debug.c b/drivers/net/wireless/ath/ath6kl/debug.c
index 1b76aff78508..15cfe30e54fd 100644
--- a/drivers/net/wireless/ath/ath6kl/debug.c
+++ b/drivers/net/wireless/ath/ath6kl/debug.c
@@ -401,8 +401,10 @@ static ssize_t ath6kl_fwlog_block_read(struct file *file,
 
 		ret = wait_for_completion_interruptible(
 			&ar->debug.fwlog_completion);
-		if (ret == -ERESTARTSYS)
+		if (ret == -ERESTARTSYS) {
+			vfree(buf);
 			return ret;
+		}
 
 		spin_lock(&ar->debug.fwlog_queue.lock);
 	}
@@ -1570,10 +1572,15 @@ static ssize_t ath6kl_bgscan_int_write(struct file *file,
 				size_t count, loff_t *ppos)
 {
 	struct ath6kl *ar = file->private_data;
+	struct ath6kl_vif *vif;
 	u16 bgscan_int;
 	char buf[32];
 	ssize_t len;
 
+	vif = ath6kl_vif_first(ar);
+	if (!vif)
+		return -EIO;
+
 	len = min(count, sizeof(buf) - 1);
 	if (copy_from_user(buf, user_buf, len))
 		return -EFAULT;
@@ -1585,6 +1592,8 @@ static ssize_t ath6kl_bgscan_int_write(struct file *file,
 	if (bgscan_int == 0)
 		bgscan_int = 0xffff;
 
+	vif->bg_scan_period = bgscan_int;
+
 	ath6kl_wmi_scanparams_cmd(ar->wmi, 0, 0, 0, bgscan_int, 0, 0, 0, 3,
 				  0, 0, 0);
 
@@ -1809,6 +1818,7 @@ int ath6kl_debug_init_fs(struct ath6kl *ar)
 void ath6kl_debug_cleanup(struct ath6kl *ar)
 {
 	skb_queue_purge(&ar->debug.fwlog_queue);
+	complete(&ar->debug.fwlog_completion);
 	kfree(ar->debug.roam_tbl);
 }
 
diff --git a/drivers/net/wireless/ath/ath6kl/htc_mbox.c b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
index 065e61516d7a..2798624d3a9d 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_mbox.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
@@ -83,10 +83,7 @@ static void ath6kl_credit_init(struct ath6kl_htc_credit_info *cred_info,
 			 * never goes inactive EVER.
 			 */
 			cur_ep_dist->dist_flags |= HTC_EP_ACTIVE;
-		} else if (cur_ep_dist->svc_id == WMI_DATA_BK_SVC)
-			/* this is the lowest priority data endpoint */
-			/* FIXME: this looks fishy, check */
-			cred_info->lowestpri_ep_dist = cur_ep_dist->list;
+		}
 
 		/*
 		 * Streams have to be created (explicit | implicit) for all
@@ -100,6 +97,13 @@ static void ath6kl_credit_init(struct ath6kl_htc_credit_info *cred_info,
 		 */
 	}
 
+	/*
+	 * For ath6kl_credit_seek function,
+	 * it use list_for_each_entry_reverse to walk around the whole ep list.
+	 * Therefore assign this lowestpri_ep_dist after walk around the ep_list
+	 */
+	cred_info->lowestpri_ep_dist = cur_ep_dist->list;
+
 	WARN_ON(cred_info->cur_free_credits <= 0);
 
 	list_for_each_entry(cur_ep_dist, ep_list, list) {
@@ -758,7 +762,7 @@ static void ath6kl_htc_tx_bundle(struct htc_endpoint *endpoint,
 	u32 txb_mask;
 	u8 ac = WMM_NUM_AC;
 
-	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) ||
+	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) &&
 	    (WMI_CONTROL_SVC != endpoint->svc_id))
 		ac = target->dev->ar->ep2ac_map[endpoint->eid];
 
@@ -793,16 +797,17 @@ static void ath6kl_htc_tx_bundle(struct htc_endpoint *endpoint,
 				 * itself
 				 */
 				txb_mask = ((1 << ac) - 1);
-		/*
-		 * when the scatter request resources drop below a
-		 * certain threshold, disable Tx bundling for all
-		 * AC's with priority lower than the current requesting
-		 * AC. Otherwise re-enable Tx bundling for them
-		 */
-		if (scat_req->scat_q_depth < ATH6KL_SCATTER_REQS)
-			target->tx_bndl_mask &= ~txb_mask;
-		else
-			target->tx_bndl_mask |= txb_mask;
+
+			/*
+			 * when the scatter request resources drop below a
+			 * certain threshold, disable Tx bundling for all
+			 * AC's with priority lower than the current requesting
+			 * AC. Otherwise re-enable Tx bundling for them
+			 */
+			if (scat_req->scat_q_depth < ATH6KL_SCATTER_REQS)
+				target->tx_bndl_mask &= ~txb_mask;
+			else
+				target->tx_bndl_mask |= txb_mask;
 		}
 
 		ath6kl_dbg(ATH6KL_DBG_HTC, "htc tx pkts to scatter: %d\n",
@@ -849,6 +854,7 @@ static void ath6kl_htc_tx_from_queue(struct htc_target *target,
 	int bundle_sent;
 	int n_pkts_bundle;
 	u8 ac = WMM_NUM_AC;
+	int status;
 
 	spin_lock_bh(&target->tx_lock);
 
@@ -866,7 +872,7 @@ static void ath6kl_htc_tx_from_queue(struct htc_target *target,
 	 */
 	INIT_LIST_HEAD(&txq);
 
-	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) ||
+	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) &&
 	    (WMI_CONTROL_SVC != endpoint->svc_id))
 		ac = target->dev->ar->ep2ac_map[endpoint->eid];
 
@@ -910,7 +916,12 @@ static void ath6kl_htc_tx_from_queue(struct htc_target *target,
 
 			ath6kl_htc_tx_prep_pkt(packet, packet->info.tx.flags,
 					       0, packet->info.tx.seqno);
-			ath6kl_htc_tx_issue(target, packet);
+			status = ath6kl_htc_tx_issue(target, packet);
+
+			if (status) {
+				packet->status = status;
+				packet->completion(packet->context, packet);
+			}
 		}
 
 		spin_lock_bh(&target->tx_lock);
diff --git a/drivers/net/wireless/ath/ath6kl/htc_pipe.c b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
index b277b3446882..f9626c723693 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_pipe.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
@@ -108,8 +108,6 @@ static void get_htc_packet_credit_based(struct htc_target *target,
 
 		/* get packet at head, but don't remove it */
 		packet = list_first_entry(&ep->txq, struct htc_packet, list);
-		if (packet == NULL)
-			break;
 
 		ath6kl_dbg(ATH6KL_DBG_HTC,
 			   "%s: got head packet:0x%p , queue depth: %d\n",
@@ -803,8 +801,6 @@ static int htc_send_packets_multiple(struct htc_target *target,
 
 	/* get first packet to find out which ep the packets will go into */
 	packet = list_first_entry(pkt_queue, struct htc_packet, list);
-	if (packet == NULL)
-		return -EINVAL;
 
 	if (packet->endpoint >= ENDPOINT_MAX) {
 		WARN_ON_ONCE(1);
@@ -1382,6 +1378,9 @@ static int ath6kl_htc_pipe_conn_service(struct htc_target *target,
 	/* copy all the callbacks */
 	ep->ep_cb = conn_req->ep_cb;
 
+	/* initialize tx_drop_packet_threshold */
+	ep->tx_drop_packet_threshold = MAX_HI_COOKIE_NUM;
+
 	status = ath6kl_hif_pipe_map_service(ar, ep->svc_id,
 					     &ep->pipe.pipeid_ul,
 					     &ep->pipe.pipeid_dl);
@@ -1636,10 +1635,6 @@ static int ath6kl_htc_pipe_add_rxbuf_multiple(struct htc_target *target,
 		return -EINVAL;
 
 	first = list_first_entry(pkt_queue, struct htc_packet, list);
-	if (first == NULL) {
-		WARN_ON_ONCE(1);
-		return -EINVAL;
-	}
 
 	if (first->endpoint >= ENDPOINT_MAX) {
 		WARN_ON_ONCE(1);
diff --git a/drivers/net/wireless/ath/ath6kl/init.c b/drivers/net/wireless/ath/ath6kl/init.c
index 29ef50ea07d5..7eb0515f458a 100644
--- a/drivers/net/wireless/ath/ath6kl/init.c
+++ b/drivers/net/wireless/ath/ath6kl/init.c
@@ -119,6 +119,24 @@ static const struct ath6kl_hw hw_list[] = {
 		.fw_board		= AR6004_HW_1_1_BOARD_DATA_FILE,
 		.fw_default_board	= AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE,
 	},
+	{
+		.id				= AR6004_HW_1_2_VERSION,
+		.name				= "ar6004 hw 1.2",
+		.dataset_patch_addr		= 0x436ecc,
+		.app_load_addr			= 0x1234,
+		.board_ext_data_addr		= 0x437000,
+		.reserved_ram_size		= 9216,
+		.board_addr			= 0x435c00,
+		.refclk_hz			= 40000000,
+		.uarttx_pin			= 11,
+
+		.fw = {
+			.dir		= AR6004_HW_1_2_FW_DIR,
+			.fw		= AR6004_HW_1_2_FIRMWARE_FILE,
+		},
+		.fw_board		= AR6004_HW_1_2_BOARD_DATA_FILE,
+		.fw_default_board	= AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE,
+	},
 };
 
 /*
@@ -445,9 +463,9 @@ static int ath6kl_target_config_wlan_params(struct ath6kl *ar, int idx)
 					      P2P_FLAG_MACADDR_REQ |
 					      P2P_FLAG_HMODEL_REQ);
 		if (ret) {
-			ath6kl_dbg(ATH6KL_DBG_TRC, "failed to request P2P "
-				   "capabilities (%d) - assuming P2P not "
-				   "supported\n", ret);
+			ath6kl_dbg(ATH6KL_DBG_TRC,
+				   "failed to request P2P capabilities (%d) - assuming P2P not supported\n",
+				   ret);
 			ar->p2p = false;
 		}
 	}
@@ -456,8 +474,9 @@ static int ath6kl_target_config_wlan_params(struct ath6kl *ar, int idx)
 		/* Enable Probe Request reporting for P2P */
 		ret = ath6kl_wmi_probe_report_req_cmd(ar->wmi, idx, true);
 		if (ret) {
-			ath6kl_dbg(ATH6KL_DBG_TRC, "failed to enable Probe "
-				   "Request reporting (%d)\n", ret);
+			ath6kl_dbg(ATH6KL_DBG_TRC,
+				   "failed to enable Probe Request reporting (%d)\n",
+				   ret);
 		}
 	}
 
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index 4d818f96c415..e5524470529c 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -421,8 +421,8 @@ void ath6kl_connect_ap_mode_bss(struct ath6kl_vif *vif, u16 channel)
 		if (!ik->valid)
 			break;
 
-		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delayed addkey for "
-			   "the initial group key for AP mode\n");
+		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+			   "Delayed addkey for the initial group key for AP mode\n");
 		memset(key_rsc, 0, sizeof(key_rsc));
 		res = ath6kl_wmi_addkey_cmd(
 			ar->wmi, vif->fw_vif_idx, ik->key_index, ik->key_type,
@@ -430,12 +430,19 @@ void ath6kl_connect_ap_mode_bss(struct ath6kl_vif *vif, u16 channel)
 			ik->key,
 			KEY_OP_INIT_VAL, NULL, SYNC_BOTH_WMIFLAG);
 		if (res) {
-			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delayed "
-				   "addkey failed: %d\n", res);
+			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+				   "Delayed addkey failed: %d\n", res);
 		}
 		break;
 	}
 
+	if (ar->want_ch_switch & (1 << vif->fw_vif_idx)) {
+		ar->want_ch_switch &= ~(1 << vif->fw_vif_idx);
+		/* we actually don't know the phymode, default to HT20 */
+		ath6kl_cfg80211_ch_switch_notify(vif, channel,
+						 WMI_11G_HT20);
+	}
+
 	ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx, NONE_BSS_FILTER, 0);
 	set_bit(CONNECTED, &vif->flags);
 	netif_carrier_on(vif->ndev);
@@ -541,7 +548,8 @@ void ath6kl_disconnect(struct ath6kl_vif *vif)
 
 /* WMI Event handlers */
 
-void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver)
+void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver,
+			enum wmi_phy_cap cap)
 {
 	struct ath6kl *ar = devt;
 
@@ -551,6 +559,7 @@ void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver)
 
 	ar->version.wlan_ver = sw_ver;
 	ar->version.abi_ver = abi_ver;
+	ar->hw.cap = cap;
 
 	snprintf(ar->wiphy->fw_version,
 		 sizeof(ar->wiphy->fw_version),
@@ -584,6 +593,45 @@ void ath6kl_scan_complete_evt(struct ath6kl_vif *vif, int status)
 	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "scan complete: %d\n", status);
 }
 
+static int ath6kl_commit_ch_switch(struct ath6kl_vif *vif, u16 channel)
+{
+
+	struct ath6kl *ar = vif->ar;
+
+	vif->next_chan = channel;
+	vif->profile.ch = cpu_to_le16(channel);
+
+	switch (vif->nw_type) {
+	case AP_NETWORK:
+		return ath6kl_wmi_ap_profile_commit(ar->wmi, vif->fw_vif_idx,
+						    &vif->profile);
+	default:
+		ath6kl_err("won't switch channels nw_type=%d\n", vif->nw_type);
+		return -ENOTSUPP;
+	}
+}
+
+static void ath6kl_check_ch_switch(struct ath6kl *ar, u16 channel)
+{
+
+	struct ath6kl_vif *vif;
+	int res = 0;
+
+	if (!ar->want_ch_switch)
+		return;
+
+	spin_lock_bh(&ar->list_lock);
+	list_for_each_entry(vif, &ar->vif_list, list) {
+		if (ar->want_ch_switch & (1 << vif->fw_vif_idx))
+			res = ath6kl_commit_ch_switch(vif, channel);
+
+		if (res)
+			ath6kl_err("channel switch failed nw_type %d res %d\n",
+				   vif->nw_type, res);
+	}
+	spin_unlock_bh(&ar->list_lock);
+}
+
 void ath6kl_connect_event(struct ath6kl_vif *vif, u16 channel, u8 *bssid,
 			  u16 listen_int, u16 beacon_int,
 			  enum network_type net_type, u8 beacon_ie_len,
@@ -601,9 +649,11 @@ void ath6kl_connect_event(struct ath6kl_vif *vif, u16 channel, u8 *bssid,
 	memcpy(vif->bssid, bssid, sizeof(vif->bssid));
 	vif->bss_ch = channel;
 
-	if ((vif->nw_type == INFRA_NETWORK))
+	if ((vif->nw_type == INFRA_NETWORK)) {
 		ath6kl_wmi_listeninterval_cmd(ar->wmi, vif->fw_vif_idx,
 					      vif->listen_intvl_t, 0);
+		ath6kl_check_ch_switch(ar, channel);
+	}
 
 	netif_wake_queue(vif->ndev);
 
@@ -926,6 +976,11 @@ void ath6kl_disconnect_event(struct ath6kl_vif *vif, u8 reason, u8 *bssid,
 	struct ath6kl *ar = vif->ar;
 
 	if (vif->nw_type == AP_NETWORK) {
+		/* disconnect due to other STA vif switching channels */
+		if (reason == BSS_DISCONNECTED &&
+		    prot_reason_status == WMI_AP_REASON_STA_ROAM)
+			ar->want_ch_switch |= 1 << vif->fw_vif_idx;
+
 		if (!ath6kl_remove_sta(ar, bssid, prot_reason_status))
 			return;
 
@@ -1090,7 +1145,7 @@ static int ath6kl_set_features(struct net_device *dev,
 static void ath6kl_set_multicast_list(struct net_device *ndev)
 {
 	struct ath6kl_vif *vif = netdev_priv(ndev);
-	bool mc_all_on = false, mc_all_off = false;
+	bool mc_all_on = false;
 	int mc_count = netdev_mc_count(ndev);
 	struct netdev_hw_addr *ha;
 	bool found;
@@ -1102,24 +1157,41 @@ static void ath6kl_set_multicast_list(struct net_device *ndev)
 	    !test_bit(WLAN_ENABLED, &vif->flags))
 		return;
 
+	/* Enable multicast-all filter. */
 	mc_all_on = !!(ndev->flags & IFF_PROMISC) ||
 		    !!(ndev->flags & IFF_ALLMULTI) ||
 		    !!(mc_count > ATH6K_MAX_MC_FILTERS_PER_LIST);
 
-	mc_all_off = !(ndev->flags & IFF_MULTICAST) || mc_count == 0;
+	if (mc_all_on)
+		set_bit(NETDEV_MCAST_ALL_ON, &vif->flags);
+	else
+		clear_bit(NETDEV_MCAST_ALL_ON, &vif->flags);
+
+	mc_all_on = mc_all_on || (vif->ar->state == ATH6KL_STATE_ON);
 
-	if (mc_all_on || mc_all_off) {
-		/* Enable/disable all multicast */
-		ath6kl_dbg(ATH6KL_DBG_TRC, "%s multicast filter\n",
-			   mc_all_on ? "enabling" : "disabling");
-		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi, vif->fw_vif_idx,
+	if (!(ndev->flags & IFF_MULTICAST)) {
+		mc_all_on = false;
+		set_bit(NETDEV_MCAST_ALL_OFF, &vif->flags);
+	} else {
+		clear_bit(NETDEV_MCAST_ALL_OFF, &vif->flags);
+	}
+
+	/* Enable/disable "multicast-all" filter*/
+	ath6kl_dbg(ATH6KL_DBG_TRC, "%s multicast-all filter\n",
+		   mc_all_on ? "enabling" : "disabling");
+
+	ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi, vif->fw_vif_idx,
 						  mc_all_on);
-		if (ret)
-			ath6kl_warn("Failed to %s multicast receive\n",
-				    mc_all_on ? "enable" : "disable");
+	if (ret) {
+		ath6kl_warn("Failed to %s multicast-all receive\n",
+			    mc_all_on ? "enable" : "disable");
 		return;
 	}
 
+	if (test_bit(NETDEV_MCAST_ALL_ON, &vif->flags))
+		return;
+
+	/* Keep the driver and firmware mcast list in sync. */
 	list_for_each_entry_safe(mc_filter, tmp, &vif->mc_filter, list) {
 		found = false;
 		netdev_for_each_mc_addr(ha, ndev) {
diff --git a/drivers/net/wireless/ath/ath6kl/sdio.c b/drivers/net/wireless/ath/ath6kl/sdio.c
index 44ea7a742101..05b95405f7b5 100644
--- a/drivers/net/wireless/ath/ath6kl/sdio.c
+++ b/drivers/net/wireless/ath/ath6kl/sdio.c
@@ -552,7 +552,7 @@ static int ath6kl_sdio_write_async(struct ath6kl *ar, u32 address, u8 *buffer,
 
 	bus_req = ath6kl_sdio_alloc_busreq(ar_sdio);
 
-	if (!bus_req)
+	if (WARN_ON_ONCE(!bus_req))
 		return -ENOMEM;
 
 	bus_req->address = address;
@@ -915,6 +915,9 @@ static int ath6kl_sdio_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
 	}
 
 cut_pwr:
+	if (func->card && func->card->host)
+		func->card->host->pm_flags &= ~MMC_PM_KEEP_POWER;
+
 	return ath6kl_cfg80211_suspend(ar, ATH6KL_CFG_SUSPEND_CUTPOWER, NULL);
 }
 
@@ -985,9 +988,8 @@ static int ath6kl_set_addrwin_reg(struct ath6kl *ar, u32 reg_addr, u32 addr)
 	}
 
 	if (status) {
-		ath6kl_err("%s: failed to write initial bytes of 0x%x "
-			   "to window reg: 0x%X\n", __func__,
-			   addr, reg_addr);
+		ath6kl_err("%s: failed to write initial bytes of 0x%x to window reg: 0x%X\n",
+			   __func__, addr, reg_addr);
 		return status;
 	}
 
@@ -1076,8 +1078,8 @@ static int ath6kl_sdio_bmi_credits(struct ath6kl *ar)
 					 (u8 *)&ar->bmi.cmd_credits, 4,
 					 HIF_RD_SYNC_BYTE_INC);
 		if (ret) {
-			ath6kl_err("Unable to decrement the command credit "
-						"count register: %d\n", ret);
+			ath6kl_err("Unable to decrement the command credit count register: %d\n",
+				   ret);
 			return ret;
 		}
 
@@ -1457,3 +1459,6 @@ MODULE_FIRMWARE(AR6004_HW_1_0_DEFAULT_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_FW_DIR "/" AR6004_HW_1_1_FIRMWARE_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_FW_DIR "/" AR6004_HW_1_2_FIRMWARE_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE);
diff --git a/drivers/net/wireless/ath/ath6kl/txrx.c b/drivers/net/wireless/ath/ath6kl/txrx.c
index 82f2f5cb475b..67206aedea6c 100644
--- a/drivers/net/wireless/ath/ath6kl/txrx.c
+++ b/drivers/net/wireless/ath/ath6kl/txrx.c
@@ -362,15 +362,11 @@ int ath6kl_data_tx(struct sk_buff *skb, struct net_device *dev)
 		   skb, skb->data, skb->len);
 
 	/* If target is not associated */
-	if (!test_bit(CONNECTED, &vif->flags)) {
-		dev_kfree_skb(skb);
-		return 0;
-	}
+	if (!test_bit(CONNECTED, &vif->flags))
+		goto fail_tx;
 
-	if (WARN_ON_ONCE(ar->state != ATH6KL_STATE_ON)) {
-		dev_kfree_skb(skb);
-		return 0;
-	}
+	if (WARN_ON_ONCE(ar->state != ATH6KL_STATE_ON))
+		goto fail_tx;
 
 	if (!test_bit(WMI_READY, &ar->flag))
 		goto fail_tx;
diff --git a/drivers/net/wireless/ath/ath6kl/usb.c b/drivers/net/wireless/ath/ath6kl/usb.c
index 44a795f14da9..3740c3d6ab88 100644
--- a/drivers/net/wireless/ath/ath6kl/usb.c
+++ b/drivers/net/wireless/ath/ath6kl/usb.c
@@ -1037,6 +1037,14 @@ static void ath6kl_usb_stop(struct ath6kl *ar)
 	hif_stop(ar);
 }
 
+static void ath6kl_usb_cleanup_scatter(struct ath6kl *ar)
+{
+	/*
+	 * USB doesn't support it. Just return.
+	 */
+	return;
+}
+
 static const struct ath6kl_hif_ops ath6kl_usb_ops = {
 	.diag_read32 = ath6kl_usb_diag_read32,
 	.diag_write32 = ath6kl_usb_diag_write32,
@@ -1049,6 +1057,7 @@ static const struct ath6kl_hif_ops ath6kl_usb_ops = {
 	.pipe_get_default = ath6kl_usb_get_default_pipe,
 	.pipe_map_service = ath6kl_usb_map_service_pipe,
 	.pipe_get_free_queue_number = ath6kl_usb_get_free_queue_number,
+	.cleanup_scatter = ath6kl_usb_cleanup_scatter,
 };
 
 /* ath6kl usb driver registered functions */
@@ -1208,3 +1217,6 @@ MODULE_FIRMWARE(AR6004_HW_1_0_DEFAULT_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_FIRMWARE_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_FIRMWARE_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE);
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.c b/drivers/net/wireless/ath/ath6kl/wmi.c
index 7c8a9977faf5..ee8ec2394c2c 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.c
+++ b/drivers/net/wireless/ath/ath6kl/wmi.c
@@ -16,6 +16,7 @@
  */
 
 #include <linux/ip.h>
+#include <linux/in.h>
 #include "core.h"
 #include "debug.h"
 #include "testmode.h"
@@ -289,6 +290,13 @@ int ath6kl_wmi_implicit_create_pstream(struct wmi *wmi, u8 if_idx,
 					layer2_priority);
 		} else
 			usr_pri = layer2_priority & 0x7;
+
+		/*
+		 * Queue the EAPOL frames in the same WMM_AC_VO queue
+		 * as that of management frames.
+		 */
+		if (skb->protocol == cpu_to_be16(ETH_P_PAE))
+			usr_pri = WMI_VOICE_USER_PRIORITY;
 	}
 
 	/*
@@ -460,8 +468,9 @@ static int ath6kl_wmi_remain_on_chnl_event_rx(struct wmi *wmi, u8 *datap,
 		   freq, dur);
 	chan = ieee80211_get_channel(ar->wiphy, freq);
 	if (!chan) {
-		ath6kl_dbg(ATH6KL_DBG_WMI, "remain_on_chnl: Unknown channel "
-			   "(freq=%u)\n", freq);
+		ath6kl_dbg(ATH6KL_DBG_WMI,
+			   "remain_on_chnl: Unknown channel (freq=%u)\n",
+			   freq);
 		return -EINVAL;
 	}
 	id = vif->last_roc_id;
@@ -488,12 +497,14 @@ static int ath6kl_wmi_cancel_remain_on_chnl_event_rx(struct wmi *wmi,
 	ev = (struct wmi_cancel_remain_on_chnl_event *) datap;
 	freq = le32_to_cpu(ev->freq);
 	dur = le32_to_cpu(ev->duration);
-	ath6kl_dbg(ATH6KL_DBG_WMI, "cancel_remain_on_chnl: freq=%u dur=%u "
-		   "status=%u\n", freq, dur, ev->status);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "cancel_remain_on_chnl: freq=%u dur=%u status=%u\n",
+		   freq, dur, ev->status);
 	chan = ieee80211_get_channel(ar->wiphy, freq);
 	if (!chan) {
-		ath6kl_dbg(ATH6KL_DBG_WMI, "cancel_remain_on_chnl: Unknown "
-			   "channel (freq=%u)\n", freq);
+		ath6kl_dbg(ATH6KL_DBG_WMI,
+			   "cancel_remain_on_chnl: Unknown channel (freq=%u)\n",
+			   freq);
 		return -EINVAL;
 	}
 	if (vif->last_cancel_roc_id &&
@@ -548,12 +559,12 @@ static int ath6kl_wmi_rx_probe_req_event_rx(struct wmi *wmi, u8 *datap, int len,
 	freq = le32_to_cpu(ev->freq);
 	dlen = le16_to_cpu(ev->len);
 	if (datap + len < ev->data + dlen) {
-		ath6kl_err("invalid wmi_p2p_rx_probe_req_event: "
-			   "len=%d dlen=%u\n", len, dlen);
+		ath6kl_err("invalid wmi_p2p_rx_probe_req_event: len=%d dlen=%u\n",
+			   len, dlen);
 		return -EINVAL;
 	}
-	ath6kl_dbg(ATH6KL_DBG_WMI, "rx_probe_req: len=%u freq=%u "
-		   "probe_req_report=%d\n",
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "rx_probe_req: len=%u freq=%u probe_req_report=%d\n",
 		   dlen, freq, vif->probe_req_report);
 
 	if (vif->probe_req_report || vif->nw_type == AP_NETWORK)
@@ -592,8 +603,8 @@ static int ath6kl_wmi_rx_action_event_rx(struct wmi *wmi, u8 *datap, int len,
 	freq = le32_to_cpu(ev->freq);
 	dlen = le16_to_cpu(ev->len);
 	if (datap + len < ev->data + dlen) {
-		ath6kl_err("invalid wmi_rx_action_event: "
-			   "len=%d dlen=%u\n", len, dlen);
+		ath6kl_err("invalid wmi_rx_action_event: len=%d dlen=%u\n",
+			   len, dlen);
 		return -EINVAL;
 	}
 	ath6kl_dbg(ATH6KL_DBG_WMI, "rx_action: len=%u freq=%u\n", dlen, freq);
@@ -687,7 +698,7 @@ static int ath6kl_wmi_ready_event_rx(struct wmi *wmi, u8 *datap, int len)
 
 	ath6kl_ready_event(wmi->parent_dev, ev->mac_addr,
 			   le32_to_cpu(ev->sw_version),
-			   le32_to_cpu(ev->abi_version));
+			   le32_to_cpu(ev->abi_version), ev->phy_cap);
 
 	return 0;
 }
@@ -777,16 +788,15 @@ static int ath6kl_wmi_connect_event_rx(struct wmi *wmi, u8 *datap, int len,
 		/* AP mode start/STA connected event */
 		struct net_device *dev = vif->ndev;
 		if (memcmp(dev->dev_addr, ev->u.ap_bss.bssid, ETH_ALEN) == 0) {
-			ath6kl_dbg(ATH6KL_DBG_WMI, "%s: freq %d bssid %pM "
-				   "(AP started)\n",
+			ath6kl_dbg(ATH6KL_DBG_WMI,
+				   "%s: freq %d bssid %pM (AP started)\n",
 				   __func__, le16_to_cpu(ev->u.ap_bss.ch),
 				   ev->u.ap_bss.bssid);
 			ath6kl_connect_ap_mode_bss(
 				vif, le16_to_cpu(ev->u.ap_bss.ch));
 		} else {
-			ath6kl_dbg(ATH6KL_DBG_WMI, "%s: aid %u mac_addr %pM "
-				   "auth=%u keymgmt=%u cipher=%u apsd_info=%u "
-				   "(STA connected)\n",
+			ath6kl_dbg(ATH6KL_DBG_WMI,
+				   "%s: aid %u mac_addr %pM auth=%u keymgmt=%u cipher=%u apsd_info=%u (STA connected)\n",
 				   __func__, ev->u.ap_sta.aid,
 				   ev->u.ap_sta.mac_addr,
 				   ev->u.ap_sta.auth,
@@ -1229,8 +1239,9 @@ static int ath6kl_wmi_neighbor_report_event_rx(struct wmi *wmi, u8 *datap,
 	ev = (struct wmi_neighbor_report_event *) datap;
 	if (sizeof(*ev) + ev->num_neighbors * sizeof(struct wmi_neighbor_info)
 	    > len) {
-		ath6kl_dbg(ATH6KL_DBG_WMI, "truncated neighbor event "
-			   "(num=%d len=%d)\n", ev->num_neighbors, len);
+		ath6kl_dbg(ATH6KL_DBG_WMI,
+			   "truncated neighbor event (num=%d len=%d)\n",
+			   ev->num_neighbors, len);
 		return -EINVAL;
 	}
 	for (i = 0; i < ev->num_neighbors; i++) {
@@ -1814,12 +1825,14 @@ int ath6kl_wmi_beginscan_cmd(struct wmi *wmi, u8 if_idx,
 			     u32 home_dwell_time, u32 force_scan_interval,
 			     s8 num_chan, u16 *ch_list, u32 no_cck, u32 *rates)
 {
+	struct ieee80211_supported_band *sband;
 	struct sk_buff *skb;
 	struct wmi_begin_scan_cmd *sc;
-	s8 size;
+	s8 size, *supp_rates;
 	int i, band, ret;
 	struct ath6kl *ar = wmi->parent_dev;
 	int num_rates;
+	u32 ratemask;
 
 	size = sizeof(struct wmi_begin_scan_cmd);
 
@@ -1846,10 +1859,13 @@ int ath6kl_wmi_beginscan_cmd(struct wmi *wmi, u8 if_idx,
 	sc->num_ch = num_chan;
 
 	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
-		struct ieee80211_supported_band *sband =
-		    ar->wiphy->bands[band];
-		u32 ratemask = rates[band];
-		u8 *supp_rates = sc->supp_rates[band].rates;
+		sband = ar->wiphy->bands[band];
+
+		if (!sband)
+			continue;
+
+		ratemask = rates[band];
+		supp_rates = sc->supp_rates[band].rates;
 		num_rates = 0;
 
 		for (i = 0; i < sband->n_bitrates; i++) {
@@ -2129,8 +2145,8 @@ int ath6kl_wmi_addkey_cmd(struct wmi *wmi, u8 if_idx, u8 key_index,
 	struct wmi_add_cipher_key_cmd *cmd;
 	int ret;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "addkey cmd: key_index=%u key_type=%d "
-		   "key_usage=%d key_len=%d key_op_ctrl=%d\n",
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "addkey cmd: key_index=%u key_type=%d key_usage=%d key_len=%d key_op_ctrl=%d\n",
 		   key_index, key_type, key_usage, key_len, key_op_ctrl);
 
 	if ((key_index > WMI_MAX_KEY_INDEX) || (key_len > WMI_MAX_KEY_LEN) ||
@@ -3047,8 +3063,8 @@ int ath6kl_wmi_ap_profile_commit(struct wmi *wmip, u8 if_idx,
 
 	res = ath6kl_wmi_cmd_send(wmip, if_idx, skb, WMI_AP_CONFIG_COMMIT_CMDID,
 				  NO_SYNC_WMIFLAG);
-	ath6kl_dbg(ATH6KL_DBG_WMI, "%s: nw_type=%u auth_mode=%u ch=%u "
-		   "ctrl_flags=0x%x-> res=%d\n",
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "%s: nw_type=%u auth_mode=%u ch=%u ctrl_flags=0x%x-> res=%d\n",
 		   __func__, p->nw_type, p->auth_mode, le16_to_cpu(p->ch),
 		   le32_to_cpu(p->ctrl_flags), res);
 	return res;
@@ -3208,8 +3224,9 @@ int ath6kl_wmi_set_appie_cmd(struct wmi *wmi, u8 if_idx, u8 mgmt_frm_type,
 	if (!skb)
 		return -ENOMEM;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "set_appie_cmd: mgmt_frm_type=%u "
-		   "ie_len=%u\n", mgmt_frm_type, ie_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "set_appie_cmd: mgmt_frm_type=%u ie_len=%u\n",
+		   mgmt_frm_type, ie_len);
 	p = (struct wmi_set_appie_cmd *) skb->data;
 	p->mgmt_frm_type = mgmt_frm_type;
 	p->ie_len = ie_len;
@@ -3310,8 +3327,9 @@ static int ath6kl_wmi_send_action_cmd(struct wmi *wmi, u8 if_idx, u32 id,
 	wmi->last_mgmt_tx_frame = buf;
 	wmi->last_mgmt_tx_frame_len = data_len;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "send_action_cmd: id=%u freq=%u wait=%u "
-		   "len=%u\n", id, freq, wait, data_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "send_action_cmd: id=%u freq=%u wait=%u len=%u\n",
+		   id, freq, wait, data_len);
 	p = (struct wmi_send_action_cmd *) skb->data;
 	p->id = cpu_to_le32(id);
 	p->freq = cpu_to_le32(freq);
@@ -3348,8 +3366,9 @@ static int __ath6kl_wmi_send_mgmt_cmd(struct wmi *wmi, u8 if_idx, u32 id,
 	wmi->last_mgmt_tx_frame = buf;
 	wmi->last_mgmt_tx_frame_len = data_len;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "send_action_cmd: id=%u freq=%u wait=%u "
-		   "len=%u\n", id, freq, wait, data_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "send_action_cmd: id=%u freq=%u wait=%u len=%u\n",
+		   id, freq, wait, data_len);
 	p = (struct wmi_send_mgmt_cmd *) skb->data;
 	p->id = cpu_to_le32(id);
 	p->freq = cpu_to_le32(freq);
@@ -3402,8 +3421,9 @@ int ath6kl_wmi_send_probe_response_cmd(struct wmi *wmi, u8 if_idx, u32 freq,
 	if (!skb)
 		return -ENOMEM;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "send_probe_response_cmd: freq=%u dst=%pM "
-		   "len=%u\n", freq, dst, data_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "send_probe_response_cmd: freq=%u dst=%pM len=%u\n",
+		   freq, dst, data_len);
 	p = (struct wmi_p2p_probe_response_cmd *) skb->data;
 	p->freq = cpu_to_le32(freq);
 	memcpy(p->destination_addr, dst, ETH_ALEN);
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.h b/drivers/net/wireless/ath/ath6kl/wmi.h
index d3d2ab5c1689..9076bec3a2ba 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.h
+++ b/drivers/net/wireless/ath/ath6kl/wmi.h
@@ -106,6 +106,8 @@ struct wmi_data_sync_bufs {
 #define WMM_AC_VI   2		/* video */
 #define WMM_AC_VO   3		/* voice */
 
+#define WMI_VOICE_USER_PRIORITY		0x7
+
 struct wmi {
 	u16 stream_exist_for_ac[WMM_NUM_AC];
 	u8 fat_pipe_exist;
@@ -1151,6 +1153,7 @@ enum wmi_phy_mode {
 	WMI_11AG_MODE = 0x3,
 	WMI_11B_MODE = 0x4,
 	WMI_11GONLY_MODE = 0x5,
+	WMI_11G_HT20	= 0x6,
 };
 
 #define WMI_MAX_CHANNELS        32
@@ -1416,6 +1419,16 @@ struct wmi_ready_event_2 {
 	u8 phy_cap;
 } __packed;
 
+/* WMI_PHY_CAPABILITY */
+enum wmi_phy_cap {
+	WMI_11A_CAP = 0x01,
+	WMI_11G_CAP = 0x02,
+	WMI_11AG_CAP = 0x03,
+	WMI_11AN_CAP = 0x04,
+	WMI_11GN_CAP = 0x05,
+	WMI_11AGN_CAP = 0x06,
+};
+
 /* Connect Event */
 struct wmi_connect_event {
 	union {
@@ -1468,6 +1481,17 @@ enum wmi_disconnect_reason {
 	IBSS_MERGE = 0xe,
 };
 
+/* AP mode disconnect proto_reasons */
+enum ap_disconnect_reason {
+	WMI_AP_REASON_STA_LEFT		= 101,
+	WMI_AP_REASON_FROM_HOST		= 102,
+	WMI_AP_REASON_COMM_TIMEOUT	= 103,
+	WMI_AP_REASON_MAX_STA		= 104,
+	WMI_AP_REASON_ACL		= 105,
+	WMI_AP_REASON_STA_ROAM		= 106,
+	WMI_AP_REASON_DFS_CHANNEL	= 107,
+};
+
 #define ATH6KL_COUNTRY_RD_SHIFT        16
 
 struct ath6kl_wmi_regdomain {
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index a0387a027db0..9fdd70fcaf5b 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -892,34 +892,6 @@ static void ar9003_hw_tx_iq_cal_reload(struct ath_hw *ah)
 		      AR_PHY_RX_IQCAL_CORR_B0_LOOPBACK_IQCORR_EN, 0x1);
 }
 
-static bool ar9003_hw_rtt_restore(struct ath_hw *ah, struct ath9k_channel *chan)
-{
-	struct ath9k_rtt_hist *hist;
-	u32 *table;
-	int i;
-	bool restore;
-
-	if (!ah->caldata)
-		return false;
-
-	hist = &ah->caldata->rtt_hist;
-	if (!hist->num_readings)
-		return false;
-
-	ar9003_hw_rtt_enable(ah);
-	ar9003_hw_rtt_set_mask(ah, 0x00);
-	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
-		if (!(ah->rxchainmask & (1 << i)))
-			continue;
-		table = &hist->table[i][hist->num_readings][0];
-		ar9003_hw_rtt_load_hist(ah, i, table);
-	}
-	restore = ar9003_hw_rtt_force_restore(ah);
-	ar9003_hw_rtt_disable(ah);
-
-	return restore;
-}
-
 static bool ar9003_hw_init_cal(struct ath_hw *ah,
 			       struct ath9k_channel *chan)
 {
@@ -942,9 +914,10 @@ static bool ar9003_hw_init_cal(struct ath_hw *ah,
 		if (!ar9003_hw_rtt_restore(ah, chan))
 			run_rtt_cal = true;
 
-		ath_dbg(common, CALIBRATE, "RTT restore %s\n",
-			run_rtt_cal ? "failed" : "succeed");
+		if (run_rtt_cal)
+			ath_dbg(common, CALIBRATE, "RTT calibration to be done\n");
 	}
+
 	run_agc_cal = run_rtt_cal;
 
 	if (run_rtt_cal) {
@@ -1069,17 +1042,14 @@ skip_tx_iqcal:
 #undef CL_TAB_ENTRY
 
 	if (run_rtt_cal && caldata) {
-		struct ath9k_rtt_hist *hist = &caldata->rtt_hist;
-		if (is_reusable && (hist->num_readings < RTT_HIST_MAX)) {
-			u32 *table;
+		if (is_reusable) {
+			if (!ath9k_hw_rfbus_req(ah))
+				ath_err(ath9k_hw_common(ah),
+					"Could not stop baseband\n");
+			else
+				ar9003_hw_rtt_fill_hist(ah);
 
-			hist->num_readings++;
-			for (i = 0; i < AR9300_MAX_CHAINS; i++) {
-				if (!(ah->rxchainmask & (1 << i)))
-					continue;
-				table = &hist->table[i][hist->num_readings][0];
-				ar9003_hw_rtt_fill_hist(ah, i, table);
-			}
+			ath9k_hw_rfbus_done(ah);
 		}
 
 		ar9003_hw_rtt_disable(ah);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.c b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
index 3cac293a2849..ffbb180f91e1 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
@@ -756,7 +756,7 @@ int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 		if (caldata) {
 			caldata->done_txiqcal_once = false;
 			caldata->done_txclcal_once = false;
-			caldata->rtt_hist.num_readings = 0;
+			caldata->rtt_done = false;
 		}
 
 		if (!ath9k_hw_init_cal(ah, chan))
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_rtt.c b/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
index 458bedf0b0ae..74de3539c2c8 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
@@ -15,6 +15,7 @@
  */
 
 #include "hw.h"
+#include "hw-ops.h"
 #include "ar9003_phy.h"
 #include "ar9003_rtt.h"
 
@@ -69,7 +70,7 @@ bool ar9003_hw_rtt_force_restore(struct ath_hw *ah)
 }
 
 static void ar9003_hw_rtt_load_hist_entry(struct ath_hw *ah, u8 chain,
-		u32 index, u32 data28)
+					  u32 index, u32 data28)
 {
 	u32 val;
 
@@ -100,12 +101,21 @@ static void ar9003_hw_rtt_load_hist_entry(struct ath_hw *ah, u8 chain,
 		      RTT_ACCESS_TIMEOUT);
 }
 
-void ar9003_hw_rtt_load_hist(struct ath_hw *ah, u8 chain, u32 *table)
+void ar9003_hw_rtt_load_hist(struct ath_hw *ah)
 {
-	int i;
+	int chain, i;
 
-	for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++)
-		ar9003_hw_rtt_load_hist_entry(ah, chain, i, table[i]);
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->rxchainmask & (1 << chain)))
+			continue;
+		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++) {
+			ar9003_hw_rtt_load_hist_entry(ah, chain, i,
+					      ah->caldata->rtt_table[chain][i]);
+			ath_dbg(ath9k_hw_common(ah), CALIBRATE,
+				"Load RTT value at idx %d, chain %d: 0x%x\n",
+				i, chain, ah->caldata->rtt_table[chain][i]);
+		}
+	}
 }
 
 static int ar9003_hw_rtt_fill_hist_entry(struct ath_hw *ah, u8 chain, u32 index)
@@ -128,27 +138,71 @@ static int ar9003_hw_rtt_fill_hist_entry(struct ath_hw *ah, u8 chain, u32 index)
 			   RTT_ACCESS_TIMEOUT))
 		return RTT_BAD_VALUE;
 
-	val = REG_READ(ah, AR_PHY_RTT_TABLE_SW_INTF_1_B(chain));
+	val = MS(REG_READ(ah, AR_PHY_RTT_TABLE_SW_INTF_1_B(chain)),
+		 AR_PHY_RTT_SW_RTT_TABLE_DATA);
+
 
 	return val;
 }
 
-void ar9003_hw_rtt_fill_hist(struct ath_hw *ah, u8 chain, u32 *table)
+void ar9003_hw_rtt_fill_hist(struct ath_hw *ah)
 {
-	int i;
+	int chain, i;
+
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->rxchainmask & (1 << chain)))
+			continue;
+		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++) {
+			ah->caldata->rtt_table[chain][i] =
+				ar9003_hw_rtt_fill_hist_entry(ah, chain, i);
+			ath_dbg(ath9k_hw_common(ah), CALIBRATE,
+				"RTT value at idx %d, chain %d is: 0x%x\n",
+				i, chain, ah->caldata->rtt_table[chain][i]);
+		}
+	}
 
-	for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++)
-		table[i] = ar9003_hw_rtt_fill_hist_entry(ah, chain, i);
+	ah->caldata->rtt_done = true;
 }
 
 void ar9003_hw_rtt_clear_hist(struct ath_hw *ah)
 {
-	int i, j;
+	int chain, i;
 
-	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
-		if (!(ah->rxchainmask & (1 << i)))
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->rxchainmask & (1 << chain)))
 			continue;
-		for (j = 0; j < MAX_RTT_TABLE_ENTRY; j++)
-			ar9003_hw_rtt_load_hist_entry(ah, i, j, 0);
+		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++)
+			ar9003_hw_rtt_load_hist_entry(ah, chain, i, 0);
 	}
+
+	if (ah->caldata)
+		ah->caldata->rtt_done = false;
+}
+
+bool ar9003_hw_rtt_restore(struct ath_hw *ah, struct ath9k_channel *chan)
+{
+	bool restore;
+
+	if (!ah->caldata)
+		return false;
+
+	if (!ah->caldata->rtt_done)
+		return false;
+
+	ar9003_hw_rtt_enable(ah);
+	ar9003_hw_rtt_set_mask(ah, 0x10);
+
+	if (!ath9k_hw_rfbus_req(ah)) {
+		ath_err(ath9k_hw_common(ah), "Could not stop baseband\n");
+		restore = false;
+		goto fail;
+	}
+
+	ar9003_hw_rtt_load_hist(ah);
+	restore = ar9003_hw_rtt_force_restore(ah);
+
+fail:
+	ath9k_hw_rfbus_done(ah);
+	ar9003_hw_rtt_disable(ah);
+	return restore;
 }
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_rtt.h b/drivers/net/wireless/ath/ath9k/ar9003_rtt.h
index 030758d087d6..a43b30d723a4 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_rtt.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_rtt.h
@@ -21,8 +21,9 @@ void ar9003_hw_rtt_enable(struct ath_hw *ah);
 void ar9003_hw_rtt_disable(struct ath_hw *ah);
 void ar9003_hw_rtt_set_mask(struct ath_hw *ah, u32 rtt_mask);
 bool ar9003_hw_rtt_force_restore(struct ath_hw *ah);
-void ar9003_hw_rtt_load_hist(struct ath_hw *ah, u8 chain, u32 *table);
-void ar9003_hw_rtt_fill_hist(struct ath_hw *ah, u8 chain, u32 *table);
+void ar9003_hw_rtt_load_hist(struct ath_hw *ah);
+void ar9003_hw_rtt_fill_hist(struct ath_hw *ah);
 void ar9003_hw_rtt_clear_hist(struct ath_hw *ah);
+bool ar9003_hw_rtt_restore(struct ath_hw *ah, struct ath9k_channel *chan);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index f84477c5ebb1..abe05ec85d50 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -1702,10 +1702,10 @@ static int ath9k_hw_do_fastcc(struct ath_hw *ah, struct ath9k_channel *chan)
 	 * For AR9462, make sure that calibration data for
 	 * re-using are present.
 	 */
-	if (AR_SREV_9462(ah) && (!ah->caldata ||
-				 !ah->caldata->done_txiqcal_once ||
-				 !ah->caldata->done_txclcal_once ||
-				 !ah->caldata->rtt_hist.num_readings))
+	if (AR_SREV_9462(ah) && (ah->caldata &&
+				 (!ah->caldata->done_txiqcal_once ||
+				  !ah->caldata->done_txclcal_once ||
+				  !ah->caldata->rtt_done)))
 		goto fail;
 
 	ath_dbg(common, RESET, "FastChannelChange for %d -> %d\n",
@@ -1941,7 +1941,6 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (caldata) {
 		caldata->done_txiqcal_once = false;
 		caldata->done_txclcal_once = false;
-		caldata->rtt_hist.num_readings = 0;
 	}
 	if (!ath9k_hw_init_cal(ah, chan))
 		return -EIO;
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 828b9bbc456d..b620c557c2a6 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -348,12 +348,6 @@ enum ath9k_int {
 	 CHANNEL_HT40MINUS)
 
 #define MAX_RTT_TABLE_ENTRY     6
-#define RTT_HIST_MAX            3
-struct ath9k_rtt_hist {
-	u32 table[AR9300_MAX_CHAINS][RTT_HIST_MAX][MAX_RTT_TABLE_ENTRY];
-	u8 num_readings;
-};
-
 #define MAX_IQCAL_MEASUREMENT	8
 #define MAX_CL_TAB_ENTRY	16
 
@@ -363,6 +357,7 @@ struct ath9k_hw_cal_data {
 	int32_t CalValid;
 	int8_t iCoff;
 	int8_t qCoff;
+	bool rtt_done;
 	bool paprd_done;
 	bool nfcal_pending;
 	bool nfcal_interference;
@@ -373,8 +368,8 @@ struct ath9k_hw_cal_data {
 	u32 num_measures[AR9300_MAX_CHAINS];
 	int tx_corr_coeff[MAX_IQCAL_MEASUREMENT][AR9300_MAX_CHAINS];
 	u32 tx_clcal[AR9300_MAX_CHAINS][MAX_CL_TAB_ENTRY];
+	u32 rtt_table[AR9300_MAX_CHAINS][MAX_RTT_TABLE_ENTRY];
 	struct ath9k_nfcal_hist nfCalHist[NUM_NF_READINGS];
-	struct ath9k_rtt_hist rtt_hist;
 };
 
 struct ath9k_channel {
diff --git a/drivers/net/wireless/b43/bus.c b/drivers/net/wireless/b43/bus.c
index 424692df239d..565fdbdd6915 100644
--- a/drivers/net/wireless/b43/bus.c
+++ b/drivers/net/wireless/b43/bus.c
@@ -107,11 +107,9 @@ struct b43_bus_dev *b43_bus_dev_bcma_init(struct bcma_device *core)
 	dev->dma_dev = core->dma_dev;
 	dev->irq = core->irq;
 
-	/*
 	dev->board_vendor = core->bus->boardinfo.vendor;
 	dev->board_type = core->bus->boardinfo.type;
-	dev->board_rev = core->bus->boardinfo.rev;
-	*/
+	dev->board_rev = core->bus->sprom.board_rev;
 
 	dev->chip_id = core->bus->chipinfo.id;
 	dev->chip_rev = core->bus->chipinfo.rev;
@@ -210,7 +208,7 @@ struct b43_bus_dev *b43_bus_dev_ssb_init(struct ssb_device *sdev)
 
 	dev->board_vendor = sdev->bus->boardinfo.vendor;
 	dev->board_type = sdev->bus->boardinfo.type;
-	dev->board_rev = sdev->bus->boardinfo.rev;
+	dev->board_rev = sdev->bus->sprom.board_rev;
 
 	dev->chip_id = sdev->bus->chip_id;
 	dev->chip_rev = sdev->bus->chip_rev;
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index b5f1b91002bb..777cd74921d7 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -1109,7 +1109,7 @@ static bool b43_dma_translation_in_low_word(struct b43_wldev *dev,
 #ifdef CONFIG_B43_SSB
 	if (dev->dev->bus_type == B43_BUS_SSB &&
 	    dev->dev->sdev->bus->bustype == SSB_BUSTYPE_PCI &&
-	    !(dev->dev->sdev->bus->host_pci->is_pcie &&
+	    !(pci_is_pcie(dev->dev->sdev->bus->host_pci) &&
 	      ssb_read32(dev->dev->sdev, SSB_TMSHIGH) & SSB_TMSHIGH_DMA64))
 			return 1;
 #endif
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 617afc8211b2..5a39b226b2e3 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -5243,10 +5243,10 @@ static void b43_sprom_fixup(struct ssb_bus *bus)
 
 	/* boardflags workarounds */
 	if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
-	    bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
+	    bus->chip_id == 0x4301 && bus->sprom.board_rev == 0x74)
 		bus->sprom.boardflags_lo |= B43_BFL_BTCOEXIST;
 	if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
-	    bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
+	    bus->boardinfo.type == 0x4E && bus->sprom.board_rev > 0x40)
 		bus->sprom.boardflags_lo |= B43_BFL_PACTRL;
 	if (bus->bustype == SSB_BUSTYPE_PCI) {
 		pdev = bus->host_pci;
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 1be214b815fb..cd9c9bc186d9 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -1573,8 +1573,6 @@ static void b43legacy_request_firmware(struct work_struct *work)
 	const char *filename;
 	int err;
 
-	/* do dummy read */
-	ssb_read32(dev->dev, SSB_TMSHIGH);
 	if (!fw->ucode) {
 		if (rev == 2)
 			filename = "ucode2";
@@ -3781,7 +3779,7 @@ static void b43legacy_sprom_fixup(struct ssb_bus *bus)
 	/* boardflags workarounds */
 	if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
 	    bus->boardinfo.type == 0x4E &&
-	    bus->boardinfo.rev > 0x40)
+	    bus->sprom.board_rev > 0x40)
 		bus->sprom.boardflags_lo |= B43legacy_BFL_PACTRL;
 }
 
diff --git a/drivers/net/wireless/b43legacy/phy.c b/drivers/net/wireless/b43legacy/phy.c
index 950334197f40..995c7d0c212a 100644
--- a/drivers/net/wireless/b43legacy/phy.c
+++ b/drivers/net/wireless/b43legacy/phy.c
@@ -408,7 +408,7 @@ static void b43legacy_phy_setupg(struct b43legacy_wldev *dev)
 
 		if (is_bcm_board_vendor(dev) &&
 		    (dev->dev->bus->boardinfo.type == 0x0416) &&
-		    (dev->dev->bus->boardinfo.rev == 0x0017))
+		    (dev->dev->bus->sprom.board_rev == 0x0017))
 			return;
 
 		b43legacy_ilt_write(dev, 0x5001, 0x0002);
@@ -424,7 +424,7 @@ static void b43legacy_phy_setupg(struct b43legacy_wldev *dev)
 
 		if (is_bcm_board_vendor(dev) &&
 		    (dev->dev->bus->boardinfo.type == 0x0416) &&
-		    (dev->dev->bus->boardinfo.rev == 0x0017))
+		    (dev->dev->bus->sprom.board_rev == 0x0017))
 			return;
 
 		b43legacy_ilt_write(dev, 0x0401, 0x0002);
diff --git a/drivers/net/wireless/b43legacy/radio.c b/drivers/net/wireless/b43legacy/radio.c
index fcbafcd603cc..896177690394 100644
--- a/drivers/net/wireless/b43legacy/radio.c
+++ b/drivers/net/wireless/b43legacy/radio.c
@@ -1998,7 +1998,7 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 			if (phy->type == B43legacy_PHYTYPE_G) {
 				if (is_bcm_board_vendor(dev) &&
 				    dev->dev->bus->boardinfo.type == 0x421 &&
-				    dev->dev->bus->boardinfo.rev >= 30)
+				    dev->dev->bus->sprom.board_rev >= 30)
 					att = 3;
 				else if (is_bcm_board_vendor(dev) &&
 					 dev->dev->bus->boardinfo.type == 0x416)
@@ -2008,7 +2008,7 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 			} else {
 				if (is_bcm_board_vendor(dev) &&
 				    dev->dev->bus->boardinfo.type == 0x421 &&
-				    dev->dev->bus->boardinfo.rev >= 30)
+				    dev->dev->bus->sprom.board_rev >= 30)
 					att = 7;
 				else
 					att = 6;
@@ -2018,7 +2018,7 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 			if (phy->type == B43legacy_PHYTYPE_G) {
 				if (is_bcm_board_vendor(dev) &&
 				    dev->dev->bus->boardinfo.type == 0x421 &&
-				    dev->dev->bus->boardinfo.rev >= 30)
+				    dev->dev->bus->sprom.board_rev >= 30)
 					att = 3;
 				else if (is_bcm_board_vendor(dev) &&
 					 dev->dev->bus->boardinfo.type ==
@@ -2052,9 +2052,9 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 	}
 	if (is_bcm_board_vendor(dev) &&
 	    dev->dev->bus->boardinfo.type == 0x421) {
-		if (dev->dev->bus->boardinfo.rev < 0x43)
+		if (dev->dev->bus->sprom.board_rev < 0x43)
 			att = 2;
-		else if (dev->dev->bus->boardinfo.rev < 0x51)
+		else if (dev->dev->bus->sprom.board_rev < 0x51)
 			att = 3;
 	}
 	if (att == 0xFFFF)
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index 4add7da24681..e2480d196276 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -85,18 +85,15 @@ int brcmf_sdio_intr_register(struct brcmf_sdio_dev *sdiodev)
 	sdiodev->irq_wake = true;
 
 	/* must configure SDIO_CCCR_IENx to enable irq */
-	data = brcmf_sdcard_cfg_read(sdiodev, SDIO_FUNC_0,
-				     SDIO_CCCR_IENx, &ret);
+	data = brcmf_sdio_regrb(sdiodev, SDIO_CCCR_IENx, &ret);
 	data |= 1 << SDIO_FUNC_1 | 1 << SDIO_FUNC_2 | 1;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_0, SDIO_CCCR_IENx,
-			       data, &ret);
+	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_IENx, data, &ret);
 
 	/* redirect, configure ane enable io for interrupt signal */
 	data = SDIO_SEPINT_MASK | SDIO_SEPINT_OE;
 	if (sdiodev->irq_flags | IRQF_TRIGGER_HIGH)
 		data |= SDIO_SEPINT_ACT_HI;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_0, SDIO_CCCR_BRCM_SEPINT,
-			       data, &ret);
+	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_BRCM_SEPINT, data, &ret);
 
 	return 0;
 }
@@ -105,9 +102,8 @@ int brcmf_sdio_intr_unregister(struct brcmf_sdio_dev *sdiodev)
 {
 	brcmf_dbg(TRACE, "Entering\n");
 
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_0, SDIO_CCCR_BRCM_SEPINT,
-			       0, NULL);
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_0, SDIO_CCCR_IENx, 0, NULL);
+	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_BRCM_SEPINT, 0, NULL);
+	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_IENx, 0, NULL);
 
 	if (sdiodev->irq_wake) {
 		disable_irq_wake(sdiodev->irq);
@@ -158,153 +154,147 @@ int brcmf_sdio_intr_unregister(struct brcmf_sdio_dev *sdiodev)
 }
 #endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
 
-u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_dev *sdiodev, uint fnc_num, u32 addr,
-			 int *err)
-{
-	int status;
-	s32 retry = 0;
-	u8 data = 0;
-
-	do {
-		if (retry)	/* wait for 1 ms till bus get settled down */
-			udelay(1000);
-		status = brcmf_sdioh_request_byte(sdiodev, SDIOH_READ, fnc_num,
-						  addr, (u8 *) &data);
-	} while (status != 0
-		 && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
-	if (err)
-		*err = status;
-
-	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, u8data = 0x%x\n",
-		  fnc_num, addr, data);
-
-	return data;
-}
-
-void
-brcmf_sdcard_cfg_write(struct brcmf_sdio_dev *sdiodev, uint fnc_num, u32 addr,
-		       u8 data, int *err)
-{
-	int status;
-	s32 retry = 0;
-
-	do {
-		if (retry)	/* wait for 1 ms till bus get settled down */
-			udelay(1000);
-		status = brcmf_sdioh_request_byte(sdiodev, SDIOH_WRITE, fnc_num,
-						  addr, (u8 *) &data);
-	} while (status != 0
-		 && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
-	if (err)
-		*err = status;
-
-	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, u8data = 0x%x\n",
-		  fnc_num, addr, data);
-}
-
 int
 brcmf_sdcard_set_sbaddr_window(struct brcmf_sdio_dev *sdiodev, u32 address)
 {
-	int err = 0;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
-			 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
-	if (!err)
-		brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_SBADDRMID,
-				       (address >> 16) & SBSDIO_SBADDRMID_MASK,
-				       &err);
-	if (!err)
-		brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_SBADDRHIGH,
-				       (address >> 24) & SBSDIO_SBADDRHIGH_MASK,
-				       &err);
+	int err = 0, i;
+	u8 addr[3];
+	s32 retry;
+
+	addr[0] = (address >> 8) & SBSDIO_SBADDRLOW_MASK;
+	addr[1] = (address >> 16) & SBSDIO_SBADDRMID_MASK;
+	addr[2] = (address >> 24) & SBSDIO_SBADDRHIGH_MASK;
+
+	for (i = 0; i < 3; i++) {
+		retry = 0;
+		do {
+			if (retry)
+				usleep_range(1000, 2000);
+			err = brcmf_sdioh_request_byte(sdiodev, SDIOH_WRITE,
+					SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW + i,
+					&addr[i]);
+		} while (err != 0 && retry++ < SDIOH_API_ACCESS_RETRY_LIMIT);
+
+		if (err) {
+			brcmf_dbg(ERROR, "failed at addr:0x%0x\n",
+				  SBSDIO_FUNC1_SBADDRLOW + i);
+			break;
+		}
+	}
 
 	return err;
 }
 
-u32 brcmf_sdcard_reg_read(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size)
+static int
+brcmf_sdio_regrw_helper(struct brcmf_sdio_dev *sdiodev, u32 addr,
+			void *data, bool write)
 {
-	int status;
-	u32 word = 0;
-	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
-
-	brcmf_dbg(INFO, "fun = 1, addr = 0x%x\n", addr);
-
-	if (bar0 != sdiodev->sbwad) {
-		if (brcmf_sdcard_set_sbaddr_window(sdiodev, bar0))
-			return 0xFFFFFFFF;
+	u8 func_num, reg_size;
+	u32 bar;
+	s32 retry = 0;
+	int ret;
 
-		sdiodev->sbwad = bar0;
+	/*
+	 * figure out how to read the register based on address range
+	 * 0x00 ~ 0x7FF: function 0 CCCR and FBR
+	 * 0x10000 ~ 0x1FFFF: function 1 miscellaneous registers
+	 * The rest: function 1 silicon backplane core registers
+	 */
+	if ((addr & ~REG_F0_REG_MASK) == 0) {
+		func_num = SDIO_FUNC_0;
+		reg_size = 1;
+	} else if ((addr & ~REG_F1_MISC_MASK) == 0) {
+		func_num = SDIO_FUNC_1;
+		reg_size = 1;
+	} else {
+		func_num = SDIO_FUNC_1;
+		reg_size = 4;
+
+		/* Set the window for SB core register */
+		bar = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
+		if (bar != sdiodev->sbwad) {
+			ret = brcmf_sdcard_set_sbaddr_window(sdiodev, bar);
+			if (ret != 0) {
+				memset(data, 0xFF, reg_size);
+				return ret;
+			}
+			sdiodev->sbwad = bar;
+		}
+		addr &= SBSDIO_SB_OFT_ADDR_MASK;
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
 	}
 
-	addr &= SBSDIO_SB_OFT_ADDR_MASK;
-	if (size == 4)
-		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+	do {
+		if (!write)
+			memset(data, 0, reg_size);
+		if (retry)	/* wait for 1 ms till bus get settled down */
+			usleep_range(1000, 2000);
+		if (reg_size == 1)
+			ret = brcmf_sdioh_request_byte(sdiodev, write,
+						       func_num, addr, data);
+		else
+			ret = brcmf_sdioh_request_word(sdiodev, write,
+						       func_num, addr, data, 4);
+	} while (ret != 0 && retry++ < SDIOH_API_ACCESS_RETRY_LIMIT);
 
-	status = brcmf_sdioh_request_word(sdiodev, SDIOH_READ, SDIO_FUNC_1,
-					  addr, &word, size);
+	if (ret != 0)
+		brcmf_dbg(ERROR, "failed with %d\n", ret);
 
-	sdiodev->regfail = (status != 0);
+	return ret;
+}
 
-	brcmf_dbg(INFO, "u32data = 0x%x\n", word);
+u8 brcmf_sdio_regrb(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret)
+{
+	u8 data;
+	int retval;
 
-	/* if ok, return appropriately masked word */
-	if (status == 0) {
-		switch (size) {
-		case sizeof(u8):
-			return word & 0xff;
-		case sizeof(u16):
-			return word & 0xffff;
-		case sizeof(u32):
-			return word;
-		default:
-			sdiodev->regfail = true;
+	brcmf_dbg(INFO, "addr:0x%08x\n", addr);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, false);
+	brcmf_dbg(INFO, "data:0x%02x\n", data);
 
-		}
-	}
+	if (ret)
+		*ret = retval;
 
-	/* otherwise, bad sdio access or invalid size */
-	brcmf_dbg(ERROR, "error reading addr 0x%04x size %d\n", addr, size);
-	return 0xFFFFFFFF;
+	return data;
 }
 
-u32 brcmf_sdcard_reg_write(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size,
-			   u32 data)
+u32 brcmf_sdio_regrl(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret)
 {
-	int status;
-	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
-	int err = 0;
+	u32 data;
+	int retval;
 
-	brcmf_dbg(INFO, "fun = 1, addr = 0x%x, uint%ddata = 0x%x\n",
-		  addr, size * 8, data);
+	brcmf_dbg(INFO, "addr:0x%08x\n", addr);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, false);
+	brcmf_dbg(INFO, "data:0x%08x\n", data);
 
-	if (bar0 != sdiodev->sbwad) {
-		err = brcmf_sdcard_set_sbaddr_window(sdiodev, bar0);
-		if (err)
-			return err;
+	if (ret)
+		*ret = retval;
 
-		sdiodev->sbwad = bar0;
-	}
+	return data;
+}
 
-	addr &= SBSDIO_SB_OFT_ADDR_MASK;
-	if (size == 4)
-		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
-	status =
-	    brcmf_sdioh_request_word(sdiodev, SDIOH_WRITE, SDIO_FUNC_1,
-				     addr, &data, size);
-	sdiodev->regfail = (status != 0);
+void brcmf_sdio_regwb(struct brcmf_sdio_dev *sdiodev, u32 addr,
+		      u8 data, int *ret)
+{
+	int retval;
 
-	if (status == 0)
-		return 0;
+	brcmf_dbg(INFO, "addr:0x%08x, data:0x%02x\n", addr, data);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, true);
 
-	brcmf_dbg(ERROR, "error writing 0x%08x to addr 0x%04x size %d\n",
-		  data, addr, size);
-	return 0xFFFFFFFF;
+	if (ret)
+		*ret = retval;
 }
 
-bool brcmf_sdcard_regfail(struct brcmf_sdio_dev *sdiodev)
+void brcmf_sdio_regwl(struct brcmf_sdio_dev *sdiodev, u32 addr,
+		      u32 data, int *ret)
 {
-	return sdiodev->regfail;
+	int retval;
+
+	brcmf_dbg(INFO, "addr:0x%08x, data:0x%08x\n", addr, data);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, true);
+
+	if (ret)
+		*ret = retval;
 }
 
 static int brcmf_sdcard_recv_prepare(struct brcmf_sdio_dev *sdiodev, uint fn,
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index dd07d33a927c..82f51dbd0d66 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -346,43 +346,17 @@ int brcmf_sdioh_request_buffer(struct brcmf_sdio_dev *sdiodev,
 	return status;
 }
 
-/* Read client card reg */
-static int
-brcmf_sdioh_card_regread(struct brcmf_sdio_dev *sdiodev, int func, u32 regaddr,
-			 int regsize, u32 *data)
-{
-
-	if ((func == 0) || (regsize == 1)) {
-		u8 temp = 0;
-
-		brcmf_sdioh_request_byte(sdiodev, SDIOH_READ, func, regaddr,
-					 &temp);
-		*data = temp;
-		*data &= 0xff;
-		brcmf_dbg(DATA, "byte read data=0x%02x\n", *data);
-	} else {
-		brcmf_sdioh_request_word(sdiodev, SDIOH_READ, func, regaddr,
-					 data, regsize);
-		if (regsize == 2)
-			*data &= 0xffff;
-
-		brcmf_dbg(DATA, "word read data=0x%08x\n", *data);
-	}
-
-	return SUCCESS;
-}
-
 static int brcmf_sdioh_get_cisaddr(struct brcmf_sdio_dev *sdiodev, u32 regaddr)
 {
 	/* read 24 bits and return valid 17 bit addr */
-	int i;
+	int i, ret;
 	u32 scratch, regdata;
 	__le32 scratch_le;
 	u8 *ptr = (u8 *)&scratch_le;
 
 	for (i = 0; i < 3; i++) {
-		if ((brcmf_sdioh_card_regread(sdiodev, 0, regaddr, 1,
-				&regdata)) != SUCCESS)
+		regdata = brcmf_sdio_regrl(sdiodev, regaddr, &ret);
+		if (ret != 0)
 			brcmf_dbg(ERROR, "Can't read!\n");
 
 		*ptr++ = (u8) regdata;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 149ee67beb2e..1dbf2be478c8 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -629,43 +629,29 @@ static bool data_ok(struct brcmf_sdio *bus)
  * Reads a register in the SDIO hardware block. This block occupies a series of
  * adresses on the 32 bit backplane bus.
  */
-static void
-r_sdreg32(struct brcmf_sdio *bus, u32 *regvar, u32 reg_offset, u32 *retryvar)
+static int
+r_sdreg32(struct brcmf_sdio *bus, u32 *regvar, u32 offset)
 {
 	u8 idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
-	*retryvar = 0;
-	do {
-		*regvar = brcmf_sdcard_reg_read(bus->sdiodev,
-				bus->ci->c_inf[idx].base + reg_offset,
-				sizeof(u32));
-	} while (brcmf_sdcard_regfail(bus->sdiodev) &&
-		 (++(*retryvar) <= retry_limit));
-	if (*retryvar) {
-		bus->regfails += (*retryvar-1);
-		if (*retryvar > retry_limit) {
-			brcmf_dbg(ERROR, "FAILED READ %Xh\n", reg_offset);
-			*regvar = 0;
-		}
-	}
+	int ret;
+
+	*regvar = brcmf_sdio_regrl(bus->sdiodev,
+				   bus->ci->c_inf[idx].base + offset, &ret);
+
+	return ret;
 }
 
-static void
-w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset, u32 *retryvar)
+static int
+w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset)
 {
 	u8 idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
-	*retryvar = 0;
-	do {
-		brcmf_sdcard_reg_write(bus->sdiodev,
-				       bus->ci->c_inf[idx].base + reg_offset,
-				       sizeof(u32), regval);
-	} while (brcmf_sdcard_regfail(bus->sdiodev) &&
-		 (++(*retryvar) <= retry_limit));
-	if (*retryvar) {
-		bus->regfails += (*retryvar-1);
-		if (*retryvar > retry_limit)
-			brcmf_dbg(ERROR, "FAILED REGISTER WRITE %Xh\n",
-				  reg_offset);
-	}
+	int ret;
+
+	brcmf_sdio_regwl(bus->sdiodev,
+			 bus->ci->c_inf[idx].base + reg_offset,
+			 regval, &ret);
+
+	return ret;
 }
 
 #define PKT_AVAILABLE()		(intstatus & I_HMB_FRAME_IND)
@@ -697,16 +683,16 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 		clkreq =
 		    bus->alp_only ? SBSDIO_ALP_AVAIL_REQ : SBSDIO_HT_AVAIL_REQ;
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 clkreq, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "HT Avail request error: %d\n", err);
 			return -EBADE;
 		}
 
 		/* Check current status */
-		clkctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		clkctl = brcmf_sdio_regrb(bus->sdiodev,
+					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "HT Avail read error: %d\n", err);
 			return -EBADE;
@@ -715,9 +701,8 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 		/* Go to pending and await interrupt if appropriate */
 		if (!SBSDIO_CLKAV(clkctl, bus->alp_only) && pendok) {
 			/* Allow only clock-available interrupt */
-			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-					SDIO_FUNC_1,
-					SBSDIO_DEVICE_CTL, &err);
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_DEVICE_CTL, &err);
 			if (err) {
 				brcmf_dbg(ERROR, "Devctl error setting CA: %d\n",
 					  err);
@@ -725,30 +710,28 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 			}
 
 			devctl |= SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 			brcmf_dbg(INFO, "CLKCTL: set PENDING\n");
 			bus->clkstate = CLK_PENDING;
 
 			return 0;
 		} else if (bus->clkstate == CLK_PENDING) {
 			/* Cancel CA-only interrupt filter */
-			devctl =
-			    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
 						  SBSDIO_DEVICE_CTL, &err);
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 		}
 
 		/* Otherwise, wait here (polling) for HT Avail */
 		timeout = jiffies +
 			  msecs_to_jiffies(PMU_MAX_TRANSITION_DLY/1000);
 		while (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
-			clkctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-						       SDIO_FUNC_1,
-						       SBSDIO_FUNC1_CHIPCLKCSR,
-						       &err);
+			clkctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_FUNC1_CHIPCLKCSR,
+						  &err);
 			if (time_after(jiffies, timeout))
 				break;
 			else
@@ -781,17 +764,16 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 
 		if (bus->clkstate == CLK_PENDING) {
 			/* Cancel CA-only interrupt filter */
-			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-					SDIO_FUNC_1,
-					SBSDIO_DEVICE_CTL, &err);
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_DEVICE_CTL, &err);
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 		}
 
 		bus->clkstate = CLK_SDONLY;
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 clkreq, &err);
 		brcmf_dbg(INFO, "CLKCTL: turned OFF\n");
 		if (err) {
 			brcmf_dbg(ERROR, "Failed access turning clock off: %d\n",
@@ -874,7 +856,7 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 
 static int brcmf_sdbrcm_bussleep(struct brcmf_sdio *bus, bool sleep)
 {
-	uint retries = 0;
+	int ret;
 
 	brcmf_dbg(INFO, "request %s (currently %s)\n",
 		  sleep ? "SLEEP" : "WAKE",
@@ -894,22 +876,20 @@ static int brcmf_sdbrcm_bussleep(struct brcmf_sdio *bus, bool sleep)
 		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
 		/* Tell device to start using OOB wakeup */
-		w_sdreg32(bus, SMB_USE_OOB,
-			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
-		if (retries > retry_limit)
+		ret = w_sdreg32(bus, SMB_USE_OOB,
+				offsetof(struct sdpcmd_regs, tosbmailbox));
+		if (ret != 0)
 			brcmf_dbg(ERROR, "CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n");
 
 		/* Turn off our contribution to the HT clock request */
 		brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_FUNC1_CHIPCLKCSR,
-			SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
 
 		/* Isolate the bus */
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_DEVICE_CTL,
-			SBSDIO_DEVCTL_PADS_ISO, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+				 SBSDIO_DEVCTL_PADS_ISO, NULL);
 
 		/* Change state */
 		bus->sleeping = true;
@@ -917,21 +897,20 @@ static int brcmf_sdbrcm_bussleep(struct brcmf_sdio *bus, bool sleep)
 	} else {
 		/* Waking up: bus power up is ok, set local state */
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 0, NULL);
 
 		/* Make sure the controller has the bus up */
 		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
 		/* Send misc interrupt to indicate OOB not needed */
-		w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, tosbmailboxdata),
-			  &retries);
-		if (retries <= retry_limit)
-			w_sdreg32(bus, SMB_DEV_INT,
-				  offsetof(struct sdpcmd_regs, tosbmailbox),
-				  &retries);
-
-		if (retries > retry_limit)
+		ret = w_sdreg32(bus, 0,
+				offsetof(struct sdpcmd_regs, tosbmailboxdata));
+		if (ret == 0)
+			ret = w_sdreg32(bus, SMB_DEV_INT,
+				offsetof(struct sdpcmd_regs, tosbmailbox));
+
+		if (ret != 0)
 			brcmf_dbg(ERROR, "CANNOT SIGNAL CHIP TO CLEAR OOB!!\n");
 
 		/* Make sure we have SD bus access */
@@ -955,17 +934,17 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 	u32 intstatus = 0;
 	u32 hmb_data;
 	u8 fcbits;
-	uint retries = 0;
+	int ret;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Read mailbox data and ack that we did so */
-	r_sdreg32(bus, &hmb_data,
-		  offsetof(struct sdpcmd_regs, tohostmailboxdata), &retries);
+	ret = r_sdreg32(bus, &hmb_data,
+			offsetof(struct sdpcmd_regs, tohostmailboxdata));
 
-	if (retries <= retry_limit)
+	if (ret == 0)
 		w_sdreg32(bus, SMB_INT_ACK,
-			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
+			  offsetof(struct sdpcmd_regs, tosbmailbox));
 	bus->f1regdata += 2;
 
 	/* Dongle recomposed rx frames, accept them again */
@@ -1040,17 +1019,16 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 	if (abort)
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_FRAMECTRL,
-			       SFC_RF_TERM, &err);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+			 SFC_RF_TERM, &err);
 	bus->f1regdata++;
 
 	/* Wait until the packet has been flushed (device/FIFO stable) */
 	for (lastrbc = retries = 0xffff; retries > 0; retries--) {
-		hi = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					   SBSDIO_FUNC1_RFRAMEBCHI, NULL);
-		lo = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					   SBSDIO_FUNC1_RFRAMEBCLO, NULL);
+		hi = brcmf_sdio_regrb(bus->sdiodev,
+				      SBSDIO_FUNC1_RFRAMEBCHI, &err);
+		lo = brcmf_sdio_regrb(bus->sdiodev,
+				      SBSDIO_FUNC1_RFRAMEBCLO, &err);
 		bus->f1regdata += 2;
 
 		if ((hi == 0) && (lo == 0))
@@ -1070,11 +1048,11 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 
 	if (rtx) {
 		bus->rxrtx++;
-		w_sdreg32(bus, SMB_NAK,
-			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
+		err = w_sdreg32(bus, SMB_NAK,
+				offsetof(struct sdpcmd_regs, tosbmailbox));
 
 		bus->f1regdata++;
-		if (retries <= retry_limit)
+		if (err == 0)
 			bus->rxskip = true;
 	}
 
@@ -1082,7 +1060,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 	bus->nextlen = 0;
 
 	/* If we can't reach the device, signal failure */
-	if (err || brcmf_sdcard_regfail(bus->sdiodev))
+	if (err)
 		bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 }
 
@@ -2178,21 +2156,16 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 		bus->tx_sderrs++;
 
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				 SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM,
-				 NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+				 SFC_WF_TERM, NULL);
 		bus->f1regdata++;
 
 		for (i = 0; i < 3; i++) {
 			u8 hi, lo;
-			hi = brcmf_sdcard_cfg_read(bus->sdiodev,
-					     SDIO_FUNC_1,
-					     SBSDIO_FUNC1_WFRAMEBCHI,
-					     NULL);
-			lo = brcmf_sdcard_cfg_read(bus->sdiodev,
-					     SDIO_FUNC_1,
-					     SBSDIO_FUNC1_WFRAMEBCLO,
-					     NULL);
+			hi = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+			lo = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCLO, NULL);
 			bus->f1regdata += 2;
 			if ((hi == 0) && (lo == 0))
 				break;
@@ -2219,7 +2192,6 @@ static uint brcmf_sdbrcm_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 {
 	struct sk_buff *pkt;
 	u32 intstatus = 0;
-	uint retries = 0;
 	int ret = 0, prec_out;
 	uint cnt = 0;
 	uint datalen;
@@ -2249,11 +2221,11 @@ static uint brcmf_sdbrcm_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 		/* In poll mode, need to check for other events */
 		if (!bus->intr && cnt) {
 			/* Check device status, signal pending interrupt */
-			r_sdreg32(bus, &intstatus,
-				  offsetof(struct sdpcmd_regs, intstatus),
-				  &retries);
+			ret = r_sdreg32(bus, &intstatus,
+					offsetof(struct sdpcmd_regs,
+						 intstatus));
 			bus->f2txdata++;
-			if (brcmf_sdcard_regfail(bus->sdiodev))
+			if (ret != 0)
 				break;
 			if (intstatus & bus->hostintmask)
 				bus->ipend = true;
@@ -2275,7 +2247,6 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 {
 	u32 local_hostintmask;
 	u8 saveclk;
-	uint retries;
 	int err;
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
@@ -2303,7 +2274,7 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
 	/* Disable and clear interrupts at the chip level also */
-	w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, hostintmask), &retries);
+	w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, hostintmask));
 	local_hostintmask = bus->hostintmask;
 	bus->hostintmask = 0;
 
@@ -2311,24 +2282,23 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 	bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 
 	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	saveclk = brcmf_sdio_regrb(bus->sdiodev,
+				   SBSDIO_FUNC1_CHIPCLKCSR, &err);
 	if (!err) {
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR,
-				       (saveclk | SBSDIO_FORCE_HT), &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 (saveclk | SBSDIO_FORCE_HT), &err);
 	}
 	if (err)
 		brcmf_dbg(ERROR, "Failed to force clock for F2: err %d\n", err);
 
 	/* Turn off the bus (F2), free any pending packets */
 	brcmf_dbg(INTR, "disable SDIO interrupts\n");
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			 SDIO_FUNC_ENABLE_1, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx, SDIO_FUNC_ENABLE_1,
+			 NULL);
 
 	/* Clear any pending interrupts now that F2 is disabled */
 	w_sdreg32(bus, local_hostintmask,
-		  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		  offsetof(struct sdpcmd_regs, intstatus));
 
 	/* Turn off the backplane clock (only) */
 	brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
@@ -2373,12 +2343,12 @@ static inline void brcmf_sdbrcm_clrintr(struct brcmf_sdio *bus)
 static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 {
 	u32 intstatus, newstatus = 0;
-	uint retries = 0;
 	uint rxlimit = bus->rxbound;	/* Rx frames to read before resched */
 	uint txlimit = bus->txbound;	/* Tx frames to send before resched */
 	uint framecnt = 0;	/* Temporary counter of tx/rx frames */
 	bool rxdone = true;	/* Flag for no more read data */
 	bool resched = false;	/* Flag indicating resched wanted */
+	int err;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
@@ -2389,13 +2359,12 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 
 	/* If waiting for HTAVAIL, check status */
 	if (bus->clkstate == CLK_PENDING) {
-		int err;
 		u8 clkctl, devctl = 0;
 
 #ifdef DEBUG
 		/* Check for inconsistent device control */
-		devctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_DEVICE_CTL, &err);
+		devctl = brcmf_sdio_regrb(bus->sdiodev,
+					  SBSDIO_DEVICE_CTL, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "error reading DEVCTL: %d\n", err);
 			bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
@@ -2403,8 +2372,8 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 #endif				/* DEBUG */
 
 		/* Read CSR, if clock on switch to AVAIL, else ignore */
-		clkctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		clkctl = brcmf_sdio_regrb(bus->sdiodev,
+					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "error reading CSR: %d\n",
 				  err);
@@ -2415,17 +2384,16 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 			  devctl, clkctl);
 
 		if (SBSDIO_HTAV(clkctl)) {
-			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-						       SDIO_FUNC_1,
-						       SBSDIO_DEVICE_CTL, &err);
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_DEVICE_CTL, &err);
 			if (err) {
 				brcmf_dbg(ERROR, "error reading DEVCTL: %d\n",
 					  err);
 				bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 			}
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 			if (err) {
 				brcmf_dbg(ERROR, "error writing DEVCTL: %d\n",
 					  err);
@@ -2447,17 +2415,17 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	/* Pending interrupt indicates new device status */
 	if (bus->ipend) {
 		bus->ipend = false;
-		r_sdreg32(bus, &newstatus,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		err = r_sdreg32(bus, &newstatus,
+				offsetof(struct sdpcmd_regs, intstatus));
 		bus->f1regdata++;
-		if (brcmf_sdcard_regfail(bus->sdiodev))
+		if (err != 0)
 			newstatus = 0;
 		newstatus &= bus->hostintmask;
 		bus->fcstate = !!(newstatus & I_HMB_FC_STATE);
 		if (newstatus) {
-			w_sdreg32(bus, newstatus,
-				  offsetof(struct sdpcmd_regs, intstatus),
-				  &retries);
+			err = w_sdreg32(bus, newstatus,
+					offsetof(struct sdpcmd_regs,
+						 intstatus));
 			bus->f1regdata++;
 		}
 	}
@@ -2472,11 +2440,11 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	 */
 	if (intstatus & I_HMB_FC_CHANGE) {
 		intstatus &= ~I_HMB_FC_CHANGE;
-		w_sdreg32(bus, I_HMB_FC_CHANGE,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		err = w_sdreg32(bus, I_HMB_FC_CHANGE,
+				offsetof(struct sdpcmd_regs, intstatus));
 
-		r_sdreg32(bus, &newstatus,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		err = r_sdreg32(bus, &newstatus,
+				offsetof(struct sdpcmd_regs, intstatus));
 		bus->f1regdata += 2;
 		bus->fcstate =
 		    !!(newstatus & (I_HMB_FC_STATE | I_HMB_FC_CHANGE));
@@ -2546,21 +2514,18 @@ clkwait:
 
 			brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-					 SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM,
-					 NULL);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+					 SFC_WF_TERM, &err);
 			bus->f1regdata++;
 
 			for (i = 0; i < 3; i++) {
 				u8 hi, lo;
-				hi = brcmf_sdcard_cfg_read(bus->sdiodev,
-						     SDIO_FUNC_1,
-						     SBSDIO_FUNC1_WFRAMEBCHI,
-						     NULL);
-				lo = brcmf_sdcard_cfg_read(bus->sdiodev,
-						     SDIO_FUNC_1,
-						     SBSDIO_FUNC1_WFRAMEBCLO,
-						     NULL);
+				hi = brcmf_sdio_regrb(bus->sdiodev,
+						      SBSDIO_FUNC1_WFRAMEBCHI,
+						      &err);
+				lo = brcmf_sdio_regrb(bus->sdiodev,
+						      SBSDIO_FUNC1_WFRAMEBCLO,
+						      &err);
 				bus->f1regdata += 2;
 				if ((hi == 0) && (lo == 0))
 					break;
@@ -2587,10 +2552,8 @@ clkwait:
 		 else await next interrupt */
 	/* On failed register access, all bets are off:
 		 no resched or interrupts */
-	if ((bus->sdiodev->bus_if->state == BRCMF_BUS_DOWN) ||
-	    brcmf_sdcard_regfail(bus->sdiodev)) {
-		brcmf_dbg(ERROR, "failed backplane access over SDIO, halting operation %d\n",
-			  brcmf_sdcard_regfail(bus->sdiodev));
+	if ((bus->sdiodev->bus_if->state == BRCMF_BUS_DOWN) || (err != 0)) {
+		brcmf_dbg(ERROR, "failed backplane access over SDIO, halting operation\n");
 		bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 		bus->intstatus = 0;
 	} else if (bus->clkstate == CLK_PENDING) {
@@ -2886,19 +2849,16 @@ static int brcmf_tx_frame(struct brcmf_sdio *bus, u8 *frame, u16 len)
 
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_FRAMECTRL,
-				       SFC_WF_TERM, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+				 SFC_WF_TERM, NULL);
 		bus->f1regdata++;
 
 		for (i = 0; i < 3; i++) {
 			u8 hi, lo;
-			hi = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-						   SBSDIO_FUNC1_WFRAMEBCHI,
-						   NULL);
-			lo = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-						   SBSDIO_FUNC1_WFRAMEBCLO,
-						   NULL);
+			hi = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+			lo = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCLO, NULL);
 			bus->f1regdata += 2;
 			if (hi == 0 && lo == 0)
 				break;
@@ -3188,7 +3148,6 @@ static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
 
 static int brcmf_sdbrcm_download_state(struct brcmf_sdio *bus, bool enter)
 {
-	uint retries;
 	int bcmerror = 0;
 	struct chip_info *ci = bus->ci;
 
@@ -3222,7 +3181,7 @@ static int brcmf_sdbrcm_download_state(struct brcmf_sdio *bus, bool enter)
 		}
 
 		w_sdreg32(bus, 0xFFFFFFFF,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+			  offsetof(struct sdpcmd_regs, intstatus));
 
 		ci->resetcore(bus->sdiodev, ci, BCMA_CORE_ARM_CM3);
 
@@ -3444,7 +3403,6 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 	struct brcmf_sdio *bus = sdiodev->bus;
 	unsigned long timeout;
-	uint retries = 0;
 	u8 ready, enable;
 	int err, ret = 0;
 	u8 saveclk;
@@ -3472,13 +3430,11 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 		goto exit;
 
 	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk =
-	    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-				  SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	saveclk = brcmf_sdio_regrb(bus->sdiodev,
+				   SBSDIO_FUNC1_CHIPCLKCSR, &err);
 	if (!err) {
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR,
-				       (saveclk | SBSDIO_FORCE_HT), &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 (saveclk | SBSDIO_FORCE_HT), &err);
 	}
 	if (err) {
 		brcmf_dbg(ERROR, "Failed to force clock for F2: err %d\n", err);
@@ -3487,17 +3443,16 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 
 	/* Enable function 2 (frame transfers) */
 	w_sdreg32(bus, SDPCM_PROT_VERSION << SMB_DATA_VERSION_SHIFT,
-		  offsetof(struct sdpcmd_regs, tosbmailboxdata), &retries);
+		  offsetof(struct sdpcmd_regs, tosbmailboxdata));
 	enable = (SDIO_FUNC_ENABLE_1 | SDIO_FUNC_ENABLE_2);
 
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			       enable, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx, enable, NULL);
 
 	timeout = jiffies + msecs_to_jiffies(BRCMF_WAIT_F2RDY);
 	ready = 0;
 	while (enable != ready) {
-		ready = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_0,
-					      SDIO_CCCR_IORx, NULL);
+		ready = brcmf_sdio_regrb(bus->sdiodev,
+					 SDIO_CCCR_IORx, NULL);
 		if (time_after(jiffies, timeout))
 			break;
 		else if (time_after(jiffies, timeout - BRCMF_WAIT_F2RDY + 50))
@@ -3512,21 +3467,18 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 		/* Set up the interrupt mask and enable interrupts */
 		bus->hostintmask = HOSTINTMASK;
 		w_sdreg32(bus, bus->hostintmask,
-			  offsetof(struct sdpcmd_regs, hostintmask), &retries);
+			  offsetof(struct sdpcmd_regs, hostintmask));
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_WATERMARK, 8, &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_WATERMARK, 8, &err);
 	} else {
 		/* Disable F2 again */
 		enable = SDIO_FUNC_ENABLE_1;
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0,
-				       SDIO_CCCR_IOEx, enable, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx, enable, NULL);
 		ret = -ENODEV;
 	}
 
 	/* Restore previous clock setting */
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
 
 	if (ret == 0) {
 		ret = brcmf_sdio_intr_register(bus->sdiodev);
@@ -3606,9 +3558,9 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 
 			if (!bus->dpc_sched) {
 				u8 devpend;
-				devpend = brcmf_sdcard_cfg_read(bus->sdiodev,
-						SDIO_FUNC_0, SDIO_CCCR_INTx,
-						NULL);
+				devpend = brcmf_sdio_regrb(bus->sdiodev,
+							   SDIO_CCCR_INTx,
+							   NULL);
 				intstatus =
 				    devpend & (INTR_STATUS_FUNC1 |
 					       INTR_STATUS_FUNC2);
@@ -3732,24 +3684,18 @@ brcmf_sdbrcm_probe_attach(struct brcmf_sdio *bus, u32 regsva)
 
 	bus->alp_only = true;
 
-	/* Return the window to backplane enumeration space for core access */
-	if (brcmf_sdcard_set_sbaddr_window(bus->sdiodev, SI_ENUM_BASE))
-		brcmf_dbg(ERROR, "FAILED to return to SI_ENUM_BASE\n");
-
 	pr_debug("F1 signature read @0x18000000=0x%4x\n",
-		 brcmf_sdcard_reg_read(bus->sdiodev, SI_ENUM_BASE, 4));
+		 brcmf_sdio_regrl(bus->sdiodev, SI_ENUM_BASE, NULL));
 
 	/*
 	 * Force PLL off until brcmf_sdio_chip_attach()
 	 * programs PLL control regs
 	 */
 
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR,
-			       BRCMF_INIT_CLKCTL1, &err);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+			 BRCMF_INIT_CLKCTL1, &err);
 	if (!err)
-		clkctl =
-		    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+		clkctl = brcmf_sdio_regrb(bus->sdiodev,
 					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
 
 	if (err || ((clkctl & ~SBSDIO_AVBITS) != BRCMF_INIT_CLKCTL1)) {
@@ -3782,9 +3728,8 @@ brcmf_sdbrcm_probe_attach(struct brcmf_sdio *bus, u32 regsva)
 	idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
 	reg_addr = bus->ci->c_inf[idx].base +
 		   offsetof(struct sdpcmd_regs, corecontrol);
-	reg_val = brcmf_sdcard_reg_read(bus->sdiodev, reg_addr, sizeof(u32));
-	brcmf_sdcard_reg_write(bus->sdiodev, reg_addr, sizeof(u32),
-			       reg_val | CC_BPRESEN);
+	reg_val = brcmf_sdio_regrl(bus->sdiodev, reg_addr, NULL);
+	brcmf_sdio_regwl(bus->sdiodev, reg_addr, reg_val | CC_BPRESEN, NULL);
 
 	brcmu_pktq_init(&bus->txq, (PRIOMASK + 1), TXQLEN);
 
@@ -3809,16 +3754,15 @@ static bool brcmf_sdbrcm_probe_init(struct brcmf_sdio *bus)
 	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Disable F2 to clear any intermediate frame state on the dongle */
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			       SDIO_FUNC_ENABLE_1, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx,
+			 SDIO_FUNC_ENABLE_1, NULL);
 
 	bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 	bus->sleeping = false;
 	bus->rxflow = false;
 
 	/* Done with backplane-dependent accesses, can drop clock... */
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
 
 	/* ...and initialize clock/power states */
 	bus->clkstate = CLK_SDONLY;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
index 1534efc21631..f8e1f1c84d08 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
@@ -93,8 +93,9 @@ brcmf_sdio_sb_corerev(struct brcmf_sdio_dev *sdiodev,
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbidhigh), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbidhigh),
+				   NULL);
 	return SBCOREREV(regdata);
 }
 
@@ -118,8 +119,9 @@ brcmf_sdio_sb_iscoreup(struct brcmf_sdio_dev *sdiodev,
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	regdata &= (SSB_TMSLOW_RESET | SSB_TMSLOW_REJECT |
 		    SSB_IMSTATE_REJECT | SSB_TMSLOW_CLOCK);
 	return (SSB_TMSLOW_CLOCK == regdata);
@@ -135,13 +137,13 @@ brcmf_sdio_ai_iscoreup(struct brcmf_sdio_dev *sdiodev,
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
+	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+				   NULL);
 	ret = (regdata & (BCMA_IOCTL_FGC | BCMA_IOCTL_CLK)) == BCMA_IOCTL_CLK;
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-					4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+				   NULL);
 	ret = ret && ((regdata & BCMA_RESET_CTL_RESET) == 0);
 
 	return ret;
@@ -151,84 +153,85 @@ static void
 brcmf_sdio_sb_coredisable(struct brcmf_sdio_dev *sdiodev,
 			  struct chip_info *ci, u16 coreid)
 {
-	u32 regdata;
+	u32 regdata, base;
 	u8 idx;
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
+	base = ci->c_inf[idx].base;
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow), NULL);
 	if (regdata & SSB_TMSLOW_RESET)
 		return;
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow), NULL);
 	if ((regdata & SSB_TMSLOW_CLOCK) != 0) {
 		/*
 		 * set target reject and spin until busy is clear
 		 * (preserve core-specific bits)
 		 */
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
-		brcmf_sdcard_reg_write(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-				4, regdata | SSB_TMSLOW_REJECT);
-
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow),
+					   NULL);
+		brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
+				 regdata | SSB_TMSLOW_REJECT, NULL);
+
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow),
+					   NULL);
 		udelay(1);
-		SPINWAIT((brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4) &
+		SPINWAIT((brcmf_sdio_regrl(sdiodev,
+					   CORE_SB(base, sbtmstatehigh),
+					   NULL) &
 			SSB_TMSHIGH_BUSY), 100000);
 
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4);
+		regdata = brcmf_sdio_regrl(sdiodev,
+					   CORE_SB(base, sbtmstatehigh),
+					   NULL);
 		if (regdata & SSB_TMSHIGH_BUSY)
 			brcmf_dbg(ERROR, "core state still busy\n");
 
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbidlow), 4);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbidlow),
+					   NULL);
 		if (regdata & SSB_IDLOW_INITIATOR) {
-			regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4) |
-				SSB_IMSTATE_REJECT;
-			brcmf_sdcard_reg_write(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4,
-				regdata);
-			regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4);
+			regdata = brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL);
+			regdata |= SSB_IMSTATE_REJECT;
+			brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbimstate),
+					 regdata, NULL);
+			regdata = brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL);
 			udelay(1);
-			SPINWAIT((brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4) &
+			SPINWAIT((brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL) &
 				SSB_IMSTATE_BUSY), 100000);
 		}
 
 		/* set reset and reject while enabling the clocks */
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-			(SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
-			SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET));
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+		regdata = SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
+			  SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET;
+		brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
+				 regdata, NULL);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow),
+					   NULL);
 		udelay(10);
 
 		/* clear the initiator reject bit */
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbidlow), 4);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbidlow),
+					   NULL);
 		if (regdata & SSB_IDLOW_INITIATOR) {
-			regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4) &
-				~SSB_IMSTATE_REJECT;
-			brcmf_sdcard_reg_write(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4,
-				regdata);
+			regdata = brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL);
+			regdata &= ~SSB_IMSTATE_REJECT;
+			brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbimstate),
+					 regdata, NULL);
 		}
 	}
 
 	/* leave reset and reject asserted */
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-		(SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET));
+	brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
+			 (SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET), NULL);
 	udelay(1);
 }
 
@@ -242,20 +245,19 @@ brcmf_sdio_ai_coredisable(struct brcmf_sdio_dev *sdiodev,
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
 	/* if core is already in reset, just return */
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-					4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+				   NULL);
 	if ((regdata & BCMA_RESET_CTL_RESET) != 0)
 		return;
 
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			       4, 0);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL, 0, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+				   NULL);
 	udelay(10);
 
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-			       4, BCMA_RESET_CTL_RESET);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+			 BCMA_RESET_CTL_RESET, NULL);
 	udelay(1);
 }
 
@@ -279,41 +281,47 @@ brcmf_sdio_sb_resetcore(struct brcmf_sdio_dev *sdiodev,
 	 * set reset while enabling the clock and
 	 * forcing them on throughout the core
 	 */
-	brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-			SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK | SSB_TMSLOW_RESET);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	brcmf_sdio_regwl(sdiodev,
+			 CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+			 SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK | SSB_TMSLOW_RESET,
+			 NULL);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	udelay(1);
 
 	/* clear any serror */
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatehigh),
+				   NULL);
 	if (regdata & SSB_TMSHIGH_SERR)
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4, 0);
+		brcmf_sdio_regwl(sdiodev,
+				 CORE_SB(ci->c_inf[idx].base, sbtmstatehigh),
+				 0, NULL);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbimstate),
+				   NULL);
 	if (regdata & (SSB_IMSTATE_IBE | SSB_IMSTATE_TO))
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbimstate), 4,
-			regdata & ~(SSB_IMSTATE_IBE | SSB_IMSTATE_TO));
+		brcmf_sdio_regwl(sdiodev,
+				 CORE_SB(ci->c_inf[idx].base, sbimstate),
+				 regdata & ~(SSB_IMSTATE_IBE | SSB_IMSTATE_TO),
+				 NULL);
 
 	/* clear reset and allow it to propagate throughout the core */
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-		SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	brcmf_sdio_regwl(sdiodev, CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+			 SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	udelay(1);
 
 	/* leave clock enabled */
-	brcmf_sdcard_reg_write(sdiodev,
-			       CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-			       4, SSB_TMSLOW_CLOCK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	brcmf_sdio_regwl(sdiodev, CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+			 SSB_TMSLOW_CLOCK, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	udelay(1);
 }
 
@@ -330,18 +338,18 @@ brcmf_sdio_ai_resetcore(struct brcmf_sdio_dev *sdiodev,
 	brcmf_sdio_ai_coredisable(sdiodev, ci, coreid);
 
 	/* now do initialization sequence */
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			       4, BCMA_IOCTL_FGC | BCMA_IOCTL_CLK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-			       4, 0);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+			 BCMA_IOCTL_FGC | BCMA_IOCTL_CLK, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+				   NULL);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+			 0, NULL);
 	udelay(1);
 
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			       4, BCMA_IOCTL_CLK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+			 BCMA_IOCTL_CLK, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+				   NULL);
 	udelay(1);
 }
 
@@ -358,8 +366,9 @@ static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
 	 */
 	ci->c_inf[0].id = BCMA_CORE_CHIPCOMMON;
 	ci->c_inf[0].base = regs;
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipid), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_CC_REG(ci->c_inf[0].base, chipid),
+				   NULL);
 	ci->chip = regdata & CID_ID_MASK;
 	ci->chiprev = (regdata & CID_REV_MASK) >> CID_REV_SHIFT;
 	ci->socitype = (regdata & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
@@ -428,8 +437,7 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 
 	/* Try forcing SDIO core to do ALPAvail request only */
 	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR,	clkset, &err);
+	brcmf_sdio_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
 	if (err) {
 		brcmf_dbg(ERROR, "error writing for HT off\n");
 		return err;
@@ -437,8 +445,8 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 
 	/* If register supported, wait for ALPAvail and then force ALP */
 	/* This may take up to 15 milliseconds */
-	clkval = brcmf_sdcard_cfg_read(sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR, NULL);
+	clkval = brcmf_sdio_regrb(sdiodev,
+				  SBSDIO_FUNC1_CHIPCLKCSR, NULL);
 
 	if ((clkval & ~SBSDIO_AVBITS) != clkset) {
 		brcmf_dbg(ERROR, "ChipClkCSR access: wrote 0x%02x read 0x%02x\n",
@@ -446,8 +454,8 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 		return -EACCES;
 	}
 
-	SPINWAIT(((clkval = brcmf_sdcard_cfg_read(sdiodev, SDIO_FUNC_1,
-				SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
+	SPINWAIT(((clkval = brcmf_sdio_regrb(sdiodev,
+					     SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
 			!SBSDIO_ALPAV(clkval)),
 			PMU_MAX_TRANSITION_DLY);
 	if (!SBSDIO_ALPAV(clkval)) {
@@ -457,13 +465,11 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 	}
 
 	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
+	brcmf_sdio_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
 	udelay(65);
 
 	/* Also, disable the extra SDIO pull-ups */
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
+	brcmf_sdio_regwb(sdiodev, SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
 
 	return 0;
 }
@@ -472,18 +478,22 @@ static void
 brcmf_sdio_chip_buscoresetup(struct brcmf_sdio_dev *sdiodev,
 			     struct chip_info *ci)
 {
+	u32 base = ci->c_inf[0].base;
+
 	/* get chipcommon rev */
 	ci->c_inf[0].rev = ci->corerev(sdiodev, ci, ci->c_inf[0].id);
 
 	/* get chipcommon capabilites */
-	ci->c_inf[0].caps =
-		brcmf_sdcard_reg_read(sdiodev,
-		CORE_CC_REG(ci->c_inf[0].base, capabilities), 4);
+	ci->c_inf[0].caps = brcmf_sdio_regrl(sdiodev,
+					     CORE_CC_REG(base, capabilities),
+					     NULL);
 
 	/* get pmu caps & rev */
 	if (ci->c_inf[0].caps & CC_CAP_PMU) {
-		ci->pmucaps = brcmf_sdcard_reg_read(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, pmucapabilities), 4);
+		ci->pmucaps =
+			brcmf_sdio_regrl(sdiodev,
+					 CORE_CC_REG(base, pmucapabilities),
+					 NULL);
 		ci->pmurev = ci->pmucaps & PCAP_REV_MASK;
 	}
 
@@ -523,10 +533,10 @@ int brcmf_sdio_chip_attach(struct brcmf_sdio_dev *sdiodev,
 
 	brcmf_sdio_chip_buscoresetup(sdiodev, ci);
 
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_CC_REG(ci->c_inf[0].base, gpiopullup), 4, 0);
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_CC_REG(ci->c_inf[0].base, gpiopulldown), 4, 0);
+	brcmf_sdio_regwl(sdiodev, CORE_CC_REG(ci->c_inf[0].base, gpiopullup),
+			 0, NULL);
+	brcmf_sdio_regwl(sdiodev, CORE_CC_REG(ci->c_inf[0].base, gpiopulldown),
+			 0, NULL);
 
 	*ci_ptr = ci;
 	return 0;
@@ -562,6 +572,7 @@ brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
 	u32 str_mask = 0;
 	u32 str_shift = 0;
 	char chn[8];
+	u32 base = ci->c_inf[0].base;
 
 	if (!(ci->c_inf[0].caps & CC_CAP_PMU))
 		return;
@@ -591,17 +602,17 @@ brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
 			}
 		}
 
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipcontrol_addr),
-			4, 1);
-		cc_data_temp = brcmf_sdcard_reg_read(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipcontrol_addr), 4);
+		brcmf_sdio_regwl(sdiodev, CORE_CC_REG(base, chipcontrol_addr),
+				 1, NULL);
+		cc_data_temp =
+			brcmf_sdio_regrl(sdiodev,
+					 CORE_CC_REG(base, chipcontrol_addr),
+					 NULL);
 		cc_data_temp &= ~str_mask;
 		drivestrength_sel <<= str_shift;
 		cc_data_temp |= drivestrength_sel;
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipcontrol_addr),
-			4, cc_data_temp);
+		brcmf_sdio_regwl(sdiodev, CORE_CC_REG(base, chipcontrol_addr),
+				 cc_data_temp, NULL);
 
 		brcmf_dbg(INFO, "SDIO: %dmA drive strength selected, set to 0x%08x\n",
 			  drivestrength, cc_data_temp);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
index 7010eaf71f99..29bf78d264e0 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
@@ -40,6 +40,10 @@
 /* Maximum number of I/O funcs */
 #define SDIOD_MAX_IOFUNCS	7
 
+/* mask of register map */
+#define REG_F0_REG_MASK		0x7FF
+#define REG_F1_MISC_MASK	0x1FFFF
+
 /* as of sdiod rev 0, supports 3 functions */
 #define SBSDIO_NUM_FUNCTION		3
 
@@ -142,7 +146,6 @@ struct brcmf_sdio_dev {
 	u8 num_funcs;			/* Supported funcs on client */
 	u32 func_cis_ptr[SDIOD_MAX_IOFUNCS];
 	u32 sbwad;			/* Save backplane window address */
-	bool regfail;			/* status of last reg_r/w call */
 	void *bus;
 	atomic_t suspend;		/* suspend flag */
 	wait_queue_head_t request_byte_wait;
@@ -164,31 +167,13 @@ struct brcmf_sdio_dev {
 extern int brcmf_sdio_intr_register(struct brcmf_sdio_dev *sdiodev);
 extern int brcmf_sdio_intr_unregister(struct brcmf_sdio_dev *sdiodev);
 
-/* Access SDIO address space (e.g. CCCR) using CMD52 (single-byte interface).
- *   fn:   function number
- *   addr: unmodified SDIO-space address
- *   data: data byte to write
- *   err:  pointer to error code (or NULL)
- */
-extern u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_dev *sdiodev, uint func,
-				u32 addr, int *err);
-extern void brcmf_sdcard_cfg_write(struct brcmf_sdio_dev *sdiodev, uint func,
-				   u32 addr, u8 data, int *err);
-
-/* Synchronous access to device (client) core registers via CMD53 to F1.
- *   addr: backplane address (i.e. >= regsva from attach)
- *   size: register width in bytes (2 or 4)
- *   data: data for register write
- */
-extern u32
-brcmf_sdcard_reg_read(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size);
-
-extern u32
-brcmf_sdcard_reg_write(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size,
-		       u32 data);
-
-/* Indicate if last reg read/write failed */
-extern bool brcmf_sdcard_regfail(struct brcmf_sdio_dev *sdiodev);
+/* sdio device register access interface */
+extern u8 brcmf_sdio_regrb(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret);
+extern u32 brcmf_sdio_regrl(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret);
+extern void brcmf_sdio_regwb(struct brcmf_sdio_dev *sdiodev, u32 addr,
+			     u8 data, int *ret);
+extern void brcmf_sdio_regwl(struct brcmf_sdio_dev *sdiodev, u32 addr,
+			     u32 data, int *ret);
 
 /* Buffer transfer to/from device (client) core via cmd53.
  *   fn:       function number
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/Makefile b/drivers/net/wireless/brcm80211/brcmsmac/Makefile
index c2eb2d0af386..e227c4c68ef9 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmsmac/Makefile
@@ -39,10 +39,7 @@ BRCMSMAC_OFILES := \
 	phy/phytbl_lcn.o \
 	phy/phytbl_n.o \
 	phy/phy_qmath.o \
-	otp.o \
-	srom.o \
 	dma.o \
-	nicpci.o \
 	brcms_trace_events.o
 
 MODULEPFX := brcmsmac
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
index c93ea35bceec..6d8b7213643a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
@@ -19,7 +19,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/delay.h>
-#include <linux/pci.h>
 
 #include <defs.h>
 #include <chipcommon.h>
@@ -29,8 +28,6 @@
 #include "types.h"
 #include "pub.h"
 #include "pmu.h"
-#include "srom.h"
-#include "nicpci.h"
 #include "aiutils.h"
 
 /* slow_clk_ctl */
@@ -321,7 +318,6 @@
 #define	IS_SIM(chippkg)	\
 	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
 
-#define PCI(sih)	(ai_get_buscoretype(sih) == PCI_CORE_ID)
 #define PCIE(sih)	(ai_get_buscoretype(sih) == PCIE_CORE_ID)
 
 #define PCI_FORCEHT(sih) (PCIE(sih) && (ai_get_chip_id(sih) == BCM4716_CHIP_ID))
@@ -454,36 +450,9 @@ struct aidmp {
 	u32 componentid3;	/* 0xffc */
 };
 
-/* return true if PCIE capability exists in the pci config space */
-static bool ai_ispcie(struct si_info *sii)
-{
-	u8 cap_ptr;
-
-	cap_ptr =
-	    pcicore_find_pci_capability(sii->pcibus, PCI_CAP_ID_EXP, NULL,
-					NULL);
-	if (!cap_ptr)
-		return false;
-
-	return true;
-}
-
-static bool ai_buscore_prep(struct si_info *sii)
-{
-	/* kludge to enable the clock on the 4306 which lacks a slowclock */
-	if (!ai_ispcie(sii))
-		ai_clkctl_xtal(&sii->pub, XTAL | PLL, ON);
-	return true;
-}
-
 static bool
 ai_buscore_setup(struct si_info *sii, struct bcma_device *cc)
 {
-	struct bcma_device *pci = NULL;
-	struct bcma_device *pcie = NULL;
-	struct bcma_device *core;
-
-
 	/* no cores found, bail out */
 	if (cc->bus->nr_cores == 0)
 		return false;
@@ -492,8 +461,7 @@ ai_buscore_setup(struct si_info *sii, struct bcma_device *cc)
 	sii->pub.ccrev = cc->id.rev;
 
 	/* get chipcommon chipstatus */
-	if (ai_get_ccrev(&sii->pub) >= 11)
-		sii->chipst = bcma_read32(cc, CHIPCREGOFFS(chipstatus));
+	sii->chipst = bcma_read32(cc, CHIPCREGOFFS(chipstatus));
 
 	/* get chipcommon capabilites */
 	sii->pub.cccaps = bcma_read32(cc, CHIPCREGOFFS(capabilities));
@@ -506,64 +474,18 @@ ai_buscore_setup(struct si_info *sii, struct bcma_device *cc)
 	}
 
 	/* figure out buscore */
-	list_for_each_entry(core, &cc->bus->cores, list) {
-		uint cid, crev;
-
-		cid = core->id.id;
-		crev = core->id.rev;
-
-		if (cid == PCI_CORE_ID) {
-			pci = core;
-		} else if (cid == PCIE_CORE_ID) {
-			pcie = core;
-		}
-	}
-
-	if (pci && pcie) {
-		if (ai_ispcie(sii))
-			pci = NULL;
-		else
-			pcie = NULL;
-	}
-	if (pci) {
-		sii->buscore = pci;
-	} else if (pcie) {
-		sii->buscore = pcie;
-	}
-
-	/* fixup necessary chip/core configurations */
-	if (!sii->pch) {
-		sii->pch = pcicore_init(&sii->pub, sii->icbus->drv_pci.core);
-		if (sii->pch == NULL)
-			return false;
-	}
-	if (ai_pci_fixcfg(&sii->pub))
-		return false;
+	sii->buscore = ai_findcore(&sii->pub, PCIE_CORE_ID, 0);
 
 	return true;
 }
 
-/*
- * get boardtype and boardrev
- */
-static __used void ai_nvram_process(struct si_info *sii)
-{
-	uint w = 0;
-
-	/* do a pci config read to get subsystem id and subvendor id */
-	pci_read_config_dword(sii->pcibus, PCI_SUBSYSTEM_VENDOR_ID, &w);
-
-	sii->pub.boardvendor = w & 0xffff;
-	sii->pub.boardtype = (w >> 16) & 0xffff;
-}
-
 static struct si_info *ai_doattach(struct si_info *sii,
 				   struct bcma_bus *pbus)
 {
 	struct si_pub *sih = &sii->pub;
 	u32 w, savewin;
 	struct bcma_device *cc;
-	uint socitype;
+	struct ssb_sprom *sprom = &pbus->sprom;
 
 	savewin = 0;
 
@@ -573,38 +495,15 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	/* switch to Chipcommon core */
 	cc = pbus->drv_cc.core;
 
-	/* bus/core/clk setup for register access */
-	if (!ai_buscore_prep(sii))
-		return NULL;
+	sih->chip = pbus->chipinfo.id;
+	sih->chiprev = pbus->chipinfo.rev;
+	sih->chippkg = pbus->chipinfo.pkg;
+	sih->boardvendor = pbus->boardinfo.vendor;
+	sih->boardtype = pbus->boardinfo.type;
 
-	/*
-	 * ChipID recognition.
-	 *   We assume we can read chipid at offset 0 from the regs arg.
-	 *   If we add other chiptypes (or if we need to support old sdio
-	 *   hosts w/o chipcommon), some way of recognizing them needs to
-	 *   be added here.
-	 */
-	w = bcma_read32(cc, CHIPCREGOFFS(chipid));
-	socitype = (w & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
-	/* Might as wll fill in chip id rev & pkg */
-	sih->chip = w & CID_ID_MASK;
-	sih->chiprev = (w & CID_REV_MASK) >> CID_REV_SHIFT;
-	sih->chippkg = (w & CID_PKG_MASK) >> CID_PKG_SHIFT;
-
-	/* scan for cores */
-	if (socitype != SOCI_AI)
-		return NULL;
-
-	SI_MSG("Found chip type AI (0x%08x)\n", w);
 	if (!ai_buscore_setup(sii, cc))
 		goto exit;
 
-	/* Init nvram from sprom/otp if they exist */
-	if (srom_var_init(&sii->pub))
-		goto exit;
-
-	ai_nvram_process(sii);
-
 	/* === NVRAM, clock is ready === */
 	bcma_write32(cc, CHIPCREGOFFS(gpiopullup), 0);
 	bcma_write32(cc, CHIPCREGOFFS(gpiopulldown), 0);
@@ -617,15 +516,13 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	}
 
 	/* setup the GPIO based LED powersave register */
-	w = getintvar(sih, BRCMS_SROM_LEDDC);
+	w = (sprom->leddc_on_time << BCMA_CC_GPIOTIMER_ONTIME_SHIFT) |
+		 (sprom->leddc_off_time << BCMA_CC_GPIOTIMER_OFFTIME_SHIFT);
 	if (w == 0)
 		w = DEFAULT_GPIOTIMERVAL;
 	ai_cc_reg(sih, offsetof(struct chipcregs, gpiotimerval),
 		  ~0, w);
 
-	if (PCIE(sih))
-		pcicore_attach(sii->pch, SI_DOATTACH);
-
 	if (ai_get_chip_id(sih) == BCM43224_CHIP_ID) {
 		/*
 		 * enable 12 mA drive strenth for 43224 and
@@ -659,9 +556,6 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	return sii;
 
  exit:
-	if (sii->pch)
-		pcicore_deinit(sii->pch);
-	sii->pch = NULL;
 
 	return NULL;
 }
@@ -700,11 +594,6 @@ void ai_detach(struct si_pub *sih)
 	if (sii == NULL)
 		return;
 
-	if (sii->pch)
-		pcicore_deinit(sii->pch);
-	sii->pch = NULL;
-
-	srom_free_vars(sih);
 	kfree(sii);
 }
 
@@ -755,21 +644,7 @@ uint ai_cc_reg(struct si_pub *sih, uint regoff, u32 mask, u32 val)
 /* return the slow clock source - LPO, XTAL, or PCI */
 static uint ai_slowclk_src(struct si_pub *sih, struct bcma_device *cc)
 {
-	struct si_info *sii;
-	u32 val;
-
-	sii = (struct si_info *)sih;
-	if (ai_get_ccrev(&sii->pub) < 6) {
-		pci_read_config_dword(sii->pcibus, PCI_GPIO_OUT,
-				      &val);
-		if (val & PCI_CFG_GPIO_SCS)
-			return SCC_SS_PCI;
-		return SCC_SS_XTAL;
-	} else if (ai_get_ccrev(&sii->pub) < 10) {
-		return bcma_read32(cc, CHIPCREGOFFS(slow_clk_ctl)) &
-		       SCC_SS_MASK;
-	} else			/* Insta-clock */
-		return SCC_SS_XTAL;
+	return SCC_SS_XTAL;
 }
 
 /*
@@ -779,36 +654,12 @@ static uint ai_slowclk_src(struct si_pub *sih, struct bcma_device *cc)
 static uint ai_slowclk_freq(struct si_pub *sih, bool max_freq,
 			    struct bcma_device *cc)
 {
-	u32 slowclk;
 	uint div;
 
-	slowclk = ai_slowclk_src(sih, cc);
-	if (ai_get_ccrev(sih) < 6) {
-		if (slowclk == SCC_SS_PCI)
-			return max_freq ? (PCIMAXFREQ / 64)
-				: (PCIMINFREQ / 64);
-		else
-			return max_freq ? (XTALMAXFREQ / 32)
-				: (XTALMINFREQ / 32);
-	} else if (ai_get_ccrev(sih) < 10) {
-		div = 4 *
-		    (((bcma_read32(cc, CHIPCREGOFFS(slow_clk_ctl)) &
-		      SCC_CD_MASK) >> SCC_CD_SHIFT) + 1);
-		if (slowclk == SCC_SS_LPO)
-			return max_freq ? LPOMAXFREQ : LPOMINFREQ;
-		else if (slowclk == SCC_SS_XTAL)
-			return max_freq ? (XTALMAXFREQ / div)
-				: (XTALMINFREQ / div);
-		else if (slowclk == SCC_SS_PCI)
-			return max_freq ? (PCIMAXFREQ / div)
-				: (PCIMINFREQ / div);
-	} else {
-		/* Chipc rev 10 is InstaClock */
-		div = bcma_read32(cc, CHIPCREGOFFS(system_clk_ctl));
-		div = 4 * ((div >> SYCC_CD_SHIFT) + 1);
-		return max_freq ? XTALMAXFREQ : (XTALMINFREQ / div);
-	}
-	return 0;
+	/* Chipc rev 10 is InstaClock */
+	div = bcma_read32(cc, CHIPCREGOFFS(system_clk_ctl));
+	div = 4 * ((div >> SYCC_CD_SHIFT) + 1);
+	return max_freq ? XTALMAXFREQ : (XTALMINFREQ / div);
 }
 
 static void
@@ -831,8 +682,7 @@ ai_clkctl_setdelay(struct si_pub *sih, struct bcma_device *cc)
 
 	/* Starting with 4318 it is ILP that is used for the delays */
 	slowmaxfreq =
-	    ai_slowclk_freq(sih,
-			    (ai_get_ccrev(sih) >= 10) ? false : true, cc);
+	    ai_slowclk_freq(sih, false, cc);
 
 	pll_on_delay = ((slowmaxfreq * pll_delay) + 999999) / 1000000;
 	fref_sel_delay = ((slowmaxfreq * FREF_DELAY) + 999999) / 1000000;
@@ -854,9 +704,8 @@ void ai_clkctl_init(struct si_pub *sih)
 		return;
 
 	/* set all Instaclk chip ILP to 1 MHz */
-	if (ai_get_ccrev(sih) >= 10)
-		bcma_maskset32(cc, CHIPCREGOFFS(system_clk_ctl), SYCC_CD_MASK,
-			       (ILP_DIV_1MHZ << SYCC_CD_SHIFT));
+	bcma_maskset32(cc, CHIPCREGOFFS(system_clk_ctl), SYCC_CD_MASK,
+		       (ILP_DIV_1MHZ << SYCC_CD_SHIFT));
 
 	ai_clkctl_setdelay(sih, cc);
 }
@@ -891,140 +740,6 @@ u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih)
 	return fpdelay;
 }
 
-/* turn primary xtal and/or pll off/on */
-int ai_clkctl_xtal(struct si_pub *sih, uint what, bool on)
-{
-	struct si_info *sii;
-	u32 in, out, outen;
-
-	sii = (struct si_info *)sih;
-
-	/* pcie core doesn't have any mapping to control the xtal pu */
-	if (PCIE(sih))
-		return -1;
-
-	pci_read_config_dword(sii->pcibus, PCI_GPIO_IN, &in);
-	pci_read_config_dword(sii->pcibus, PCI_GPIO_OUT, &out);
-	pci_read_config_dword(sii->pcibus, PCI_GPIO_OUTEN, &outen);
-
-	/*
-	 * Avoid glitching the clock if GPRS is already using it.
-	 * We can't actually read the state of the PLLPD so we infer it
-	 * by the value of XTAL_PU which *is* readable via gpioin.
-	 */
-	if (on && (in & PCI_CFG_GPIO_XTAL))
-		return 0;
-
-	if (what & XTAL)
-		outen |= PCI_CFG_GPIO_XTAL;
-	if (what & PLL)
-		outen |= PCI_CFG_GPIO_PLL;
-
-	if (on) {
-		/* turn primary xtal on */
-		if (what & XTAL) {
-			out |= PCI_CFG_GPIO_XTAL;
-			if (what & PLL)
-				out |= PCI_CFG_GPIO_PLL;
-			pci_write_config_dword(sii->pcibus,
-					       PCI_GPIO_OUT, out);
-			pci_write_config_dword(sii->pcibus,
-					       PCI_GPIO_OUTEN, outen);
-			udelay(XTAL_ON_DELAY);
-		}
-
-		/* turn pll on */
-		if (what & PLL) {
-			out &= ~PCI_CFG_GPIO_PLL;
-			pci_write_config_dword(sii->pcibus,
-					       PCI_GPIO_OUT, out);
-			mdelay(2);
-		}
-	} else {
-		if (what & XTAL)
-			out &= ~PCI_CFG_GPIO_XTAL;
-		if (what & PLL)
-			out |= PCI_CFG_GPIO_PLL;
-		pci_write_config_dword(sii->pcibus,
-				       PCI_GPIO_OUT, out);
-		pci_write_config_dword(sii->pcibus,
-				       PCI_GPIO_OUTEN, outen);
-	}
-
-	return 0;
-}
-
-/* clk control mechanism through chipcommon, no policy checking */
-static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
-{
-	struct bcma_device *cc;
-	u32 scc;
-
-	/* chipcommon cores prior to rev6 don't support dynamic clock control */
-	if (ai_get_ccrev(&sii->pub) < 6)
-		return false;
-
-	cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
-
-	if (!(ai_get_cccaps(&sii->pub) & CC_CAP_PWR_CTL) &&
-	    (ai_get_ccrev(&sii->pub) < 20))
-		return mode == CLK_FAST;
-
-	switch (mode) {
-	case CLK_FAST:		/* FORCEHT, fast (pll) clock */
-		if (ai_get_ccrev(&sii->pub) < 10) {
-			/*
-			 * don't forget to force xtal back
-			 * on before we clear SCC_DYN_XTAL..
-			 */
-			ai_clkctl_xtal(&sii->pub, XTAL, ON);
-			bcma_maskset32(cc, CHIPCREGOFFS(slow_clk_ctl),
-				       (SCC_XC | SCC_FS | SCC_IP), SCC_IP);
-		} else if (ai_get_ccrev(&sii->pub) < 20) {
-			bcma_set32(cc, CHIPCREGOFFS(system_clk_ctl), SYCC_HR);
-		} else {
-			bcma_set32(cc, CHIPCREGOFFS(clk_ctl_st), CCS_FORCEHT);
-		}
-
-		/* wait for the PLL */
-		if (ai_get_cccaps(&sii->pub) & CC_CAP_PMU) {
-			u32 htavail = CCS_HTAVAIL;
-			SPINWAIT(((bcma_read32(cc, CHIPCREGOFFS(clk_ctl_st)) &
-				   htavail) == 0), PMU_MAX_TRANSITION_DLY);
-		} else {
-			udelay(PLL_DELAY);
-		}
-		break;
-
-	case CLK_DYNAMIC:	/* enable dynamic clock control */
-		if (ai_get_ccrev(&sii->pub) < 10) {
-			scc = bcma_read32(cc, CHIPCREGOFFS(slow_clk_ctl));
-			scc &= ~(SCC_FS | SCC_IP | SCC_XC);
-			if ((scc & SCC_SS_MASK) != SCC_SS_XTAL)
-				scc |= SCC_XC;
-			bcma_write32(cc, CHIPCREGOFFS(slow_clk_ctl), scc);
-
-			/*
-			 * for dynamic control, we have to
-			 * release our xtal_pu "force on"
-			 */
-			if (scc & SCC_XC)
-				ai_clkctl_xtal(&sii->pub, XTAL, OFF);
-		} else if (ai_get_ccrev(&sii->pub) < 20) {
-			/* Instaclock */
-			bcma_mask32(cc, CHIPCREGOFFS(system_clk_ctl), ~SYCC_HR);
-		} else {
-			bcma_mask32(cc, CHIPCREGOFFS(clk_ctl_st), ~CCS_FORCEHT);
-		}
-		break;
-
-	default:
-		break;
-	}
-
-	return mode == CLK_FAST;
-}
-
 /*
  *  clock control policy function throught chipcommon
  *
@@ -1033,133 +748,53 @@ static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
  *    this is a wrapper over the next internal function
  *      to allow flexible policy settings for outside caller
  */
-bool ai_clkctl_cc(struct si_pub *sih, uint mode)
+bool ai_clkctl_cc(struct si_pub *sih, enum bcma_clkmode mode)
 {
 	struct si_info *sii;
+	struct bcma_device *cc;
 
 	sii = (struct si_info *)sih;
 
-	/* chipcommon cores prior to rev6 don't support dynamic clock control */
-	if (ai_get_ccrev(sih) < 6)
-		return false;
-
 	if (PCI_FORCEHT(sih))
-		return mode == CLK_FAST;
+		return mode == BCMA_CLKMODE_FAST;
 
-	return _ai_clkctl_cc(sii, mode);
+	cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
+	bcma_core_set_clockmode(cc, mode);
+	return mode == BCMA_CLKMODE_FAST;
 }
 
 void ai_pci_up(struct si_pub *sih)
 {
 	struct si_info *sii;
+	struct bcma_device *cc;
 
 	sii = (struct si_info *)sih;
 
-	if (PCI_FORCEHT(sih))
-		_ai_clkctl_cc(sii, CLK_FAST);
+	if (PCI_FORCEHT(sih)) {
+		cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
+		bcma_core_set_clockmode(cc, BCMA_CLKMODE_FAST);
+	}
 
 	if (PCIE(sih))
-		pcicore_up(sii->pch, SI_PCIUP);
-
-}
-
-/* Unconfigure and/or apply various WARs when system is going to sleep mode */
-void ai_pci_sleep(struct si_pub *sih)
-{
-	struct si_info *sii;
-
-	sii = (struct si_info *)sih;
-
-	pcicore_sleep(sii->pch);
+		bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, true);
 }
 
 /* Unconfigure and/or apply various WARs when going down */
 void ai_pci_down(struct si_pub *sih)
 {
 	struct si_info *sii;
+	struct bcma_device *cc;
 
 	sii = (struct si_info *)sih;
 
 	/* release FORCEHT since chip is going to "down" state */
-	if (PCI_FORCEHT(sih))
-		_ai_clkctl_cc(sii, CLK_DYNAMIC);
-
-	pcicore_down(sii->pch, SI_PCIDOWN);
-}
-
-/*
- * Configure the pci core for pci client (NIC) action
- * coremask is the bitvec of cores by index to be enabled.
- */
-void ai_pci_setup(struct si_pub *sih, uint coremask)
-{
-	struct si_info *sii;
-	u32 w;
-
-	sii = (struct si_info *)sih;
-
-	/*
-	 * Enable sb->pci interrupts.  Assume
-	 * PCI rev 2.3 support was added in pci core rev 6 and things changed..
-	 */
-	if (PCIE(sih) || (PCI(sih) && (ai_get_buscorerev(sih) >= 6))) {
-		/* pci config write to set this core bit in PCIIntMask */
-		pci_read_config_dword(sii->pcibus, PCI_INT_MASK, &w);
-		w |= (coremask << PCI_SBIM_SHIFT);
-		pci_write_config_dword(sii->pcibus, PCI_INT_MASK, w);
-	}
-
-	if (PCI(sih)) {
-		pcicore_pci_setup(sii->pch);
+	if (PCI_FORCEHT(sih)) {
+		cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
+		bcma_core_set_clockmode(cc, BCMA_CLKMODE_DYNAMIC);
 	}
-}
 
-/*
- * Fixup SROMless PCI device's configuration.
- * The current core may be changed upon return.
- */
-int ai_pci_fixcfg(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-
-	/* Fixup PI in SROM shadow area to enable the correct PCI core access */
-	/* check 'pi' is correct and fix it if not */
-	pcicore_fixcfg(sii->pch);
-	pcicore_hwup(sii->pch);
-	return 0;
-}
-
-/* mask&set gpiocontrol bits */
-u32 ai_gpiocontrol(struct si_pub *sih, u32 mask, u32 val, u8 priority)
-{
-	uint regoff;
-
-	regoff = offsetof(struct chipcregs, gpiocontrol);
-	return ai_cc_reg(sih, regoff, mask, val);
-}
-
-void ai_chipcontrl_epa4331(struct si_pub *sih, bool on)
-{
-	struct bcma_device *cc;
-	u32 val;
-
-	cc = ai_findcore(sih, CC_CORE_ID, 0);
-
-	if (on) {
-		if (ai_get_chippkg(sih) == 9 || ai_get_chippkg(sih) == 0xb)
-			/* Ext PA Controls for 4331 12x9 Package */
-			bcma_set32(cc, CHIPCREGOFFS(chipcontrol),
-				   CCTRL4331_EXTPA_EN |
-				   CCTRL4331_EXTPA_ON_GPIO2_5);
-		else
-			/* Ext PA Controls for 4331 12x12 Package */
-			bcma_set32(cc, CHIPCREGOFFS(chipcontrol),
-				   CCTRL4331_EXTPA_EN);
-	} else {
-		val &= ~(CCTRL4331_EXTPA_EN | CCTRL4331_EXTPA_ON_GPIO2_5);
-		bcma_mask32(cc, CHIPCREGOFFS(chipcontrol),
-			    ~(CCTRL4331_EXTPA_EN | CCTRL4331_EXTPA_ON_GPIO2_5));
-	}
+	if (PCIE(sih))
+		bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, false);
 }
 
 /* Enable BT-COEX & Ex-PA for 4313 */
@@ -1181,6 +816,9 @@ bool ai_deviceremoved(struct si_pub *sih)
 
 	sii = (struct si_info *)sih;
 
+	if (sii->icbus->hosttype != BCMA_HOSTTYPE_PCI)
+		return false;
+
 	pci_read_config_dword(sii->pcibus, PCI_VENDOR_ID, &w);
 	if ((w & 0xFFFF) != PCI_VENDOR_ID_BROADCOM)
 		return true;
@@ -1188,45 +826,6 @@ bool ai_deviceremoved(struct si_pub *sih)
 	return false;
 }
 
-bool ai_is_sprom_available(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-
-	if (ai_get_ccrev(sih) >= 31) {
-		struct bcma_device *cc;
-		u32 sromctrl;
-
-		if ((ai_get_cccaps(sih) & CC_CAP_SROM) == 0)
-			return false;
-
-		cc = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-		sromctrl = bcma_read32(cc, CHIPCREGOFFS(sromcontrol));
-		return sromctrl & SRC_PRESENT;
-	}
-
-	switch (ai_get_chip_id(sih)) {
-	case BCM4313_CHIP_ID:
-		return (sii->chipst & CST4313_SPROM_PRESENT) != 0;
-	default:
-		return true;
-	}
-}
-
-bool ai_is_otp_disabled(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-
-	switch (ai_get_chip_id(sih)) {
-	case BCM4313_CHIP_ID:
-		return (sii->chipst & CST4313_OTP_PRESENT) == 0;
-		/* These chips always have their OTP on */
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-	default:
-		return false;
-	}
-}
-
 uint ai_get_buscoretype(struct si_pub *sih)
 {
 	struct si_info *sii = (struct si_info *)sih;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
index f84c6f781692..d9f04a683bdb 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
@@ -113,10 +113,6 @@
 #define	XTAL			0x1	/* primary crystal oscillator (2050) */
 #define	PLL			0x2	/* main chip pll */
 
-/* clkctl clk mode */
-#define	CLK_FAST		0	/* force fast (pll) clock */
-#define	CLK_DYNAMIC		2	/* enable dynamic clock control */
-
 /* GPIO usage priorities */
 #define GPIO_DRV_PRIORITY	0	/* Driver */
 #define GPIO_APP_PRIORITY	1	/* Application */
@@ -172,9 +168,7 @@ struct si_info {
 	struct si_pub pub;	/* back plane public state (must be first) */
 	struct bcma_bus *icbus;	/* handle to soc interconnect bus */
 	struct pci_dev *pcibus;	/* handle to pci bus */
-	struct pcicore_info *pch; /* PCI/E core handle */
 	struct bcma_device *buscore;
-	struct list_head var_list; /* list of srom variables */
 
 	u32 chipst;		/* chip status */
 };
@@ -197,38 +191,20 @@ extern u32 ai_core_cflags(struct bcma_device *core, u32 mask, u32 val);
 extern struct si_pub *ai_attach(struct bcma_bus *pbus);
 extern void ai_detach(struct si_pub *sih);
 extern uint ai_cc_reg(struct si_pub *sih, uint regoff, u32 mask, u32 val);
-extern void ai_pci_setup(struct si_pub *sih, uint coremask);
 extern void ai_clkctl_init(struct si_pub *sih);
 extern u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih);
 extern bool ai_clkctl_cc(struct si_pub *sih, uint mode);
-extern int ai_clkctl_xtal(struct si_pub *sih, uint what, bool on);
 extern bool ai_deviceremoved(struct si_pub *sih);
-extern u32 ai_gpiocontrol(struct si_pub *sih, u32 mask, u32 val,
-			     u8 priority);
-
-/* OTP status */
-extern bool ai_is_otp_disabled(struct si_pub *sih);
-
-/* SPROM availability */
-extern bool ai_is_sprom_available(struct si_pub *sih);
 
-extern void ai_pci_sleep(struct si_pub *sih);
 extern void ai_pci_down(struct si_pub *sih);
 extern void ai_pci_up(struct si_pub *sih);
-extern int ai_pci_fixcfg(struct si_pub *sih);
 
-extern void ai_chipcontrl_epa4331(struct si_pub *sih, bool on);
 /* Enable Ex-PA for 4313 */
 extern void ai_epa_4313war(struct si_pub *sih);
 
 extern uint ai_get_buscoretype(struct si_pub *sih);
 extern uint ai_get_buscorerev(struct si_pub *sih);
 
-static inline int ai_get_ccrev(struct si_pub *sih)
-{
-	return sih->ccrev;
-}
-
 static inline u32 ai_get_cccaps(struct si_pub *sih)
 {
 	return sih->cccaps;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/antsel.c b/drivers/net/wireless/brcm80211/brcmsmac/antsel.c
index a47ce25cb9a2..55e12c327911 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/antsel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/antsel.c
@@ -108,7 +108,7 @@ brcms_c_antsel_init_cfg(struct antsel_info *asi, struct brcms_antselcfg *antsel,
 struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 {
 	struct antsel_info *asi;
-	struct si_pub *sih = wlc->hw->sih;
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
 
 	asi = kzalloc(sizeof(struct antsel_info), GFP_ATOMIC);
 	if (!asi)
@@ -118,7 +118,7 @@ struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 	asi->pub = wlc->pub;
 	asi->antsel_type = ANTSEL_NA;
 	asi->antsel_avail = false;
-	asi->antsel_antswitch = (u8) getintvar(sih, BRCMS_SROM_ANTSWITCH);
+	asi->antsel_antswitch = sprom->antswitch;
 
 	if ((asi->pub->sromrev >= 4) && (asi->antsel_antswitch != 0)) {
 		switch (asi->antsel_antswitch) {
@@ -128,12 +128,12 @@ struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 			/* 4321/2 board with 2x3 switch logic */
 			asi->antsel_type = ANTSEL_2x3;
 			/* Antenna selection availability */
-			if (((u16) getintvar(sih, BRCMS_SROM_AA2G) == 7) ||
-			    ((u16) getintvar(sih, BRCMS_SROM_AA5G) == 7)) {
+			if ((sprom->ant_available_bg == 7) ||
+			    (sprom->ant_available_a == 7)) {
 				asi->antsel_avail = true;
 			} else if (
-				(u16) getintvar(sih, BRCMS_SROM_AA2G) == 3 ||
-				(u16) getintvar(sih, BRCMS_SROM_AA5G) == 3) {
+				sprom->ant_available_bg == 3 ||
+				sprom->ant_available_a == 3) {
 				asi->antsel_avail = false;
 			} else {
 				asi->antsel_avail = false;
@@ -146,8 +146,8 @@ struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 			break;
 		}
 	} else if ((asi->pub->sromrev == 4) &&
-		   ((u16) getintvar(sih, BRCMS_SROM_AA2G) == 7) &&
-		   ((u16) getintvar(sih, BRCMS_SROM_AA5G) == 0)) {
+		   (sprom->ant_available_bg == 7) &&
+		   (sprom->ant_available_a == 0)) {
 		/* hack to match old 4321CB2 cards with 2of3 antenna switch */
 		asi->antsel_type = ANTSEL_2x3;
 		asi->antsel_avail = true;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.c b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
index 0efe88e25a9a..eb77ac3cfb6b 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
@@ -1110,7 +1110,7 @@ struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
 	char country_abbrev[BRCM_CNTRY_BUF_SZ];
 	const struct country_info *country;
 	struct brcms_pub *pub = wlc->pub;
-	char *ccode;
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
 
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
@@ -1122,9 +1122,8 @@ struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
 	wlc->cmi = wlc_cm;
 
 	/* store the country code for passing up as a regulatory hint */
-	ccode = getvar(wlc->hw->sih, BRCMS_SROM_CCODE);
-	if (ccode && brcms_c_country_valid(ccode))
-		strncpy(wlc->pub->srom_ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
+	if (sprom->alpha2 && brcms_c_country_valid(sprom->alpha2))
+		strncpy(wlc->pub->srom_ccode, sprom->alpha2, sizeof(sprom->alpha2));
 
 	/*
 	 * internal country information which must match
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index aa15558f75c8..50f92a0b7c41 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -25,7 +25,6 @@
 #include <linux/bcma/bcma.h>
 #include <net/mac80211.h>
 #include <defs.h>
-#include "nicpci.h"
 #include "phy/phy_int.h"
 #include "d11.h"
 #include "channel.h"
@@ -770,7 +769,7 @@ void brcms_dpc(unsigned long data)
  * Precondition: Since this function is called in brcms_pci_probe() context,
  * no locking is required.
  */
-static int brcms_request_fw(struct brcms_info *wl, struct pci_dev *pdev)
+static int brcms_request_fw(struct brcms_info *wl, struct bcma_device *pdev)
 {
 	int status;
 	struct device *device = &pdev->dev;
@@ -1022,7 +1021,7 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 	spin_lock_init(&wl->isr_lock);
 
 	/* prepare ucode */
-	if (brcms_request_fw(wl, pdev->bus->host_pci) < 0) {
+	if (brcms_request_fw(wl, pdev) < 0) {
 		wiphy_err(wl->wiphy, "%s: Failed to find firmware usually in "
 			  "%s\n", KBUILD_MODNAME, "/lib/firmware/brcm");
 		brcms_release_fw(wl);
@@ -1043,12 +1042,12 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 	wl->pub->ieee_hw = hw;
 
 	/* register our interrupt handler */
-	if (request_irq(pdev->bus->host_pci->irq, brcms_isr,
+	if (request_irq(pdev->irq, brcms_isr,
 			IRQF_SHARED, KBUILD_MODNAME, wl)) {
 		wiphy_err(wl->wiphy, "wl%d: request_irq() failed\n", unit);
 		goto fail;
 	}
-	wl->irq = pdev->bus->host_pci->irq;
+	wl->irq = pdev->irq;
 
 	/* register module */
 	brcms_c_module_register(wl->pub, "linux", wl, NULL);
@@ -1098,7 +1097,7 @@ static int __devinit brcms_bcma_probe(struct bcma_device *pdev)
 
 	dev_info(&pdev->dev, "mfg %x core %x rev %d class %d irq %d\n",
 		 pdev->id.manuf, pdev->id.id, pdev->id.rev, pdev->id.class,
-		 pdev->bus->host_pci->irq);
+		 pdev->irq);
 
 	if ((pdev->id.manuf != BCMA_MANUF_BCM) ||
 	    (pdev->id.id != BCMA_CORE_80211))
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index b4d92792c502..19db4052c44c 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -1219,7 +1219,7 @@ static void brcms_b_wait_for_wake(struct brcms_hardware *wlc_hw)
 }
 
 /* control chip clock to save power, enable dynamic clock or force fast clock */
-static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
+static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, enum bcma_clkmode mode)
 {
 	if (ai_get_cccaps(wlc_hw->sih) & CC_CAP_PMU) {
 		/* new chips with PMU, CCS_FORCEHT will distribute the HT clock
@@ -1229,7 +1229,7 @@ static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
 		 */
 
 		if (wlc_hw->clk) {
-			if (mode == CLK_FAST) {
+			if (mode == BCMA_CLKMODE_FAST) {
 				bcma_set32(wlc_hw->d11core,
 					   D11REGOFFS(clk_ctl_st),
 					   CCS_FORCEHT);
@@ -1260,7 +1260,7 @@ static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
 					~CCS_FORCEHT);
 			}
 		}
-		wlc_hw->forcefastclk = (mode == CLK_FAST);
+		wlc_hw->forcefastclk = (mode == BCMA_CLKMODE_FAST);
 	} else {
 
 		/* old chips w/o PMU, force HT through cc,
@@ -1567,7 +1567,7 @@ void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw)
 	/* request FAST clock if not on */
 	fastclk = wlc_hw->forcefastclk;
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	wlc_phy_bw_state_set(wlc_hw->band->pi, bw);
 
@@ -1576,7 +1576,7 @@ void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw)
 
 	/* restore the clk */
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 }
 
 static void brcms_b_upd_synthpu(struct brcms_hardware *wlc_hw)
@@ -1882,27 +1882,20 @@ static bool brcms_c_validboardtype(struct brcms_hardware *wlc_hw)
 	return true;
 }
 
-static char *brcms_c_get_macaddr(struct brcms_hardware *wlc_hw)
+static void brcms_c_get_macaddr(struct brcms_hardware *wlc_hw, u8 etheraddr[ETH_ALEN])
 {
-	enum brcms_srom_id var_id = BRCMS_SROM_MACADDR;
-	char *macaddr;
+	struct ssb_sprom *sprom = &wlc_hw->d11core->bus->sprom;
 
 	/* If macaddr exists, use it (Sromrev4, CIS, ...). */
-	macaddr = getvar(wlc_hw->sih, var_id);
-	if (macaddr != NULL)
-		return macaddr;
+	if (!is_zero_ether_addr(sprom->il0mac)) {
+		memcpy(etheraddr, sprom->il0mac, 6);
+		return;
+	}
 
 	if (wlc_hw->_nbands > 1)
-		var_id = BRCMS_SROM_ET1MACADDR;
+		memcpy(etheraddr, sprom->et1mac, 6);
 	else
-		var_id = BRCMS_SROM_IL0MACADDR;
-
-	macaddr = getvar(wlc_hw->sih, var_id);
-	if (macaddr == NULL)
-		wiphy_err(wlc_hw->wlc->wiphy, "wl%d: wlc_get_macaddr: macaddr "
-			  "getvar(%d) not found\n", wlc_hw->unit, var_id);
-
-	return macaddr;
+		memcpy(etheraddr, sprom->il0mac, 6);
 }
 
 /* power both the pll and external oscillator on/off */
@@ -1917,9 +1910,6 @@ static void brcms_b_xtal(struct brcms_hardware *wlc_hw, bool want)
 	if (!want && wlc_hw->pllreq)
 		return;
 
-	if (wlc_hw->sih)
-		ai_clkctl_xtal(wlc_hw->sih, XTAL | PLL, want);
-
 	wlc_hw->sbclk = want;
 	if (!wlc_hw->sbclk) {
 		wlc_hw->clk = false;
@@ -2004,7 +1994,7 @@ void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
 	/* request FAST clock if not on  */
 	fastclk = wlc_hw->forcefastclk;
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/* reset the dma engines except first time thru */
 	if (bcma_core_is_enabled(wlc_hw->d11core)) {
@@ -2053,7 +2043,7 @@ void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
 	brcms_c_mctrl_reset(wlc_hw);
 
 	if (ai_get_cccaps(wlc_hw->sih) & CC_CAP_PMU)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	brcms_b_phy_reset(wlc_hw);
 
@@ -2065,7 +2055,7 @@ void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
 
 	/* restore the clk setting */
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 }
 
 /* txfifo sizes needs to be modified(increased) since the newer cores
@@ -2218,7 +2208,7 @@ static void brcms_c_gpio_init(struct brcms_c_info *wlc)
 		gm |= gc |= BOARD_GPIO_PACTRL;
 
 	/* apply to gpiocontrol register */
-	ai_gpiocontrol(wlc_hw->sih, gm, gc, GPIO_DRV_PRIORITY);
+	bcma_chipco_gpio_control(&wlc_hw->d11core->bus->drv_cc, gm, gc);
 }
 
 static void brcms_ucode_write(struct brcms_hardware *wlc_hw,
@@ -3371,7 +3361,7 @@ static brcms_b_init(struct brcms_hardware *wlc_hw, u16 chanspec) {
 	/* request FAST clock if not on */
 	fastclk = wlc_hw->forcefastclk;
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/* disable interrupts */
 	macintmask = brcms_intrsoff(wlc->wl);
@@ -3405,7 +3395,7 @@ static brcms_b_init(struct brcms_hardware *wlc_hw, u16 chanspec) {
 
 	/* restore the clk */
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 }
 
 static void brcms_c_set_phy_chanspec(struct brcms_c_info *wlc,
@@ -4436,17 +4426,22 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 			  uint unit, bool piomode)
 {
 	struct brcms_hardware *wlc_hw;
-	char *macaddr = NULL;
 	uint err = 0;
 	uint j;
 	bool wme = false;
 	struct shared_phy_params sha_params;
 	struct wiphy *wiphy = wlc->wiphy;
 	struct pci_dev *pcidev = core->bus->host_pci;
+	struct ssb_sprom *sprom = &core->bus->sprom;
 
-	BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit,
-	       pcidev->vendor,
-	       pcidev->device);
+	if (core->bus->hosttype == BCMA_HOSTTYPE_PCI)
+		BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit,
+		       pcidev->vendor,
+		       pcidev->device);
+	else
+		BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit,
+		       core->bus->boardinfo.vendor,
+		       core->bus->boardinfo.type);
 
 	wme = true;
 
@@ -4472,7 +4467,8 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	}
 
 	/* verify again the device is supported */
-	if (!brcms_c_chipmatch(pcidev->vendor, pcidev->device)) {
+	if (core->bus->hosttype == BCMA_HOSTTYPE_PCI &&
+	    !brcms_c_chipmatch(pcidev->vendor, pcidev->device)) {
 		wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported "
 			"vendor/device (0x%x/0x%x)\n",
 			 unit, pcidev->vendor, pcidev->device);
@@ -4480,8 +4476,13 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 		goto fail;
 	}
 
-	wlc_hw->vendorid = pcidev->vendor;
-	wlc_hw->deviceid = pcidev->device;
+	if (core->bus->hosttype == BCMA_HOSTTYPE_PCI) {
+		wlc_hw->vendorid = pcidev->vendor;
+		wlc_hw->deviceid = pcidev->device;
+	} else {
+		wlc_hw->vendorid = core->bus->boardinfo.vendor;
+		wlc_hw->deviceid = core->bus->boardinfo.type;
+	}
 
 	wlc_hw->d11core = core;
 	wlc_hw->corerev = core->id.rev;
@@ -4501,7 +4502,7 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	 *   is still false; But it will be called again inside wlc_corereset,
 	 *   after d11 is out of reset.
 	 */
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 	brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
 
 	if (!brcms_b_validate_chip_access(wlc_hw)) {
@@ -4512,7 +4513,7 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	}
 
 	/* get the board rev, used just below */
-	j = getintvar(wlc_hw->sih, BRCMS_SROM_BOARDREV);
+	j = sprom->board_rev;
 	/* promote srom boardrev of 0xFF to 1 */
 	if (j == BOARDREV_PROMOTABLE)
 		j = BOARDREV_PROMOTED;
@@ -4525,11 +4526,9 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 		err = 15;
 		goto fail;
 	}
-	wlc_hw->sromrev = (u8) getintvar(wlc_hw->sih, BRCMS_SROM_REV);
-	wlc_hw->boardflags = (u32) getintvar(wlc_hw->sih,
-					     BRCMS_SROM_BOARDFLAGS);
-	wlc_hw->boardflags2 = (u32) getintvar(wlc_hw->sih,
-					      BRCMS_SROM_BOARDFLAGS2);
+	wlc_hw->sromrev = sprom->revision;
+	wlc_hw->boardflags = sprom->boardflags_lo + (sprom->boardflags_hi << 16);
+	wlc_hw->boardflags2 = sprom->boardflags2_lo + (sprom->boardflags2_hi << 16);
 
 	if (wlc_hw->boardflags & BFL_NOPLLDOWN)
 		brcms_b_pllreq(wlc_hw, true, BRCMS_PLLREQ_SHARED);
@@ -4702,25 +4701,18 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	 */
 
 	/* init etheraddr state variables */
-	macaddr = brcms_c_get_macaddr(wlc_hw);
-	if (macaddr == NULL) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: macaddr not found\n",
-			  unit);
-		err = 21;
-		goto fail;
-	}
-	if (!mac_pton(macaddr, wlc_hw->etheraddr) ||
-	    is_broadcast_ether_addr(wlc_hw->etheraddr) ||
+	brcms_c_get_macaddr(wlc_hw, wlc_hw->etheraddr);
+
+	if (is_broadcast_ether_addr(wlc_hw->etheraddr) ||
 	    is_zero_ether_addr(wlc_hw->etheraddr)) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: bad macaddr %s\n",
-			  unit, macaddr);
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: bad macaddr\n",
+			  unit);
 		err = 22;
 		goto fail;
 	}
 
-	BCMMSG(wlc->wiphy, "deviceid 0x%x nbands %d board 0x%x macaddr: %s\n",
-	       wlc_hw->deviceid, wlc_hw->_nbands, ai_get_boardtype(wlc_hw->sih),
-	       macaddr);
+	BCMMSG(wlc->wiphy, "deviceid 0x%x nbands %d board 0x%x\n",
+	       wlc_hw->deviceid, wlc_hw->_nbands, ai_get_boardtype(wlc_hw->sih));
 
 	return err;
 
@@ -4770,16 +4762,16 @@ static bool brcms_c_attach_stf_ant_init(struct brcms_c_info *wlc)
 	int aa;
 	uint unit;
 	int bandtype;
-	struct si_pub *sih = wlc->hw->sih;
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
 
 	unit = wlc->pub->unit;
 	bandtype = wlc->band->bandtype;
 
 	/* get antennas available */
 	if (bandtype == BRCM_BAND_5G)
-		aa = (s8) getintvar(sih, BRCMS_SROM_AA5G);
+		aa = sprom->ant_available_a;
 	else
-		aa = (s8) getintvar(sih, BRCMS_SROM_AA2G);
+		aa = sprom->ant_available_bg;
 
 	if ((aa < 1) || (aa > 15)) {
 		wiphy_err(wlc->wiphy, "wl%d: %s: Invalid antennas available in"
@@ -4799,9 +4791,9 @@ static bool brcms_c_attach_stf_ant_init(struct brcms_c_info *wlc)
 
 	/* Compute Antenna Gain */
 	if (bandtype == BRCM_BAND_5G)
-		wlc->band->antgain = (s8) getintvar(sih, BRCMS_SROM_AG1);
+		wlc->band->antgain = sprom->antenna_gain.a1;
 	else
-		wlc->band->antgain = (s8) getintvar(sih, BRCMS_SROM_AG0);
+		wlc->band->antgain = sprom->antenna_gain.a0;
 
 	brcms_c_attach_antgain_init(wlc);
 
@@ -4952,15 +4944,6 @@ static int brcms_b_detach(struct brcms_c_info *wlc)
 
 	callbacks = 0;
 
-	if (wlc_hw->sih) {
-		/*
-		 * detach interrupt sync mechanism since interrupt is disabled
-		 * and per-port interrupt object may has been freed. this must
-		 * be done before sb core switch
-		 */
-		ai_pci_sleep(wlc_hw->sih);
-	}
-
 	brcms_b_detach_dmapio(wlc_hw);
 
 	band = wlc_hw->band;
@@ -5047,9 +5030,7 @@ static void brcms_b_hw_up(struct brcms_hardware *wlc_hw)
 	 */
 	brcms_b_xtal(wlc_hw, ON);
 	ai_clkctl_init(wlc_hw->sih);
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	ai_pci_fixcfg(wlc_hw->sih);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/*
 	 * TODO: test suspend/resume
@@ -5078,8 +5059,6 @@ static void brcms_b_hw_up(struct brcms_hardware *wlc_hw)
 
 static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
 {
-	uint coremask;
-
 	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
 
 	/*
@@ -5088,15 +5067,14 @@ static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
 	 */
 	brcms_b_xtal(wlc_hw, ON);
 	ai_clkctl_init(wlc_hw->sih);
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/*
 	 * Configure pci/pcmcia here instead of in brcms_c_attach()
 	 * to allow mfg hotswap:  down, hotswap (chip power cycle), up.
 	 */
-	coremask = (1 << wlc_hw->wlc->core->coreidx);
-
-	ai_pci_setup(wlc_hw->sih, coremask);
+	bcma_core_pci_irq_ctl(&wlc_hw->d11core->bus->drv_pci, wlc_hw->d11core,
+			      true);
 
 	/*
 	 * Need to read the hwradio status here to cover the case where the
@@ -5126,7 +5104,7 @@ static int brcms_b_up_finish(struct brcms_hardware *wlc_hw)
 	wlc_phy_hw_state_upd(wlc_hw->band->pi, true);
 
 	/* FULLY enable dynamic power control and d11 core interrupt */
-	brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 	brcms_intrson(wlc_hw->wlc->wl);
 	return 0;
 }
@@ -5267,7 +5245,7 @@ static int brcms_b_bmac_down_prep(struct brcms_hardware *wlc_hw)
 		brcms_intrsoff(wlc_hw->wlc->wl);
 
 		/* ensure we're running on the pll clock again */
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 	}
 	/* down phy at the last of this stage */
 	callbacks += wlc_phy_down(wlc_hw->band->pi);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.c b/drivers/net/wireless/brcm80211/brcmsmac/nicpci.c
deleted file mode 100644
index 7fad6dc19258..000000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.c
+++ /dev/null
@@ -1,826 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#include <linux/slab.h>
-#include <linux/delay.h>
-#include <linux/pci.h>
-
-#include <defs.h>
-#include <soc.h>
-#include <chipcommon.h>
-#include "aiutils.h"
-#include "pub.h"
-#include "nicpci.h"
-
-/* SPROM offsets */
-#define SRSH_ASPM_OFFSET		4	/* word 4 */
-#define SRSH_ASPM_ENB			0x18	/* bit 3, 4 */
-#define SRSH_ASPM_L1_ENB		0x10	/* bit 4 */
-#define SRSH_ASPM_L0s_ENB		0x8	/* bit 3 */
-
-#define SRSH_PCIE_MISC_CONFIG		5	/* word 5 */
-#define SRSH_L23READY_EXIT_NOPERST	0x8000	/* bit 15 */
-#define SRSH_CLKREQ_OFFSET_REV5		20	/* word 20 for srom rev <= 5 */
-#define SRSH_CLKREQ_ENB			0x0800	/* bit 11 */
-#define SRSH_BD_OFFSET                  6	/* word 6 */
-
-/* chipcontrol */
-#define CHIPCTRL_4321_PLL_DOWN		0x800000/* serdes PLL down override */
-
-/* MDIO control */
-#define MDIOCTL_DIVISOR_MASK		0x7f	/* clock to be used on MDIO */
-#define MDIOCTL_DIVISOR_VAL		0x2
-#define MDIOCTL_PREAM_EN		0x80	/* Enable preamble sequnce */
-#define MDIOCTL_ACCESS_DONE		0x100	/* Transaction complete */
-
-/* MDIO Data */
-#define MDIODATA_MASK			0x0000ffff	/* data 2 bytes */
-#define MDIODATA_TA			0x00020000	/* Turnaround */
-
-#define MDIODATA_REGADDR_SHF		18		/* Regaddr shift */
-#define MDIODATA_REGADDR_MASK		0x007c0000	/* Regaddr Mask */
-#define MDIODATA_DEVADDR_SHF		23	/* Physmedia devaddr shift */
-#define MDIODATA_DEVADDR_MASK		0x0f800000
-						/* Physmedia devaddr Mask */
-
-/* MDIO Data for older revisions < 10 */
-#define MDIODATA_REGADDR_SHF_OLD	18	/* Regaddr shift */
-#define MDIODATA_REGADDR_MASK_OLD	0x003c0000
-						/* Regaddr Mask */
-#define MDIODATA_DEVADDR_SHF_OLD	22	/* Physmedia devaddr shift  */
-#define MDIODATA_DEVADDR_MASK_OLD	0x0fc00000
-						/* Physmedia devaddr Mask */
-
-/* Transactions flags */
-#define MDIODATA_WRITE			0x10000000
-#define MDIODATA_READ			0x20000000
-#define MDIODATA_START			0x40000000
-
-#define MDIODATA_DEV_ADDR		0x0	/* dev address for serdes */
-#define	MDIODATA_BLK_ADDR		0x1F	/* blk address for serdes */
-
-/* serdes regs (rev < 10) */
-#define MDIODATA_DEV_PLL		0x1d	/* SERDES PLL Dev */
-#define MDIODATA_DEV_TX			0x1e	/* SERDES TX Dev */
-#define MDIODATA_DEV_RX			0x1f	/* SERDES RX Dev */
-
-/* SERDES RX registers */
-#define SERDES_RX_CTRL			1	/* Rx cntrl */
-#define SERDES_RX_TIMER1		2	/* Rx Timer1 */
-#define SERDES_RX_CDR			6	/* CDR */
-#define SERDES_RX_CDRBW			7	/* CDR BW */
-/* SERDES RX control register */
-#define SERDES_RX_CTRL_FORCE		0x80	/* rxpolarity_force */
-#define SERDES_RX_CTRL_POLARITY		0x40	/* rxpolarity_value */
-
-/* SERDES PLL registers */
-#define SERDES_PLL_CTRL                 1	/* PLL control reg */
-#define PLL_CTRL_FREQDET_EN             0x4000	/* bit 14 is FREQDET on */
-
-/* Linkcontrol reg offset in PCIE Cap */
-#define PCIE_CAP_LINKCTRL_OFFSET	16	/* offset in pcie cap */
-#define PCIE_CAP_LCREG_ASPML0s		0x01	/* ASPM L0s in linkctrl */
-#define PCIE_CAP_LCREG_ASPML1		0x02	/* ASPM L1 in linkctrl */
-#define PCIE_CLKREQ_ENAB		0x100	/* CLKREQ Enab in linkctrl */
-
-#define PCIE_ASPM_ENAB			3	/* ASPM L0s & L1 in linkctrl */
-#define PCIE_ASPM_L1_ENAB		2	/* ASPM L0s & L1 in linkctrl */
-#define PCIE_ASPM_L0s_ENAB		1	/* ASPM L0s & L1 in linkctrl */
-#define PCIE_ASPM_DISAB			0	/* ASPM L0s & L1 in linkctrl */
-
-/* Power management threshold */
-#define PCIE_L1THRESHOLDTIME_MASK       0xFF00	/* bits 8 - 15 */
-#define PCIE_L1THRESHOLDTIME_SHIFT      8	/* PCIE_L1THRESHOLDTIME_SHIFT */
-#define PCIE_L1THRESHOLD_WARVAL         0x72	/* WAR value */
-#define PCIE_ASPMTIMER_EXTEND		0x01000000
-						/* > rev7:
-						 * enable extend ASPM timer
-						 */
-
-/* different register spaces to access thru pcie indirect access */
-#define PCIE_CONFIGREGS		1	/* Access to config space */
-#define PCIE_PCIEREGS		2	/* Access to pcie registers */
-
-/* PCIE protocol PHY diagnostic registers */
-#define	PCIE_PLP_STATUSREG		0x204	/* Status */
-
-/* Status reg PCIE_PLP_STATUSREG */
-#define PCIE_PLP_POLARITYINV_STAT	0x10
-
-/* PCIE protocol DLLP diagnostic registers */
-#define PCIE_DLLP_LCREG			0x100	/* Link Control */
-#define PCIE_DLLP_PMTHRESHREG		0x128	/* Power Management Threshold */
-
-/* PCIE protocol TLP diagnostic registers */
-#define PCIE_TLP_WORKAROUNDSREG		0x004	/* TLP Workarounds */
-
-/* Sonics to PCI translation types */
-#define	SBTOPCI_PREF	0x4		/* prefetch enable */
-#define	SBTOPCI_BURST	0x8		/* burst enable */
-#define	SBTOPCI_RC_READMULTI	0x20	/* memory read multiple */
-
-#define PCI_CLKRUN_DSBL	0x8000	/* Bit 15 forceClkrun */
-
-/* PCI core index in SROM shadow area */
-#define SRSH_PI_OFFSET	0	/* first word */
-#define SRSH_PI_MASK	0xf000	/* bit 15:12 */
-#define SRSH_PI_SHIFT	12	/* bit 15:12 */
-
-#define PCIREGOFFS(field)	offsetof(struct sbpciregs, field)
-#define PCIEREGOFFS(field)	offsetof(struct sbpcieregs, field)
-
-/* Sonics side: PCI core and host control registers */
-struct sbpciregs {
-	u32 control;		/* PCI control */
-	u32 PAD[3];
-	u32 arbcontrol;		/* PCI arbiter control */
-	u32 clkrun;		/* Clkrun Control (>=rev11) */
-	u32 PAD[2];
-	u32 intstatus;		/* Interrupt status */
-	u32 intmask;		/* Interrupt mask */
-	u32 sbtopcimailbox;	/* Sonics to PCI mailbox */
-	u32 PAD[9];
-	u32 bcastaddr;		/* Sonics broadcast address */
-	u32 bcastdata;		/* Sonics broadcast data */
-	u32 PAD[2];
-	u32 gpioin;		/* ro: gpio input (>=rev2) */
-	u32 gpioout;		/* rw: gpio output (>=rev2) */
-	u32 gpioouten;		/* rw: gpio output enable (>= rev2) */
-	u32 gpiocontrol;	/* rw: gpio control (>= rev2) */
-	u32 PAD[36];
-	u32 sbtopci0;		/* Sonics to PCI translation 0 */
-	u32 sbtopci1;		/* Sonics to PCI translation 1 */
-	u32 sbtopci2;		/* Sonics to PCI translation 2 */
-	u32 PAD[189];
-	u32 pcicfg[4][64];	/* 0x400 - 0x7FF, PCI Cfg Space (>=rev8) */
-	u16 sprom[36];		/* SPROM shadow Area */
-	u32 PAD[46];
-};
-
-/* SB side: PCIE core and host control registers */
-struct sbpcieregs {
-	u32 control;		/* host mode only */
-	u32 PAD[2];
-	u32 biststatus;		/* bist Status: 0x00C */
-	u32 gpiosel;		/* PCIE gpio sel: 0x010 */
-	u32 gpioouten;		/* PCIE gpio outen: 0x14 */
-	u32 PAD[2];
-	u32 intstatus;		/* Interrupt status: 0x20 */
-	u32 intmask;		/* Interrupt mask: 0x24 */
-	u32 sbtopcimailbox;	/* sb to pcie mailbox: 0x028 */
-	u32 PAD[53];
-	u32 sbtopcie0;		/* sb to pcie translation 0: 0x100 */
-	u32 sbtopcie1;		/* sb to pcie translation 1: 0x104 */
-	u32 sbtopcie2;		/* sb to pcie translation 2: 0x108 */
-	u32 PAD[5];
-
-	/* pcie core supports in direct access to config space */
-	u32 configaddr;	/* pcie config space access: Address field: 0x120 */
-	u32 configdata;	/* pcie config space access: Data field: 0x124 */
-
-	/* mdio access to serdes */
-	u32 mdiocontrol;	/* controls the mdio access: 0x128 */
-	u32 mdiodata;		/* Data to the mdio access: 0x12c */
-
-	/* pcie protocol phy/dllp/tlp register indirect access mechanism */
-	u32 pcieindaddr;	/* indirect access to
-				 * the internal register: 0x130
-				 */
-	u32 pcieinddata;	/* Data to/from the internal regsiter: 0x134 */
-
-	u32 clkreqenctrl;	/* >= rev 6, Clkreq rdma control : 0x138 */
-	u32 PAD[177];
-	u32 pciecfg[4][64];	/* 0x400 - 0x7FF, PCIE Cfg Space */
-	u16 sprom[64];		/* SPROM shadow Area */
-};
-
-struct pcicore_info {
-	struct bcma_device *core;
-	struct si_pub *sih;	/* System interconnect handle */
-	struct pci_dev *dev;
-	u8 pciecap_lcreg_offset;/* PCIE capability LCreg offset
-				 * in the config space
-				 */
-	bool pcie_pr42767;
-	u8 pcie_polarity;
-	u8 pcie_war_aspm_ovr;	/* Override ASPM/Clkreq settings */
-
-	u8 pmecap_offset;	/* PM Capability offset in the config space */
-	bool pmecap;		/* Capable of generating PME */
-};
-
-#define PCIE_ASPM(sih)							\
-	((ai_get_buscoretype(sih) == PCIE_CORE_ID) &&			\
-	 ((ai_get_buscorerev(sih) >= 3) &&				\
-	  (ai_get_buscorerev(sih) <= 5)))
-
-
-/* delay needed between the mdio control/ mdiodata register data access */
-static void pr28829_delay(void)
-{
-	udelay(10);
-}
-
-/* Initialize the PCI core.
- * It's caller's responsibility to make sure that this is done only once
- */
-struct pcicore_info *pcicore_init(struct si_pub *sih, struct bcma_device *core)
-{
-	struct pcicore_info *pi;
-
-	/* alloc struct pcicore_info */
-	pi = kzalloc(sizeof(struct pcicore_info), GFP_ATOMIC);
-	if (pi == NULL)
-		return NULL;
-
-	pi->sih = sih;
-	pi->dev = core->bus->host_pci;
-	pi->core = core;
-
-	if (core->id.id == PCIE_CORE_ID) {
-		u8 cap_ptr;
-		cap_ptr = pcicore_find_pci_capability(pi->dev, PCI_CAP_ID_EXP,
-						      NULL, NULL);
-		pi->pciecap_lcreg_offset = cap_ptr + PCIE_CAP_LINKCTRL_OFFSET;
-	}
-	return pi;
-}
-
-void pcicore_deinit(struct pcicore_info *pch)
-{
-	kfree(pch);
-}
-
-/* return cap_offset if requested capability exists in the PCI config space */
-/* Note that it's caller's responsibility to make sure it's a pci bus */
-u8
-pcicore_find_pci_capability(struct pci_dev *dev, u8 req_cap_id,
-			    unsigned char *buf, u32 *buflen)
-{
-	u8 cap_id;
-	u8 cap_ptr = 0;
-	u32 bufsize;
-	u8 byte_val;
-
-	/* check for Header type 0 */
-	pci_read_config_byte(dev, PCI_HEADER_TYPE, &byte_val);
-	if ((byte_val & 0x7f) != PCI_HEADER_TYPE_NORMAL)
-		goto end;
-
-	/* check if the capability pointer field exists */
-	pci_read_config_byte(dev, PCI_STATUS, &byte_val);
-	if (!(byte_val & PCI_STATUS_CAP_LIST))
-		goto end;
-
-	pci_read_config_byte(dev, PCI_CAPABILITY_LIST, &cap_ptr);
-	/* check if the capability pointer is 0x00 */
-	if (cap_ptr == 0x00)
-		goto end;
-
-	/* loop thru the capability list
-	 * and see if the pcie capability exists
-	 */
-
-	pci_read_config_byte(dev, cap_ptr, &cap_id);
-
-	while (cap_id != req_cap_id) {
-		pci_read_config_byte(dev, cap_ptr + 1, &cap_ptr);
-		if (cap_ptr == 0x00)
-			break;
-		pci_read_config_byte(dev, cap_ptr, &cap_id);
-	}
-	if (cap_id != req_cap_id)
-		goto end;
-
-	/* found the caller requested capability */
-	if (buf != NULL && buflen != NULL) {
-		u8 cap_data;
-
-		bufsize = *buflen;
-		if (!bufsize)
-			goto end;
-		*buflen = 0;
-		/* copy the capability data excluding cap ID and next ptr */
-		cap_data = cap_ptr + 2;
-		if ((bufsize + cap_data) > PCI_SZPCR)
-			bufsize = PCI_SZPCR - cap_data;
-		*buflen = bufsize;
-		while (bufsize--) {
-			pci_read_config_byte(dev, cap_data, buf);
-			cap_data++;
-			buf++;
-		}
-	}
-end:
-	return cap_ptr;
-}
-
-/* ***** Register Access API */
-static uint
-pcie_readreg(struct bcma_device *core, uint addrtype, uint offset)
-{
-	uint retval = 0xFFFFFFFF;
-
-	switch (addrtype) {
-	case PCIE_CONFIGREGS:
-		bcma_write32(core, PCIEREGOFFS(configaddr), offset);
-		(void)bcma_read32(core, PCIEREGOFFS(configaddr));
-		retval = bcma_read32(core, PCIEREGOFFS(configdata));
-		break;
-	case PCIE_PCIEREGS:
-		bcma_write32(core, PCIEREGOFFS(pcieindaddr), offset);
-		(void)bcma_read32(core, PCIEREGOFFS(pcieindaddr));
-		retval = bcma_read32(core, PCIEREGOFFS(pcieinddata));
-		break;
-	}
-
-	return retval;
-}
-
-static uint pcie_writereg(struct bcma_device *core, uint addrtype,
-			  uint offset, uint val)
-{
-	switch (addrtype) {
-	case PCIE_CONFIGREGS:
-		bcma_write32(core, PCIEREGOFFS(configaddr), offset);
-		bcma_write32(core, PCIEREGOFFS(configdata), val);
-		break;
-	case PCIE_PCIEREGS:
-		bcma_write32(core, PCIEREGOFFS(pcieindaddr), offset);
-		bcma_write32(core, PCIEREGOFFS(pcieinddata), val);
-		break;
-	default:
-		break;
-	}
-	return 0;
-}
-
-static bool pcie_mdiosetblock(struct pcicore_info *pi, uint blk)
-{
-	uint mdiodata, i = 0;
-	uint pcie_serdes_spinwait = 200;
-
-	mdiodata = (MDIODATA_START | MDIODATA_WRITE | MDIODATA_TA |
-		    (MDIODATA_DEV_ADDR << MDIODATA_DEVADDR_SHF) |
-		    (MDIODATA_BLK_ADDR << MDIODATA_REGADDR_SHF) |
-		    (blk << 4));
-	bcma_write32(pi->core, PCIEREGOFFS(mdiodata), mdiodata);
-
-	pr28829_delay();
-	/* retry till the transaction is complete */
-	while (i < pcie_serdes_spinwait) {
-		if (bcma_read32(pi->core, PCIEREGOFFS(mdiocontrol)) &
-		    MDIOCTL_ACCESS_DONE)
-			break;
-
-		udelay(1000);
-		i++;
-	}
-
-	if (i >= pcie_serdes_spinwait)
-		return false;
-
-	return true;
-}
-
-static int
-pcie_mdioop(struct pcicore_info *pi, uint physmedia, uint regaddr, bool write,
-	    uint *val)
-{
-	uint mdiodata;
-	uint i = 0;
-	uint pcie_serdes_spinwait = 10;
-
-	/* enable mdio access to SERDES */
-	bcma_write32(pi->core, PCIEREGOFFS(mdiocontrol),
-		     MDIOCTL_PREAM_EN | MDIOCTL_DIVISOR_VAL);
-
-	if (ai_get_buscorerev(pi->sih) >= 10) {
-		/* new serdes is slower in rw,
-		 * using two layers of reg address mapping
-		 */
-		if (!pcie_mdiosetblock(pi, physmedia))
-			return 1;
-		mdiodata = ((MDIODATA_DEV_ADDR << MDIODATA_DEVADDR_SHF) |
-			    (regaddr << MDIODATA_REGADDR_SHF));
-		pcie_serdes_spinwait *= 20;
-	} else {
-		mdiodata = ((physmedia << MDIODATA_DEVADDR_SHF_OLD) |
-			    (regaddr << MDIODATA_REGADDR_SHF_OLD));
-	}
-
-	if (!write)
-		mdiodata |= (MDIODATA_START | MDIODATA_READ | MDIODATA_TA);
-	else
-		mdiodata |= (MDIODATA_START | MDIODATA_WRITE | MDIODATA_TA |
-			     *val);
-
-	bcma_write32(pi->core, PCIEREGOFFS(mdiodata), mdiodata);
-
-	pr28829_delay();
-
-	/* retry till the transaction is complete */
-	while (i < pcie_serdes_spinwait) {
-		if (bcma_read32(pi->core, PCIEREGOFFS(mdiocontrol)) &
-		    MDIOCTL_ACCESS_DONE) {
-			if (!write) {
-				pr28829_delay();
-				*val = (bcma_read32(pi->core,
-						    PCIEREGOFFS(mdiodata)) &
-					MDIODATA_MASK);
-			}
-			/* Disable mdio access to SERDES */
-			bcma_write32(pi->core, PCIEREGOFFS(mdiocontrol), 0);
-			return 0;
-		}
-		udelay(1000);
-		i++;
-	}
-
-	/* Timed out. Disable mdio access to SERDES. */
-	bcma_write32(pi->core, PCIEREGOFFS(mdiocontrol), 0);
-	return 1;
-}
-
-/* use the mdio interface to read from mdio slaves */
-static int
-pcie_mdioread(struct pcicore_info *pi, uint physmedia, uint regaddr,
-	      uint *regval)
-{
-	return pcie_mdioop(pi, physmedia, regaddr, false, regval);
-}
-
-/* use the mdio interface to write to mdio slaves */
-static int
-pcie_mdiowrite(struct pcicore_info *pi, uint physmedia, uint regaddr, uint val)
-{
-	return pcie_mdioop(pi, physmedia, regaddr, true, &val);
-}
-
-/* ***** Support functions ***** */
-static u8 pcie_clkreq(struct pcicore_info *pi, u32 mask, u32 val)
-{
-	u32 reg_val;
-	u8 offset;
-
-	offset = pi->pciecap_lcreg_offset;
-	if (!offset)
-		return 0;
-
-	pci_read_config_dword(pi->dev, offset, &reg_val);
-	/* set operation */
-	if (mask) {
-		if (val)
-			reg_val |= PCIE_CLKREQ_ENAB;
-		else
-			reg_val &= ~PCIE_CLKREQ_ENAB;
-		pci_write_config_dword(pi->dev, offset, reg_val);
-		pci_read_config_dword(pi->dev, offset, &reg_val);
-	}
-	if (reg_val & PCIE_CLKREQ_ENAB)
-		return 1;
-	else
-		return 0;
-}
-
-static void pcie_extendL1timer(struct pcicore_info *pi, bool extend)
-{
-	u32 w;
-	struct si_pub *sih = pi->sih;
-
-	if (ai_get_buscoretype(sih) != PCIE_CORE_ID ||
-	    ai_get_buscorerev(sih) < 7)
-		return;
-
-	w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_PMTHRESHREG);
-	if (extend)
-		w |= PCIE_ASPMTIMER_EXTEND;
-	else
-		w &= ~PCIE_ASPMTIMER_EXTEND;
-	pcie_writereg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_PMTHRESHREG, w);
-	w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_PMTHRESHREG);
-}
-
-/* centralized clkreq control policy */
-static void pcie_clkreq_upd(struct pcicore_info *pi, uint state)
-{
-	struct si_pub *sih = pi->sih;
-
-	switch (state) {
-	case SI_DOATTACH:
-		if (PCIE_ASPM(sih))
-			pcie_clkreq(pi, 1, 0);
-		break;
-	case SI_PCIDOWN:
-		/* turn on serdes PLL down */
-		if (ai_get_buscorerev(sih) == 6) {
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_addr),
-				  ~0, 0);
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_data),
-				  ~0x40, 0);
-		} else if (pi->pcie_pr42767) {
-			pcie_clkreq(pi, 1, 1);
-		}
-		break;
-	case SI_PCIUP:
-		/* turn off serdes PLL down */
-		if (ai_get_buscorerev(sih) == 6) {
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_addr),
-				  ~0, 0);
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_data),
-				  ~0x40, 0x40);
-		} else if (PCIE_ASPM(sih)) {	/* disable clkreq */
-			pcie_clkreq(pi, 1, 0);
-		}
-		break;
-	}
-}
-
-/* ***** PCI core WARs ***** */
-/* Done only once at attach time */
-static void pcie_war_polarity(struct pcicore_info *pi)
-{
-	u32 w;
-
-	if (pi->pcie_polarity != 0)
-		return;
-
-	w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_PLP_STATUSREG);
-
-	/* Detect the current polarity at attach and force that polarity and
-	 * disable changing the polarity
-	 */
-	if ((w & PCIE_PLP_POLARITYINV_STAT) == 0)
-		pi->pcie_polarity = SERDES_RX_CTRL_FORCE;
-	else
-		pi->pcie_polarity = (SERDES_RX_CTRL_FORCE |
-				     SERDES_RX_CTRL_POLARITY);
-}
-
-/* enable ASPM and CLKREQ if srom doesn't have it */
-/* Needs to happen when update to shadow SROM is needed
- *   : Coming out of 'standby'/'hibernate'
- *   : If pcie_war_aspm_ovr state changed
- */
-static void pcie_war_aspm_clkreq(struct pcicore_info *pi)
-{
-	struct si_pub *sih = pi->sih;
-	u16 val16;
-	u32 w;
-
-	if (!PCIE_ASPM(sih))
-		return;
-
-	/* bypass this on QT or VSIM */
-	val16 = bcma_read16(pi->core, PCIEREGOFFS(sprom[SRSH_ASPM_OFFSET]));
-
-	val16 &= ~SRSH_ASPM_ENB;
-	if (pi->pcie_war_aspm_ovr == PCIE_ASPM_ENAB)
-		val16 |= SRSH_ASPM_ENB;
-	else if (pi->pcie_war_aspm_ovr == PCIE_ASPM_L1_ENAB)
-		val16 |= SRSH_ASPM_L1_ENB;
-	else if (pi->pcie_war_aspm_ovr == PCIE_ASPM_L0s_ENAB)
-		val16 |= SRSH_ASPM_L0s_ENB;
-
-	bcma_write16(pi->core, PCIEREGOFFS(sprom[SRSH_ASPM_OFFSET]), val16);
-
-	pci_read_config_dword(pi->dev, pi->pciecap_lcreg_offset, &w);
-	w &= ~PCIE_ASPM_ENAB;
-	w |= pi->pcie_war_aspm_ovr;
-	pci_write_config_dword(pi->dev, pi->pciecap_lcreg_offset, w);
-
-	val16 = bcma_read16(pi->core,
-			    PCIEREGOFFS(sprom[SRSH_CLKREQ_OFFSET_REV5]));
-
-	if (pi->pcie_war_aspm_ovr != PCIE_ASPM_DISAB) {
-		val16 |= SRSH_CLKREQ_ENB;
-		pi->pcie_pr42767 = true;
-	} else
-		val16 &= ~SRSH_CLKREQ_ENB;
-
-	bcma_write16(pi->core, PCIEREGOFFS(sprom[SRSH_CLKREQ_OFFSET_REV5]),
-		     val16);
-}
-
-/* Apply the polarity determined at the start */
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_war_serdes(struct pcicore_info *pi)
-{
-	u32 w = 0;
-
-	if (pi->pcie_polarity != 0)
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_CTRL,
-			       pi->pcie_polarity);
-
-	pcie_mdioread(pi, MDIODATA_DEV_PLL, SERDES_PLL_CTRL, &w);
-	if (w & PLL_CTRL_FREQDET_EN) {
-		w &= ~PLL_CTRL_FREQDET_EN;
-		pcie_mdiowrite(pi, MDIODATA_DEV_PLL, SERDES_PLL_CTRL, w);
-	}
-}
-
-/* Fix MISC config to allow coming out of L2/L3-Ready state w/o PRST */
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_misc_config_fixup(struct pcicore_info *pi)
-{
-	u16 val16;
-
-	val16 = bcma_read16(pi->core,
-			    PCIEREGOFFS(sprom[SRSH_PCIE_MISC_CONFIG]));
-
-	if ((val16 & SRSH_L23READY_EXIT_NOPERST) == 0) {
-		val16 |= SRSH_L23READY_EXIT_NOPERST;
-		bcma_write16(pi->core,
-			     PCIEREGOFFS(sprom[SRSH_PCIE_MISC_CONFIG]), val16);
-	}
-}
-
-/* quick hack for testing */
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_war_noplldown(struct pcicore_info *pi)
-{
-	/* turn off serdes PLL down */
-	ai_cc_reg(pi->sih, offsetof(struct chipcregs, chipcontrol),
-		  CHIPCTRL_4321_PLL_DOWN, CHIPCTRL_4321_PLL_DOWN);
-
-	/* clear srom shadow backdoor */
-	bcma_write16(pi->core, PCIEREGOFFS(sprom[SRSH_BD_OFFSET]), 0);
-}
-
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_war_pci_setup(struct pcicore_info *pi)
-{
-	struct si_pub *sih = pi->sih;
-	u32 w;
-
-	if (ai_get_buscorerev(sih) == 0 || ai_get_buscorerev(sih) == 1) {
-		w = pcie_readreg(pi->core, PCIE_PCIEREGS,
-				 PCIE_TLP_WORKAROUNDSREG);
-		w |= 0x8;
-		pcie_writereg(pi->core, PCIE_PCIEREGS,
-			      PCIE_TLP_WORKAROUNDSREG, w);
-	}
-
-	if (ai_get_buscorerev(sih) == 1) {
-		w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_LCREG);
-		w |= 0x40;
-		pcie_writereg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_LCREG, w);
-	}
-
-	if (ai_get_buscorerev(sih) == 0) {
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_TIMER1, 0x8128);
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_CDR, 0x0100);
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_CDRBW, 0x1466);
-	} else if (PCIE_ASPM(sih)) {
-		/* Change the L1 threshold for better performance */
-		w = pcie_readreg(pi->core, PCIE_PCIEREGS,
-				 PCIE_DLLP_PMTHRESHREG);
-		w &= ~PCIE_L1THRESHOLDTIME_MASK;
-		w |= PCIE_L1THRESHOLD_WARVAL << PCIE_L1THRESHOLDTIME_SHIFT;
-		pcie_writereg(pi->core, PCIE_PCIEREGS,
-			      PCIE_DLLP_PMTHRESHREG, w);
-
-		pcie_war_serdes(pi);
-
-		pcie_war_aspm_clkreq(pi);
-	} else if (ai_get_buscorerev(pi->sih) == 7)
-		pcie_war_noplldown(pi);
-
-	/* Note that the fix is actually in the SROM,
-	 * that's why this is open-ended
-	 */
-	if (ai_get_buscorerev(pi->sih) >= 6)
-		pcie_misc_config_fixup(pi);
-}
-
-/* ***** Functions called during driver state changes ***** */
-void pcicore_attach(struct pcicore_info *pi, int state)
-{
-	struct si_pub *sih = pi->sih;
-	u32 bfl2 = (u32)getintvar(sih, BRCMS_SROM_BOARDFLAGS2);
-
-	/* Determine if this board needs override */
-	if (PCIE_ASPM(sih)) {
-		if (bfl2 & BFL2_PCIEWAR_OVR)
-			pi->pcie_war_aspm_ovr = PCIE_ASPM_DISAB;
-		else
-			pi->pcie_war_aspm_ovr = PCIE_ASPM_ENAB;
-	}
-
-	/* These need to happen in this order only */
-	pcie_war_polarity(pi);
-
-	pcie_war_serdes(pi);
-
-	pcie_war_aspm_clkreq(pi);
-
-	pcie_clkreq_upd(pi, state);
-
-}
-
-void pcicore_hwup(struct pcicore_info *pi)
-{
-	if (!pi || ai_get_buscoretype(pi->sih) != PCIE_CORE_ID)
-		return;
-
-	pcie_war_pci_setup(pi);
-}
-
-void pcicore_up(struct pcicore_info *pi, int state)
-{
-	if (!pi || ai_get_buscoretype(pi->sih) != PCIE_CORE_ID)
-		return;
-
-	/* Restore L1 timer for better performance */
-	pcie_extendL1timer(pi, true);
-
-	pcie_clkreq_upd(pi, state);
-}
-
-/* When the device is going to enter D3 state
- * (or the system is going to enter S3/S4 states)
- */
-void pcicore_sleep(struct pcicore_info *pi)
-{
-	u32 w;
-
-	if (!pi || !PCIE_ASPM(pi->sih))
-		return;
-
-	pci_read_config_dword(pi->dev, pi->pciecap_lcreg_offset, &w);
-	w &= ~PCIE_CAP_LCREG_ASPML1;
-	pci_write_config_dword(pi->dev, pi->pciecap_lcreg_offset, w);
-
-	pi->pcie_pr42767 = false;
-}
-
-void pcicore_down(struct pcicore_info *pi, int state)
-{
-	if (!pi || ai_get_buscoretype(pi->sih) != PCIE_CORE_ID)
-		return;
-
-	pcie_clkreq_upd(pi, state);
-
-	/* Reduce L1 timer for better power savings */
-	pcie_extendL1timer(pi, false);
-}
-
-void pcicore_fixcfg(struct pcicore_info *pi)
-{
-	struct bcma_device *core = pi->core;
-	u16 val16;
-	uint regoff;
-
-	switch (pi->core->id.id) {
-	case BCMA_CORE_PCI:
-		regoff = PCIREGOFFS(sprom[SRSH_PI_OFFSET]);
-		break;
-
-	case BCMA_CORE_PCIE:
-		regoff = PCIEREGOFFS(sprom[SRSH_PI_OFFSET]);
-		break;
-
-	default:
-		return;
-	}
-
-	val16 = bcma_read16(pi->core, regoff);
-	if (((val16 & SRSH_PI_MASK) >> SRSH_PI_SHIFT) !=
-	    (u16)core->core_index) {
-		val16 = ((u16)core->core_index << SRSH_PI_SHIFT) |
-			(val16 & ~SRSH_PI_MASK);
-		bcma_write16(pi->core, regoff, val16);
-	}
-}
-
-/* precondition: current core is pci core */
-void
-pcicore_pci_setup(struct pcicore_info *pi)
-{
-	bcma_set32(pi->core, PCIREGOFFS(sbtopci2),
-		   SBTOPCI_PREF | SBTOPCI_BURST);
-
-	if (pi->core->id.rev >= 11) {
-		bcma_set32(pi->core, PCIREGOFFS(sbtopci2),
-			   SBTOPCI_RC_READMULTI);
-		bcma_set32(pi->core, PCIREGOFFS(clkrun), PCI_CLKRUN_DSBL);
-		(void)bcma_read32(pi->core, PCIREGOFFS(clkrun));
-	}
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.h b/drivers/net/wireless/brcm80211/brcmsmac/nicpci.h
deleted file mode 100644
index 9fc3ead540a8..000000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#ifndef	_BRCM_NICPCI_H_
-#define	_BRCM_NICPCI_H_
-
-#include "types.h"
-
-/* PCI configuration address space size */
-#define PCI_SZPCR		256
-
-/* Brcm PCI configuration registers */
-/* backplane address space accessed by BAR0 */
-#define PCI_BAR0_WIN		0x80
-/* sprom property control */
-#define PCI_SPROM_CONTROL	0x88
-/* mask of PCI and other cores interrupts */
-#define PCI_INT_MASK		0x94
-/* backplane core interrupt mask bits offset */
-#define  PCI_SBIM_SHIFT		8
-/* backplane address space accessed by second 4KB of BAR0 */
-#define PCI_BAR0_WIN2		0xac
-/* pci config space gpio input (>=rev3) */
-#define PCI_GPIO_IN		0xb0
-/* pci config space gpio output (>=rev3) */
-#define PCI_GPIO_OUT		0xb4
-/* pci config space gpio output enable (>=rev3) */
-#define PCI_GPIO_OUTEN		0xb8
-
-/* bar0 + 4K accesses external sprom */
-#define PCI_BAR0_SPROM_OFFSET	(4 * 1024)
-/* bar0 + 6K accesses pci core registers */
-#define PCI_BAR0_PCIREGS_OFFSET	(6 * 1024)
-/*
- * pci core SB registers are at the end of the
- * 8KB window, so their address is the "regular"
- * address plus 4K
- */
-#define PCI_BAR0_PCISBR_OFFSET	(4 * 1024)
-/* bar0 window size Match with corerev 13 */
-#define PCI_BAR0_WINSZ		(16 * 1024)
-/* On pci corerev >= 13 and all pcie, the bar0 is now 16KB and it maps: */
-/* bar0 + 8K accesses pci/pcie core registers */
-#define PCI_16KB0_PCIREGS_OFFSET (8 * 1024)
-/* bar0 + 12K accesses chipc core registers */
-#define PCI_16KB0_CCREGS_OFFSET	(12 * 1024)
-
-struct sbpciregs;
-struct sbpcieregs;
-
-extern struct pcicore_info *pcicore_init(struct si_pub *sih,
-					 struct bcma_device *core);
-extern void pcicore_deinit(struct pcicore_info *pch);
-extern void pcicore_attach(struct pcicore_info *pch, int state);
-extern void pcicore_hwup(struct pcicore_info *pch);
-extern void pcicore_up(struct pcicore_info *pch, int state);
-extern void pcicore_sleep(struct pcicore_info *pch);
-extern void pcicore_down(struct pcicore_info *pch, int state);
-extern u8 pcicore_find_pci_capability(struct pci_dev *dev, u8 req_cap_id,
-				      unsigned char *buf, u32 *buflen);
-extern void pcicore_fixcfg(struct pcicore_info *pch);
-extern void pcicore_pci_setup(struct pcicore_info *pch);
-
-#endif /* _BRCM_NICPCI_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/otp.c b/drivers/net/wireless/brcm80211/brcmsmac/otp.c
deleted file mode 100644
index f1ca12625860..000000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/otp.c
+++ /dev/null
@@ -1,410 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#include <linux/io.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-
-#include <brcm_hw_ids.h>
-#include <chipcommon.h>
-#include "aiutils.h"
-#include "otp.h"
-
-#define OTPS_GUP_MASK		0x00000f00
-#define OTPS_GUP_SHIFT		8
-/* h/w subregion is programmed */
-#define OTPS_GUP_HW		0x00000100
-/* s/w subregion is programmed */
-#define OTPS_GUP_SW		0x00000200
-/* chipid/pkgopt subregion is programmed */
-#define OTPS_GUP_CI		0x00000400
-/* fuse subregion is programmed */
-#define OTPS_GUP_FUSE		0x00000800
-
-/* Fields in otpprog in rev >= 21 */
-#define OTPP_COL_MASK		0x000000ff
-#define OTPP_COL_SHIFT		0
-#define OTPP_ROW_MASK		0x0000ff00
-#define OTPP_ROW_SHIFT		8
-#define OTPP_OC_MASK		0x0f000000
-#define OTPP_OC_SHIFT		24
-#define OTPP_READERR		0x10000000
-#define OTPP_VALUE_MASK		0x20000000
-#define OTPP_VALUE_SHIFT	29
-#define OTPP_START_BUSY		0x80000000
-#define	OTPP_READ		0x40000000
-
-/* Opcodes for OTPP_OC field */
-#define OTPPOC_READ		0
-#define OTPPOC_BIT_PROG		1
-#define OTPPOC_VERIFY		3
-#define OTPPOC_INIT		4
-#define OTPPOC_SET		5
-#define OTPPOC_RESET		6
-#define OTPPOC_OCST		7
-#define OTPPOC_ROW_LOCK		8
-#define OTPPOC_PRESCN_TEST	9
-
-#define OTPTYPE_IPX(ccrev)	((ccrev) == 21 || (ccrev) >= 23)
-
-#define OTPP_TRIES	10000000	/* # of tries for OTPP */
-
-#define MAXNUMRDES		9	/* Maximum OTP redundancy entries */
-
-/* Fixed size subregions sizes in words */
-#define OTPGU_CI_SZ		2
-
-struct otpinfo;
-
-/* OTP function struct */
-struct otp_fn_s {
-	int (*init)(struct si_pub *sih, struct otpinfo *oi);
-	int (*read_region)(struct otpinfo *oi, int region, u16 *data,
-			   uint *wlen);
-};
-
-struct otpinfo {
-	struct bcma_device *core; /* chipc core */
-	const struct otp_fn_s *fn;	/* OTP functions */
-	struct si_pub *sih;		/* Saved sb handle */
-
-	/* IPX OTP section */
-	u16 wsize;		/* Size of otp in words */
-	u16 rows;		/* Geometry */
-	u16 cols;		/* Geometry */
-	u32 status;		/* Flag bits (lock/prog/rv).
-				 * (Reflected only when OTP is power cycled)
-				 */
-	u16 hwbase;		/* hardware subregion offset */
-	u16 hwlim;		/* hardware subregion boundary */
-	u16 swbase;		/* software subregion offset */
-	u16 swlim;		/* software subregion boundary */
-	u16 fbase;		/* fuse subregion offset */
-	u16 flim;		/* fuse subregion boundary */
-	int otpgu_base;		/* offset to General Use Region */
-};
-
-/* OTP layout */
-/* CC revs 21, 24 and 27 OTP General Use Region word offset */
-#define REVA4_OTPGU_BASE	12
-
-/* CC revs 23, 25, 26, 28 and above OTP General Use Region word offset */
-#define REVB8_OTPGU_BASE	20
-
-/* CC rev 36 OTP General Use Region word offset */
-#define REV36_OTPGU_BASE	12
-
-/* Subregion word offsets in General Use region */
-#define OTPGU_HSB_OFF		0
-#define OTPGU_SFB_OFF		1
-#define OTPGU_CI_OFF		2
-#define OTPGU_P_OFF		3
-#define OTPGU_SROM_OFF		4
-
-/* Flag bit offsets in General Use region  */
-#define OTPGU_HWP_OFF		60
-#define OTPGU_SWP_OFF		61
-#define OTPGU_CIP_OFF		62
-#define OTPGU_FUSEP_OFF		63
-#define OTPGU_CIP_MSK		0x4000
-#define OTPGU_P_MSK		0xf000
-#define OTPGU_P_SHIFT		(OTPGU_HWP_OFF % 16)
-
-/* OTP Size */
-#define OTP_SZ_FU_324		((roundup(324, 8))/8)	/* 324 bits */
-#define OTP_SZ_FU_288		(288/8)	/* 288 bits */
-#define OTP_SZ_FU_216		(216/8)	/* 216 bits */
-#define OTP_SZ_FU_72		(72/8)	/* 72 bits */
-#define OTP_SZ_CHECKSUM		(16/8)	/* 16 bits */
-#define OTP4315_SWREG_SZ	178	/* 178 bytes */
-#define OTP_SZ_FU_144		(144/8)	/* 144 bits */
-
-static u16
-ipxotp_otpr(struct otpinfo *oi, uint wn)
-{
-	return bcma_read16(oi->core,
-			   CHIPCREGOFFS(sromotp[wn]));
-}
-
-/*
- * Calculate max HW/SW region byte size by subtracting fuse region
- * and checksum size, osizew is oi->wsize (OTP size - GU size) in words
- */
-static int ipxotp_max_rgnsz(struct si_pub *sih, int osizew)
-{
-	int ret = 0;
-
-	switch (ai_get_chip_id(sih)) {
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-		ret = osizew * 2 - OTP_SZ_FU_72 - OTP_SZ_CHECKSUM;
-		break;
-	case BCM4313_CHIP_ID:
-		ret = osizew * 2 - OTP_SZ_FU_72 - OTP_SZ_CHECKSUM;
-		break;
-	default:
-		break;	/* Don't know about this chip */
-	}
-
-	return ret;
-}
-
-static void _ipxotp_init(struct otpinfo *oi)
-{
-	uint k;
-	u32 otpp, st;
-	int ccrev = ai_get_ccrev(oi->sih);
-
-
-	/*
-	 * record word offset of General Use Region
-	 * for various chipcommon revs
-	 */
-	if (ccrev == 21 || ccrev == 24
-	    || ccrev == 27) {
-		oi->otpgu_base = REVA4_OTPGU_BASE;
-	} else if (ccrev == 36) {
-		/*
-		 * OTP size greater than equal to 2KB (128 words),
-		 * otpgu_base is similar to rev23
-		 */
-		if (oi->wsize >= 128)
-			oi->otpgu_base = REVB8_OTPGU_BASE;
-		else
-			oi->otpgu_base = REV36_OTPGU_BASE;
-	} else if (ccrev == 23 || ccrev >= 25) {
-		oi->otpgu_base = REVB8_OTPGU_BASE;
-	}
-
-	/* First issue an init command so the status is up to date */
-	otpp =
-	    OTPP_START_BUSY | ((OTPPOC_INIT << OTPP_OC_SHIFT) & OTPP_OC_MASK);
-
-	bcma_write32(oi->core, CHIPCREGOFFS(otpprog), otpp);
-	st = bcma_read32(oi->core, CHIPCREGOFFS(otpprog));
-	for (k = 0; (st & OTPP_START_BUSY) && (k < OTPP_TRIES); k++)
-		st = bcma_read32(oi->core, CHIPCREGOFFS(otpprog));
-	if (k >= OTPP_TRIES)
-		return;
-
-	/* Read OTP lock bits and subregion programmed indication bits */
-	oi->status = bcma_read32(oi->core, CHIPCREGOFFS(otpstatus));
-
-	if ((ai_get_chip_id(oi->sih) == BCM43224_CHIP_ID)
-	    || (ai_get_chip_id(oi->sih) == BCM43225_CHIP_ID)) {
-		u32 p_bits;
-		p_bits = (ipxotp_otpr(oi, oi->otpgu_base + OTPGU_P_OFF) &
-			  OTPGU_P_MSK) >> OTPGU_P_SHIFT;
-		oi->status |= (p_bits << OTPS_GUP_SHIFT);
-	}
-
-	/*
-	 * h/w region base and fuse region limit are fixed to
-	 * the top and the bottom of the general use region.
-	 * Everything else can be flexible.
-	 */
-	oi->hwbase = oi->otpgu_base + OTPGU_SROM_OFF;
-	oi->hwlim = oi->wsize;
-	if (oi->status & OTPS_GUP_HW) {
-		oi->hwlim =
-		    ipxotp_otpr(oi, oi->otpgu_base + OTPGU_HSB_OFF) / 16;
-		oi->swbase = oi->hwlim;
-	} else
-		oi->swbase = oi->hwbase;
-
-	/* subtract fuse and checksum from beginning */
-	oi->swlim = ipxotp_max_rgnsz(oi->sih, oi->wsize) / 2;
-
-	if (oi->status & OTPS_GUP_SW) {
-		oi->swlim =
-		    ipxotp_otpr(oi, oi->otpgu_base + OTPGU_SFB_OFF) / 16;
-		oi->fbase = oi->swlim;
-	} else
-		oi->fbase = oi->swbase;
-
-	oi->flim = oi->wsize;
-}
-
-static int ipxotp_init(struct si_pub *sih, struct otpinfo *oi)
-{
-	/* Make sure we're running IPX OTP */
-	if (!OTPTYPE_IPX(ai_get_ccrev(sih)))
-		return -EBADE;
-
-	/* Make sure OTP is not disabled */
-	if (ai_is_otp_disabled(sih))
-		return -EBADE;
-
-	/* Check for otp size */
-	switch ((ai_get_cccaps(sih) & CC_CAP_OTPSIZE) >> CC_CAP_OTPSIZE_SHIFT) {
-	case 0:
-		/* Nothing there */
-		return -EBADE;
-	case 1:		/* 32x64 */
-		oi->rows = 32;
-		oi->cols = 64;
-		oi->wsize = 128;
-		break;
-	case 2:		/* 64x64 */
-		oi->rows = 64;
-		oi->cols = 64;
-		oi->wsize = 256;
-		break;
-	case 5:		/* 96x64 */
-		oi->rows = 96;
-		oi->cols = 64;
-		oi->wsize = 384;
-		break;
-	case 7:		/* 16x64 *//* 1024 bits */
-		oi->rows = 16;
-		oi->cols = 64;
-		oi->wsize = 64;
-		break;
-	default:
-		/* Don't know the geometry */
-		return -EBADE;
-	}
-
-	/* Retrieve OTP region info */
-	_ipxotp_init(oi);
-	return 0;
-}
-
-static int
-ipxotp_read_region(struct otpinfo *oi, int region, u16 *data, uint *wlen)
-{
-	uint base, i, sz;
-
-	/* Validate region selection */
-	switch (region) {
-	case OTP_HW_RGN:
-		sz = (uint) oi->hwlim - oi->hwbase;
-		if (!(oi->status & OTPS_GUP_HW)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->hwbase;
-		break;
-	case OTP_SW_RGN:
-		sz = ((uint) oi->swlim - oi->swbase);
-		if (!(oi->status & OTPS_GUP_SW)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->swbase;
-		break;
-	case OTP_CI_RGN:
-		sz = OTPGU_CI_SZ;
-		if (!(oi->status & OTPS_GUP_CI)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->otpgu_base + OTPGU_CI_OFF;
-		break;
-	case OTP_FUSE_RGN:
-		sz = (uint) oi->flim - oi->fbase;
-		if (!(oi->status & OTPS_GUP_FUSE)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->fbase;
-		break;
-	case OTP_ALL_RGN:
-		sz = ((uint) oi->flim - oi->hwbase);
-		if (!(oi->status & (OTPS_GUP_HW | OTPS_GUP_SW))) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->hwbase;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	/* Read the data */
-	for (i = 0; i < sz; i++)
-		data[i] = ipxotp_otpr(oi, base + i);
-
-	*wlen = sz;
-	return 0;
-}
-
-static const struct otp_fn_s ipxotp_fn = {
-	(int (*)(struct si_pub *, struct otpinfo *)) ipxotp_init,
-	(int (*)(struct otpinfo *, int, u16 *, uint *)) ipxotp_read_region,
-};
-
-static int otp_init(struct si_pub *sih, struct otpinfo *oi)
-{
-	int ret;
-
-	memset(oi, 0, sizeof(struct otpinfo));
-
-	oi->core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-
-	if (OTPTYPE_IPX(ai_get_ccrev(sih)))
-		oi->fn = &ipxotp_fn;
-
-	if (oi->fn == NULL)
-		return -EBADE;
-
-	oi->sih = sih;
-
-	ret = (oi->fn->init)(sih, oi);
-
-	return ret;
-}
-
-int
-otp_read_region(struct si_pub *sih, int region, u16 *data, uint *wlen) {
-	struct otpinfo otpinfo;
-	struct otpinfo *oi = &otpinfo;
-	int err = 0;
-
-	if (ai_is_otp_disabled(sih)) {
-		err = -EPERM;
-		goto out;
-	}
-
-	err = otp_init(sih, oi);
-	if (err)
-		goto out;
-
-	err = ((oi)->fn->read_region)(oi, region, data, wlen);
-
- out:
-	return err;
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/otp.h b/drivers/net/wireless/brcm80211/brcmsmac/otp.h
deleted file mode 100644
index 6b6d31cf9569..000000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/otp.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#ifndef	_BRCM_OTP_H_
-#define	_BRCM_OTP_H_
-
-#include "types.h"
-
-/* OTP regions */
-#define OTP_HW_RGN	1
-#define OTP_SW_RGN	2
-#define OTP_CI_RGN	4
-#define OTP_FUSE_RGN	8
-/* From h/w region to end of OTP including checksum */
-#define OTP_ALL_RGN	0xf
-
-/* OTP Size */
-#define OTP_SZ_MAX		(6144/8)	/* maximum bytes in one CIS */
-
-extern int otp_read_region(struct si_pub *sih, int region, u16 *data,
-			   uint *wlen);
-
-#endif				/* _BRCM_OTP_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
index 0fce56235f38..abfd78822fb8 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
@@ -4817,28 +4817,23 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 	s8 txpwr = 0;
 	int i;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
-	struct phy_shim_info *shim = pi->sh->physhim;
+	struct ssb_sprom *sprom = &pi->d11core->bus->sprom;
 
 	if (CHSPEC_IS2G(pi->radio_chanspec)) {
 		u16 cckpo = 0;
 		u32 offset_ofdm, offset_mcs;
 
-		pi_lcn->lcnphy_tr_isolation_mid =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TRISO2G);
+		pi_lcn->lcnphy_tr_isolation_mid = sprom->fem.ghz2.tr_iso;
 
-		pi_lcn->lcnphy_rx_power_offset =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_RXPO2G);
+		pi_lcn->lcnphy_rx_power_offset = sprom->rxpo2g;
 
-		pi->txpa_2g[0] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B0);
-		pi->txpa_2g[1] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B1);
-		pi->txpa_2g[2] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B2);
+		pi->txpa_2g[0] = sprom->pa0b0;
+		pi->txpa_2g[1] = sprom->pa0b1;
+		pi->txpa_2g[2] = sprom->pa0b2;
 
-		pi_lcn->lcnphy_rssi_vf =
-				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISMF2G);
-		pi_lcn->lcnphy_rssi_vc =
-				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISMC2G);
-		pi_lcn->lcnphy_rssi_gs =
-				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISAV2G);
+		pi_lcn->lcnphy_rssi_vf = sprom->rssismf2g;
+		pi_lcn->lcnphy_rssi_vc = sprom->rssismc2g;
+		pi_lcn->lcnphy_rssi_gs = sprom->rssisav2g;
 
 		pi_lcn->lcnphy_rssi_vf_lowtemp = pi_lcn->lcnphy_rssi_vf;
 		pi_lcn->lcnphy_rssi_vc_lowtemp = pi_lcn->lcnphy_rssi_vc;
@@ -4848,7 +4843,7 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 		pi_lcn->lcnphy_rssi_vc_hightemp = pi_lcn->lcnphy_rssi_vc;
 		pi_lcn->lcnphy_rssi_gs_hightemp = pi_lcn->lcnphy_rssi_gs;
 
-		txpwr = (s8)wlapi_getintvar(shim, BRCMS_SROM_MAXP2GA0);
+		txpwr = sprom->core_pwr_info[0].maxpwr_2g;
 		pi->tx_srom_max_2g = txpwr;
 
 		for (i = 0; i < PWRTBL_NUM_COEFF; i++) {
@@ -4856,8 +4851,8 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 			pi->txpa_2g_high_temp[i] = pi->txpa_2g[i];
 		}
 
-		cckpo = (u16)wlapi_getintvar(shim, BRCMS_SROM_CCK2GPO);
-		offset_ofdm = (u32)wlapi_getintvar(shim, BRCMS_SROM_OFDM2GPO);
+		cckpo = sprom->cck2gpo;
+		offset_ofdm = sprom->ofdm2gpo;
 		if (cckpo) {
 			uint max_pwr_chan = txpwr;
 
@@ -4876,7 +4871,7 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 		} else {
 			u8 opo = 0;
 
-			opo = (u8)wlapi_getintvar(shim, BRCMS_SROM_OPO);
+			opo = sprom->opo;
 
 			for (i = TXP_FIRST_CCK; i <= TXP_LAST_CCK; i++)
 				pi->tx_srom_max_rate_2g[i] = txpwr;
@@ -4886,12 +4881,8 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 						((offset_ofdm & 0xf) * 2);
 				offset_ofdm >>= 4;
 			}
-			offset_mcs =
-				wlapi_getintvar(shim,
-						BRCMS_SROM_MCS2GPO1) << 16;
-			offset_mcs |=
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO0);
+			offset_mcs = sprom->mcs2gpo[1] << 16;
+			offset_mcs |= sprom->mcs2gpo[0];
 			pi_lcn->lcnphy_mcs20_po = offset_mcs;
 			for (i = TXP_FIRST_SISO_MCS_20;
 			     i <= TXP_LAST_SISO_MCS_20; i++) {
@@ -4901,25 +4892,17 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 			}
 		}
 
-		pi_lcn->lcnphy_rawtempsense =
-			(u16)wlapi_getintvar(shim, BRCMS_SROM_RAWTEMPSENSE);
-		pi_lcn->lcnphy_measPower =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_MEASPOWER);
-		pi_lcn->lcnphy_tempsense_slope =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPSENSE_SLOPE);
-		pi_lcn->lcnphy_hw_iqcal_en =
-			(bool)wlapi_getintvar(shim, BRCMS_SROM_HW_IQCAL_EN);
-		pi_lcn->lcnphy_iqcal_swp_dis =
-			(bool)wlapi_getintvar(shim, BRCMS_SROM_IQCAL_SWP_DIS);
-		pi_lcn->lcnphy_tempcorrx =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPCORRX);
-		pi_lcn->lcnphy_tempsense_option =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPSENSE_OPTION);
-		pi_lcn->lcnphy_freqoffset_corr =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_FREQOFFSET_CORR);
-		if ((u8)wlapi_getintvar(shim, BRCMS_SROM_AA2G) > 1)
+		pi_lcn->lcnphy_rawtempsense = sprom->rawtempsense;
+		pi_lcn->lcnphy_measPower = sprom->measpower;
+		pi_lcn->lcnphy_tempsense_slope = sprom->tempsense_slope;
+		pi_lcn->lcnphy_hw_iqcal_en = sprom->hw_iqcal_en;
+		pi_lcn->lcnphy_iqcal_swp_dis = sprom->iqcal_swp_dis;
+		pi_lcn->lcnphy_tempcorrx = sprom->tempcorrx;
+		pi_lcn->lcnphy_tempsense_option = sprom->tempsense_option;
+		pi_lcn->lcnphy_freqoffset_corr = sprom->freqoffset_corr;
+		if (sprom->ant_available_bg > 1)
 			wlc_phy_ant_rxdiv_set((struct brcms_phy_pub *) pi,
-				(u8) wlapi_getintvar(shim, BRCMS_SROM_AA2G));
+				sprom->ant_available_bg);
 	}
 	pi_lcn->lcnphy_cck_dig_filt_type = -1;
 
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
index 812b6e38526e..13b261517cce 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
@@ -14386,30 +14386,30 @@ static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
 {
 	u16 bw40po, cddpo, stbcpo, bwduppo;
 	uint band_num;
-	struct phy_shim_info *shim = pi->sh->physhim;
+	struct ssb_sprom *sprom = &pi->d11core->bus->sprom;
 
 	if (pi->sh->sromrev >= 9)
 		return;
 
-	bw40po = (u16) wlapi_getintvar(shim, BRCMS_SROM_BW40PO);
+	bw40po = sprom->bw40po;
 	pi->bw402gpo = bw40po & 0xf;
 	pi->bw405gpo = (bw40po & 0xf0) >> 4;
 	pi->bw405glpo = (bw40po & 0xf00) >> 8;
 	pi->bw405ghpo = (bw40po & 0xf000) >> 12;
 
-	cddpo = (u16) wlapi_getintvar(shim, BRCMS_SROM_CDDPO);
+	cddpo = sprom->cddpo;
 	pi->cdd2gpo = cddpo & 0xf;
 	pi->cdd5gpo = (cddpo & 0xf0) >> 4;
 	pi->cdd5glpo = (cddpo & 0xf00) >> 8;
 	pi->cdd5ghpo = (cddpo & 0xf000) >> 12;
 
-	stbcpo = (u16) wlapi_getintvar(shim, BRCMS_SROM_STBCPO);
+	stbcpo = sprom->stbcpo;
 	pi->stbc2gpo = stbcpo & 0xf;
 	pi->stbc5gpo = (stbcpo & 0xf0) >> 4;
 	pi->stbc5glpo = (stbcpo & 0xf00) >> 8;
 	pi->stbc5ghpo = (stbcpo & 0xf000) >> 12;
 
-	bwduppo = (u16) wlapi_getintvar(shim, BRCMS_SROM_BWDUPPO);
+	bwduppo = sprom->bwduppo;
 	pi->bwdup2gpo = bwduppo & 0xf;
 	pi->bwdup5gpo = (bwduppo & 0xf0) >> 4;
 	pi->bwdup5glpo = (bwduppo & 0xf00) >> 8;
@@ -14419,242 +14419,137 @@ static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
 	     band_num++) {
 		switch (band_num) {
 		case 0:
-
 			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_2g =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP2GA0);
+				sprom->core_pwr_info[0].maxpwr_2g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_2g =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP2GA1);
+				sprom->core_pwr_info[1].maxpwr_2g;
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW0A0);
+				sprom->core_pwr_info[0].pa_2g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW0A1);
+				sprom->core_pwr_info[1].pa_2g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW1A0);
+				sprom->core_pwr_info[0].pa_2g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW1A1);
+				sprom->core_pwr_info[1].pa_2g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW2A0);
+				sprom->core_pwr_info[0].pa_2g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW2A1);
+				sprom->core_pwr_info[1].pa_2g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_2g =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT2GA0);
+				sprom->core_pwr_info[0].itssi_2g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_2g =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT2GA1);
-
-			pi->cck2gpo = (u16) wlapi_getintvar(shim,
-							    BRCMS_SROM_CCK2GPO);
-
-			pi->ofdm2gpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM2GPO);
-
-			pi->mcs2gpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO0);
-			pi->mcs2gpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO1);
-			pi->mcs2gpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO2);
-			pi->mcs2gpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO3);
-			pi->mcs2gpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO4);
-			pi->mcs2gpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO5);
-			pi->mcs2gpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO6);
-			pi->mcs2gpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO7);
+				sprom->core_pwr_info[1].itssi_2g;
+
+			pi->cck2gpo = sprom->cck2gpo;
+
+			pi->ofdm2gpo = sprom->ofdm2gpo;
+
+			pi->mcs2gpo[0] = sprom->mcs2gpo[0];
+			pi->mcs2gpo[1] = sprom->mcs2gpo[1];
+			pi->mcs2gpo[2] = sprom->mcs2gpo[2];
+			pi->mcs2gpo[3] = sprom->mcs2gpo[3];
+			pi->mcs2gpo[4] = sprom->mcs2gpo[4];
+			pi->mcs2gpo[5] = sprom->mcs2gpo[5];
+			pi->mcs2gpo[6] = sprom->mcs2gpo[6];
+			pi->mcs2gpo[7] = sprom->mcs2gpo[7];
 			break;
 		case 1:
 
 			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_5gm =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_MAXP5GA0);
+				sprom->core_pwr_info[0].maxpwr_5g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_5gm =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GA1);
+				sprom->core_pwr_info[1].maxpwr_5g;
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW0A0);
+				sprom->core_pwr_info[0].pa_5g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW0A1);
+				sprom->core_pwr_info[1].pa_5g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW1A0);
+				sprom->core_pwr_info[0].pa_5g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW1A1);
+				sprom->core_pwr_info[1].pa_5g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW2A0);
+				sprom->core_pwr_info[0].pa_5g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW2A1);
+				sprom->core_pwr_info[1].pa_5g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_5gm =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT5GA0);
+				sprom->core_pwr_info[0].itssi_5g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_5gm =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT5GA1);
-
-			pi->ofdm5gpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM5GPO);
-
-			pi->mcs5gpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO0);
-			pi->mcs5gpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO1);
-			pi->mcs5gpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO2);
-			pi->mcs5gpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO3);
-			pi->mcs5gpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO4);
-			pi->mcs5gpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO5);
-			pi->mcs5gpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO6);
-			pi->mcs5gpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO7);
+				sprom->core_pwr_info[1].itssi_5g;
+
+			pi->ofdm5gpo = sprom->ofdm5gpo;
+
+			pi->mcs5gpo[0] = sprom->mcs5gpo[0];
+			pi->mcs5gpo[1] = sprom->mcs5gpo[1];
+			pi->mcs5gpo[2] = sprom->mcs5gpo[2];
+			pi->mcs5gpo[3] = sprom->mcs5gpo[3];
+			pi->mcs5gpo[4] = sprom->mcs5gpo[4];
+			pi->mcs5gpo[5] = sprom->mcs5gpo[5];
+			pi->mcs5gpo[6] = sprom->mcs5gpo[6];
+			pi->mcs5gpo[7] = sprom->mcs5gpo[7];
 			break;
 		case 2:
 
 			pi->nphy_pwrctrl_info[0].max_pwr_5gl =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GLA0);
+				sprom->core_pwr_info[0].maxpwr_5gl;
 			pi->nphy_pwrctrl_info[1].max_pwr_5gl =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GLA1);
+				sprom->core_pwr_info[1].maxpwr_5gl;
 			pi->nphy_pwrctrl_info[0].pwrdet_5gl_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW0A0);
+				sprom->core_pwr_info[0].pa_5gl[0];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gl_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW0A1);
+				sprom->core_pwr_info[1].pa_5gl[0];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW1A0);
+				sprom->core_pwr_info[0].pa_5gl[1];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW1A1);
+				sprom->core_pwr_info[1].pa_5gl[1];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW2A0);
+				sprom->core_pwr_info[0].pa_5gl[2];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW2A1);
+				sprom->core_pwr_info[1].pa_5gl[2];
 			pi->nphy_pwrctrl_info[0].idle_targ_5gl = 0;
 			pi->nphy_pwrctrl_info[1].idle_targ_5gl = 0;
 
-			pi->ofdm5glpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM5GLPO);
-
-			pi->mcs5glpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO0);
-			pi->mcs5glpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO1);
-			pi->mcs5glpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO2);
-			pi->mcs5glpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO3);
-			pi->mcs5glpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO4);
-			pi->mcs5glpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO5);
-			pi->mcs5glpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO6);
-			pi->mcs5glpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO7);
+			pi->ofdm5glpo = sprom->ofdm5glpo;
+
+			pi->mcs5glpo[0] = sprom->mcs5glpo[0];
+			pi->mcs5glpo[1] = sprom->mcs5glpo[1];
+			pi->mcs5glpo[2] = sprom->mcs5glpo[2];
+			pi->mcs5glpo[3] = sprom->mcs5glpo[3];
+			pi->mcs5glpo[4] = sprom->mcs5glpo[4];
+			pi->mcs5glpo[5] = sprom->mcs5glpo[5];
+			pi->mcs5glpo[6] = sprom->mcs5glpo[6];
+			pi->mcs5glpo[7] = sprom->mcs5glpo[7];
 			break;
 		case 3:
 
 			pi->nphy_pwrctrl_info[0].max_pwr_5gh =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GHA0);
+				sprom->core_pwr_info[0].maxpwr_5gh;
 			pi->nphy_pwrctrl_info[1].max_pwr_5gh =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GHA1);
+				sprom->core_pwr_info[1].maxpwr_5gh;
 			pi->nphy_pwrctrl_info[0].pwrdet_5gh_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW0A0);
+				sprom->core_pwr_info[0].pa_5gh[0];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gh_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW0A1);
+				sprom->core_pwr_info[1].pa_5gh[0];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW1A0);
+				sprom->core_pwr_info[0].pa_5gh[1];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW1A1);
+				sprom->core_pwr_info[1].pa_5gh[1];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW2A0);
+				sprom->core_pwr_info[0].pa_5gh[2];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW2A1);
+				sprom->core_pwr_info[1].pa_5gh[2];
 			pi->nphy_pwrctrl_info[0].idle_targ_5gh = 0;
 			pi->nphy_pwrctrl_info[1].idle_targ_5gh = 0;
 
-			pi->ofdm5ghpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM5GHPO);
-
-			pi->mcs5ghpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO0);
-			pi->mcs5ghpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO1);
-			pi->mcs5ghpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO2);
-			pi->mcs5ghpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO3);
-			pi->mcs5ghpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO4);
-			pi->mcs5ghpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO5);
-			pi->mcs5ghpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO6);
-			pi->mcs5ghpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO7);
+			pi->ofdm5ghpo = sprom->ofdm5ghpo;
+
+			pi->mcs5ghpo[0] = sprom->mcs5ghpo[0];
+			pi->mcs5ghpo[1] = sprom->mcs5ghpo[1];
+			pi->mcs5ghpo[2] = sprom->mcs5ghpo[2];
+			pi->mcs5ghpo[3] = sprom->mcs5ghpo[3];
+			pi->mcs5ghpo[4] = sprom->mcs5ghpo[4];
+			pi->mcs5ghpo[5] = sprom->mcs5ghpo[5];
+			pi->mcs5ghpo[6] = sprom->mcs5ghpo[6];
+			pi->mcs5ghpo[7] = sprom->mcs5ghpo[7];
 			break;
 		}
 	}
@@ -14664,45 +14559,34 @@ static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
 
 static bool wlc_phy_txpwr_srom_read_nphy(struct brcms_phy *pi)
 {
-	struct phy_shim_info *shim = pi->sh->physhim;
-
-	pi->antswitch = (u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWITCH);
-	pi->aa2g = (u8) wlapi_getintvar(shim, BRCMS_SROM_AA2G);
-	pi->aa5g = (u8) wlapi_getintvar(shim, BRCMS_SROM_AA5G);
-
-	pi->srom_fem2g.tssipos = (u8) wlapi_getintvar(shim,
-						      BRCMS_SROM_TSSIPOS2G);
-	pi->srom_fem2g.extpagain = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_EXTPAGAIN2G);
-	pi->srom_fem2g.pdetrange = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_PDETRANGE2G);
-	pi->srom_fem2g.triso = (u8) wlapi_getintvar(shim, BRCMS_SROM_TRISO2G);
-	pi->srom_fem2g.antswctrllut =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL2G);
-
-	pi->srom_fem5g.tssipos = (u8) wlapi_getintvar(shim,
-						      BRCMS_SROM_TSSIPOS5G);
-	pi->srom_fem5g.extpagain = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_EXTPAGAIN5G);
-	pi->srom_fem5g.pdetrange = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_PDETRANGE5G);
-	pi->srom_fem5g.triso = (u8) wlapi_getintvar(shim, BRCMS_SROM_TRISO5G);
-	if (wlapi_getvar(shim, BRCMS_SROM_ANTSWCTL5G))
-		pi->srom_fem5g.antswctrllut =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL5G);
+	struct ssb_sprom *sprom = &pi->d11core->bus->sprom;
+
+	pi->antswitch = sprom->antswitch;
+	pi->aa2g = sprom->ant_available_bg;
+	pi->aa5g = sprom->ant_available_a;
+
+	pi->srom_fem2g.tssipos = sprom->fem.ghz2.tssipos;
+	pi->srom_fem2g.extpagain = sprom->fem.ghz2.extpa_gain;
+	pi->srom_fem2g.pdetrange = sprom->fem.ghz2.pdet_range;
+	pi->srom_fem2g.triso = sprom->fem.ghz2.tr_iso;
+	pi->srom_fem2g.antswctrllut = sprom->fem.ghz2.antswlut;
+
+	pi->srom_fem5g.tssipos = sprom->fem.ghz5.tssipos;
+	pi->srom_fem5g.extpagain = sprom->fem.ghz5.extpa_gain;
+	pi->srom_fem5g.pdetrange = sprom->fem.ghz5.pdet_range;
+	pi->srom_fem5g.triso = sprom->fem.ghz5.tr_iso;
+	if (sprom->fem.ghz5.antswlut)
+		pi->srom_fem5g.antswctrllut = sprom->fem.ghz5.antswlut;
 	else
-		pi->srom_fem5g.antswctrllut =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL2G);
+		pi->srom_fem5g.antswctrllut = sprom->fem.ghz2.antswlut;
 
 	wlc_phy_txpower_ipa_upd(pi);
 
-	pi->phy_txcore_disable_temp =
-			(s16) wlapi_getintvar(shim, BRCMS_SROM_TEMPTHRESH);
+	pi->phy_txcore_disable_temp = sprom->tempthresh;
 	if (pi->phy_txcore_disable_temp == 0)
 		pi->phy_txcore_disable_temp = PHY_CHAIN_TX_DISABLE_TEMP;
 
-	pi->phy_tempsense_offset = (s8) wlapi_getintvar(shim,
-							BRCMS_SROM_TEMPOFFSET);
+	pi->phy_tempsense_offset = sprom->tempoffset;
 	if (pi->phy_tempsense_offset != 0) {
 		if (pi->phy_tempsense_offset >
 		    (NPHY_SROM_TEMPSHIFT + NPHY_SROM_MAXTEMPOFFSET))
@@ -14717,8 +14601,7 @@ static bool wlc_phy_txpwr_srom_read_nphy(struct brcms_phy *pi)
 	pi->phy_txcore_enable_temp =
 		pi->phy_txcore_disable_temp - PHY_HYSTERESIS_DELTATEMP;
 
-	pi->phycal_tempdelta =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_PHYCAL_TEMPDELTA);
+	pi->phycal_tempdelta = sprom->phycal_tempdelta;
 	if (pi->phycal_tempdelta > NPHY_CAL_MAXTEMPDELTA)
 		pi->phycal_tempdelta = 0;
 
@@ -21460,7 +21343,7 @@ void wlc_phy_antsel_init(struct brcms_phy_pub *ppi, bool lut_init)
 		write_phy_reg(pi, 0xc8, 0x0);
 		write_phy_reg(pi, 0xc9, 0x0);
 
-		ai_gpiocontrol(pi->sh->sih, mask, mask, GPIO_DRV_PRIORITY);
+		bcma_chipco_gpio_control(&pi->d11core->bus->drv_cc, mask, mask);
 
 		mc = bcma_read32(pi->d11core, D11REGOFFS(maccontrol));
 		mc &= ~MCTL_GPOUT_SEL_MASK;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c
index 5926854f62e2..a0de5db0cd64 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c
@@ -214,12 +214,3 @@ wlapi_copyto_objmem(struct phy_shim_info *physhim, uint offset, const void *buf,
 {
 	brcms_b_copyto_objmem(physhim->wlc_hw, offset, buf, l, sel);
 }
-
-char *wlapi_getvar(struct phy_shim_info *physhim, enum brcms_srom_id id)
-{
-	return getvar(physhim->wlc_hw->sih, id);
-}
-int wlapi_getintvar(struct phy_shim_info *physhim, enum brcms_srom_id id)
-{
-	return getintvar(physhim->wlc_hw->sih, id);
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h
index 9168c459b185..2c5b66b75970 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h
@@ -175,8 +175,5 @@ extern void wlapi_copyto_objmem(struct phy_shim_info *physhim, uint,
 extern void wlapi_high_update_phy_mode(struct phy_shim_info *physhim,
 				       u32 phy_mode);
 extern u16 wlapi_bmac_get_txant(struct phy_shim_info *physhim);
-extern char *wlapi_getvar(struct phy_shim_info *physhim, enum brcms_srom_id id);
-extern int wlapi_getintvar(struct phy_shim_info *physhim,
-			   enum brcms_srom_id id);
 
 #endif				/* _BRCM_PHY_SHIM_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pub.h b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
index f0038ad7d7bf..aa5d67f8d874 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pub.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
@@ -22,232 +22,6 @@
 #include "types.h"
 #include "defs.h"
 
-enum brcms_srom_id {
-	BRCMS_SROM_NULL,
-	BRCMS_SROM_CONT,
-	BRCMS_SROM_AA2G,
-	BRCMS_SROM_AA5G,
-	BRCMS_SROM_AG0,
-	BRCMS_SROM_AG1,
-	BRCMS_SROM_AG2,
-	BRCMS_SROM_AG3,
-	BRCMS_SROM_ANTSWCTL2G,
-	BRCMS_SROM_ANTSWCTL5G,
-	BRCMS_SROM_ANTSWITCH,
-	BRCMS_SROM_BOARDFLAGS2,
-	BRCMS_SROM_BOARDFLAGS,
-	BRCMS_SROM_BOARDNUM,
-	BRCMS_SROM_BOARDREV,
-	BRCMS_SROM_BOARDTYPE,
-	BRCMS_SROM_BW40PO,
-	BRCMS_SROM_BWDUPPO,
-	BRCMS_SROM_BXA2G,
-	BRCMS_SROM_BXA5G,
-	BRCMS_SROM_CC,
-	BRCMS_SROM_CCK2GPO,
-	BRCMS_SROM_CCKBW202GPO,
-	BRCMS_SROM_CCKBW20UL2GPO,
-	BRCMS_SROM_CCODE,
-	BRCMS_SROM_CDDPO,
-	BRCMS_SROM_DEVID,
-	BRCMS_SROM_ET1MACADDR,
-	BRCMS_SROM_EXTPAGAIN2G,
-	BRCMS_SROM_EXTPAGAIN5G,
-	BRCMS_SROM_FREQOFFSET_CORR,
-	BRCMS_SROM_HW_IQCAL_EN,
-	BRCMS_SROM_IL0MACADDR,
-	BRCMS_SROM_IQCAL_SWP_DIS,
-	BRCMS_SROM_LEDBH0,
-	BRCMS_SROM_LEDBH1,
-	BRCMS_SROM_LEDBH2,
-	BRCMS_SROM_LEDBH3,
-	BRCMS_SROM_LEDDC,
-	BRCMS_SROM_LEGOFDM40DUPPO,
-	BRCMS_SROM_LEGOFDMBW202GPO,
-	BRCMS_SROM_LEGOFDMBW205GHPO,
-	BRCMS_SROM_LEGOFDMBW205GLPO,
-	BRCMS_SROM_LEGOFDMBW205GMPO,
-	BRCMS_SROM_LEGOFDMBW20UL2GPO,
-	BRCMS_SROM_LEGOFDMBW20UL5GHPO,
-	BRCMS_SROM_LEGOFDMBW20UL5GLPO,
-	BRCMS_SROM_LEGOFDMBW20UL5GMPO,
-	BRCMS_SROM_MACADDR,
-	BRCMS_SROM_MCS2GPO0,
-	BRCMS_SROM_MCS2GPO1,
-	BRCMS_SROM_MCS2GPO2,
-	BRCMS_SROM_MCS2GPO3,
-	BRCMS_SROM_MCS2GPO4,
-	BRCMS_SROM_MCS2GPO5,
-	BRCMS_SROM_MCS2GPO6,
-	BRCMS_SROM_MCS2GPO7,
-	BRCMS_SROM_MCS32PO,
-	BRCMS_SROM_MCS5GHPO0,
-	BRCMS_SROM_MCS5GHPO1,
-	BRCMS_SROM_MCS5GHPO2,
-	BRCMS_SROM_MCS5GHPO3,
-	BRCMS_SROM_MCS5GHPO4,
-	BRCMS_SROM_MCS5GHPO5,
-	BRCMS_SROM_MCS5GHPO6,
-	BRCMS_SROM_MCS5GHPO7,
-	BRCMS_SROM_MCS5GLPO0,
-	BRCMS_SROM_MCS5GLPO1,
-	BRCMS_SROM_MCS5GLPO2,
-	BRCMS_SROM_MCS5GLPO3,
-	BRCMS_SROM_MCS5GLPO4,
-	BRCMS_SROM_MCS5GLPO5,
-	BRCMS_SROM_MCS5GLPO6,
-	BRCMS_SROM_MCS5GLPO7,
-	BRCMS_SROM_MCS5GPO0,
-	BRCMS_SROM_MCS5GPO1,
-	BRCMS_SROM_MCS5GPO2,
-	BRCMS_SROM_MCS5GPO3,
-	BRCMS_SROM_MCS5GPO4,
-	BRCMS_SROM_MCS5GPO5,
-	BRCMS_SROM_MCS5GPO6,
-	BRCMS_SROM_MCS5GPO7,
-	BRCMS_SROM_MCSBW202GPO,
-	BRCMS_SROM_MCSBW205GHPO,
-	BRCMS_SROM_MCSBW205GLPO,
-	BRCMS_SROM_MCSBW205GMPO,
-	BRCMS_SROM_MCSBW20UL2GPO,
-	BRCMS_SROM_MCSBW20UL5GHPO,
-	BRCMS_SROM_MCSBW20UL5GLPO,
-	BRCMS_SROM_MCSBW20UL5GMPO,
-	BRCMS_SROM_MCSBW402GPO,
-	BRCMS_SROM_MCSBW405GHPO,
-	BRCMS_SROM_MCSBW405GLPO,
-	BRCMS_SROM_MCSBW405GMPO,
-	BRCMS_SROM_MEASPOWER,
-	BRCMS_SROM_OFDM2GPO,
-	BRCMS_SROM_OFDM5GHPO,
-	BRCMS_SROM_OFDM5GLPO,
-	BRCMS_SROM_OFDM5GPO,
-	BRCMS_SROM_OPO,
-	BRCMS_SROM_PA0B0,
-	BRCMS_SROM_PA0B1,
-	BRCMS_SROM_PA0B2,
-	BRCMS_SROM_PA0ITSSIT,
-	BRCMS_SROM_PA0MAXPWR,
-	BRCMS_SROM_PA1B0,
-	BRCMS_SROM_PA1B1,
-	BRCMS_SROM_PA1B2,
-	BRCMS_SROM_PA1HIB0,
-	BRCMS_SROM_PA1HIB1,
-	BRCMS_SROM_PA1HIB2,
-	BRCMS_SROM_PA1HIMAXPWR,
-	BRCMS_SROM_PA1ITSSIT,
-	BRCMS_SROM_PA1LOB0,
-	BRCMS_SROM_PA1LOB1,
-	BRCMS_SROM_PA1LOB2,
-	BRCMS_SROM_PA1LOMAXPWR,
-	BRCMS_SROM_PA1MAXPWR,
-	BRCMS_SROM_PDETRANGE2G,
-	BRCMS_SROM_PDETRANGE5G,
-	BRCMS_SROM_PHYCAL_TEMPDELTA,
-	BRCMS_SROM_RAWTEMPSENSE,
-	BRCMS_SROM_REGREV,
-	BRCMS_SROM_REV,
-	BRCMS_SROM_RSSISAV2G,
-	BRCMS_SROM_RSSISAV5G,
-	BRCMS_SROM_RSSISMC2G,
-	BRCMS_SROM_RSSISMC5G,
-	BRCMS_SROM_RSSISMF2G,
-	BRCMS_SROM_RSSISMF5G,
-	BRCMS_SROM_RXCHAIN,
-	BRCMS_SROM_RXPO2G,
-	BRCMS_SROM_RXPO5G,
-	BRCMS_SROM_STBCPO,
-	BRCMS_SROM_TEMPCORRX,
-	BRCMS_SROM_TEMPOFFSET,
-	BRCMS_SROM_TEMPSENSE_OPTION,
-	BRCMS_SROM_TEMPSENSE_SLOPE,
-	BRCMS_SROM_TEMPTHRESH,
-	BRCMS_SROM_TRI2G,
-	BRCMS_SROM_TRI5GH,
-	BRCMS_SROM_TRI5GL,
-	BRCMS_SROM_TRI5G,
-	BRCMS_SROM_TRISO2G,
-	BRCMS_SROM_TRISO5G,
-	BRCMS_SROM_TSSIPOS2G,
-	BRCMS_SROM_TSSIPOS5G,
-	BRCMS_SROM_TXCHAIN,
-	/*
-	 * per-path identifiers (see srom.c)
-	 */
-	BRCMS_SROM_ITT2GA0,
-	BRCMS_SROM_ITT2GA1,
-	BRCMS_SROM_ITT2GA2,
-	BRCMS_SROM_ITT2GA3,
-	BRCMS_SROM_ITT5GA0,
-	BRCMS_SROM_ITT5GA1,
-	BRCMS_SROM_ITT5GA2,
-	BRCMS_SROM_ITT5GA3,
-	BRCMS_SROM_MAXP2GA0,
-	BRCMS_SROM_MAXP2GA1,
-	BRCMS_SROM_MAXP2GA2,
-	BRCMS_SROM_MAXP2GA3,
-	BRCMS_SROM_MAXP5GA0,
-	BRCMS_SROM_MAXP5GA1,
-	BRCMS_SROM_MAXP5GA2,
-	BRCMS_SROM_MAXP5GA3,
-	BRCMS_SROM_MAXP5GHA0,
-	BRCMS_SROM_MAXP5GHA1,
-	BRCMS_SROM_MAXP5GHA2,
-	BRCMS_SROM_MAXP5GHA3,
-	BRCMS_SROM_MAXP5GLA0,
-	BRCMS_SROM_MAXP5GLA1,
-	BRCMS_SROM_MAXP5GLA2,
-	BRCMS_SROM_MAXP5GLA3,
-	BRCMS_SROM_PA2GW0A0,
-	BRCMS_SROM_PA2GW0A1,
-	BRCMS_SROM_PA2GW0A2,
-	BRCMS_SROM_PA2GW0A3,
-	BRCMS_SROM_PA2GW1A0,
-	BRCMS_SROM_PA2GW1A1,
-	BRCMS_SROM_PA2GW1A2,
-	BRCMS_SROM_PA2GW1A3,
-	BRCMS_SROM_PA2GW2A0,
-	BRCMS_SROM_PA2GW2A1,
-	BRCMS_SROM_PA2GW2A2,
-	BRCMS_SROM_PA2GW2A3,
-	BRCMS_SROM_PA5GHW0A0,
-	BRCMS_SROM_PA5GHW0A1,
-	BRCMS_SROM_PA5GHW0A2,
-	BRCMS_SROM_PA5GHW0A3,
-	BRCMS_SROM_PA5GHW1A0,
-	BRCMS_SROM_PA5GHW1A1,
-	BRCMS_SROM_PA5GHW1A2,
-	BRCMS_SROM_PA5GHW1A3,
-	BRCMS_SROM_PA5GHW2A0,
-	BRCMS_SROM_PA5GHW2A1,
-	BRCMS_SROM_PA5GHW2A2,
-	BRCMS_SROM_PA5GHW2A3,
-	BRCMS_SROM_PA5GLW0A0,
-	BRCMS_SROM_PA5GLW0A1,
-	BRCMS_SROM_PA5GLW0A2,
-	BRCMS_SROM_PA5GLW0A3,
-	BRCMS_SROM_PA5GLW1A0,
-	BRCMS_SROM_PA5GLW1A1,
-	BRCMS_SROM_PA5GLW1A2,
-	BRCMS_SROM_PA5GLW1A3,
-	BRCMS_SROM_PA5GLW2A0,
-	BRCMS_SROM_PA5GLW2A1,
-	BRCMS_SROM_PA5GLW2A2,
-	BRCMS_SROM_PA5GLW2A3,
-	BRCMS_SROM_PA5GW0A0,
-	BRCMS_SROM_PA5GW0A1,
-	BRCMS_SROM_PA5GW0A2,
-	BRCMS_SROM_PA5GW0A3,
-	BRCMS_SROM_PA5GW1A0,
-	BRCMS_SROM_PA5GW1A1,
-	BRCMS_SROM_PA5GW1A2,
-	BRCMS_SROM_PA5GW1A3,
-	BRCMS_SROM_PA5GW2A0,
-	BRCMS_SROM_PA5GW2A1,
-	BRCMS_SROM_PA5GW2A2,
-	BRCMS_SROM_PA5GW2A3,
-};
-
 #define	BRCMS_NUMRATES	16	/* max # of rates in a rateset */
 
 /* phy types */
@@ -565,8 +339,6 @@ extern void brcms_c_ampdu_flush(struct brcms_c_info *wlc,
 			    struct ieee80211_sta *sta, u16 tid);
 extern void brcms_c_ampdu_tx_operational(struct brcms_c_info *wlc, u8 tid,
 					 u8 ba_wsize, uint max_rx_ampdu_bytes);
-extern char *getvar(struct si_pub *sih, enum brcms_srom_id id);
-extern int getintvar(struct si_pub *sih, enum brcms_srom_id id);
 extern int brcms_c_module_register(struct brcms_pub *pub,
 				   const char *name, struct brcms_info *hdl,
 				   int (*down_fn)(void *handle));
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/srom.c b/drivers/net/wireless/brcm80211/brcmsmac/srom.c
deleted file mode 100644
index b96f4b9d74bd..000000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/srom.c
+++ /dev/null
@@ -1,980 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/io.h>
-#include <linux/etherdevice.h>
-#include <linux/crc8.h>
-#include <stdarg.h>
-
-#include <chipcommon.h>
-#include <brcmu_utils.h>
-#include "pub.h"
-#include "nicpci.h"
-#include "aiutils.h"
-#include "otp.h"
-#include "srom.h"
-#include "soc.h"
-
-/*
- * SROM CRC8 polynomial value:
- *
- * x^8 + x^7 +x^6 + x^4 + x^2 + 1
- */
-#define SROM_CRC8_POLY		0xAB
-
-/* Maximum srom: 6 Kilobits == 768 bytes */
-#define	SROM_MAX		768
-
-/* PCI fields */
-#define PCI_F0DEVID		48
-
-#define	SROM_WORDS		64
-
-#define	SROM_SSID		2
-
-#define	SROM_WL1LHMAXP		29
-
-#define	SROM_WL1LPAB0		30
-#define	SROM_WL1LPAB1		31
-#define	SROM_WL1LPAB2		32
-
-#define	SROM_WL1HPAB0		33
-#define	SROM_WL1HPAB1		34
-#define	SROM_WL1HPAB2		35
-
-#define	SROM_MACHI_IL0		36
-#define	SROM_MACMID_IL0		37
-#define	SROM_MACLO_IL0		38
-#define	SROM_MACHI_ET1		42
-#define	SROM_MACMID_ET1		43
-#define	SROM_MACLO_ET1		44
-
-#define	SROM_BXARSSI2G		40
-#define	SROM_BXARSSI5G		41
-
-#define	SROM_TRI52G		42
-#define	SROM_TRI5GHL		43
-
-#define	SROM_RXPO52G		45
-
-#define	SROM_AABREV		46
-/* Fields in AABREV */
-#define	SROM_BR_MASK		0x00ff
-#define	SROM_CC_MASK		0x0f00
-#define	SROM_CC_SHIFT		8
-#define	SROM_AA0_MASK		0x3000
-#define	SROM_AA0_SHIFT		12
-#define	SROM_AA1_MASK		0xc000
-#define	SROM_AA1_SHIFT		14
-
-#define	SROM_WL0PAB0		47
-#define	SROM_WL0PAB1		48
-#define	SROM_WL0PAB2		49
-
-#define	SROM_LEDBH10		50
-#define	SROM_LEDBH32		51
-
-#define	SROM_WL10MAXP		52
-
-#define	SROM_WL1PAB0		53
-#define	SROM_WL1PAB1		54
-#define	SROM_WL1PAB2		55
-
-#define	SROM_ITT		56
-
-#define	SROM_BFL		57
-#define	SROM_BFL2		28
-
-#define	SROM_AG10		58
-
-#define	SROM_CCODE		59
-
-#define	SROM_OPO		60
-
-#define	SROM_CRCREV		63
-
-#define	SROM4_WORDS		220
-
-#define SROM4_TXCHAIN_MASK	0x000f
-#define SROM4_RXCHAIN_MASK	0x00f0
-#define SROM4_SWITCH_MASK	0xff00
-
-/* Per-path fields */
-#define	MAX_PATH_SROM		4
-
-#define	SROM4_CRCREV		219
-
-/* SROM Rev 8: Make space for a 48word hardware header for PCIe rev >= 6.
- * This is acombined srom for both MIMO and SISO boards, usable in
- * the .130 4Kilobit OTP with hardware redundancy.
- */
-#define	SROM8_BREV		65
-
-#define	SROM8_BFL0		66
-#define	SROM8_BFL1		67
-#define	SROM8_BFL2		68
-#define	SROM8_BFL3		69
-
-#define	SROM8_MACHI		70
-#define	SROM8_MACMID		71
-#define	SROM8_MACLO		72
-
-#define	SROM8_CCODE		73
-#define	SROM8_REGREV		74
-
-#define	SROM8_LEDBH10		75
-#define	SROM8_LEDBH32		76
-
-#define	SROM8_LEDDC		77
-
-#define	SROM8_AA		78
-
-#define	SROM8_AG10		79
-#define	SROM8_AG32		80
-
-#define	SROM8_TXRXC		81
-
-#define	SROM8_BXARSSI2G		82
-#define	SROM8_BXARSSI5G		83
-#define	SROM8_TRI52G		84
-#define	SROM8_TRI5GHL		85
-#define	SROM8_RXPO52G		86
-
-#define SROM8_FEM2G		87
-#define SROM8_FEM5G		88
-#define SROM8_FEM_ANTSWLUT_MASK		0xf800
-#define SROM8_FEM_ANTSWLUT_SHIFT	11
-#define SROM8_FEM_TR_ISO_MASK		0x0700
-#define SROM8_FEM_TR_ISO_SHIFT		8
-#define SROM8_FEM_PDET_RANGE_MASK	0x00f8
-#define SROM8_FEM_PDET_RANGE_SHIFT	3
-#define SROM8_FEM_EXTPA_GAIN_MASK	0x0006
-#define SROM8_FEM_EXTPA_GAIN_SHIFT	1
-#define SROM8_FEM_TSSIPOS_MASK		0x0001
-#define SROM8_FEM_TSSIPOS_SHIFT		0
-
-#define SROM8_THERMAL		89
-
-/* Temp sense related entries */
-#define SROM8_MPWR_RAWTS		90
-#define SROM8_TS_SLP_OPT_CORRX	91
-/* FOC: freiquency offset correction, HWIQ: H/W IOCAL enable,
- * IQSWP: IQ CAL swap disable */
-#define SROM8_FOC_HWIQ_IQSWP	92
-
-/* Temperature delta for PHY calibration */
-#define SROM8_PHYCAL_TEMPDELTA	93
-
-/* Per-path offsets & fields */
-#define	SROM8_PATH0		96
-#define	SROM8_PATH1		112
-#define	SROM8_PATH2		128
-#define	SROM8_PATH3		144
-
-#define	SROM8_2G_ITT_MAXP	0
-#define	SROM8_2G_PA		1
-#define	SROM8_5G_ITT_MAXP	4
-#define	SROM8_5GLH_MAXP		5
-#define	SROM8_5G_PA		6
-#define	SROM8_5GL_PA		9
-#define	SROM8_5GH_PA		12
-
-/* All the miriad power offsets */
-#define	SROM8_2G_CCKPO		160
-
-#define	SROM8_2G_OFDMPO		161
-#define	SROM8_5G_OFDMPO		163
-#define	SROM8_5GL_OFDMPO	165
-#define	SROM8_5GH_OFDMPO	167
-
-#define	SROM8_2G_MCSPO		169
-#define	SROM8_5G_MCSPO		177
-#define	SROM8_5GL_MCSPO		185
-#define	SROM8_5GH_MCSPO		193
-
-#define	SROM8_CDDPO		201
-#define	SROM8_STBCPO		202
-#define	SROM8_BW40PO		203
-#define	SROM8_BWDUPPO		204
-
-/* SISO PA parameters are in the path0 spaces */
-#define	SROM8_SISO		96
-
-/* Legacy names for SISO PA paramters */
-#define	SROM8_W0_ITTMAXP	(SROM8_SISO + SROM8_2G_ITT_MAXP)
-#define	SROM8_W0_PAB0		(SROM8_SISO + SROM8_2G_PA)
-#define	SROM8_W0_PAB1		(SROM8_SISO + SROM8_2G_PA + 1)
-#define	SROM8_W0_PAB2		(SROM8_SISO + SROM8_2G_PA + 2)
-#define	SROM8_W1_ITTMAXP	(SROM8_SISO + SROM8_5G_ITT_MAXP)
-#define	SROM8_W1_MAXP_LCHC	(SROM8_SISO + SROM8_5GLH_MAXP)
-#define	SROM8_W1_PAB0		(SROM8_SISO + SROM8_5G_PA)
-#define	SROM8_W1_PAB1		(SROM8_SISO + SROM8_5G_PA + 1)
-#define	SROM8_W1_PAB2		(SROM8_SISO + SROM8_5G_PA + 2)
-#define	SROM8_W1_PAB0_LC	(SROM8_SISO + SROM8_5GL_PA)
-#define	SROM8_W1_PAB1_LC	(SROM8_SISO + SROM8_5GL_PA + 1)
-#define	SROM8_W1_PAB2_LC	(SROM8_SISO + SROM8_5GL_PA + 2)
-#define	SROM8_W1_PAB0_HC	(SROM8_SISO + SROM8_5GH_PA)
-#define	SROM8_W1_PAB1_HC	(SROM8_SISO + SROM8_5GH_PA + 1)
-#define	SROM8_W1_PAB2_HC	(SROM8_SISO + SROM8_5GH_PA + 2)
-
-/* SROM REV 9 */
-#define SROM9_2GPO_CCKBW20	160
-#define SROM9_2GPO_CCKBW20UL	161
-#define SROM9_2GPO_LOFDMBW20	162
-#define SROM9_2GPO_LOFDMBW20UL	164
-
-#define SROM9_5GLPO_LOFDMBW20	166
-#define SROM9_5GLPO_LOFDMBW20UL	168
-#define SROM9_5GMPO_LOFDMBW20	170
-#define SROM9_5GMPO_LOFDMBW20UL	172
-#define SROM9_5GHPO_LOFDMBW20	174
-#define SROM9_5GHPO_LOFDMBW20UL	176
-
-#define SROM9_2GPO_MCSBW20	178
-#define SROM9_2GPO_MCSBW20UL	180
-#define SROM9_2GPO_MCSBW40	182
-
-#define SROM9_5GLPO_MCSBW20	184
-#define SROM9_5GLPO_MCSBW20UL	186
-#define SROM9_5GLPO_MCSBW40	188
-#define SROM9_5GMPO_MCSBW20	190
-#define SROM9_5GMPO_MCSBW20UL	192
-#define SROM9_5GMPO_MCSBW40	194
-#define SROM9_5GHPO_MCSBW20	196
-#define SROM9_5GHPO_MCSBW20UL	198
-#define SROM9_5GHPO_MCSBW40	200
-
-#define SROM9_PO_MCS32		202
-#define SROM9_PO_LOFDM40DUP	203
-
-/* SROM flags (see sromvar_t) */
-
-/* value continues as described by the next entry */
-#define SRFL_MORE	1
-#define	SRFL_NOFFS	2	/* value bits can't be all one's */
-#define	SRFL_PRHEX	4	/* value is in hexdecimal format */
-#define	SRFL_PRSIGN	8	/* value is in signed decimal format */
-#define	SRFL_CCODE	0x10	/* value is in country code format */
-#define	SRFL_ETHADDR	0x20	/* value is an Ethernet address */
-#define SRFL_LEDDC	0x40	/* value is an LED duty cycle */
-/* do not generate a nvram param, entry is for mfgc */
-#define SRFL_NOVAR	0x80
-
-/* Max. nvram variable table size */
-#define	MAXSZ_NVRAM_VARS	4096
-
-/*
- * indicates type of value.
- */
-enum brcms_srom_var_type {
-	BRCMS_SROM_STRING,
-	BRCMS_SROM_SNUMBER,
-	BRCMS_SROM_UNUMBER
-};
-
-/*
- * storage type for srom variable.
- *
- * var_list: for linked list operations.
- * varid: identifier of the variable.
- * var_type: type of variable.
- * buf: variable value when var_type == BRCMS_SROM_STRING.
- * uval: unsigned variable value when var_type == BRCMS_SROM_UNUMBER.
- * sval: signed variable value when var_type == BRCMS_SROM_SNUMBER.
- */
-struct brcms_srom_list_head {
-	struct list_head var_list;
-	enum brcms_srom_id varid;
-	enum brcms_srom_var_type var_type;
-	union {
-		char buf[0];
-		u32 uval;
-		s32 sval;
-	};
-};
-
-struct brcms_sromvar {
-	enum brcms_srom_id varid;
-	u32 revmask;
-	u32 flags;
-	u16 off;
-	u16 mask;
-};
-
-struct brcms_varbuf {
-	char *base;		/* pointer to buffer base */
-	char *buf;		/* pointer to current position */
-	unsigned int size;	/* current (residual) size in bytes */
-};
-
-/*
- * Assumptions:
- * - Ethernet address spans across 3 consecutive words
- *
- * Table rules:
- * - Add multiple entries next to each other if a value spans across multiple
- *   words (even multiple fields in the same word) with each entry except the
- *   last having it's SRFL_MORE bit set.
- * - Ethernet address entry does not follow above rule and must not have
- *   SRFL_MORE bit set. Its SRFL_ETHADDR bit implies it takes multiple words.
- * - The last entry's name field must be NULL to indicate the end of the table.
- *   Other entries must have non-NULL name.
- */
-static const struct brcms_sromvar pci_sromvars[] = {
-	{BRCMS_SROM_DEVID, 0xffffff00, SRFL_PRHEX | SRFL_NOVAR, PCI_F0DEVID,
-	 0xffff},
-	{BRCMS_SROM_BOARDREV, 0xffffff00, SRFL_PRHEX, SROM8_BREV, 0xffff},
-	{BRCMS_SROM_BOARDFLAGS, 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL0,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_BFL1, 0xffff},
-	{BRCMS_SROM_BOARDFLAGS2, 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL2,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_BFL3, 0xffff},
-	{BRCMS_SROM_BOARDTYPE, 0xfffffffc, SRFL_PRHEX, SROM_SSID, 0xffff},
-	{BRCMS_SROM_BOARDNUM, 0xffffff00, 0, SROM8_MACLO, 0xffff},
-	{BRCMS_SROM_REGREV, 0xffffff00, 0, SROM8_REGREV, 0x00ff},
-	{BRCMS_SROM_LEDBH0, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0x00ff},
-	{BRCMS_SROM_LEDBH1, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0xff00},
-	{BRCMS_SROM_LEDBH2, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0x00ff},
-	{BRCMS_SROM_LEDBH3, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0xff00},
-	{BRCMS_SROM_PA0B0, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB0, 0xffff},
-	{BRCMS_SROM_PA0B1, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB1, 0xffff},
-	{BRCMS_SROM_PA0B2, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB2, 0xffff},
-	{BRCMS_SROM_PA0ITSSIT, 0xffffff00, 0, SROM8_W0_ITTMAXP, 0xff00},
-	{BRCMS_SROM_PA0MAXPWR, 0xffffff00, 0, SROM8_W0_ITTMAXP, 0x00ff},
-	{BRCMS_SROM_OPO, 0xffffff00, 0, SROM8_2G_OFDMPO, 0x00ff},
-	{BRCMS_SROM_AA2G, 0xffffff00, 0, SROM8_AA, 0x00ff},
-	{BRCMS_SROM_AA5G, 0xffffff00, 0, SROM8_AA, 0xff00},
-	{BRCMS_SROM_AG0, 0xffffff00, 0, SROM8_AG10, 0x00ff},
-	{BRCMS_SROM_AG1, 0xffffff00, 0, SROM8_AG10, 0xff00},
-	{BRCMS_SROM_AG2, 0xffffff00, 0, SROM8_AG32, 0x00ff},
-	{BRCMS_SROM_AG3, 0xffffff00, 0, SROM8_AG32, 0xff00},
-	{BRCMS_SROM_PA1B0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0, 0xffff},
-	{BRCMS_SROM_PA1B1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1, 0xffff},
-	{BRCMS_SROM_PA1B2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2, 0xffff},
-	{BRCMS_SROM_PA1LOB0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_LC, 0xffff},
-	{BRCMS_SROM_PA1LOB1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_LC, 0xffff},
-	{BRCMS_SROM_PA1LOB2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_LC, 0xffff},
-	{BRCMS_SROM_PA1HIB0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_HC, 0xffff},
-	{BRCMS_SROM_PA1HIB1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_HC, 0xffff},
-	{BRCMS_SROM_PA1HIB2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_HC, 0xffff},
-	{BRCMS_SROM_PA1ITSSIT, 0xffffff00, 0, SROM8_W1_ITTMAXP, 0xff00},
-	{BRCMS_SROM_PA1MAXPWR, 0xffffff00, 0, SROM8_W1_ITTMAXP, 0x00ff},
-	{BRCMS_SROM_PA1LOMAXPWR, 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0xff00},
-	{BRCMS_SROM_PA1HIMAXPWR, 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0x00ff},
-	{BRCMS_SROM_BXA2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x1800},
-	{BRCMS_SROM_RSSISAV2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x0700},
-	{BRCMS_SROM_RSSISMC2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x00f0},
-	{BRCMS_SROM_RSSISMF2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x000f},
-	{BRCMS_SROM_BXA5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x1800},
-	{BRCMS_SROM_RSSISAV5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x0700},
-	{BRCMS_SROM_RSSISMC5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x00f0},
-	{BRCMS_SROM_RSSISMF5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x000f},
-	{BRCMS_SROM_TRI2G, 0xffffff00, 0, SROM8_TRI52G, 0x00ff},
-	{BRCMS_SROM_TRI5G, 0xffffff00, 0, SROM8_TRI52G, 0xff00},
-	{BRCMS_SROM_TRI5GL, 0xffffff00, 0, SROM8_TRI5GHL, 0x00ff},
-	{BRCMS_SROM_TRI5GH, 0xffffff00, 0, SROM8_TRI5GHL, 0xff00},
-	{BRCMS_SROM_RXPO2G, 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0x00ff},
-	{BRCMS_SROM_RXPO5G, 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0xff00},
-	{BRCMS_SROM_TXCHAIN, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
-	 SROM4_TXCHAIN_MASK},
-	{BRCMS_SROM_RXCHAIN, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
-	 SROM4_RXCHAIN_MASK},
-	{BRCMS_SROM_ANTSWITCH, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
-	 SROM4_SWITCH_MASK},
-	{BRCMS_SROM_TSSIPOS2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_TSSIPOS_MASK},
-	{BRCMS_SROM_EXTPAGAIN2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_EXTPA_GAIN_MASK},
-	{BRCMS_SROM_PDETRANGE2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_PDET_RANGE_MASK},
-	{BRCMS_SROM_TRISO2G, 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_TR_ISO_MASK},
-	{BRCMS_SROM_ANTSWCTL2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_ANTSWLUT_MASK},
-	{BRCMS_SROM_TSSIPOS5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_TSSIPOS_MASK},
-	{BRCMS_SROM_EXTPAGAIN5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_EXTPA_GAIN_MASK},
-	{BRCMS_SROM_PDETRANGE5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_PDET_RANGE_MASK},
-	{BRCMS_SROM_TRISO5G, 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_TR_ISO_MASK},
-	{BRCMS_SROM_ANTSWCTL5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_ANTSWLUT_MASK},
-	{BRCMS_SROM_TEMPTHRESH, 0xffffff00, 0, SROM8_THERMAL, 0xff00},
-	{BRCMS_SROM_TEMPOFFSET, 0xffffff00, 0, SROM8_THERMAL, 0x00ff},
-
-	{BRCMS_SROM_CCODE, 0xffffff00, SRFL_CCODE, SROM8_CCODE, 0xffff},
-	{BRCMS_SROM_MACADDR, 0xffffff00, SRFL_ETHADDR, SROM8_MACHI, 0xffff},
-	{BRCMS_SROM_LEDDC, 0xffffff00, SRFL_NOFFS | SRFL_LEDDC, SROM8_LEDDC,
-	 0xffff},
-	{BRCMS_SROM_RAWTEMPSENSE, 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS,
-	 0x01ff},
-	{BRCMS_SROM_MEASPOWER, 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS,
-	 0xfe00},
-	{BRCMS_SROM_TEMPSENSE_SLOPE, 0xffffff00, SRFL_PRHEX,
-	 SROM8_TS_SLP_OPT_CORRX, 0x00ff},
-	{BRCMS_SROM_TEMPCORRX, 0xffffff00, SRFL_PRHEX, SROM8_TS_SLP_OPT_CORRX,
-	 0xfc00},
-	{BRCMS_SROM_TEMPSENSE_OPTION, 0xffffff00, SRFL_PRHEX,
-	 SROM8_TS_SLP_OPT_CORRX, 0x0300},
-	{BRCMS_SROM_FREQOFFSET_CORR, 0xffffff00, SRFL_PRHEX,
-	 SROM8_FOC_HWIQ_IQSWP, 0x000f},
-	{BRCMS_SROM_IQCAL_SWP_DIS, 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP,
-	 0x0010},
-	{BRCMS_SROM_HW_IQCAL_EN, 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP,
-	 0x0020},
-	{BRCMS_SROM_PHYCAL_TEMPDELTA, 0xffffff00, 0, SROM8_PHYCAL_TEMPDELTA,
-	 0x00ff},
-
-	{BRCMS_SROM_CCK2GPO, 0x00000100, 0, SROM8_2G_CCKPO, 0xffff},
-	{BRCMS_SROM_OFDM2GPO, 0x00000100, SRFL_MORE, SROM8_2G_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_2G_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_OFDM5GPO, 0x00000100, SRFL_MORE, SROM8_5G_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_5G_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_OFDM5GLPO, 0x00000100, SRFL_MORE, SROM8_5GL_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_5GL_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_OFDM5GHPO, 0x00000100, SRFL_MORE, SROM8_5GH_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_5GH_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_MCS2GPO0, 0x00000100, 0, SROM8_2G_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS2GPO1, 0x00000100, 0, SROM8_2G_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS2GPO2, 0x00000100, 0, SROM8_2G_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS2GPO3, 0x00000100, 0, SROM8_2G_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS2GPO4, 0x00000100, 0, SROM8_2G_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS2GPO5, 0x00000100, 0, SROM8_2G_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS2GPO6, 0x00000100, 0, SROM8_2G_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS2GPO7, 0x00000100, 0, SROM8_2G_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_MCS5GPO0, 0x00000100, 0, SROM8_5G_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS5GPO1, 0x00000100, 0, SROM8_5G_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS5GPO2, 0x00000100, 0, SROM8_5G_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS5GPO3, 0x00000100, 0, SROM8_5G_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS5GPO4, 0x00000100, 0, SROM8_5G_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS5GPO5, 0x00000100, 0, SROM8_5G_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS5GPO6, 0x00000100, 0, SROM8_5G_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS5GPO7, 0x00000100, 0, SROM8_5G_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_MCS5GLPO0, 0x00000100, 0, SROM8_5GL_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS5GLPO1, 0x00000100, 0, SROM8_5GL_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS5GLPO2, 0x00000100, 0, SROM8_5GL_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS5GLPO3, 0x00000100, 0, SROM8_5GL_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS5GLPO4, 0x00000100, 0, SROM8_5GL_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS5GLPO5, 0x00000100, 0, SROM8_5GL_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS5GLPO6, 0x00000100, 0, SROM8_5GL_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS5GLPO7, 0x00000100, 0, SROM8_5GL_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_MCS5GHPO0, 0x00000100, 0, SROM8_5GH_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS5GHPO1, 0x00000100, 0, SROM8_5GH_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS5GHPO2, 0x00000100, 0, SROM8_5GH_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS5GHPO3, 0x00000100, 0, SROM8_5GH_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS5GHPO4, 0x00000100, 0, SROM8_5GH_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS5GHPO5, 0x00000100, 0, SROM8_5GH_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS5GHPO6, 0x00000100, 0, SROM8_5GH_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS5GHPO7, 0x00000100, 0, SROM8_5GH_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_CDDPO, 0x00000100, 0, SROM8_CDDPO, 0xffff},
-	{BRCMS_SROM_STBCPO, 0x00000100, 0, SROM8_STBCPO, 0xffff},
-	{BRCMS_SROM_BW40PO, 0x00000100, 0, SROM8_BW40PO, 0xffff},
-	{BRCMS_SROM_BWDUPPO, 0x00000100, 0, SROM8_BWDUPPO, 0xffff},
-
-	/* power per rate from sromrev 9 */
-	{BRCMS_SROM_CCKBW202GPO, 0xfffffe00, 0, SROM9_2GPO_CCKBW20, 0xffff},
-	{BRCMS_SROM_CCKBW20UL2GPO, 0xfffffe00, 0, SROM9_2GPO_CCKBW20UL, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW202GPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_2GPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL2GPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_2GPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW205GLPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GLPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL5GLPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GLPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW205GMPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GMPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL5GMPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GMPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW205GHPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GHPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL5GHPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GHPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW202GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL2GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20UL,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW402GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW205GLPO, 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL5GLPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GLPO_MCSBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW405GLPO, 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW205GMPO, 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL5GMPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GMPO_MCSBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW405GMPO, 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW205GHPO, 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL5GHPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GHPO_MCSBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW405GHPO, 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCS32PO, 0xfffffe00, 0, SROM9_PO_MCS32, 0xffff},
-	{BRCMS_SROM_LEGOFDM40DUPPO, 0xfffffe00, 0, SROM9_PO_LOFDM40DUP, 0xffff},
-
-	{BRCMS_SROM_NULL, 0, 0, 0, 0}
-};
-
-static const struct brcms_sromvar perpath_pci_sromvars[] = {
-	{BRCMS_SROM_MAXP2GA0, 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0x00ff},
-	{BRCMS_SROM_ITT2GA0, 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0xff00},
-	{BRCMS_SROM_ITT5GA0, 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0xff00},
-	{BRCMS_SROM_PA2GW0A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA, 0xffff},
-	{BRCMS_SROM_PA2GW1A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 1, 0xffff},
-	{BRCMS_SROM_PA2GW2A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 2, 0xffff},
-	{BRCMS_SROM_MAXP5GA0, 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0x00ff},
-	{BRCMS_SROM_MAXP5GHA0, 0xffffff00, 0, SROM8_5GLH_MAXP, 0x00ff},
-	{BRCMS_SROM_MAXP5GLA0, 0xffffff00, 0, SROM8_5GLH_MAXP, 0xff00},
-	{BRCMS_SROM_PA5GW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA, 0xffff},
-	{BRCMS_SROM_PA5GW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 1, 0xffff},
-	{BRCMS_SROM_PA5GW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 2, 0xffff},
-	{BRCMS_SROM_PA5GLW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA, 0xffff},
-	{BRCMS_SROM_PA5GLW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 1,
-	 0xffff},
-	{BRCMS_SROM_PA5GLW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 2,
-	 0xffff},
-	{BRCMS_SROM_PA5GHW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA, 0xffff},
-	{BRCMS_SROM_PA5GHW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 1,
-	 0xffff},
-	{BRCMS_SROM_PA5GHW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 2,
-	 0xffff},
-	{BRCMS_SROM_NULL, 0, 0, 0, 0}
-};
-
-/* crc table has the same contents for every device instance, so it can be
- * shared between devices. */
-static u8 brcms_srom_crc8_table[CRC8_TABLE_SIZE];
-
-static uint mask_shift(u16 mask)
-{
-	uint i;
-	for (i = 0; i < (sizeof(mask) << 3); i++) {
-		if (mask & (1 << i))
-			return i;
-	}
-	return 0;
-}
-
-static uint mask_width(u16 mask)
-{
-	int i;
-	for (i = (sizeof(mask) << 3) - 1; i >= 0; i--) {
-		if (mask & (1 << i))
-			return (uint) (i - mask_shift(mask) + 1);
-	}
-	return 0;
-}
-
-static inline void le16_to_cpu_buf(u16 *buf, uint nwords)
-{
-	while (nwords--)
-		*(buf + nwords) = le16_to_cpu(*(__le16 *)(buf + nwords));
-}
-
-static inline void cpu_to_le16_buf(u16 *buf, uint nwords)
-{
-	while (nwords--)
-		*(__le16 *)(buf + nwords) = cpu_to_le16(*(buf + nwords));
-}
-
-/*
- * convert binary srom data into linked list of srom variable items.
- */
-static int
-_initvars_srom_pci(u8 sromrev, u16 *srom, struct list_head *var_list)
-{
-	struct brcms_srom_list_head *entry;
-	enum brcms_srom_id id;
-	u16 w;
-	u32 val = 0;
-	const struct brcms_sromvar *srv;
-	uint width;
-	uint flags;
-	u32 sr = (1 << sromrev);
-	uint p;
-	uint pb =  SROM8_PATH0;
-	const uint psz = SROM8_PATH1 - SROM8_PATH0;
-
-	/* first store the srom revision */
-	entry = kzalloc(sizeof(struct brcms_srom_list_head), GFP_KERNEL);
-	if (!entry)
-		return -ENOMEM;
-
-	entry->varid = BRCMS_SROM_REV;
-	entry->var_type = BRCMS_SROM_UNUMBER;
-	entry->uval = sromrev;
-	list_add(&entry->var_list, var_list);
-
-	for (srv = pci_sromvars; srv->varid != BRCMS_SROM_NULL; srv++) {
-		enum brcms_srom_var_type type;
-		u8 ea[ETH_ALEN];
-		u8 extra_space = 0;
-
-		if ((srv->revmask & sr) == 0)
-			continue;
-
-		flags = srv->flags;
-		id = srv->varid;
-
-		/* This entry is for mfgc only. Don't generate param for it, */
-		if (flags & SRFL_NOVAR)
-			continue;
-
-		if (flags & SRFL_ETHADDR) {
-			/*
-			 * stored in string format XX:XX:XX:XX:XX:XX (17 chars)
-			 */
-			ea[0] = (srom[srv->off] >> 8) & 0xff;
-			ea[1] = srom[srv->off] & 0xff;
-			ea[2] = (srom[srv->off + 1] >> 8) & 0xff;
-			ea[3] = srom[srv->off + 1] & 0xff;
-			ea[4] = (srom[srv->off + 2] >> 8) & 0xff;
-			ea[5] = srom[srv->off + 2] & 0xff;
-			/* 17 characters + string terminator - union size */
-			extra_space = 18 - sizeof(s32);
-			type = BRCMS_SROM_STRING;
-		} else {
-			w = srom[srv->off];
-			val = (w & srv->mask) >> mask_shift(srv->mask);
-			width = mask_width(srv->mask);
-
-			while (srv->flags & SRFL_MORE) {
-				srv++;
-				if (srv->off == 0)
-					continue;
-
-				w = srom[srv->off];
-				val +=
-				    ((w & srv->mask) >> mask_shift(srv->
-								   mask)) <<
-				    width;
-				width += mask_width(srv->mask);
-			}
-
-			if ((flags & SRFL_NOFFS)
-			    && ((int)val == (1 << width) - 1))
-				continue;
-
-			if (flags & SRFL_CCODE) {
-				type = BRCMS_SROM_STRING;
-			} else if (flags & SRFL_LEDDC) {
-				/* LED Powersave duty cycle has to be scaled:
-				 *(oncount >> 24) (offcount >> 8)
-				 */
-				u32 w32 = /* oncount */
-					  (((val >> 8) & 0xff) << 24) |
-					  /* offcount */
-					  (((val & 0xff)) << 8);
-				type = BRCMS_SROM_UNUMBER;
-				val = w32;
-			} else if ((flags & SRFL_PRSIGN)
-				 && (val & (1 << (width - 1)))) {
-				type = BRCMS_SROM_SNUMBER;
-				val |= ~0 << width;
-			} else
-				type = BRCMS_SROM_UNUMBER;
-		}
-
-		entry = kzalloc(sizeof(struct brcms_srom_list_head) +
-				extra_space, GFP_KERNEL);
-		if (!entry)
-			return -ENOMEM;
-		entry->varid = id;
-		entry->var_type = type;
-		if (flags & SRFL_ETHADDR) {
-			snprintf(entry->buf, 18, "%pM", ea);
-		} else if (flags & SRFL_CCODE) {
-			if (val == 0)
-				entry->buf[0] = '\0';
-			else
-				snprintf(entry->buf, 3, "%c%c",
-					 (val >> 8), (val & 0xff));
-		} else {
-			entry->uval = val;
-		}
-
-		list_add(&entry->var_list, var_list);
-	}
-
-	for (p = 0; p < MAX_PATH_SROM; p++) {
-		for (srv = perpath_pci_sromvars;
-		     srv->varid != BRCMS_SROM_NULL; srv++) {
-			if ((srv->revmask & sr) == 0)
-				continue;
-
-			if (srv->flags & SRFL_NOVAR)
-				continue;
-
-			w = srom[pb + srv->off];
-			val = (w & srv->mask) >> mask_shift(srv->mask);
-			width = mask_width(srv->mask);
-
-			/* Cheating: no per-path var is more than
-			 * 1 word */
-			if ((srv->flags & SRFL_NOFFS)
-			    && ((int)val == (1 << width) - 1))
-				continue;
-
-			entry =
-			    kzalloc(sizeof(struct brcms_srom_list_head),
-				    GFP_KERNEL);
-			if (!entry)
-				return -ENOMEM;
-			entry->varid = srv->varid+p;
-			entry->var_type = BRCMS_SROM_UNUMBER;
-			entry->uval = val;
-			list_add(&entry->var_list, var_list);
-		}
-		pb += psz;
-	}
-	return 0;
-}
-
-/*
- * The crc check is done on a little-endian array, we need
- * to switch the bytes around before checking crc (and
- * then switch it back).
- */
-static int do_crc_check(u16 *buf, unsigned nwords)
-{
-	u8 crc;
-
-	cpu_to_le16_buf(buf, nwords);
-	crc = crc8(brcms_srom_crc8_table, (void *)buf, nwords << 1, CRC8_INIT_VALUE);
-	le16_to_cpu_buf(buf, nwords);
-
-	return crc == CRC8_GOOD_VALUE(brcms_srom_crc8_table);
-}
-
-/*
- * Read in and validate sprom.
- * Return 0 on success, nonzero on error.
- */
-static int
-sprom_read_pci(struct si_pub *sih, u16 *buf, uint nwords, bool check_crc)
-{
-	int err = 0;
-	uint i;
-	struct bcma_device *core;
-	uint sprom_offset;
-
-	/* determine core to read */
-	if (ai_get_ccrev(sih) < 32) {
-		core = ai_findcore(sih, BCMA_CORE_80211, 0);
-		sprom_offset = PCI_BAR0_SPROM_OFFSET;
-	} else {
-		core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-		sprom_offset = CHIPCREGOFFS(sromotp);
-	}
-
-	/* read the sprom */
-	for (i = 0; i < nwords; i++)
-		buf[i] = bcma_read16(core, sprom_offset+i*2);
-
-	if (buf[0] == 0xffff)
-		/*
-		 * The hardware thinks that an srom that starts with
-		 * 0xffff is blank, regardless of the rest of the
-		 * content, so declare it bad.
-		 */
-		return -ENODATA;
-
-	if (check_crc && !do_crc_check(buf, nwords))
-		err = -EIO;
-
-	return err;
-}
-
-static int otp_read_pci(struct si_pub *sih, u16 *buf, uint nwords)
-{
-	u8 *otp;
-	uint sz = OTP_SZ_MAX / 2;	/* size in words */
-	int err = 0;
-
-	otp = kzalloc(OTP_SZ_MAX, GFP_ATOMIC);
-	if (otp == NULL)
-		return -ENOMEM;
-
-	err = otp_read_region(sih, OTP_HW_RGN, (u16 *) otp, &sz);
-
-	sz = min_t(uint, sz, nwords);
-	memcpy(buf, otp, sz * 2);
-
-	kfree(otp);
-
-	/* Check CRC */
-	if (buf[0] == 0xffff)
-		/* The hardware thinks that an srom that starts with 0xffff
-		 * is blank, regardless of the rest of the content, so declare
-		 * it bad.
-		 */
-		return -ENODATA;
-
-	/* fixup the endianness so crc8 will pass */
-	cpu_to_le16_buf(buf, sz);
-	if (crc8(brcms_srom_crc8_table, (u8 *) buf, sz * 2,
-		 CRC8_INIT_VALUE) != CRC8_GOOD_VALUE(brcms_srom_crc8_table))
-		err = -EIO;
-	else
-		/* now correct the endianness of the byte array */
-		le16_to_cpu_buf(buf, sz);
-
-	return err;
-}
-
-/*
- * Initialize nonvolatile variable table from sprom.
- * Return 0 on success, nonzero on error.
- */
-int srom_var_init(struct si_pub *sih)
-{
-	u16 *srom;
-	u8 sromrev = 0;
-	u32 sr;
-	int err = 0;
-
-	/*
-	 * Apply CRC over SROM content regardless SROM is present or not.
-	 */
-	srom = kmalloc(SROM_MAX, GFP_ATOMIC);
-	if (!srom)
-		return -ENOMEM;
-
-	crc8_populate_lsb(brcms_srom_crc8_table, SROM_CRC8_POLY);
-	if (ai_is_sprom_available(sih)) {
-		err = sprom_read_pci(sih, srom, SROM4_WORDS, true);
-
-		if (err == 0)
-			/* srom read and passed crc */
-			/* top word of sprom contains version and crc8 */
-			sromrev = srom[SROM4_CRCREV] & 0xff;
-	} else {
-		/* Use OTP if SPROM not available */
-		err = otp_read_pci(sih, srom, SROM4_WORDS);
-		if (err == 0)
-			/* OTP only contain SROM rev8/rev9 for now */
-			sromrev = srom[SROM4_CRCREV] & 0xff;
-	}
-
-	if (!err) {
-		struct si_info *sii = (struct si_info *)sih;
-
-		/* Bitmask for the sromrev */
-		sr = 1 << sromrev;
-
-		/*
-		 * srom version check: Current valid versions: 8, 9
-		 */
-		if ((sr & 0x300) == 0) {
-			err = -EINVAL;
-			goto errout;
-		}
-
-		INIT_LIST_HEAD(&sii->var_list);
-
-		/* parse SROM into name=value pairs. */
-		err = _initvars_srom_pci(sromrev, srom, &sii->var_list);
-		if (err)
-			srom_free_vars(sih);
-	}
-
-errout:
-	kfree(srom);
-	return err;
-}
-
-void srom_free_vars(struct si_pub *sih)
-{
-	struct si_info *sii;
-	struct brcms_srom_list_head *entry, *next;
-
-	sii = (struct si_info *)sih;
-	list_for_each_entry_safe(entry, next, &sii->var_list, var_list) {
-		list_del(&entry->var_list);
-		kfree(entry);
-	}
-}
-
-/*
- * Search the name=value vars for a specific one and return its value.
- * Returns NULL if not found.
- */
-char *getvar(struct si_pub *sih, enum brcms_srom_id id)
-{
-	struct si_info *sii;
-	struct brcms_srom_list_head *entry;
-
-	sii = (struct si_info *)sih;
-
-	list_for_each_entry(entry, &sii->var_list, var_list)
-		if (entry->varid == id)
-			return &entry->buf[0];
-
-	/* nothing found */
-	return NULL;
-}
-
-/*
- * Search the vars for a specific one and return its value as
- * an integer. Returns 0 if not found.-
- */
-int getintvar(struct si_pub *sih, enum brcms_srom_id id)
-{
-	struct si_info *sii;
-	struct brcms_srom_list_head *entry;
-	unsigned long res;
-
-	sii = (struct si_info *)sih;
-
-	list_for_each_entry(entry, &sii->var_list, var_list)
-		if (entry->varid == id) {
-			if (entry->var_type == BRCMS_SROM_SNUMBER ||
-			    entry->var_type == BRCMS_SROM_UNUMBER)
-				return (int)entry->sval;
-			else if (!kstrtoul(&entry->buf[0], 0, &res))
-				return (int)res;
-		}
-
-	return 0;
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/srom.h b/drivers/net/wireless/brcm80211/brcmsmac/srom.h
deleted file mode 100644
index f2a58f262c99..000000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/srom.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#ifndef	_BRCM_SROM_H_
-#define	_BRCM_SROM_H_
-
-#include "types.h"
-
-/* Prototypes */
-extern int srom_var_init(struct si_pub *sih);
-extern void srom_free_vars(struct si_pub *sih);
-
-extern int srom_read(struct si_pub *sih, uint bus, void *curmap,
-		     uint byteoff, uint nbytes, u16 *buf, bool check_crc);
-
-#endif				/* _BRCM_SROM_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/stf.c b/drivers/net/wireless/brcm80211/brcmsmac/stf.c
index d8f528eb180c..ed1d1aa71d2d 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/stf.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/stf.c
@@ -370,9 +370,11 @@ void brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc)
 
 void brcms_c_stf_phy_chain_calc(struct brcms_c_info *wlc)
 {
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
+
 	/* get available rx/tx chains */
-	wlc->stf->hw_txchain = (u8) getintvar(wlc->hw->sih, BRCMS_SROM_TXCHAIN);
-	wlc->stf->hw_rxchain = (u8) getintvar(wlc->hw->sih, BRCMS_SROM_RXCHAIN);
+	wlc->stf->hw_txchain = sprom->txchain;
+	wlc->stf->hw_rxchain = sprom->rxchain;
 
 	/* these parameter are intended to be used for all PHY types */
 	if (wlc->stf->hw_txchain == 0 || wlc->stf->hw_txchain == 0xf) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
index 01dc44267317..e55ec6c8a920 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
@@ -31,6 +31,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/sched.h>
+#include <net/mac80211.h>
 
 #include "iwl-dev.h"
 #include "iwl-io.h"
@@ -273,9 +274,20 @@ void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
 		return;
 	}
 
+	/*
+	 * Possible situations when BT needs to take over for receive,
+	 * at the same time where STA needs to response to AP's frame(s),
+	 * reduce the tx power of the required response frames, by that,
+	 * allow the concurrent BT receive & WiFi transmit
+	 * (BT - ANT A, WiFi -ANT B), without interference to one another
+	 *
+	 * Reduced tx power apply to control frames only (ACK/Back/CTS)
+	 * when indicated by the BT config command
+	 */
 	basic.kill_ack_mask = priv->kill_ack_mask;
 	basic.kill_cts_mask = priv->kill_cts_mask;
-	basic.reduce_txpower = priv->reduced_txpower;
+	if (priv->reduced_txpower)
+		basic.reduce_txpower = IWLAGN_BT_REDUCED_TX_PWR;
 	basic.valid = priv->bt_valid;
 
 	/*
@@ -589,13 +601,31 @@ static bool iwlagn_set_kill_msk(struct iwl_priv *priv,
 	return need_update;
 }
 
+/*
+ * Upon RSSI changes, sends a bt config command with following changes
+ *  1. enable/disable "reduced control frames tx power
+ *  2. update the "kill)ack_mask" and "kill_cts_mask"
+ *
+ * If "reduced tx power" is enabled, uCode shall
+ *  1. ACK/Back/CTS rate shall reduced to 6Mbps
+ *  2. not use duplciate 20/40MHz mode
+ */
 static bool iwlagn_fill_txpower_mode(struct iwl_priv *priv,
 				struct iwl_bt_uart_msg *uart_msg)
 {
 	bool need_update = false;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	int ave_rssi;
 
+	ave_rssi = ieee80211_ave_rssi(ctx->vif);
+	if (!ave_rssi) {
+		/* no rssi data, no changes to reduce tx power */
+		IWL_DEBUG_COEX(priv, "no rssi data available\n");
+		return need_update;
+	}
 	if (!priv->reduced_txpower &&
 	    !iwl_is_associated(priv, IWL_RXON_CTX_PAN) &&
+	    (ave_rssi > BT_ENABLE_REDUCED_TXPOWER_THRESHOLD) &&
 	    (uart_msg->frame3 & (BT_UART_MSG_FRAME3ACL_MSK |
 	    BT_UART_MSG_FRAME3OBEX_MSK)) &&
 	    !(uart_msg->frame3 & (BT_UART_MSG_FRAME3SCOESCO_MSK |
@@ -606,13 +636,14 @@ static bool iwlagn_fill_txpower_mode(struct iwl_priv *priv,
 		need_update = true;
 	} else if (priv->reduced_txpower &&
 		   (iwl_is_associated(priv, IWL_RXON_CTX_PAN) ||
+		   (ave_rssi < BT_DISABLE_REDUCED_TXPOWER_THRESHOLD) ||
 		   (uart_msg->frame3 & (BT_UART_MSG_FRAME3SCOESCO_MSK |
 		   BT_UART_MSG_FRAME3SNIFF_MSK | BT_UART_MSG_FRAME3A2DP_MSK)) ||
 		   !(uart_msg->frame3 & (BT_UART_MSG_FRAME3ACL_MSK |
 		   BT_UART_MSG_FRAME3OBEX_MSK)))) {
 		/* disable reduced tx power */
 		priv->reduced_txpower = false;
-		priv->bt_valid &= ~IWLAGN_BT_VALID_REDUCED_TX_PWR;
+		priv->bt_valid |= IWLAGN_BT_VALID_REDUCED_TX_PWR;
 		need_update = true;
 	}
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
index 74fbee627306..0a3aa7c83003 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
@@ -61,6 +61,10 @@ void iwl_connection_init_rx_config(struct iwl_priv *priv,
 						  RXON_FILTER_ACCEPT_GRP_MSK;
 		break;
 
+	case NL80211_IFTYPE_MONITOR:
+		ctx->staging.dev_type = RXON_DEV_TYPE_SNIFFER;
+		break;
+
 	default:
 		IWL_ERR(priv, "Unsupported interface type %d\n",
 			ctx->vif->type);
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
index f2e9f298a947..3366e2e2f00f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
@@ -590,11 +590,17 @@ turn_off:
 	spin_unlock_bh(&priv->sta_lock);
 
 	if (test_bit(txq_id, priv->agg_q_alloc)) {
-		/* If the transport didn't know that we wanted to start
-		 * agreggation, don't tell it that we want to stop them
+		/*
+		 * If the transport didn't know that we wanted to start
+		 * agreggation, don't tell it that we want to stop them.
+		 * This can happen when we don't get the addBA response on
+		 * time, or we hadn't time to drain the AC queues.
 		 */
-		if (agg_state != IWL_AGG_STARTING)
+		if (agg_state == IWL_AGG_ON)
 			iwl_trans_tx_agg_disable(priv->trans, txq_id);
+		else
+			IWL_DEBUG_TX_QUEUES(priv, "Don't disable tx agg: %d\n",
+					    agg_state);
 		iwlagn_dealloc_agg_txq(priv, txq_id);
 	}
 
@@ -1300,10 +1306,11 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 			   (u8 *) &ba_resp->sta_addr_lo32,
 			   ba_resp->sta_id);
 	IWL_DEBUG_TX_REPLY(priv, "TID = %d, SeqCtl = %d, bitmap = 0x%llx, "
-			   "scd_flow = %d, scd_ssn = %d\n",
+			   "scd_flow = %d, scd_ssn = %d sent:%d, acked:%d\n",
 			   ba_resp->tid, le16_to_cpu(ba_resp->seq_ctl),
 			   (unsigned long long)le64_to_cpu(ba_resp->bitmap),
-			   scd_flow, ba_resp_scd_ssn);
+			   scd_flow, ba_resp_scd_ssn, ba_resp->txed,
+			   ba_resp->txed_2_done);
 
 	/* Mark that the expected block-ack response arrived */
 	agg->wait_for_ba = false;
@@ -1319,8 +1326,6 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 		 */
 		ba_resp->txed = ba_resp->txed_2_done;
 	}
-	IWL_DEBUG_HT(priv, "agg frames sent:%d, acked:%d\n",
-			ba_resp->txed, ba_resp->txed_2_done);
 
 	priv->tid_data[sta_id][tid].next_reclaimed = ba_resp_scd_ssn;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index 8d7637083fcf..ec36e2b020b6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -603,7 +603,7 @@ void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 	priv->contexts[IWL_RXON_CTX_BSS].wep_key_cmd = REPLY_WEPKEY;
 	priv->contexts[IWL_RXON_CTX_BSS].bcast_sta_id = IWLAGN_BROADCAST_ID;
 	priv->contexts[IWL_RXON_CTX_BSS].exclusive_interface_modes =
-		BIT(NL80211_IFTYPE_ADHOC);
+		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_MONITOR);
 	priv->contexts[IWL_RXON_CTX_BSS].interface_modes =
 		BIT(NL80211_IFTYPE_STATION);
 	priv->contexts[IWL_RXON_CTX_BSS].ap_devtype = RXON_DEV_TYPE_AP;
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/iwl-commands.h
index 83a6930f3658..9af6a239b384 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -1910,6 +1910,8 @@ enum iwl_bt_kill_idx {
 					IWLAGN_BT_VALID_REDUCED_TX_PWR | \
 					IWLAGN_BT_VALID_3W_LUT)
 
+#define IWLAGN_BT_REDUCED_TX_PWR	BIT(0)
+
 #define IWLAGN_BT_DECISION_LUT_SIZE	12
 
 struct iwl_basic_bt_cmd {
@@ -1923,6 +1925,10 @@ struct iwl_basic_bt_cmd {
 	u8 bt3_timer_t2_value;
 	__le16 bt4_reaction_time; /* unused */
 	__le32 bt3_lookup_table[IWLAGN_BT_DECISION_LUT_SIZE];
+	/*
+	 * bit 0: use reduced tx power for control frame
+	 * bit 1 - 7: reserved
+	 */
 	u8 reduce_txpower;
 	u8 reserved;
 	__le16 valid;
@@ -2272,7 +2278,6 @@ struct iwl_ssid_ie {
 #define IWL_GOOD_CRC_TH_DISABLED	0
 #define IWL_GOOD_CRC_TH_DEFAULT		cpu_to_le16(1)
 #define IWL_GOOD_CRC_TH_NEVER		cpu_to_le16(0xffff)
-#define IWL_MAX_SCAN_SIZE 1024
 #define IWL_MAX_CMD_SIZE 4096
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/iwl-mac80211.c b/drivers/net/wireless/iwlwifi/iwl-mac80211.c
index d33cc9cc7d3f..ab2f4d7500a4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-mac80211.c
+++ b/drivers/net/wireless/iwlwifi/iwl-mac80211.c
@@ -150,6 +150,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 		    IEEE80211_HW_QUEUE_CONTROL |
 		    IEEE80211_HW_SUPPORTS_PS |
 		    IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
+		    IEEE80211_HW_WANT_MONITOR_VIF |
 		    IEEE80211_HW_SCAN_WHILE_IDLE;
 
 	hw->offchannel_tx_hw_queue = IWL_AUX_QUEUE;
@@ -223,8 +224,8 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 		hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
 	hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX;
-	/* we create the 802.11 header and a zero-length SSID element */
-	hw->wiphy->max_scan_ie_len = capa->max_probe_length - 24 - 2;
+	/* we create the 802.11 header and a max-length SSID element */
+	hw->wiphy->max_scan_ie_len = capa->max_probe_length - 24 - 34;
 
 	/*
 	 * We don't use all queues: 4 and 9 are unused and any
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/iwl-power.c
index 8352265dbc4b..544ddf17f5bd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.c
+++ b/drivers/net/wireless/iwlwifi/iwl-power.c
@@ -253,6 +253,8 @@ static void iwl_static_sleep_cmd(struct iwl_priv *priv,
 
 	IWL_DEBUG_POWER(priv, "numSkipDtim = %u, dtimPeriod = %d\n",
 			skip, period);
+	/* The power level here is 0-4 (used as array index), but user expects
+	to see 1-5 (according to spec). */
 	IWL_DEBUG_POWER(priv, "Sleep command for index %d\n", lvl + 1);
 }
 
@@ -308,10 +310,12 @@ static void iwl_power_build_cmd(struct iwl_priv *priv,
 				     priv->power_data.debug_sleep_level_override,
 				     dtimper);
 	else {
+		/* Note that the user parameter is 1-5 (according to spec),
+		but we pass 0-4 because it acts as an array index. */
 		if (iwlwifi_mod_params.power_level > IWL_POWER_INDEX_1 &&
-		    iwlwifi_mod_params.power_level <= IWL_POWER_INDEX_5)
+		    iwlwifi_mod_params.power_level <= IWL_POWER_NUM)
 			iwl_static_sleep_cmd(priv, cmd,
-				iwlwifi_mod_params.power_level, dtimper);
+				iwlwifi_mod_params.power_level - 1, dtimper);
 		else
 			iwl_static_sleep_cmd(priv, cmd,
 				IWL_POWER_INDEX_1, dtimper);
diff --git a/drivers/net/wireless/iwlwifi/iwl-scan.c b/drivers/net/wireless/iwlwifi/iwl-scan.c
index a8437a6bc18e..031d8e21f82f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-scan.c
+++ b/drivers/net/wireless/iwlwifi/iwl-scan.c
@@ -52,6 +52,7 @@
 #define IWL_PASSIVE_DWELL_TIME_52   (10)
 #define IWL_PASSIVE_DWELL_BASE      (100)
 #define IWL_CHANNEL_TUNE_TIME       5
+#define MAX_SCAN_CHANNEL	    50
 
 static int iwl_send_scan_abort(struct iwl_priv *priv)
 {
@@ -616,7 +617,8 @@ static int iwl_get_channels_for_scan(struct iwl_priv *priv,
  */
 
 static u16 iwl_fill_probe_req(struct ieee80211_mgmt *frame, const u8 *ta,
-			      const u8 *ies, int ie_len, int left)
+			      const u8 *ies, int ie_len, const u8 *ssid,
+			      u8 ssid_len, int left)
 {
 	int len = 0;
 	u8 *pos = NULL;
@@ -638,14 +640,18 @@ static u16 iwl_fill_probe_req(struct ieee80211_mgmt *frame, const u8 *ta,
 	/* ...next IE... */
 	pos = &frame->u.probe_req.variable[0];
 
-	/* fill in our indirect SSID IE */
-	left -= 2;
+	/* fill in our SSID IE */
+	left -= ssid_len + 2;
 	if (left < 0)
 		return 0;
 	*pos++ = WLAN_EID_SSID;
-	*pos++ = 0;
+	*pos++ = ssid_len;
+	if (ssid && ssid_len) {
+		memcpy(pos, ssid, ssid_len);
+		pos += ssid_len;
+	}
 
-	len += 2;
+	len += ssid_len + 2;
 
 	if (WARN_ON(left < ie_len))
 		return len;
@@ -679,6 +685,15 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	u8 active_chains;
 	u8 scan_tx_antennas = priv->hw_params.valid_tx_ant;
 	int ret;
+	int scan_cmd_size = sizeof(struct iwl_scan_cmd) +
+			    MAX_SCAN_CHANNEL * sizeof(struct iwl_scan_channel) +
+			    priv->fw->ucode_capa.max_probe_length;
+	const u8 *ssid = NULL;
+	u8 ssid_len = 0;
+
+	if (WARN_ON_ONCE(priv->scan_request &&
+			 priv->scan_request->n_channels > MAX_SCAN_CHANNEL))
+		return -EINVAL;
 
 	lockdep_assert_held(&priv->mutex);
 
@@ -686,8 +701,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		ctx = iwl_rxon_ctx_from_vif(vif);
 
 	if (!priv->scan_cmd) {
-		priv->scan_cmd = kmalloc(sizeof(struct iwl_scan_cmd) +
-					 IWL_MAX_SCAN_SIZE, GFP_KERNEL);
+		priv->scan_cmd = kmalloc(scan_cmd_size, GFP_KERNEL);
 		if (!priv->scan_cmd) {
 			IWL_DEBUG_SCAN(priv,
 				       "fail to allocate memory for scan\n");
@@ -695,7 +709,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		}
 	}
 	scan = priv->scan_cmd;
-	memset(scan, 0, sizeof(struct iwl_scan_cmd) + IWL_MAX_SCAN_SIZE);
+	memset(scan, 0, scan_cmd_size);
 
 	scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
 	scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
@@ -746,10 +760,18 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		if (priv->scan_request->n_ssids) {
 			int i, p = 0;
 			IWL_DEBUG_SCAN(priv, "Kicking off active scan\n");
-			for (i = 0; i < priv->scan_request->n_ssids; i++) {
-				/* always does wildcard anyway */
-				if (!priv->scan_request->ssids[i].ssid_len)
-					continue;
+			/*
+			 * The highest priority SSID is inserted to the
+			 * probe request template.
+			 */
+			ssid_len = priv->scan_request->ssids[0].ssid_len;
+			ssid = priv->scan_request->ssids[0].ssid;
+
+			/*
+			 * Invert the order of ssids, the firmware will invert
+			 * it back.
+			 */
+			for (i = priv->scan_request->n_ssids - 1; i >= 1; i--) {
 				scan->direct_scan[p].id = WLAN_EID_SSID;
 				scan->direct_scan[p].len =
 					priv->scan_request->ssids[i].ssid_len;
@@ -883,7 +905,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 					vif->addr,
 					priv->scan_request->ie,
 					priv->scan_request->ie_len,
-					IWL_MAX_SCAN_SIZE - sizeof(*scan));
+					ssid, ssid_len,
+					scan_cmd_size - sizeof(*scan));
 		break;
 	case IWL_SCAN_RADIO_RESET:
 	case IWL_SCAN_ROC:
@@ -891,7 +914,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		cmd_len = iwl_fill_probe_req(
 					(struct ieee80211_mgmt *)scan->data,
 					iwl_bcast_addr, NULL, 0,
-					IWL_MAX_SCAN_SIZE - sizeof(*scan));
+					NULL, 0,
+					scan_cmd_size - sizeof(*scan));
 		break;
 	default:
 		BUG();
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 03c0c6b1372c..fb787df01666 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -746,6 +746,11 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		hwsim_check_sta_magic(txi->control.sta);
 
 	ieee80211_tx_info_clear_status(txi);
+
+	/* frame was transmitted at most favorable rate at first attempt */
+	txi->control.rates[0].count = 1;
+	txi->control.rates[1].idx = -1;
+
 	if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK) && ack)
 		txi->flags |= IEEE80211_TX_STAT_ACK;
 	ieee80211_tx_status_irqsafe(hw, skb);
diff --git a/drivers/net/wireless/mwifiex/Makefile b/drivers/net/wireless/mwifiex/Makefile
index 5c1a46bf1e11..3f66ebb0a630 100644
--- a/drivers/net/wireless/mwifiex/Makefile
+++ b/drivers/net/wireless/mwifiex/Makefile
@@ -29,6 +29,8 @@ mwifiex-y += scan.o
 mwifiex-y += join.o
 mwifiex-y += sta_ioctl.o
 mwifiex-y += sta_cmd.o
+mwifiex-y += uap_cmd.o
+mwifiex-y += ie.o
 mwifiex-y += sta_cmdresp.o
 mwifiex-y += sta_event.o
 mwifiex-y += sta_tx.o
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index c78ea873a63a..87671446e24b 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -20,6 +20,23 @@
 #include "cfg80211.h"
 #include "main.h"
 
+static const struct ieee80211_iface_limit mwifiex_ap_sta_limits[] = {
+	{
+		.max = 1, .types = BIT(NL80211_IFTYPE_STATION),
+	},
+	{
+		.max = 1, .types = BIT(NL80211_IFTYPE_AP),
+	},
+};
+
+static const struct ieee80211_iface_combination mwifiex_iface_comb_ap_sta = {
+	.limits = mwifiex_ap_sta_limits,
+	.num_different_channels = 1,
+	.n_limits = ARRAY_SIZE(mwifiex_ap_sta_limits),
+	.max_interfaces = MWIFIEX_MAX_BSS_NUM,
+	.beacon_int_infra_match = true,
+};
+
 /*
  * This function maps the nl802.11 channel type into driver channel type.
  *
@@ -67,7 +84,7 @@ mwifiex_is_alg_wep(u32 cipher)
 /*
  * This function retrieves the private structure from kernel wiphy structure.
  */
-static void *mwifiex_cfg80211_get_priv(struct wiphy *wiphy)
+static void *mwifiex_cfg80211_get_adapter(struct wiphy *wiphy)
 {
 	return (void *) (*(unsigned long *) wiphy_priv(wiphy));
 }
@@ -80,8 +97,10 @@ mwifiex_cfg80211_del_key(struct wiphy *wiphy, struct net_device *netdev,
 			 u8 key_index, bool pairwise, const u8 *mac_addr)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(netdev);
+	const u8 bc_mac[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	const u8 *peer_mac = pairwise ? mac_addr : bc_mac;
 
-	if (mwifiex_set_encode(priv, NULL, 0, key_index, 1)) {
+	if (mwifiex_set_encode(priv, NULL, 0, key_index, peer_mac, 1)) {
 		wiphy_err(wiphy, "deleting the crypto keys\n");
 		return -EFAULT;
 	}
@@ -98,7 +117,8 @@ mwifiex_cfg80211_set_tx_power(struct wiphy *wiphy,
 			      enum nl80211_tx_power_setting type,
 			      int mbm)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
 	struct mwifiex_power_cfg power_cfg;
 	int dbm = MBM_TO_DBM(mbm);
 
@@ -109,6 +129,8 @@ mwifiex_cfg80211_set_tx_power(struct wiphy *wiphy,
 		power_cfg.is_power_auto = 1;
 	}
 
+	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
+
 	return mwifiex_set_tx_power(priv, &power_cfg);
 }
 
@@ -148,7 +170,7 @@ mwifiex_cfg80211_set_default_key(struct wiphy *wiphy, struct net_device *netdev,
 	if (!priv->sec_info.wep_enabled)
 		return 0;
 
-	if (mwifiex_set_encode(priv, NULL, 0, key_index, 0)) {
+	if (mwifiex_set_encode(priv, NULL, 0, key_index, NULL, 0)) {
 		wiphy_err(wiphy, "set default Tx key index\n");
 		return -EFAULT;
 	}
@@ -165,9 +187,11 @@ mwifiex_cfg80211_add_key(struct wiphy *wiphy, struct net_device *netdev,
 			 struct key_params *params)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(netdev);
+	const u8 bc_mac[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	const u8 *peer_mac = pairwise ? mac_addr : bc_mac;
 
 	if (mwifiex_set_encode(priv, params->key, params->key_len,
-			       key_index, 0)) {
+			       key_index, peer_mac, 0)) {
 		wiphy_err(wiphy, "crypto keys added\n");
 		return -EFAULT;
 	}
@@ -192,13 +216,13 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 	enum ieee80211_band band;
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_channel *ch;
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
-	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
 	struct mwifiex_802_11d_domain_reg *domain_info = &adapter->domain_reg;
 
 	/* Set country code */
-	domain_info->country_code[0] = priv->country_code[0];
-	domain_info->country_code[1] = priv->country_code[1];
+	domain_info->country_code[0] = adapter->country_code[0];
+	domain_info->country_code[1] = adapter->country_code[1];
 	domain_info->country_code[2] = ' ';
 
 	band = mwifiex_band_to_radio_type(adapter->config_bands);
@@ -250,6 +274,8 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 
 	domain_info->no_of_triplet = no_of_triplet;
 
+	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
+
 	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
 				   HostCmd_ACT_GEN_SET, 0, NULL)) {
 		wiphy_err(wiphy, "11D: setting domain info in FW\n");
@@ -272,12 +298,12 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 static int mwifiex_reg_notifier(struct wiphy *wiphy,
 				struct regulatory_request *request)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
 
-	wiphy_dbg(wiphy, "info: cfg80211 regulatory domain callback for domain"
-			" %c%c\n", request->alpha2[0], request->alpha2[1]);
+	wiphy_dbg(wiphy, "info: cfg80211 regulatory domain callback for %c%c\n",
+		  request->alpha2[0], request->alpha2[1]);
 
-	memcpy(priv->country_code, request->alpha2, sizeof(request->alpha2));
+	memcpy(adapter->country_code, request->alpha2, sizeof(request->alpha2));
 
 	switch (request->initiator) {
 	case NL80211_REGDOM_SET_BY_DRIVER:
@@ -361,33 +387,10 @@ mwifiex_set_rf_channel(struct mwifiex_private *priv,
 	if (mwifiex_bss_set_channel(priv, &cfp))
 		return -EFAULT;
 
-	return mwifiex_drv_change_adhoc_chan(priv, cfp.channel);
-}
-
-/*
- * CFG802.11 operation handler to set channel.
- *
- * This function can only be used when station is not connected.
- */
-static int
-mwifiex_cfg80211_set_channel(struct wiphy *wiphy, struct net_device *dev,
-			     struct ieee80211_channel *chan,
-			     enum nl80211_channel_type channel_type)
-{
-	struct mwifiex_private *priv;
-
-	if (dev)
-		priv = mwifiex_netdev_get_priv(dev);
+	if (priv->bss_type == MWIFIEX_BSS_TYPE_STA)
+		return mwifiex_drv_change_adhoc_chan(priv, cfp.channel);
 	else
-		priv = mwifiex_cfg80211_get_priv(wiphy);
-
-	if (priv->media_connected) {
-		wiphy_err(wiphy, "This setting is valid only when station "
-				"is not connected\n");
-		return -EINVAL;
-	}
-
-	return mwifiex_set_rf_channel(priv, chan, channel_type);
+		return mwifiex_uap_set_channel(priv, cfp.channel);
 }
 
 /*
@@ -399,18 +402,13 @@ mwifiex_cfg80211_set_channel(struct wiphy *wiphy, struct net_device *dev,
 static int
 mwifiex_set_frag(struct mwifiex_private *priv, u32 frag_thr)
 {
-	int ret;
-
 	if (frag_thr < MWIFIEX_FRAG_MIN_VALUE ||
 	    frag_thr > MWIFIEX_FRAG_MAX_VALUE)
-		return -EINVAL;
-
-	/* Send request to firmware */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-				    HostCmd_ACT_GEN_SET, FRAG_THRESH_I,
-				    &frag_thr);
+		frag_thr = MWIFIEX_FRAG_MAX_VALUE;
 
-	return ret;
+	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
+				     HostCmd_ACT_GEN_SET, FRAG_THRESH_I,
+				     &frag_thr);
 }
 
 /*
@@ -439,19 +437,85 @@ mwifiex_set_rts(struct mwifiex_private *priv, u32 rts_thr)
 static int
 mwifiex_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
-	int ret = 0;
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
+	struct mwifiex_uap_bss_param *bss_cfg;
+	int ret, bss_started, i;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		priv = adapter->priv[i];
+
+		switch (priv->bss_role) {
+		case MWIFIEX_BSS_ROLE_UAP:
+			bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param),
+					  GFP_KERNEL);
+			if (!bss_cfg)
+				return -ENOMEM;
+
+			mwifiex_set_sys_config_invalid_data(bss_cfg);
+
+			if (changed & WIPHY_PARAM_RTS_THRESHOLD)
+				bss_cfg->rts_threshold = wiphy->rts_threshold;
+			if (changed & WIPHY_PARAM_FRAG_THRESHOLD)
+				bss_cfg->frag_threshold = wiphy->frag_threshold;
+			if (changed & WIPHY_PARAM_RETRY_LONG)
+				bss_cfg->retry_limit = wiphy->retry_long;
+
+			bss_started = priv->bss_started;
+
+			ret = mwifiex_send_cmd_sync(priv,
+						    HostCmd_CMD_UAP_BSS_STOP,
+						    HostCmd_ACT_GEN_SET, 0,
+						    NULL);
+			if (ret) {
+				wiphy_err(wiphy, "Failed to stop the BSS\n");
+				kfree(bss_cfg);
+				return ret;
+			}
 
-	if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
-		ret = mwifiex_set_rts(priv, wiphy->rts_threshold);
-		if (ret)
-			return ret;
-	}
+			ret = mwifiex_send_cmd_async(priv,
+						     HostCmd_CMD_UAP_SYS_CONFIG,
+						     HostCmd_ACT_GEN_SET,
+						     UAP_BSS_PARAMS_I, bss_cfg);
 
-	if (changed & WIPHY_PARAM_FRAG_THRESHOLD)
-		ret = mwifiex_set_frag(priv, wiphy->frag_threshold);
+			kfree(bss_cfg);
 
-	return ret;
+			if (ret) {
+				wiphy_err(wiphy, "Failed to set bss config\n");
+				return ret;
+			}
+
+			if (!bss_started)
+				break;
+
+			ret = mwifiex_send_cmd_async(priv,
+						     HostCmd_CMD_UAP_BSS_START,
+						     HostCmd_ACT_GEN_SET, 0,
+						     NULL);
+			if (ret) {
+				wiphy_err(wiphy, "Failed to start BSS\n");
+				return ret;
+			}
+
+			break;
+		case MWIFIEX_BSS_ROLE_STA:
+			if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
+				ret = mwifiex_set_rts(priv,
+						      wiphy->rts_threshold);
+				if (ret)
+					return ret;
+			}
+			if (changed & WIPHY_PARAM_FRAG_THRESHOLD) {
+				ret = mwifiex_set_frag(priv,
+						       wiphy->frag_threshold);
+				if (ret)
+					return ret;
+			}
+			break;
+		}
+	}
+
+	return 0;
 }
 
 /*
@@ -466,31 +530,59 @@ mwifiex_cfg80211_change_virtual_intf(struct wiphy *wiphy,
 	int ret;
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 
-	if (priv->bss_mode == type) {
-		wiphy_warn(wiphy, "already set to required type\n");
-		return 0;
-	}
-
-	priv->bss_mode = type;
-
-	switch (type) {
+	switch (dev->ieee80211_ptr->iftype) {
 	case NL80211_IFTYPE_ADHOC:
-		dev->ieee80211_ptr->iftype = NL80211_IFTYPE_ADHOC;
-		wiphy_dbg(wiphy, "info: setting interface type to adhoc\n");
+		switch (type) {
+		case NL80211_IFTYPE_STATION:
+			break;
+		case NL80211_IFTYPE_UNSPECIFIED:
+			wiphy_warn(wiphy, "%s: kept type as IBSS\n", dev->name);
+		case NL80211_IFTYPE_ADHOC:	/* This shouldn't happen */
+			return 0;
+		case NL80211_IFTYPE_AP:
+		default:
+			wiphy_err(wiphy, "%s: changing to %d not supported\n",
+				  dev->name, type);
+			return -EOPNOTSUPP;
+		}
 		break;
 	case NL80211_IFTYPE_STATION:
-		dev->ieee80211_ptr->iftype = NL80211_IFTYPE_STATION;
-		wiphy_dbg(wiphy, "info: setting interface type to managed\n");
+		switch (type) {
+		case NL80211_IFTYPE_ADHOC:
+			break;
+		case NL80211_IFTYPE_UNSPECIFIED:
+			wiphy_warn(wiphy, "%s: kept type as STA\n", dev->name);
+		case NL80211_IFTYPE_STATION:	/* This shouldn't happen */
+			return 0;
+		case NL80211_IFTYPE_AP:
+		default:
+			wiphy_err(wiphy, "%s: changing to %d not supported\n",
+				  dev->name, type);
+			return -EOPNOTSUPP;
+		}
+		break;
+	case NL80211_IFTYPE_AP:
+		switch (type) {
+		case NL80211_IFTYPE_UNSPECIFIED:
+			wiphy_warn(wiphy, "%s: kept type as AP\n", dev->name);
+		case NL80211_IFTYPE_AP:		/* This shouldn't happen */
+			return 0;
+		case NL80211_IFTYPE_ADHOC:
+		case NL80211_IFTYPE_STATION:
+		default:
+			wiphy_err(wiphy, "%s: changing to %d not supported\n",
+				  dev->name, type);
+			return -EOPNOTSUPP;
+		}
 		break;
-	case NL80211_IFTYPE_UNSPECIFIED:
-		dev->ieee80211_ptr->iftype = NL80211_IFTYPE_STATION;
-		wiphy_dbg(wiphy, "info: setting interface type to auto\n");
-		return 0;
 	default:
-		wiphy_err(wiphy, "unknown interface type: %d\n", type);
-		return -EINVAL;
+		wiphy_err(wiphy, "%s: unknown iftype: %d\n",
+			  dev->name, dev->ieee80211_ptr->iftype);
+		return -EOPNOTSUPP;
 	}
 
+	dev->ieee80211_ptr->iftype = type;
+	priv->bss_mode = type;
 	mwifiex_deauthenticate(priv, NULL);
 
 	priv->sec_info.authentication_mode = NL80211_AUTHTYPE_OPEN_SYSTEM;
@@ -804,6 +896,90 @@ static int mwifiex_cfg80211_set_cqm_rssi_config(struct wiphy *wiphy,
 	return 0;
 }
 
+/* cfg80211 operation handler for stop ap.
+ * Function stops BSS running at uAP interface.
+ */
+static int mwifiex_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (mwifiex_del_mgmt_ies(priv))
+		wiphy_err(wiphy, "Failed to delete mgmt IEs!\n");
+
+	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
+				  HostCmd_ACT_GEN_SET, 0, NULL)) {
+		wiphy_err(wiphy, "Failed to stop the BSS\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/* cfg80211 operation handler for start_ap.
+ * Function sets beacon period, DTIM period, SSID and security into
+ * AP config structure.
+ * AP is configured with these settings and BSS is started.
+ */
+static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
+				     struct net_device *dev,
+				     struct cfg80211_ap_settings *params)
+{
+	struct mwifiex_uap_bss_param *bss_cfg;
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP)
+		return -1;
+	if (mwifiex_set_mgmt_ies(priv, params))
+		return -1;
+
+	bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param), GFP_KERNEL);
+	if (!bss_cfg)
+		return -ENOMEM;
+
+	mwifiex_set_sys_config_invalid_data(bss_cfg);
+
+	if (params->beacon_interval)
+		bss_cfg->beacon_period = params->beacon_interval;
+	if (params->dtim_period)
+		bss_cfg->dtim_period = params->dtim_period;
+
+	if (params->ssid && params->ssid_len) {
+		memcpy(bss_cfg->ssid.ssid, params->ssid, params->ssid_len);
+		bss_cfg->ssid.ssid_len = params->ssid_len;
+	}
+
+	if (mwifiex_set_secure_params(priv, bss_cfg, params)) {
+		kfree(bss_cfg);
+		wiphy_err(wiphy, "Failed to parse secuirty parameters!\n");
+		return -1;
+	}
+
+	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
+				  HostCmd_ACT_GEN_SET, 0, NULL)) {
+		wiphy_err(wiphy, "Failed to stop the BSS\n");
+		kfree(bss_cfg);
+		return -1;
+	}
+
+	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+				   HostCmd_ACT_GEN_SET,
+				   UAP_BSS_PARAMS_I, bss_cfg)) {
+		wiphy_err(wiphy, "Failed to set the SSID\n");
+		kfree(bss_cfg);
+		return -1;
+	}
+
+	kfree(bss_cfg);
+
+	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_BSS_START,
+				   HostCmd_ACT_GEN_SET, 0, NULL)) {
+		wiphy_err(wiphy, "Failed to start the BSS\n");
+		return -1;
+	}
+
+	return 0;
+}
+
 /*
  * CFG802.11 operation handler for disconnection request.
  *
@@ -923,7 +1099,7 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len, u8 *ssid,
 	priv->wep_key_curr_index = 0;
 	priv->sec_info.encryption_mode = 0;
 	priv->sec_info.is_authtype_auto = 0;
-	ret = mwifiex_set_encode(priv, NULL, 0, 0, 1);
+	ret = mwifiex_set_encode(priv, NULL, 0, 0, NULL, 1);
 
 	if (mode == NL80211_IFTYPE_ADHOC) {
 		/* "privacy" is set only for ad-hoc mode */
@@ -971,7 +1147,7 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len, u8 *ssid,
 				" with key len %d\n", sme->key_len);
 			priv->wep_key_curr_index = sme->key_idx;
 			ret = mwifiex_set_encode(priv, sme->key, sme->key_len,
-							sme->key_idx, 0);
+						 sme->key_idx, NULL, 0);
 		}
 	}
 done:
@@ -1050,6 +1226,11 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 		goto done;
 	}
 
+	if (priv->bss_mode == NL80211_IFTYPE_AP) {
+		wiphy_err(wiphy, "skip association request for AP interface\n");
+		goto done;
+	}
+
 	wiphy_dbg(wiphy, "info: Trying to associate to %s and bssid %pM\n",
 		  (char *) sme->ssid, sme->bssid);
 
@@ -1283,15 +1464,12 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 					    u32 *flags,
 					    struct vif_params *params)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
-	struct mwifiex_adapter *adapter;
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
 	struct net_device *dev;
 	void *mdev_priv;
+	struct wireless_dev *wdev;
 
-	if (!priv)
-		return NULL;
-
-	adapter = priv->adapter;
 	if (!adapter)
 		return NULL;
 
@@ -1299,12 +1477,21 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	case NL80211_IFTYPE_UNSPECIFIED:
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_ADHOC:
+		priv = adapter->priv[MWIFIEX_BSS_TYPE_STA];
 		if (priv->bss_mode) {
-			wiphy_err(wiphy, "cannot create multiple"
-					" station/adhoc interfaces\n");
+			wiphy_err(wiphy,
+				  "cannot create multiple sta/adhoc ifaces\n");
 			return NULL;
 		}
 
+		wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+		if (!wdev)
+			return NULL;
+
+		wdev->wiphy = wiphy;
+		priv->wdev = wdev;
+		wdev->iftype = NL80211_IFTYPE_STATION;
+
 		if (type == NL80211_IFTYPE_UNSPECIFIED)
 			priv->bss_mode = NL80211_IFTYPE_STATION;
 		else
@@ -1312,11 +1499,36 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 		priv->bss_type = MWIFIEX_BSS_TYPE_STA;
 		priv->frame_type = MWIFIEX_DATA_FRAME_TYPE_ETH_II;
-		priv->bss_priority = 0;
+		priv->bss_priority = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_role = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_num = 0;
 
 		break;
+	case NL80211_IFTYPE_AP:
+		priv = adapter->priv[MWIFIEX_BSS_TYPE_UAP];
+
+		if (priv->bss_mode) {
+			wiphy_err(wiphy, "Can't create multiple AP interfaces");
+			return NULL;
+		}
+
+		wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+		if (!wdev)
+			return NULL;
+
+		priv->wdev = wdev;
+		wdev->wiphy = wiphy;
+		wdev->iftype = NL80211_IFTYPE_AP;
+
+		priv->bss_type = MWIFIEX_BSS_TYPE_UAP;
+		priv->frame_type = MWIFIEX_DATA_FRAME_TYPE_ETH_II;
+		priv->bss_priority = MWIFIEX_BSS_ROLE_UAP;
+		priv->bss_role = MWIFIEX_BSS_ROLE_UAP;
+		priv->bss_started = 0;
+		priv->bss_num = 0;
+		priv->bss_mode = type;
+
+		break;
 	default:
 		wiphy_err(wiphy, "type not supported\n");
 		return NULL;
@@ -1329,6 +1541,15 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		goto error;
 	}
 
+	mwifiex_init_priv_params(priv, dev);
+	priv->netdev = dev;
+
+	mwifiex_setup_ht_caps(&wiphy->bands[IEEE80211_BAND_2GHZ]->ht_cap, priv);
+
+	if (adapter->config_bands & BAND_A)
+		mwifiex_setup_ht_caps(
+			&wiphy->bands[IEEE80211_BAND_5GHZ]->ht_cap, priv);
+
 	dev_net_set(dev, wiphy_net(wiphy));
 	dev->ieee80211_ptr = priv->wdev;
 	dev->ieee80211_ptr->iftype = priv->bss_mode;
@@ -1343,9 +1564,6 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	mdev_priv = netdev_priv(dev);
 	*((unsigned long *) mdev_priv) = (unsigned long) priv;
 
-	priv->netdev = dev;
-	mwifiex_init_priv_params(priv, dev);
-
 	SET_NETDEV_DEV(dev, adapter->dev);
 
 	/* Register network device */
@@ -1417,7 +1635,6 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.get_station = mwifiex_cfg80211_get_station,
 	.dump_station = mwifiex_cfg80211_dump_station,
 	.set_wiphy_params = mwifiex_cfg80211_set_wiphy_params,
-	.set_channel = mwifiex_cfg80211_set_channel,
 	.join_ibss = mwifiex_cfg80211_join_ibss,
 	.leave_ibss = mwifiex_cfg80211_leave_ibss,
 	.add_key = mwifiex_cfg80211_add_key,
@@ -1426,6 +1643,8 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.set_power_mgmt = mwifiex_cfg80211_set_power_mgmt,
 	.set_tx_power = mwifiex_cfg80211_set_tx_power,
 	.set_bitrate_mask = mwifiex_cfg80211_set_bitrate_mask,
+	.start_ap = mwifiex_cfg80211_start_ap,
+	.stop_ap = mwifiex_cfg80211_stop_ap,
 	.set_cqm_rssi_config = mwifiex_cfg80211_set_cqm_rssi_config,
 };
 
@@ -1436,82 +1655,67 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
  * default parameters and handler function pointers, and finally
  * registers the device.
  */
-int mwifiex_register_cfg80211(struct mwifiex_private *priv)
+
+int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 {
 	int ret;
 	void *wdev_priv;
-	struct wireless_dev *wdev;
-	struct ieee80211_sta_ht_cap *ht_info;
+	struct wiphy *wiphy;
+	struct mwifiex_private *priv = adapter->priv[MWIFIEX_BSS_TYPE_STA];
 	u8 *country_code;
 
-	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
-	if (!wdev) {
-		dev_err(priv->adapter->dev, "%s: allocating wireless device\n",
-			__func__);
-		return -ENOMEM;
-	}
-	wdev->wiphy =
-		wiphy_new(&mwifiex_cfg80211_ops,
-			  sizeof(struct mwifiex_private *));
-	if (!wdev->wiphy) {
-		kfree(wdev);
+	/* create a new wiphy for use with cfg80211 */
+	wiphy = wiphy_new(&mwifiex_cfg80211_ops,
+			  sizeof(struct mwifiex_adapter *));
+	if (!wiphy) {
+		dev_err(adapter->dev, "%s: creating new wiphy\n", __func__);
 		return -ENOMEM;
 	}
-	wdev->iftype = NL80211_IFTYPE_STATION;
-	wdev->wiphy->max_scan_ssids = 10;
-	wdev->wiphy->max_scan_ie_len = MWIFIEX_MAX_VSIE_LEN;
-	wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
-				       BIT(NL80211_IFTYPE_ADHOC);
-
-	wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &mwifiex_band_2ghz;
-	ht_info = &wdev->wiphy->bands[IEEE80211_BAND_2GHZ]->ht_cap;
-	mwifiex_setup_ht_caps(ht_info, priv);
-
-	if (priv->adapter->config_bands & BAND_A) {
-		wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &mwifiex_band_5ghz;
-		ht_info = &wdev->wiphy->bands[IEEE80211_BAND_5GHZ]->ht_cap;
-		mwifiex_setup_ht_caps(ht_info, priv);
-	} else {
-		wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
-	}
+	wiphy->max_scan_ssids = MWIFIEX_MAX_SSID_LIST_LENGTH;
+	wiphy->max_scan_ie_len = MWIFIEX_MAX_VSIE_LEN;
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+				 BIT(NL80211_IFTYPE_ADHOC) |
+				 BIT(NL80211_IFTYPE_AP);
+
+	wiphy->bands[IEEE80211_BAND_2GHZ] = &mwifiex_band_2ghz;
+	if (adapter->config_bands & BAND_A)
+		wiphy->bands[IEEE80211_BAND_5GHZ] = &mwifiex_band_5ghz;
+	else
+		wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
+
+	wiphy->iface_combinations = &mwifiex_iface_comb_ap_sta;
+	wiphy->n_iface_combinations = 1;
 
 	/* Initialize cipher suits */
-	wdev->wiphy->cipher_suites = mwifiex_cipher_suites;
-	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(mwifiex_cipher_suites);
+	wiphy->cipher_suites = mwifiex_cipher_suites;
+	wiphy->n_cipher_suites = ARRAY_SIZE(mwifiex_cipher_suites);
 
-	memcpy(wdev->wiphy->perm_addr, priv->curr_addr, ETH_ALEN);
-	wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+	memcpy(wiphy->perm_addr, priv->curr_addr, ETH_ALEN);
+	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+	wiphy->flags |= WIPHY_FLAG_HAVE_AP_SME | WIPHY_FLAG_CUSTOM_REGULATORY;
 
 	/* Reserve space for mwifiex specific private data for BSS */
-	wdev->wiphy->bss_priv_size = sizeof(struct mwifiex_bss_priv);
-
-	wdev->wiphy->reg_notifier = mwifiex_reg_notifier;
+	wiphy->bss_priv_size = sizeof(struct mwifiex_bss_priv);
 
-	/* Set struct mwifiex_private pointer in wiphy_priv */
-	wdev_priv = wiphy_priv(wdev->wiphy);
+	wiphy->reg_notifier = mwifiex_reg_notifier;
 
-	*(unsigned long *) wdev_priv = (unsigned long) priv;
+	/* Set struct mwifiex_adapter pointer in wiphy_priv */
+	wdev_priv = wiphy_priv(wiphy);
+	*(unsigned long *)wdev_priv = (unsigned long)adapter;
 
-	set_wiphy_dev(wdev->wiphy, (struct device *) priv->adapter->dev);
+	set_wiphy_dev(wiphy, (struct device *)priv->adapter->dev);
 
-	ret = wiphy_register(wdev->wiphy);
+	ret = wiphy_register(wiphy);
 	if (ret < 0) {
-		dev_err(priv->adapter->dev, "%s: registering cfg80211 device\n",
-			__func__);
-		wiphy_free(wdev->wiphy);
-		kfree(wdev);
+		dev_err(adapter->dev,
+			"%s: wiphy_register failed: %d\n", __func__, ret);
+		wiphy_free(wiphy);
 		return ret;
-	} else {
-		dev_dbg(priv->adapter->dev,
-			"info: successfully registered wiphy device\n");
 	}
-
 	country_code = mwifiex_11d_code_2_region(priv->adapter->region_code);
-	if (country_code && regulatory_hint(wdev->wiphy, country_code))
-		dev_err(priv->adapter->dev,
-			"%s: regulatory_hint failed\n", __func__);
-
-	priv->wdev = wdev;
+	if (country_code && regulatory_hint(wiphy, country_code))
+		dev_err(adapter->dev, "regulatory_hint() failed\n");
 
+	adapter->wiphy = wiphy;
 	return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/cfg80211.h b/drivers/net/wireless/mwifiex/cfg80211.h
index 76c76c60438b..c5848934f111 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.h
+++ b/drivers/net/wireless/mwifiex/cfg80211.h
@@ -24,6 +24,6 @@
 
 #include "main.h"
 
-int mwifiex_register_cfg80211(struct mwifiex_private *);
+int mwifiex_register_cfg80211(struct mwifiex_adapter *);
 
 #endif
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 1710beffb93a..51e023ec1de4 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -440,6 +440,11 @@ int mwifiex_process_event(struct mwifiex_adapter *adapter)
 		do_gettimeofday(&tstamp);
 		dev_dbg(adapter->dev, "event: %lu.%lu: cause: %#x\n",
 			tstamp.tv_sec, tstamp.tv_usec, eventcause);
+	} else {
+		/* Handle PS_SLEEP/AWAKE events on STA */
+		priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
+		if (!priv)
+			priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 	}
 
 	ret = mwifiex_process_sta_event(priv);
@@ -540,8 +545,20 @@ int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
 
 	/* Prepare command */
 	if (cmd_no) {
-		ret = mwifiex_sta_prepare_cmd(priv, cmd_no, cmd_action,
-					      cmd_oid, data_buf, cmd_ptr);
+		switch (cmd_no) {
+		case HostCmd_CMD_UAP_SYS_CONFIG:
+		case HostCmd_CMD_UAP_BSS_START:
+		case HostCmd_CMD_UAP_BSS_STOP:
+			ret = mwifiex_uap_prepare_cmd(priv, cmd_no, cmd_action,
+						      cmd_oid, data_buf,
+						      cmd_ptr);
+			break;
+		default:
+			ret = mwifiex_sta_prepare_cmd(priv, cmd_no, cmd_action,
+						      cmd_oid, data_buf,
+						      cmd_ptr);
+			break;
+		}
 	} else {
 		ret = mwifiex_cmd_host_cmd(priv, cmd_ptr, data_buf);
 		cmd_node->cmd_flag |= CMD_F_HOSTCMD;
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index d04aba4131dc..f918f66e5e27 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -28,7 +28,7 @@
 #include <linux/ieee80211.h>
 
 
-#define MWIFIEX_MAX_BSS_NUM         (1)
+#define MWIFIEX_MAX_BSS_NUM         (2)
 
 #define MWIFIEX_MIN_DATA_HEADER_LEN 36	/* sizeof(mwifiex_txpd)
 					 *   + 4 byte alignment
@@ -55,11 +55,17 @@
 #define MWIFIEX_RX_DATA_BUF_SIZE     (4 * 1024)
 #define MWIFIEX_RX_CMD_BUF_SIZE	     (2 * 1024)
 
+#define MAX_BEACON_PERIOD                  (4000)
+#define MIN_BEACON_PERIOD                  (50)
+#define MAX_DTIM_PERIOD                    (100)
+#define MIN_DTIM_PERIOD                    (1)
+
 #define MWIFIEX_RTS_MIN_VALUE              (0)
 #define MWIFIEX_RTS_MAX_VALUE              (2347)
 #define MWIFIEX_FRAG_MIN_VALUE             (256)
 #define MWIFIEX_FRAG_MAX_VALUE             (2346)
 
+#define MWIFIEX_RETRY_LIMIT                14
 #define MWIFIEX_SDIO_BLOCK_SIZE            256
 
 #define MWIFIEX_BUF_FLAG_REQUEUED_PKT      BIT(0)
@@ -92,6 +98,11 @@ struct mwifiex_fw_image {
 	u32 fw_len;
 };
 
+struct mwifiex_802_11_ssid {
+	u32 ssid_len;
+	u8 ssid[IEEE80211_MAX_SSID_LEN];
+};
+
 struct mwifiex_wait_queue {
 	wait_queue_head_t wait;
 	int status;
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index 5f6adeb9b950..9f674bbebe65 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -93,6 +93,20 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define CAL_SNR(RSSI, NF)		((s16)((s16)(RSSI)-(s16)(NF)))
 
+#define UAP_BSS_PARAMS_I			0
+#define UAP_CUSTOM_IE_I				1
+#define MWIFIEX_AUTO_IDX_MASK			0xffff
+#define MWIFIEX_DELETE_MASK			0x0000
+#define MGMT_MASK_ASSOC_REQ			0x01
+#define MGMT_MASK_REASSOC_REQ			0x04
+#define MGMT_MASK_ASSOC_RESP			0x02
+#define MGMT_MASK_REASSOC_RESP			0x08
+#define MGMT_MASK_PROBE_REQ			0x10
+#define MGMT_MASK_PROBE_RESP			0x20
+#define MGMT_MASK_BEACON			0x100
+
+#define TLV_TYPE_UAP_SSID			0x0000
+
 #define PROPRIETARY_TLV_BASE_ID                 0x0100
 #define TLV_TYPE_KEY_MATERIAL       (PROPRIETARY_TLV_BASE_ID + 0)
 #define TLV_TYPE_CHANLIST           (PROPRIETARY_TLV_BASE_ID + 1)
@@ -104,14 +118,26 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_TSFTIMESTAMP       (PROPRIETARY_TLV_BASE_ID + 19)
 #define TLV_TYPE_RSSI_HIGH          (PROPRIETARY_TLV_BASE_ID + 22)
 #define TLV_TYPE_AUTH_TYPE          (PROPRIETARY_TLV_BASE_ID + 31)
+#define TLV_TYPE_STA_MAC_ADDR       (PROPRIETARY_TLV_BASE_ID + 32)
 #define TLV_TYPE_CHANNELBANDLIST    (PROPRIETARY_TLV_BASE_ID + 42)
+#define TLV_TYPE_UAP_BEACON_PERIOD  (PROPRIETARY_TLV_BASE_ID + 44)
+#define TLV_TYPE_UAP_DTIM_PERIOD    (PROPRIETARY_TLV_BASE_ID + 45)
+#define TLV_TYPE_UAP_RTS_THRESHOLD  (PROPRIETARY_TLV_BASE_ID + 51)
+#define TLV_TYPE_UAP_WPA_PASSPHRASE (PROPRIETARY_TLV_BASE_ID + 60)
+#define TLV_TYPE_UAP_ENCRY_PROTOCOL (PROPRIETARY_TLV_BASE_ID + 64)
+#define TLV_TYPE_UAP_AKMP           (PROPRIETARY_TLV_BASE_ID + 65)
+#define TLV_TYPE_UAP_FRAG_THRESHOLD (PROPRIETARY_TLV_BASE_ID + 70)
 #define TLV_TYPE_RATE_DROP_CONTROL  (PROPRIETARY_TLV_BASE_ID + 82)
 #define TLV_TYPE_RATE_SCOPE         (PROPRIETARY_TLV_BASE_ID + 83)
 #define TLV_TYPE_POWER_GROUP        (PROPRIETARY_TLV_BASE_ID + 84)
+#define TLV_TYPE_UAP_RETRY_LIMIT    (PROPRIETARY_TLV_BASE_ID + 93)
 #define TLV_TYPE_WAPI_IE            (PROPRIETARY_TLV_BASE_ID + 94)
+#define TLV_TYPE_UAP_MGMT_FRAME     (PROPRIETARY_TLV_BASE_ID + 104)
 #define TLV_TYPE_MGMT_IE            (PROPRIETARY_TLV_BASE_ID + 105)
 #define TLV_TYPE_AUTO_DS_PARAM      (PROPRIETARY_TLV_BASE_ID + 113)
 #define TLV_TYPE_PS_PARAM           (PROPRIETARY_TLV_BASE_ID + 114)
+#define TLV_TYPE_PWK_CIPHER         (PROPRIETARY_TLV_BASE_ID + 145)
+#define TLV_TYPE_GWK_CIPHER         (PROPRIETARY_TLV_BASE_ID + 146)
 
 #define MWIFIEX_TX_DATA_BUF_SIZE_2K        2048
 
@@ -209,6 +235,9 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_RSSI_INFO                         0x00a4
 #define HostCmd_CMD_FUNC_INIT                         0x00a9
 #define HostCmd_CMD_FUNC_SHUTDOWN                     0x00aa
+#define HostCmd_CMD_UAP_SYS_CONFIG                    0x00b0
+#define HostCmd_CMD_UAP_BSS_START                     0x00b1
+#define HostCmd_CMD_UAP_BSS_STOP                      0x00b2
 #define HostCmd_CMD_11N_CFG                           0x00cd
 #define HostCmd_CMD_11N_ADDBA_REQ                     0x00ce
 #define HostCmd_CMD_11N_ADDBA_RSP                     0x00cf
@@ -223,6 +252,19 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_SET_BSS_MODE                      0x00f7
 #define HostCmd_CMD_PCIE_DESC_DETAILS                 0x00fa
 
+#define PROTOCOL_NO_SECURITY        0x01
+#define PROTOCOL_STATIC_WEP         0x02
+#define PROTOCOL_WPA                0x08
+#define PROTOCOL_WPA2               0x20
+#define PROTOCOL_WPA2_MIXED         0x28
+#define PROTOCOL_EAP                0x40
+#define KEY_MGMT_NONE               0x04
+#define KEY_MGMT_PSK                0x02
+#define KEY_MGMT_EAP                0x01
+#define CIPHER_TKIP                 0x04
+#define CIPHER_AES_CCMP             0x08
+#define VALID_CIPHER_BITMAP         0x0c
+
 enum ENH_PS_MODES {
 	EN_PS = 1,
 	DIS_PS = 2,
@@ -313,15 +355,20 @@ enum ENH_PS_MODES {
 #define EVENT_DATA_SNR_HIGH             0x00000027
 #define EVENT_LINK_QUALITY              0x00000028
 #define EVENT_PORT_RELEASE              0x0000002b
+#define EVENT_UAP_STA_DEAUTH            0x0000002c
+#define EVENT_UAP_STA_ASSOC             0x0000002d
+#define EVENT_UAP_BSS_START             0x0000002e
 #define EVENT_PRE_BEACON_LOST           0x00000031
 #define EVENT_ADDBA                     0x00000033
 #define EVENT_DELBA                     0x00000034
 #define EVENT_BA_STREAM_TIEMOUT         0x00000037
 #define EVENT_AMSDU_AGGR_CTRL           0x00000042
+#define EVENT_UAP_BSS_IDLE              0x00000043
+#define EVENT_UAP_BSS_ACTIVE            0x00000044
 #define EVENT_WEP_ICV_ERR               0x00000046
 #define EVENT_HS_ACT_REQ                0x00000047
 #define EVENT_BW_CHANGE                 0x00000048
-
+#define EVENT_UAP_MIC_COUNTERMEASURES   0x0000004c
 #define EVENT_HOSTWAKE_STAIE		0x0000004d
 
 #define EVENT_ID_MASK                   0xffff
@@ -1103,6 +1150,101 @@ struct host_cmd_ds_802_11_eeprom_access {
 	u8 value;
 } __packed;
 
+struct host_cmd_tlv {
+	__le16 type;
+	__le16 len;
+} __packed;
+
+struct mwifiex_assoc_event {
+	u8 sta_addr[ETH_ALEN];
+	__le16 type;
+	__le16 len;
+	__le16 frame_control;
+	__le16 cap_info;
+	__le16 listen_interval;
+	u8 data[0];
+} __packed;
+
+struct host_cmd_ds_sys_config {
+	__le16 action;
+	u8 tlv[0];
+};
+
+struct host_cmd_tlv_akmp {
+	struct host_cmd_tlv tlv;
+	__le16 key_mgmt;
+	__le16 key_mgmt_operation;
+} __packed;
+
+struct host_cmd_tlv_pwk_cipher {
+	struct host_cmd_tlv tlv;
+	__le16 proto;
+	u8 cipher;
+	u8 reserved;
+} __packed;
+
+struct host_cmd_tlv_gwk_cipher {
+	struct host_cmd_tlv tlv;
+	u8 cipher;
+	u8 reserved;
+} __packed;
+
+struct host_cmd_tlv_passphrase {
+	struct host_cmd_tlv tlv;
+	u8 passphrase[0];
+} __packed;
+
+struct host_cmd_tlv_auth_type {
+	struct host_cmd_tlv tlv;
+	u8 auth_type;
+} __packed;
+
+struct host_cmd_tlv_encrypt_protocol {
+	struct host_cmd_tlv tlv;
+	__le16 proto;
+} __packed;
+
+struct host_cmd_tlv_ssid {
+	struct host_cmd_tlv tlv;
+	u8 ssid[0];
+} __packed;
+
+struct host_cmd_tlv_beacon_period {
+	struct host_cmd_tlv tlv;
+	__le16 period;
+} __packed;
+
+struct host_cmd_tlv_dtim_period {
+	struct host_cmd_tlv tlv;
+	u8 period;
+} __packed;
+
+struct host_cmd_tlv_frag_threshold {
+	struct host_cmd_tlv tlv;
+	__le16 frag_thr;
+} __packed;
+
+struct host_cmd_tlv_rts_threshold {
+	struct host_cmd_tlv tlv;
+	__le16 rts_thr;
+} __packed;
+
+struct host_cmd_tlv_retry_limit {
+	struct host_cmd_tlv tlv;
+	u8 limit;
+} __packed;
+
+struct host_cmd_tlv_mac_addr {
+	struct host_cmd_tlv tlv;
+	u8 mac_addr[ETH_ALEN];
+} __packed;
+
+struct host_cmd_tlv_channel_band {
+	struct host_cmd_tlv tlv;
+	u8 band_config;
+	u8 channel;
+} __packed;
+
 struct host_cmd_ds_802_11_rf_channel {
 	__le16 action;
 	__le16 current_channel;
@@ -1167,6 +1309,20 @@ struct host_cmd_ds_802_11_subsc_evt {
 	__le16 events;
 } __packed;
 
+struct mwifiex_ie {
+	__le16 ie_index;
+	__le16 mgmt_subtype_mask;
+	__le16 ie_length;
+	u8 ie_buffer[IEEE_MAX_IE_SIZE];
+} __packed;
+
+#define MAX_MGMT_IE_INDEX	16
+struct mwifiex_ie_list {
+	__le16 type;
+	__le16 len;
+	struct mwifiex_ie ie_list[MAX_MGMT_IE_INDEX];
+} __packed;
+
 struct host_cmd_ds_command {
 	__le16 command;
 	__le16 size;
@@ -1217,6 +1373,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_pcie_details pcie_host_spec;
 		struct host_cmd_ds_802_11_eeprom_access eeprom;
 		struct host_cmd_ds_802_11_subsc_evt subsc_evt;
+		struct host_cmd_ds_sys_config uap_sys_config;
 	} params;
 } __packed;
 
diff --git a/drivers/net/wireless/mwifiex/ie.c b/drivers/net/wireless/mwifiex/ie.c
new file mode 100644
index 000000000000..ceb82cd749cc
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/ie.c
@@ -0,0 +1,396 @@
+/*
+ * Marvell Wireless LAN device driver: management IE handling- setting and
+ * deleting IE.
+ *
+ * Copyright (C) 2012, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+#include "main.h"
+
+/* This function checks if current IE index is used by any on other interface.
+ * Return: -1: yes, current IE index is used by someone else.
+ *          0: no, current IE index is NOT used by other interface.
+ */
+static int
+mwifiex_ie_index_used_by_other_intf(struct mwifiex_private *priv, u16 idx)
+{
+	int i;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_ie *ie;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		if (adapter->priv[i] != priv) {
+			ie = &adapter->priv[i]->mgmt_ie[idx];
+			if (ie->mgmt_subtype_mask && ie->ie_length)
+				return -1;
+		}
+	}
+
+	return 0;
+}
+
+/* Get unused IE index. This index will be used for setting new IE */
+static int
+mwifiex_ie_get_autoidx(struct mwifiex_private *priv, u16 subtype_mask,
+		       struct mwifiex_ie *ie, u16 *index)
+{
+	u16 mask, len, i;
+
+	for (i = 0; i < priv->adapter->max_mgmt_ie_index; i++) {
+		mask = le16_to_cpu(priv->mgmt_ie[i].mgmt_subtype_mask);
+		len = le16_to_cpu(priv->mgmt_ie[i].ie_length) +
+		      le16_to_cpu(ie->ie_length);
+
+		if (mask == MWIFIEX_AUTO_IDX_MASK)
+			continue;
+
+		if (mask == subtype_mask) {
+			if (len > IEEE_MAX_IE_SIZE)
+				continue;
+
+			*index = i;
+			return 0;
+		}
+
+		if (!priv->mgmt_ie[i].ie_length) {
+			if (mwifiex_ie_index_used_by_other_intf(priv, i))
+				continue;
+
+			*index = i;
+			return 0;
+		}
+	}
+
+	return -1;
+}
+
+/* This function prepares IE data buffer for command to be sent to FW */
+static int
+mwifiex_update_autoindex_ies(struct mwifiex_private *priv,
+			     struct mwifiex_ie_list *ie_list)
+{
+	u16 travel_len, index, mask;
+	s16 input_len;
+	struct mwifiex_ie *ie;
+	u8 *tmp;
+
+	input_len = le16_to_cpu(ie_list->len);
+	travel_len = sizeof(struct host_cmd_tlv);
+
+	ie_list->len = 0;
+
+	while (input_len > 0) {
+		ie = (struct mwifiex_ie *)(((u8 *)ie_list) + travel_len);
+		input_len -= le16_to_cpu(ie->ie_length) + MWIFIEX_IE_HDR_SIZE;
+		travel_len += le16_to_cpu(ie->ie_length) + MWIFIEX_IE_HDR_SIZE;
+
+		index = le16_to_cpu(ie->ie_index);
+		mask = le16_to_cpu(ie->mgmt_subtype_mask);
+
+		if (index == MWIFIEX_AUTO_IDX_MASK) {
+			/* automatic addition */
+			if (mwifiex_ie_get_autoidx(priv, mask, ie, &index))
+				return -1;
+			if (index == MWIFIEX_AUTO_IDX_MASK)
+				return -1;
+
+			tmp = (u8 *)&priv->mgmt_ie[index].ie_buffer;
+			tmp += le16_to_cpu(priv->mgmt_ie[index].ie_length);
+			memcpy(tmp, &ie->ie_buffer, le16_to_cpu(ie->ie_length));
+			le16_add_cpu(&priv->mgmt_ie[index].ie_length,
+				     le16_to_cpu(ie->ie_length));
+			priv->mgmt_ie[index].ie_index = cpu_to_le16(index);
+			priv->mgmt_ie[index].mgmt_subtype_mask =
+							cpu_to_le16(mask);
+
+			ie->ie_index = cpu_to_le16(index);
+			ie->ie_length = priv->mgmt_ie[index].ie_length;
+			memcpy(&ie->ie_buffer, &priv->mgmt_ie[index].ie_buffer,
+			       le16_to_cpu(priv->mgmt_ie[index].ie_length));
+		} else {
+			if (mask != MWIFIEX_DELETE_MASK)
+				return -1;
+			/*
+			 * Check if this index is being used on any
+			 * other interface.
+			 */
+			if (mwifiex_ie_index_used_by_other_intf(priv, index))
+				return -1;
+
+			ie->ie_length = 0;
+			memcpy(&priv->mgmt_ie[index], ie,
+			       sizeof(struct mwifiex_ie));
+		}
+
+		le16_add_cpu(&ie_list->len,
+			     le16_to_cpu(priv->mgmt_ie[index].ie_length) +
+			     MWIFIEX_IE_HDR_SIZE);
+	}
+
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP)
+		return mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+					      HostCmd_ACT_GEN_SET,
+					      UAP_CUSTOM_IE_I, ie_list);
+
+	return 0;
+}
+
+/* Copy individual custom IEs for beacon, probe response and assoc response
+ * and prepare single structure for IE setting.
+ * This function also updates allocated IE indices from driver.
+ */
+static int
+mwifiex_update_uap_custom_ie(struct mwifiex_private *priv,
+			     struct mwifiex_ie *beacon_ie, u16 *beacon_idx,
+			     struct mwifiex_ie *pr_ie, u16 *probe_idx,
+			     struct mwifiex_ie *ar_ie, u16 *assoc_idx)
+{
+	struct mwifiex_ie_list *ap_custom_ie;
+	u8 *pos;
+	u16 len;
+	int ret;
+
+	ap_custom_ie = kzalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+	if (!ap_custom_ie)
+		return -ENOMEM;
+
+	ap_custom_ie->type = cpu_to_le16(TLV_TYPE_MGMT_IE);
+	pos = (u8 *)ap_custom_ie->ie_list;
+
+	if (beacon_ie) {
+		len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(beacon_ie->ie_length);
+		memcpy(pos, beacon_ie, len);
+		pos += len;
+		le16_add_cpu(&ap_custom_ie->len, len);
+	}
+	if (pr_ie) {
+		len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(pr_ie->ie_length);
+		memcpy(pos, pr_ie, len);
+		pos += len;
+		le16_add_cpu(&ap_custom_ie->len, len);
+	}
+	if (ar_ie) {
+		len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(ar_ie->ie_length);
+		memcpy(pos, ar_ie, len);
+		pos += len;
+		le16_add_cpu(&ap_custom_ie->len, len);
+	}
+
+	ret = mwifiex_update_autoindex_ies(priv, ap_custom_ie);
+
+	pos = (u8 *)(&ap_custom_ie->ie_list[0].ie_index);
+	if (beacon_ie && *beacon_idx == MWIFIEX_AUTO_IDX_MASK) {
+		/* save beacon ie index after auto-indexing */
+		*beacon_idx = le16_to_cpu(ap_custom_ie->ie_list[0].ie_index);
+		len = sizeof(*beacon_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(beacon_ie->ie_length);
+		pos += len;
+	}
+	if (pr_ie && le16_to_cpu(pr_ie->ie_index) == MWIFIEX_AUTO_IDX_MASK) {
+		/* save probe resp ie index after auto-indexing */
+		*probe_idx = *((u16 *)pos);
+		len = sizeof(*pr_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(pr_ie->ie_length);
+		pos += len;
+	}
+	if (ar_ie && le16_to_cpu(ar_ie->ie_index) == MWIFIEX_AUTO_IDX_MASK)
+		/* save assoc resp ie index after auto-indexing */
+		*assoc_idx = *((u16 *)pos);
+
+	return ret;
+}
+
+/* This function parses different IEs- Tail IEs, beacon IEs, probe response IEs,
+ * association response IEs from cfg80211_ap_settings function and sets these IE
+ * to FW.
+ */
+int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
+			 struct cfg80211_ap_settings *params)
+{
+	struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL;
+	struct mwifiex_ie *ar_ie = NULL, *rsn_ie = NULL;
+	struct ieee_types_header *ie = NULL;
+	u16 beacon_idx = MWIFIEX_AUTO_IDX_MASK, pr_idx = MWIFIEX_AUTO_IDX_MASK;
+	u16 ar_idx = MWIFIEX_AUTO_IDX_MASK, rsn_idx = MWIFIEX_AUTO_IDX_MASK;
+	u16 mask;
+	int ret = 0;
+
+	if (params->beacon.tail && params->beacon.tail_len) {
+		ie = (void *)cfg80211_find_ie(WLAN_EID_RSN, params->beacon.tail,
+					      params->beacon.tail_len);
+		if (ie) {
+			rsn_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+			if (!rsn_ie)
+				return -ENOMEM;
+
+			rsn_ie->ie_index = cpu_to_le16(rsn_idx);
+			mask = MGMT_MASK_BEACON | MGMT_MASK_PROBE_RESP |
+			       MGMT_MASK_ASSOC_RESP;
+			rsn_ie->mgmt_subtype_mask = cpu_to_le16(mask);
+			rsn_ie->ie_length = cpu_to_le16(ie->len + 2);
+			memcpy(rsn_ie->ie_buffer, ie, ie->len + 2);
+
+			if (mwifiex_update_uap_custom_ie(priv, rsn_ie, &rsn_idx,
+							 NULL, NULL,
+							 NULL, NULL)) {
+				ret = -1;
+				goto done;
+			}
+
+			priv->rsn_idx = rsn_idx;
+		}
+	}
+
+	if (params->beacon.beacon_ies && params->beacon.beacon_ies_len) {
+		beacon_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!beacon_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+
+		beacon_ie->ie_index = cpu_to_le16(beacon_idx);
+		beacon_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON);
+		beacon_ie->ie_length =
+				cpu_to_le16(params->beacon.beacon_ies_len);
+		memcpy(beacon_ie->ie_buffer, params->beacon.beacon_ies,
+		       params->beacon.beacon_ies_len);
+	}
+
+	if (params->beacon.proberesp_ies && params->beacon.proberesp_ies_len) {
+		pr_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!pr_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+
+		pr_ie->ie_index = cpu_to_le16(pr_idx);
+		pr_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_PROBE_RESP);
+		pr_ie->ie_length =
+				cpu_to_le16(params->beacon.proberesp_ies_len);
+		memcpy(pr_ie->ie_buffer, params->beacon.proberesp_ies,
+		       params->beacon.proberesp_ies_len);
+	}
+
+	if (params->beacon.assocresp_ies && params->beacon.assocresp_ies_len) {
+		ar_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!ar_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+
+		ar_ie->ie_index = cpu_to_le16(ar_idx);
+		mask = MGMT_MASK_ASSOC_RESP | MGMT_MASK_REASSOC_RESP;
+		ar_ie->mgmt_subtype_mask = cpu_to_le16(mask);
+		ar_ie->ie_length =
+				cpu_to_le16(params->beacon.assocresp_ies_len);
+		memcpy(ar_ie->ie_buffer, params->beacon.assocresp_ies,
+		       params->beacon.assocresp_ies_len);
+	}
+
+	if (beacon_ie || pr_ie || ar_ie) {
+		ret = mwifiex_update_uap_custom_ie(priv, beacon_ie,
+						   &beacon_idx, pr_ie,
+						   &pr_idx, ar_ie, &ar_idx);
+		if (ret)
+			goto done;
+	}
+
+	priv->beacon_idx = beacon_idx;
+	priv->proberesp_idx = pr_idx;
+	priv->assocresp_idx = ar_idx;
+
+done:
+	kfree(beacon_ie);
+	kfree(pr_ie);
+	kfree(ar_ie);
+	kfree(rsn_ie);
+
+	return ret;
+}
+
+/* This function removes management IE set */
+int mwifiex_del_mgmt_ies(struct mwifiex_private *priv)
+{
+	struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL;
+	struct mwifiex_ie *ar_ie = NULL, *rsn_ie = NULL;
+	int ret = 0;
+
+	if (priv->rsn_idx != MWIFIEX_AUTO_IDX_MASK) {
+		rsn_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!rsn_ie)
+			return -ENOMEM;
+
+		rsn_ie->ie_index = cpu_to_le16(priv->rsn_idx);
+		rsn_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		rsn_ie->ie_length = 0;
+		if (mwifiex_update_uap_custom_ie(priv, rsn_ie, &priv->rsn_idx,
+						 NULL, &priv->proberesp_idx,
+						 NULL, &priv->assocresp_idx)) {
+			ret = -1;
+			goto done;
+		}
+
+		priv->rsn_idx = MWIFIEX_AUTO_IDX_MASK;
+	}
+
+	if (priv->beacon_idx != MWIFIEX_AUTO_IDX_MASK) {
+		beacon_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!beacon_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		beacon_ie->ie_index = cpu_to_le16(priv->beacon_idx);
+		beacon_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		beacon_ie->ie_length = 0;
+	}
+	if (priv->proberesp_idx != MWIFIEX_AUTO_IDX_MASK) {
+		pr_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!pr_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		pr_ie->ie_index = cpu_to_le16(priv->proberesp_idx);
+		pr_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		pr_ie->ie_length = 0;
+	}
+	if (priv->assocresp_idx != MWIFIEX_AUTO_IDX_MASK) {
+		ar_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!ar_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		ar_ie->ie_index = cpu_to_le16(priv->assocresp_idx);
+		ar_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		ar_ie->ie_length = 0;
+	}
+
+	if (beacon_ie || pr_ie || ar_ie)
+		ret = mwifiex_update_uap_custom_ie(priv,
+						   beacon_ie, &priv->beacon_idx,
+						   pr_ie, &priv->proberesp_idx,
+						   ar_ie, &priv->assocresp_idx);
+
+done:
+	kfree(beacon_ie);
+	kfree(pr_ie);
+	kfree(ar_ie);
+	kfree(rsn_ie);
+
+	return ret;
+}
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index d440c3eb640b..c1cb004db913 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -279,6 +279,7 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	memset(&adapter->arp_filter, 0, sizeof(adapter->arp_filter));
 	adapter->arp_filter_size = 0;
 	adapter->channel_type = NL80211_CHAN_HT20;
+	adapter->max_mgmt_ie_index = MAX_MGMT_IE_INDEX;
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index f0f95524e96b..e6be6ee75951 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -62,6 +62,36 @@ enum {
 	BAND_AN = 16,
 };
 
+#define MWIFIEX_WPA_PASSHPHRASE_LEN 64
+struct wpa_param {
+	u8 pairwise_cipher_wpa;
+	u8 pairwise_cipher_wpa2;
+	u8 group_cipher;
+	u32 length;
+	u8 passphrase[MWIFIEX_WPA_PASSHPHRASE_LEN];
+};
+
+#define KEY_MGMT_ON_HOST        0x03
+#define MWIFIEX_AUTH_MODE_AUTO  0xFF
+#define BAND_CONFIG_MANUAL      0x00
+struct mwifiex_uap_bss_param {
+	u8 channel;
+	u8 band_cfg;
+	u16 rts_threshold;
+	u16 frag_threshold;
+	u8 retry_limit;
+	struct mwifiex_802_11_ssid ssid;
+	u8 bcast_ssid_ctl;
+	u8 radio_ctl;
+	u8 dtim_period;
+	u16 beacon_period;
+	u16 auth_mode;
+	u16 protocol;
+	u16 key_mgmt;
+	u16 key_mgmt_operation;
+	struct wpa_param wpa_cfg;
+};
+
 enum {
 	ADHOC_IDLE,
 	ADHOC_STARTED,
@@ -269,6 +299,8 @@ struct mwifiex_ds_read_eeprom {
 
 #define IEEE_MAX_IE_SIZE		256
 
+#define MWIFIEX_IE_HDR_SIZE	(sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE)
+
 struct mwifiex_ds_misc_gen_ie {
 	u32 type;
 	u32 len;
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index 8a390982463e..d6b4fb04011f 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -1374,22 +1374,28 @@ static int mwifiex_deauthenticate_infra(struct mwifiex_private *priv, u8 *mac)
  *
  * In case of infra made, it sends deauthentication request, and
  * in case of ad-hoc mode, a stop network request is sent to the firmware.
+ * In AP mode, a command to stop bss is sent to firmware.
  */
 int mwifiex_deauthenticate(struct mwifiex_private *priv, u8 *mac)
 {
-	int ret = 0;
+	if (!priv->media_connected)
+		return 0;
 
-	if (priv->media_connected) {
-		if (priv->bss_mode == NL80211_IFTYPE_STATION) {
-			ret = mwifiex_deauthenticate_infra(priv, mac);
-		} else if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
-			ret = mwifiex_send_cmd_sync(priv,
-						HostCmd_CMD_802_11_AD_HOC_STOP,
-						HostCmd_ACT_GEN_SET, 0, NULL);
-		}
+	switch (priv->bss_mode) {
+	case NL80211_IFTYPE_STATION:
+		return mwifiex_deauthenticate_infra(priv, mac);
+	case NL80211_IFTYPE_ADHOC:
+		return mwifiex_send_cmd_sync(priv,
+					     HostCmd_CMD_802_11_AD_HOC_STOP,
+					     HostCmd_ACT_GEN_SET, 0, NULL);
+	case NL80211_IFTYPE_AP:
+		return mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
+					     HostCmd_ACT_GEN_SET, 0, NULL);
+	default:
+		break;
 	}
 
-	return ret;
+	return 0;
 }
 EXPORT_SYMBOL_GPL(mwifiex_deauthenticate);
 
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index be0f0e583f75..3192855c31c0 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -64,17 +64,17 @@ static int mwifiex_register(void *card, struct mwifiex_if_ops *if_ops,
 
 	adapter->priv_num = 0;
 
-	/* Allocate memory for private structure */
-	adapter->priv[0] = kzalloc(sizeof(struct mwifiex_private), GFP_KERNEL);
-	if (!adapter->priv[0]) {
-		dev_err(adapter->dev,
-			"%s: failed to alloc priv[0]\n", __func__);
-		goto error;
-	}
-
-	adapter->priv_num++;
+	for (i = 0; i < MWIFIEX_MAX_BSS_NUM; i++) {
+		/* Allocate memory for private structure */
+		adapter->priv[i] =
+			kzalloc(sizeof(struct mwifiex_private), GFP_KERNEL);
+		if (!adapter->priv[i])
+			goto error;
 
-	adapter->priv[0]->adapter = adapter;
+		adapter->priv[i]->adapter = adapter;
+		adapter->priv[i]->bss_priority = i;
+		adapter->priv_num++;
+	}
 	mwifiex_init_lock_list(adapter);
 
 	init_timer(&adapter->cmd_timer);
@@ -349,19 +349,26 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	if (adapter->hw_status != MWIFIEX_HW_STATUS_READY)
 		goto done;
 
-	priv = adapter->priv[0];
-	if (mwifiex_register_cfg80211(priv) != 0) {
+	priv = adapter->priv[MWIFIEX_BSS_ROLE_STA];
+	if (mwifiex_register_cfg80211(adapter)) {
 		dev_err(adapter->dev, "cannot register with cfg80211\n");
 		goto err_init_fw;
 	}
 
 	rtnl_lock();
 	/* Create station interface by default */
-	if (!mwifiex_add_virtual_intf(priv->wdev->wiphy, "mlan%d",
+	if (!mwifiex_add_virtual_intf(adapter->wiphy, "mlan%d",
 				      NL80211_IFTYPE_STATION, NULL, NULL)) {
 		dev_err(adapter->dev, "cannot create default STA interface\n");
 		goto err_add_intf;
 	}
+
+	/* Create AP interface by default */
+	if (!mwifiex_add_virtual_intf(adapter->wiphy, "uap%d",
+				      NL80211_IFTYPE_AP, NULL, NULL)) {
+		dev_err(adapter->dev, "cannot create default AP interface\n");
+		goto err_add_intf;
+	}
 	rtnl_unlock();
 
 	mwifiex_drv_get_driver_version(adapter, fmt, sizeof(fmt) - 1);
@@ -369,7 +376,7 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	goto done;
 
 err_add_intf:
-	mwifiex_del_virtual_intf(priv->wdev->wiphy, priv->netdev);
+	mwifiex_del_virtual_intf(adapter->wiphy, priv->netdev);
 	rtnl_unlock();
 err_init_fw:
 	pr_debug("info: %s: unregister device\n", __func__);
@@ -633,6 +640,12 @@ void mwifiex_init_priv_params(struct mwifiex_private *priv,
 	priv->current_key_index = 0;
 	priv->media_connected = false;
 	memset(&priv->nick_name, 0, sizeof(priv->nick_name));
+	memset(priv->mgmt_ie, 0,
+	       sizeof(struct mwifiex_ie) * MAX_MGMT_IE_INDEX);
+	priv->beacon_idx = MWIFIEX_AUTO_IDX_MASK;
+	priv->proberesp_idx = MWIFIEX_AUTO_IDX_MASK;
+	priv->assocresp_idx = MWIFIEX_AUTO_IDX_MASK;
+	priv->rsn_idx = MWIFIEX_AUTO_IDX_MASK;
 	priv->num_tx_timeout = 0;
 	memcpy(dev->dev_addr, priv->curr_addr, ETH_ALEN);
 }
@@ -830,19 +843,21 @@ int mwifiex_remove_card(struct mwifiex_adapter *adapter, struct semaphore *sem)
 
 		rtnl_lock();
 		if (priv->wdev && priv->netdev)
-			mwifiex_del_virtual_intf(priv->wdev->wiphy,
-						 priv->netdev);
+			mwifiex_del_virtual_intf(adapter->wiphy, priv->netdev);
 		rtnl_unlock();
 	}
 
 	priv = adapter->priv[0];
-	if (!priv)
+	if (!priv || !priv->wdev)
 		goto exit_remove;
 
-	if (priv->wdev) {
-		wiphy_unregister(priv->wdev->wiphy);
-		wiphy_free(priv->wdev->wiphy);
-		kfree(priv->wdev);
+	wiphy_unregister(priv->wdev->wiphy);
+	wiphy_free(priv->wdev->wiphy);
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		priv = adapter->priv[i];
+		if (priv)
+			kfree(priv->wdev);
 	}
 
 	mwifiex_terminate_workqueue(adapter);
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index 324ad390cacd..bd3b0bf94b9e 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -116,6 +116,7 @@ enum {
 #define MAX_FREQUENCY_BAND_BG   2484
 
 #define MWIFIEX_EVENT_HEADER_LEN           4
+#define MWIFIEX_UAP_EVENT_EXTRA_HEADER	   2
 
 #define MWIFIEX_TYPE_LEN			4
 #define MWIFIEX_USB_TYPE_CMD			0xF00DFACE
@@ -370,6 +371,7 @@ struct mwifiex_private {
 	u8 bss_role;
 	u8 bss_priority;
 	u8 bss_num;
+	u8 bss_started;
 	u8 frame_type;
 	u8 curr_addr[ETH_ALEN];
 	u8 media_connected;
@@ -470,12 +472,16 @@ struct mwifiex_private {
 	struct cfg80211_scan_request *scan_request;
 	struct mwifiex_user_scan_cfg *user_scan_cfg;
 	u8 cfg_bssid[6];
-	u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
 	struct wps wps;
 	u8 scan_block;
 	s32 cqm_rssi_thold;
 	u32 cqm_rssi_hyst;
 	u8 subsc_evt_rssi_state;
+	struct mwifiex_ie mgmt_ie[MAX_MGMT_IE_INDEX];
+	u16 beacon_idx;
+	u16 proberesp_idx;
+	u16 assocresp_idx;
+	u16 rsn_idx;
 };
 
 enum mwifiex_ba_status {
@@ -571,6 +577,7 @@ struct mwifiex_adapter {
 	char fw_name[32];
 	int winner;
 	struct device *dev;
+	struct wiphy *wiphy;
 	bool surprise_removed;
 	u32 fw_release_number;
 	u16 init_wait_q_woken;
@@ -677,6 +684,8 @@ struct mwifiex_adapter {
 	struct cmd_ctrl_node *cmd_queued;
 	spinlock_t queue_lock;		/* lock for tx queues */
 	struct completion fw_load;
+	u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
+	u16 max_mgmt_ie_index;
 };
 
 int mwifiex_init_lock_list(struct mwifiex_adapter *adapter);
@@ -760,6 +769,9 @@ int mwifiex_process_rx_packet(struct mwifiex_adapter *adapter,
 int mwifiex_sta_prepare_cmd(struct mwifiex_private *, uint16_t cmd_no,
 			    u16 cmd_action, u32 cmd_oid,
 			    void *data_buf, void *cmd_buf);
+int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
+			    u16 cmd_action, u32 cmd_oid,
+			    void *data_buf, void *cmd_buf);
 int mwifiex_process_sta_cmdresp(struct mwifiex_private *, u16 cmdresp_no,
 				struct host_cmd_ds_command *resp);
 int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *,
@@ -820,6 +832,9 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 int is_command_pending(struct mwifiex_adapter *adapter);
 void mwifiex_init_priv_params(struct mwifiex_private *priv,
 						struct net_device *dev);
+int mwifiex_set_secure_params(struct mwifiex_private *priv,
+			      struct mwifiex_uap_bss_param *bss_config,
+			      struct cfg80211_ap_settings *params);
 
 /*
  * This function checks if the queuing is RA based or not.
@@ -933,7 +948,8 @@ int mwifiex_set_radio(struct mwifiex_private *priv, u8 option);
 int mwifiex_drv_change_adhoc_chan(struct mwifiex_private *priv, u16 channel);
 
 int mwifiex_set_encode(struct mwifiex_private *priv, const u8 *key,
-		       int key_len, u8 key_index, int disable);
+		       int key_len, u8 key_index, const u8 *mac_addr,
+		       int disable);
 
 int mwifiex_set_gen_ie(struct mwifiex_private *priv, u8 *ie, int ie_len);
 
@@ -969,6 +985,7 @@ int mwifiex_set_tx_power(struct mwifiex_private *priv,
 
 int mwifiex_main_process(struct mwifiex_adapter *);
 
+int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel);
 int mwifiex_bss_set_channel(struct mwifiex_private *,
 			    struct mwifiex_chan_freq_power *cfp);
 int mwifiex_get_bss_info(struct mwifiex_private *,
@@ -986,6 +1003,11 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 					u32 *flags, struct vif_params *params);
 int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct net_device *dev);
 
+void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config);
+
+int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
+			 struct cfg80211_ap_settings *params);
+int mwifiex_del_mgmt_ies(struct mwifiex_private *priv);
 u8 *mwifiex_11d_code_2_region(u8 code);
 
 #ifdef CONFIG_DEBUG_FS
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index 87ed2a1f6cd9..40e025da6bc2 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -498,7 +498,8 @@ mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
 {
 	struct host_cmd_ds_802_11_key_material *key_material =
 		&cmd->params.key_material;
-	u16 key_param_len = 0;
+	struct host_cmd_tlv_mac_addr *tlv_mac;
+	u16 key_param_len = 0, cmd_size;
 	int ret = 0;
 	const u8 bc_mac[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
@@ -614,11 +615,26 @@ mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
 			cpu_to_le16((u16) enc_key->key_len +
 				    KEYPARAMSET_FIXED_LEN);
 
-		key_param_len = (u16) (enc_key->key_len + KEYPARAMSET_FIXED_LEN)
+		key_param_len = (u16)(enc_key->key_len + KEYPARAMSET_FIXED_LEN)
 				+ sizeof(struct mwifiex_ie_types_header);
 
 		cmd->size = cpu_to_le16(sizeof(key_material->action) + S_DS_GEN
 					+ key_param_len);
+
+		if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP) {
+			tlv_mac = (void *)((u8 *)&key_material->key_param_set +
+					   key_param_len);
+			tlv_mac->tlv.type = cpu_to_le16(TLV_TYPE_STA_MAC_ADDR);
+			tlv_mac->tlv.len = cpu_to_le16(ETH_ALEN);
+			memcpy(tlv_mac->mac_addr, enc_key->mac_addr, ETH_ALEN);
+			cmd_size = key_param_len + S_DS_GEN +
+				   sizeof(key_material->action) +
+				   sizeof(struct host_cmd_tlv_mac_addr);
+		} else {
+			cmd_size = key_param_len + S_DS_GEN +
+				   sizeof(key_material->action);
+		}
+		cmd->size = cpu_to_le16(cmd_size);
 	}
 
 	return ret;
@@ -1248,13 +1264,15 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 		if (ret)
 			return -1;
 
-		/* Enable IEEE PS by default */
-		priv->adapter->ps_mode = MWIFIEX_802_11_POWER_MODE_PSP;
-		ret = mwifiex_send_cmd_async(priv,
-					     HostCmd_CMD_802_11_PS_MODE_ENH,
-					     EN_AUTO_PS, BITMAP_STA_PS, NULL);
-		if (ret)
-			return -1;
+		if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
+			/* Enable IEEE PS by default */
+			priv->adapter->ps_mode = MWIFIEX_802_11_POWER_MODE_PSP;
+			ret = mwifiex_send_cmd_async(
+					priv, HostCmd_CMD_802_11_PS_MODE_ENH,
+					EN_AUTO_PS, BITMAP_STA_PS, NULL);
+			if (ret)
+				return -1;
+		}
 	}
 
 	/* get tx rate */
@@ -1270,12 +1288,14 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	if (ret)
 		return -1;
 
-	/* set ibss coalescing_status */
-	ret = mwifiex_send_cmd_async(priv,
-				     HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
-				     HostCmd_ACT_GEN_SET, 0, &enable);
-	if (ret)
-		return -1;
+	if (priv->bss_type == MWIFIEX_BSS_TYPE_STA) {
+		/* set ibss coalescing_status */
+		ret = mwifiex_send_cmd_async(
+				priv, HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
+				HostCmd_ACT_GEN_SET, 0, &enable);
+		if (ret)
+			return -1;
+	}
 
 	memset(&amsdu_aggr_ctrl, 0, sizeof(amsdu_aggr_ctrl));
 	amsdu_aggr_ctrl.enable = true;
@@ -1293,7 +1313,8 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	if (ret)
 		return -1;
 
-	if (first_sta && (priv->adapter->iface_type != MWIFIEX_USB)) {
+	if (first_sta && priv->adapter->iface_type != MWIFIEX_USB &&
+	    priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
 		/* Enable auto deep sleep */
 		auto_ds.auto_ds = DEEP_SLEEP_ON;
 		auto_ds.idle_time = DEEP_SLEEP_IDLE_TIME;
@@ -1305,12 +1326,16 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 			return -1;
 	}
 
-	/* Send cmd to FW to enable/disable 11D function */
-	state_11d = ENABLE_11D;
-	ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11_SNMP_MIB,
-				     HostCmd_ACT_GEN_SET, DOT11D_I, &state_11d);
-	if (ret)
-		dev_err(priv->adapter->dev, "11D: failed to enable 11D\n");
+	if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
+		/* Send cmd to FW to enable/disable 11D function */
+		state_11d = ENABLE_11D;
+		ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11_SNMP_MIB,
+					     HostCmd_ACT_GEN_SET, DOT11D_I,
+					     &state_11d);
+		if (ret)
+			dev_err(priv->adapter->dev,
+				"11D: failed to enable 11D\n");
+	}
 
 	/* Send cmd to FW to configure 11n specific configuration
 	 * (Short GI, Channel BW, Green field support etc.) for transmit
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 3aa54243dea9..a79ed9bd9695 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -944,6 +944,14 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 	case HostCmd_CMD_802_11_SUBSCRIBE_EVENT:
 		ret = mwifiex_ret_subsc_evt(priv, resp, data_buf);
 		break;
+	case HostCmd_CMD_UAP_SYS_CONFIG:
+		break;
+	case HostCmd_CMD_UAP_BSS_START:
+		priv->bss_started = 1;
+		break;
+	case HostCmd_CMD_UAP_BSS_STOP:
+		priv->bss_started = 0;
+		break;
 	default:
 		dev_err(adapter->dev, "CMD_RESP: unknown cmd response %#x\n",
 			resp->command);
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index f6bbb9307f86..4ace5a3dcd23 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -184,8 +184,10 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv)
 int mwifiex_process_sta_event(struct mwifiex_private *priv)
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
-	int ret = 0;
+	int len, ret = 0;
 	u32 eventcause = adapter->event_cause;
+	struct station_info sinfo;
+	struct mwifiex_assoc_event *event;
 
 	switch (eventcause) {
 	case EVENT_DUMMY_HOST_WAKEUP_SIGNAL:
@@ -402,6 +404,53 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 	case EVENT_HOSTWAKE_STAIE:
 		dev_dbg(adapter->dev, "event: HOSTWAKE_STAIE %d\n", eventcause);
 		break;
+
+	case EVENT_UAP_STA_ASSOC:
+		skb_pull(adapter->event_skb, MWIFIEX_UAP_EVENT_EXTRA_HEADER);
+		memset(&sinfo, 0, sizeof(sinfo));
+		event = (struct mwifiex_assoc_event *)adapter->event_skb->data;
+		if (le16_to_cpu(event->type) == TLV_TYPE_UAP_MGMT_FRAME) {
+			len = -1;
+
+			if (ieee80211_is_assoc_req(event->frame_control))
+				len = 0;
+			else if (ieee80211_is_reassoc_req(event->frame_control))
+				/* There will be ETH_ALEN bytes of
+				 * current_ap_addr before the re-assoc ies.
+				 */
+				len = ETH_ALEN;
+
+			if (len != -1) {
+				sinfo.filled = STATION_INFO_ASSOC_REQ_IES;
+				sinfo.assoc_req_ies = (u8 *)&event->data[len];
+				len = (u8 *)sinfo.assoc_req_ies -
+				      (u8 *)&event->frame_control;
+				sinfo.assoc_req_ies_len =
+					le16_to_cpu(event->len) - (u16)len;
+			}
+		}
+		cfg80211_new_sta(priv->netdev, event->sta_addr, &sinfo,
+				 GFP_KERNEL);
+		break;
+	case EVENT_UAP_STA_DEAUTH:
+		skb_pull(adapter->event_skb, MWIFIEX_UAP_EVENT_EXTRA_HEADER);
+		cfg80211_del_sta(priv->netdev, adapter->event_skb->data,
+				 GFP_KERNEL);
+		break;
+	case EVENT_UAP_BSS_IDLE:
+		priv->media_connected = false;
+		break;
+	case EVENT_UAP_BSS_ACTIVE:
+		priv->media_connected = true;
+		break;
+	case EVENT_UAP_BSS_START:
+		dev_dbg(adapter->dev, "AP EVENT: event id: %#x\n", eventcause);
+		memcpy(priv->netdev->dev_addr, adapter->event_body+2, ETH_ALEN);
+		break;
+	case EVENT_UAP_MIC_COUNTERMEASURES:
+		/* For future development */
+		dev_dbg(adapter->dev, "AP EVENT: event id: %#x\n", eventcause);
+		break;
 	default:
 		dev_dbg(adapter->dev, "event: unknown event id: %#x\n",
 			eventcause);
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index 58970e0f7d13..106c449477b2 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -462,7 +462,7 @@ int mwifiex_get_bss_info(struct mwifiex_private *priv,
 
 	info->bss_chan = bss_desc->channel;
 
-	memcpy(info->country_code, priv->country_code,
+	memcpy(info->country_code, adapter->country_code,
 	       IEEE80211_COUNTRY_STRING_LEN);
 
 	info->media_connected = priv->media_connected;
@@ -1219,7 +1219,8 @@ mwifiex_drv_get_driver_version(struct mwifiex_adapter *adapter, char *version,
  * with requisite parameters and calls the IOCTL handler.
  */
 int mwifiex_set_encode(struct mwifiex_private *priv, const u8 *key,
-			int key_len, u8 key_index, int disable)
+			int key_len, u8 key_index,
+			const u8 *mac_addr, int disable)
 {
 	struct mwifiex_ds_encrypt_key encrypt_key;
 
@@ -1229,8 +1230,12 @@ int mwifiex_set_encode(struct mwifiex_private *priv, const u8 *key,
 		encrypt_key.key_index = key_index;
 		if (key_len)
 			memcpy(encrypt_key.key_material, key, key_len);
+		if (mac_addr)
+			memcpy(encrypt_key.mac_addr, mac_addr, ETH_ALEN);
 	} else {
 		encrypt_key.key_disable = true;
+		if (mac_addr)
+			memcpy(encrypt_key.mac_addr, mac_addr, ETH_ALEN);
 	}
 
 	return mwifiex_sec_ioctl_encrypt_key(priv, &encrypt_key);
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
new file mode 100644
index 000000000000..76dfbc42a732
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -0,0 +1,432 @@
+/*
+ * Marvell Wireless LAN device driver: AP specific command handling
+ *
+ * Copyright (C) 2012, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+#include "main.h"
+
+/* This function parses security related parameters from cfg80211_ap_settings
+ * and sets into FW understandable bss_config structure.
+ */
+int mwifiex_set_secure_params(struct mwifiex_private *priv,
+			      struct mwifiex_uap_bss_param *bss_config,
+			      struct cfg80211_ap_settings *params) {
+	int i;
+
+	switch (params->auth_type) {
+	case NL80211_AUTHTYPE_OPEN_SYSTEM:
+		bss_config->auth_mode = WLAN_AUTH_OPEN;
+		break;
+	case NL80211_AUTHTYPE_SHARED_KEY:
+		bss_config->auth_mode = WLAN_AUTH_SHARED_KEY;
+		break;
+	case NL80211_AUTHTYPE_NETWORK_EAP:
+		bss_config->auth_mode = WLAN_AUTH_LEAP;
+		break;
+	default:
+		bss_config->auth_mode = MWIFIEX_AUTH_MODE_AUTO;
+		break;
+	}
+
+	bss_config->key_mgmt_operation |= KEY_MGMT_ON_HOST;
+
+	for (i = 0; i < params->crypto.n_akm_suites; i++) {
+		switch (params->crypto.akm_suites[i]) {
+		case WLAN_AKM_SUITE_8021X:
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_1) {
+				bss_config->protocol = PROTOCOL_WPA;
+				bss_config->key_mgmt = KEY_MGMT_EAP;
+			}
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_2) {
+				bss_config->protocol = PROTOCOL_WPA2;
+				bss_config->key_mgmt = KEY_MGMT_EAP;
+			}
+			break;
+		case WLAN_AKM_SUITE_PSK:
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_1) {
+				bss_config->protocol = PROTOCOL_WPA;
+				bss_config->key_mgmt = KEY_MGMT_PSK;
+			}
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_2) {
+				bss_config->protocol = PROTOCOL_WPA2;
+				bss_config->key_mgmt = KEY_MGMT_PSK;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	for (i = 0; i < params->crypto.n_ciphers_pairwise; i++) {
+		switch (params->crypto.ciphers_pairwise[i]) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			bss_config->wpa_cfg.pairwise_cipher_wpa = CIPHER_TKIP;
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			bss_config->wpa_cfg.pairwise_cipher_wpa2 =
+								CIPHER_AES_CCMP;
+		default:
+			break;
+		}
+	}
+
+	switch (params->crypto.cipher_group) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		bss_config->wpa_cfg.group_cipher = CIPHER_TKIP;
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		bss_config->wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/* This function initializes some of mwifiex_uap_bss_param variables.
+ * This helps FW in ignoring invalid values. These values may or may not
+ * be get updated to valid ones at later stage.
+ */
+void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config)
+{
+	config->bcast_ssid_ctl = 0x7F;
+	config->radio_ctl = 0x7F;
+	config->dtim_period = 0x7F;
+	config->beacon_period = 0x7FFF;
+	config->auth_mode = 0x7F;
+	config->rts_threshold = 0x7FFF;
+	config->frag_threshold = 0x7FFF;
+	config->retry_limit = 0x7F;
+}
+
+/* This function parses BSS related parameters from structure
+ * and prepares TLVs. These TLVs are appended to command buffer.
+*/
+static int
+mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
+{
+	struct host_cmd_tlv_dtim_period *dtim_period;
+	struct host_cmd_tlv_beacon_period *beacon_period;
+	struct host_cmd_tlv_ssid *ssid;
+	struct host_cmd_tlv_channel_band *chan_band;
+	struct host_cmd_tlv_frag_threshold *frag_threshold;
+	struct host_cmd_tlv_rts_threshold *rts_threshold;
+	struct host_cmd_tlv_retry_limit *retry_limit;
+	struct host_cmd_tlv_pwk_cipher *pwk_cipher;
+	struct host_cmd_tlv_gwk_cipher *gwk_cipher;
+	struct host_cmd_tlv_encrypt_protocol *encrypt_protocol;
+	struct host_cmd_tlv_auth_type *auth_type;
+	struct host_cmd_tlv_passphrase *passphrase;
+	struct host_cmd_tlv_akmp *tlv_akmp;
+	struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
+	u16 cmd_size = *param_size;
+
+	if (bss_cfg->ssid.ssid_len) {
+		ssid = (struct host_cmd_tlv_ssid *)tlv;
+		ssid->tlv.type = cpu_to_le16(TLV_TYPE_UAP_SSID);
+		ssid->tlv.len = cpu_to_le16((u16)bss_cfg->ssid.ssid_len);
+		memcpy(ssid->ssid, bss_cfg->ssid.ssid, bss_cfg->ssid.ssid_len);
+		cmd_size += sizeof(struct host_cmd_tlv) +
+			    bss_cfg->ssid.ssid_len;
+		tlv += sizeof(struct host_cmd_tlv) + bss_cfg->ssid.ssid_len;
+	}
+	if (bss_cfg->channel && bss_cfg->channel <= MAX_CHANNEL_BAND_BG) {
+		chan_band = (struct host_cmd_tlv_channel_band *)tlv;
+		chan_band->tlv.type = cpu_to_le16(TLV_TYPE_CHANNELBANDLIST);
+		chan_band->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_channel_band) -
+				    sizeof(struct host_cmd_tlv));
+		chan_band->band_config = bss_cfg->band_cfg;
+		chan_band->channel = bss_cfg->channel;
+		cmd_size += sizeof(struct host_cmd_tlv_channel_band);
+		tlv += sizeof(struct host_cmd_tlv_channel_band);
+	}
+	if (bss_cfg->beacon_period >= MIN_BEACON_PERIOD &&
+	    bss_cfg->beacon_period <= MAX_BEACON_PERIOD) {
+		beacon_period = (struct host_cmd_tlv_beacon_period *)tlv;
+		beacon_period->tlv.type =
+					cpu_to_le16(TLV_TYPE_UAP_BEACON_PERIOD);
+		beacon_period->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_beacon_period) -
+				    sizeof(struct host_cmd_tlv));
+		beacon_period->period = cpu_to_le16(bss_cfg->beacon_period);
+		cmd_size += sizeof(struct host_cmd_tlv_beacon_period);
+		tlv += sizeof(struct host_cmd_tlv_beacon_period);
+	}
+	if (bss_cfg->dtim_period >= MIN_DTIM_PERIOD &&
+	    bss_cfg->dtim_period <= MAX_DTIM_PERIOD) {
+		dtim_period = (struct host_cmd_tlv_dtim_period *)tlv;
+		dtim_period->tlv.type = cpu_to_le16(TLV_TYPE_UAP_DTIM_PERIOD);
+		dtim_period->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_dtim_period) -
+				    sizeof(struct host_cmd_tlv));
+		dtim_period->period = bss_cfg->dtim_period;
+		cmd_size += sizeof(struct host_cmd_tlv_dtim_period);
+		tlv += sizeof(struct host_cmd_tlv_dtim_period);
+	}
+	if (bss_cfg->rts_threshold <= MWIFIEX_RTS_MAX_VALUE) {
+		rts_threshold = (struct host_cmd_tlv_rts_threshold *)tlv;
+		rts_threshold->tlv.type =
+					cpu_to_le16(TLV_TYPE_UAP_RTS_THRESHOLD);
+		rts_threshold->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_rts_threshold) -
+				    sizeof(struct host_cmd_tlv));
+		rts_threshold->rts_thr = cpu_to_le16(bss_cfg->rts_threshold);
+		cmd_size += sizeof(struct host_cmd_tlv_frag_threshold);
+		tlv += sizeof(struct host_cmd_tlv_frag_threshold);
+	}
+	if ((bss_cfg->frag_threshold >= MWIFIEX_FRAG_MIN_VALUE) &&
+	    (bss_cfg->frag_threshold <= MWIFIEX_FRAG_MAX_VALUE)) {
+		frag_threshold = (struct host_cmd_tlv_frag_threshold *)tlv;
+		frag_threshold->tlv.type =
+				cpu_to_le16(TLV_TYPE_UAP_FRAG_THRESHOLD);
+		frag_threshold->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_frag_threshold) -
+				    sizeof(struct host_cmd_tlv));
+		frag_threshold->frag_thr = cpu_to_le16(bss_cfg->frag_threshold);
+		cmd_size += sizeof(struct host_cmd_tlv_frag_threshold);
+		tlv += sizeof(struct host_cmd_tlv_frag_threshold);
+	}
+	if (bss_cfg->retry_limit <= MWIFIEX_RETRY_LIMIT) {
+		retry_limit = (struct host_cmd_tlv_retry_limit *)tlv;
+		retry_limit->tlv.type = cpu_to_le16(TLV_TYPE_UAP_RETRY_LIMIT);
+		retry_limit->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_retry_limit) -
+				    sizeof(struct host_cmd_tlv));
+		retry_limit->limit = (u8)bss_cfg->retry_limit;
+		cmd_size += sizeof(struct host_cmd_tlv_retry_limit);
+		tlv += sizeof(struct host_cmd_tlv_retry_limit);
+	}
+	if ((bss_cfg->protocol & PROTOCOL_WPA) ||
+	    (bss_cfg->protocol & PROTOCOL_WPA2) ||
+	    (bss_cfg->protocol & PROTOCOL_EAP)) {
+		tlv_akmp = (struct host_cmd_tlv_akmp *)tlv;
+		tlv_akmp->tlv.type = cpu_to_le16(TLV_TYPE_UAP_AKMP);
+		tlv_akmp->tlv.len =
+		    cpu_to_le16(sizeof(struct host_cmd_tlv_akmp) -
+				sizeof(struct host_cmd_tlv));
+		tlv_akmp->key_mgmt_operation =
+			cpu_to_le16(bss_cfg->key_mgmt_operation);
+		tlv_akmp->key_mgmt = cpu_to_le16(bss_cfg->key_mgmt);
+		cmd_size += sizeof(struct host_cmd_tlv_akmp);
+		tlv += sizeof(struct host_cmd_tlv_akmp);
+
+		if (bss_cfg->wpa_cfg.pairwise_cipher_wpa &
+				VALID_CIPHER_BITMAP) {
+			pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+			pwk_cipher->tlv.type =
+				cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+			pwk_cipher->tlv.len = cpu_to_le16(
+				sizeof(struct host_cmd_tlv_pwk_cipher) -
+				sizeof(struct host_cmd_tlv));
+			pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA);
+			pwk_cipher->cipher =
+				bss_cfg->wpa_cfg.pairwise_cipher_wpa;
+			cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+			tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+		}
+		if (bss_cfg->wpa_cfg.pairwise_cipher_wpa2 &
+				VALID_CIPHER_BITMAP) {
+			pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+			pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+			pwk_cipher->tlv.len = cpu_to_le16(
+				sizeof(struct host_cmd_tlv_pwk_cipher) -
+				sizeof(struct host_cmd_tlv));
+			pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA2);
+			pwk_cipher->cipher =
+				bss_cfg->wpa_cfg.pairwise_cipher_wpa2;
+			cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+			tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+		}
+		if (bss_cfg->wpa_cfg.group_cipher & VALID_CIPHER_BITMAP) {
+			gwk_cipher = (struct host_cmd_tlv_gwk_cipher *)tlv;
+			gwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_GWK_CIPHER);
+			gwk_cipher->tlv.len = cpu_to_le16(
+				sizeof(struct host_cmd_tlv_gwk_cipher) -
+				sizeof(struct host_cmd_tlv));
+			gwk_cipher->cipher = bss_cfg->wpa_cfg.group_cipher;
+			cmd_size += sizeof(struct host_cmd_tlv_gwk_cipher);
+			tlv += sizeof(struct host_cmd_tlv_gwk_cipher);
+		}
+		if (bss_cfg->wpa_cfg.length) {
+			passphrase = (struct host_cmd_tlv_passphrase *)tlv;
+			passphrase->tlv.type =
+				cpu_to_le16(TLV_TYPE_UAP_WPA_PASSPHRASE);
+			passphrase->tlv.len =
+				cpu_to_le16(bss_cfg->wpa_cfg.length);
+			memcpy(passphrase->passphrase,
+			       bss_cfg->wpa_cfg.passphrase,
+			       bss_cfg->wpa_cfg.length);
+			cmd_size += sizeof(struct host_cmd_tlv) +
+				    bss_cfg->wpa_cfg.length;
+			tlv += sizeof(struct host_cmd_tlv) +
+			       bss_cfg->wpa_cfg.length;
+		}
+	}
+	if ((bss_cfg->auth_mode <= WLAN_AUTH_SHARED_KEY) ||
+	    (bss_cfg->auth_mode == MWIFIEX_AUTH_MODE_AUTO)) {
+		auth_type = (struct host_cmd_tlv_auth_type *)tlv;
+		auth_type->tlv.type = cpu_to_le16(TLV_TYPE_AUTH_TYPE);
+		auth_type->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_auth_type) -
+			sizeof(struct host_cmd_tlv));
+		auth_type->auth_type = (u8)bss_cfg->auth_mode;
+		cmd_size += sizeof(struct host_cmd_tlv_auth_type);
+		tlv += sizeof(struct host_cmd_tlv_auth_type);
+	}
+	if (bss_cfg->protocol) {
+		encrypt_protocol = (struct host_cmd_tlv_encrypt_protocol *)tlv;
+		encrypt_protocol->tlv.type =
+			cpu_to_le16(TLV_TYPE_UAP_ENCRY_PROTOCOL);
+		encrypt_protocol->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_encrypt_protocol)
+			- sizeof(struct host_cmd_tlv));
+		encrypt_protocol->proto = cpu_to_le16(bss_cfg->protocol);
+		cmd_size += sizeof(struct host_cmd_tlv_encrypt_protocol);
+		tlv += sizeof(struct host_cmd_tlv_encrypt_protocol);
+	}
+
+	*param_size = cmd_size;
+
+	return 0;
+}
+
+/* This function parses custom IEs from IE list and prepares command buffer */
+static int mwifiex_uap_custom_ie_prepare(u8 *tlv, void *cmd_buf, u16 *ie_size)
+{
+	struct mwifiex_ie_list *ap_ie = cmd_buf;
+	struct host_cmd_tlv *tlv_ie = (struct host_cmd_tlv *)tlv;
+
+	if (!ap_ie || !ap_ie->len || !ap_ie->ie_list)
+		return -1;
+
+	*ie_size += le16_to_cpu(ap_ie->len) + sizeof(struct host_cmd_tlv);
+
+	tlv_ie->type = cpu_to_le16(TLV_TYPE_MGMT_IE);
+	tlv_ie->len = ap_ie->len;
+	tlv += sizeof(struct host_cmd_tlv);
+
+	memcpy(tlv, ap_ie->ie_list, le16_to_cpu(ap_ie->len));
+
+	return 0;
+}
+
+/* Parse AP config structure and prepare TLV based command structure
+ * to be sent to FW for uAP configuration
+ */
+static int
+mwifiex_cmd_uap_sys_config(struct host_cmd_ds_command *cmd, u16 cmd_action,
+			   u32 type, void *cmd_buf)
+{
+	u8 *tlv;
+	u16 cmd_size, param_size, ie_size;
+	struct host_cmd_ds_sys_config *sys_cfg;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_UAP_SYS_CONFIG);
+	cmd_size = (u16)(sizeof(struct host_cmd_ds_sys_config) + S_DS_GEN);
+	sys_cfg = (struct host_cmd_ds_sys_config *)&cmd->params.uap_sys_config;
+	sys_cfg->action = cpu_to_le16(cmd_action);
+	tlv = sys_cfg->tlv;
+
+	switch (type) {
+	case UAP_BSS_PARAMS_I:
+		param_size = cmd_size;
+		if (mwifiex_uap_bss_param_prepare(tlv, cmd_buf, &param_size))
+			return -1;
+		cmd->size = cpu_to_le16(param_size);
+		break;
+	case UAP_CUSTOM_IE_I:
+		ie_size = cmd_size;
+		if (mwifiex_uap_custom_ie_prepare(tlv, cmd_buf, &ie_size))
+			return -1;
+		cmd->size = cpu_to_le16(ie_size);
+		break;
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+/* This function prepares the AP specific commands before sending them
+ * to the firmware.
+ * This is a generic function which calls specific command preparation
+ * routines based upon the command number.
+ */
+int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, u16 cmd_no,
+			    u16 cmd_action, u32 type,
+			    void *data_buf, void *cmd_buf)
+{
+	struct host_cmd_ds_command *cmd = cmd_buf;
+
+	switch (cmd_no) {
+	case HostCmd_CMD_UAP_SYS_CONFIG:
+		if (mwifiex_cmd_uap_sys_config(cmd, cmd_action, type, data_buf))
+			return -1;
+		break;
+	case HostCmd_CMD_UAP_BSS_START:
+	case HostCmd_CMD_UAP_BSS_STOP:
+		cmd->command = cpu_to_le16(cmd_no);
+		cmd->size = cpu_to_le16(S_DS_GEN);
+		break;
+	default:
+		dev_err(priv->adapter->dev,
+			"PREP_CMD: unknown cmd %#x\n", cmd_no);
+		return -1;
+	}
+
+	return 0;
+}
+
+/* This function sets the RF channel for AP.
+ *
+ * This function populates channel information in AP config structure
+ * and sends command to configure channel information in AP.
+ */
+int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel)
+{
+	struct mwifiex_uap_bss_param *bss_cfg;
+	struct wiphy *wiphy = priv->wdev->wiphy;
+
+	bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param), GFP_KERNEL);
+	if (!bss_cfg)
+		return -ENOMEM;
+
+	bss_cfg->band_cfg = BAND_CONFIG_MANUAL;
+	bss_cfg->channel = channel;
+
+	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+				   HostCmd_ACT_GEN_SET,
+				   UAP_BSS_PARAMS_I, bss_cfg)) {
+		wiphy_err(wiphy, "Failed to set the uAP channel\n");
+		kfree(bss_cfg);
+		return -1;
+	}
+
+	kfree(bss_cfg);
+	return 0;
+}
diff --git a/drivers/net/wireless/mwifiex/wmm.c b/drivers/net/wireless/mwifiex/wmm.c
index 429a1dee2d26..f3fc65515857 100644
--- a/drivers/net/wireless/mwifiex/wmm.c
+++ b/drivers/net/wireless/mwifiex/wmm.c
@@ -885,6 +885,10 @@ mwifiex_wmm_get_highest_priolist_ptr(struct mwifiex_adapter *adapter,
 				tid_ptr = &(priv_tmp)->wmm.
 					tid_tbl_ptr[tos_to_tid[i]];
 
+				/* For non-STA ra_list_curr may be NULL */
+				if (!tid_ptr->ra_list_curr)
+					continue;
+
 				spin_lock_irqsave(&tid_ptr->tid_tbl_lock,
 						  flags);
 				is_list_empty =
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index c5404eb82b2f..2e9e6af21362 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -505,9 +505,6 @@ static int rndis_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 
 static int rndis_leave_ibss(struct wiphy *wiphy, struct net_device *dev);
 
-static int rndis_set_channel(struct wiphy *wiphy, struct net_device *dev,
-	struct ieee80211_channel *chan, enum nl80211_channel_type channel_type);
-
 static int rndis_add_key(struct wiphy *wiphy, struct net_device *netdev,
 			 u8 key_index, bool pairwise, const u8 *mac_addr,
 			 struct key_params *params);
@@ -549,7 +546,6 @@ static const struct cfg80211_ops rndis_config_ops = {
 	.disconnect = rndis_disconnect,
 	.join_ibss = rndis_join_ibss,
 	.leave_ibss = rndis_leave_ibss,
-	.set_channel = rndis_set_channel,
 	.add_key = rndis_add_key,
 	.del_key = rndis_del_key,
 	.set_default_key = rndis_set_default_key,
@@ -2398,16 +2394,6 @@ static int rndis_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 	return deauthenticate(usbdev);
 }
 
-static int rndis_set_channel(struct wiphy *wiphy, struct net_device *netdev,
-	struct ieee80211_channel *chan, enum nl80211_channel_type channel_type)
-{
-	struct rndis_wlan_private *priv = wiphy_priv(wiphy);
-	struct usbnet *usbdev = priv->usbdev;
-
-	return set_channel(usbdev,
-			ieee80211_frequency_to_channel(chan->center_freq));
-}
-
 static int rndis_add_key(struct wiphy *wiphy, struct net_device *netdev,
 			 u8 key_index, bool pairwise, const u8 *mac_addr,
 			 struct key_params *params)
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index 931331d95217..cad25bfebd7a 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -1192,6 +1192,7 @@ static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
 	{ PCI_DEVICE(0x1814, 0x5390) },
 	{ PCI_DEVICE(0x1814, 0x5392) },
 	{ PCI_DEVICE(0x1814, 0x539a) },
+	{ PCI_DEVICE(0x1814, 0x539b) },
 	{ PCI_DEVICE(0x1814, 0x539f) },
 #endif
 	{ 0, }
diff --git a/drivers/net/wireless/ti/wl12xx/Kconfig b/drivers/net/wireless/ti/wl12xx/Kconfig
index 5b92329122c4..c2183594655a 100644
--- a/drivers/net/wireless/ti/wl12xx/Kconfig
+++ b/drivers/net/wireless/ti/wl12xx/Kconfig
@@ -1,5 +1,6 @@
 config WL12XX
        tristate "TI wl12xx support"
+	depends on MAC80211
        select WLCORE
        ---help---
 	  This module adds support for wireless adapters based on TI wl1271,
diff --git a/drivers/net/wireless/ti/wlcore/Kconfig b/drivers/net/wireless/ti/wlcore/Kconfig
index 9d04c38938bc..54156b0b5c2d 100644
--- a/drivers/net/wireless/ti/wlcore/Kconfig
+++ b/drivers/net/wireless/ti/wlcore/Kconfig
@@ -1,6 +1,6 @@
 config WLCORE
 	tristate "TI wlcore support"
-	depends on WL_TI && GENERIC_HARDIRQS
+	depends on WL_TI && GENERIC_HARDIRQS && MAC80211
 	depends on INET
 	select FW_LOADER
 	---help---
diff --git a/drivers/net/wireless/ti/wlcore/acx.c b/drivers/net/wireless/ti/wlcore/acx.c
index 5912541a925e..509aa881d790 100644
--- a/drivers/net/wireless/ti/wlcore/acx.c
+++ b/drivers/net/wireless/ti/wlcore/acx.c
@@ -1714,3 +1714,83 @@ out:
 	return ret;
 
 }
+
+/* Set the global behaviour of RX filters - On/Off + default action */
+int wl1271_acx_default_rx_filter_enable(struct wl1271 *wl, bool enable,
+					enum rx_filter_action action)
+{
+	struct acx_default_rx_filter *acx;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx default rx filter en: %d act: %d",
+		     enable, action);
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx)
+		return -ENOMEM;
+
+	acx->enable = enable;
+	acx->default_action = action;
+
+	ret = wl1271_cmd_configure(wl, ACX_ENABLE_RX_DATA_FILTER, acx,
+				   sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx default rx filter enable failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
+/* Configure or disable a specific RX filter pattern */
+int wl1271_acx_set_rx_filter(struct wl1271 *wl, u8 index, bool enable,
+			     struct wl12xx_rx_filter *filter)
+{
+	struct acx_rx_filter_cfg *acx;
+	int fields_size = 0;
+	int acx_size;
+	int ret;
+
+	WARN_ON(enable && !filter);
+	WARN_ON(index >= WL1271_MAX_RX_FILTERS);
+
+	wl1271_debug(DEBUG_ACX,
+		     "acx set rx filter idx: %d enable: %d filter: %p",
+		     index, enable, filter);
+
+	if (enable) {
+		fields_size = wl1271_rx_filter_get_fields_size(filter);
+
+		wl1271_debug(DEBUG_ACX, "act: %d num_fields: %d field_size: %d",
+		      filter->action, filter->num_fields, fields_size);
+	}
+
+	acx_size = ALIGN(sizeof(*acx) + fields_size, 4);
+	acx = kzalloc(acx_size, GFP_KERNEL);
+
+	if (!acx)
+		return -ENOMEM;
+
+	acx->enable = enable;
+	acx->index = index;
+
+	if (enable) {
+		acx->num_fields = filter->num_fields;
+		acx->action = filter->action;
+		wl1271_rx_filter_flatten_fields(filter, acx->fields);
+	}
+
+	wl1271_dump(DEBUG_ACX, "RX_FILTER: ", acx, acx_size);
+
+	ret = wl1271_cmd_configure(wl, ACX_SET_RX_DATA_FILTER, acx, acx_size);
+	if (ret < 0) {
+		wl1271_warning("setting rx filter failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wlcore/acx.h b/drivers/net/wireless/ti/wlcore/acx.h
index b2f88831b7a9..8106b2ebfe60 100644
--- a/drivers/net/wireless/ti/wlcore/acx.h
+++ b/drivers/net/wireless/ti/wlcore/acx.h
@@ -1147,6 +1147,32 @@ struct wl12xx_acx_config_hangover {
 	u8 padding[2];
 } __packed;
 
+
+struct acx_default_rx_filter {
+	struct acx_header header;
+	u8 enable;
+
+	/* action of type FILTER_XXX */
+	u8 default_action;
+
+	u8 pad[2];
+} __packed;
+
+
+struct acx_rx_filter_cfg {
+	struct acx_header header;
+
+	u8 enable;
+
+	/* 0 - WL1271_MAX_RX_FILTERS-1 */
+	u8 index;
+
+	u8 action;
+
+	u8 num_fields;
+	u8 fields[0];
+} __packed;
+
 enum {
 	ACX_WAKE_UP_CONDITIONS           = 0x0000,
 	ACX_MEM_CFG                      = 0x0001,
@@ -1304,5 +1330,9 @@ int wl1271_acx_set_inconnection_sta(struct wl1271 *wl, u8 *addr);
 int wl1271_acx_fm_coex(struct wl1271 *wl);
 int wl12xx_acx_set_rate_mgmt_params(struct wl1271 *wl);
 int wl12xx_acx_config_hangover(struct wl1271 *wl);
+int wl1271_acx_default_rx_filter_enable(struct wl1271 *wl, bool enable,
+					enum rx_filter_action action);
+int wl1271_acx_set_rx_filter(struct wl1271 *wl, u8 index, bool enable,
+			     struct wl12xx_rx_filter *filter);
 
 #endif /* __WL1271_ACX_H__ */
diff --git a/drivers/net/wireless/ti/wlcore/boot.c b/drivers/net/wireless/ti/wlcore/boot.c
index 3a2207db5405..9b98230f84ce 100644
--- a/drivers/net/wireless/ti/wlcore/boot.c
+++ b/drivers/net/wireless/ti/wlcore/boot.c
@@ -72,7 +72,7 @@ static int wlcore_boot_fw_version(struct wl1271 *wl)
 	struct wl1271_static_data *static_data;
 	int ret;
 
-	static_data = kmalloc(sizeof(*static_data), GFP_DMA);
+	static_data = kmalloc(sizeof(*static_data), GFP_KERNEL | GFP_DMA);
 	if (!static_data) {
 		wl1271_error("Couldn't allocate memory for static data!");
 		return -ENOMEM;
@@ -413,6 +413,7 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
 
 	/* unmask required mbox events  */
 	wl->event_mask = BSS_LOSE_EVENT_ID |
+		REGAINED_BSS_EVENT_ID |
 		SCAN_COMPLETE_EVENT_ID |
 		ROLE_STOP_COMPLETE_EVENT_ID |
 		RSSI_SNR_TRIGGER_0_EVENT_ID |
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index 5c4716c6f040..5b128a971449 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -123,7 +123,9 @@ static int wl1271_cmd_wait_for_event_or_timeout(struct wl1271 *wl, u32 mask)
 	unsigned long timeout;
 	int ret = 0;
 
-	events_vector = kmalloc(sizeof(*events_vector), GFP_DMA);
+	events_vector = kmalloc(sizeof(*events_vector), GFP_KERNEL | GFP_DMA);
+	if (!events_vector)
+		return -ENOMEM;
 
 	timeout = jiffies + msecs_to_jiffies(WL1271_EVENT_TIMEOUT);
 
@@ -1034,7 +1036,7 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	skb_reserve(skb, sizeof(*hdr) + WL1271_EXTRA_SPACE_MAX);
 
 	tmpl = (struct wl12xx_arp_rsp_template *)skb_put(skb, sizeof(*tmpl));
-	memset(tmpl, 0, sizeof(tmpl));
+	memset(tmpl, 0, sizeof(*tmpl));
 
 	/* llc layer */
 	memcpy(tmpl->llc_hdr, rfc1042_header, sizeof(rfc1042_header));
@@ -1083,7 +1085,7 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	/* mac80211 header */
 	hdr = (struct ieee80211_hdr_3addr *)skb_push(skb, sizeof(*hdr));
-	memset(hdr, 0, sizeof(hdr));
+	memset(hdr, 0, sizeof(*hdr));
 	fc = IEEE80211_FTYPE_DATA | IEEE80211_FCTL_TODS;
 	if (wlvif->sta.qos)
 		fc |= IEEE80211_STYPE_QOS_DATA;
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index 292632ddf890..28e2a633c3be 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -103,7 +103,6 @@ static int wl1271_event_process(struct wl1271 *wl)
 	struct ieee80211_vif *vif;
 	struct wl12xx_vif *wlvif;
 	u32 vector;
-	bool beacon_loss = false;
 	bool disconnect_sta = false;
 	unsigned long sta_bitmap = 0;
 
@@ -141,20 +140,23 @@ static int wl1271_event_process(struct wl1271 *wl)
 					       mbox->soft_gemini_sense_info);
 
 	/*
-	 * The BSS_LOSE_EVENT_ID is only needed while psm (and hence beacon
-	 * filtering) is enabled. Without PSM, the stack will receive all
-	 * beacons and can detect beacon loss by itself.
-	 *
-	 * As there's possibility that the driver disables PSM before receiving
-	 * BSS_LOSE_EVENT, beacon loss has to be reported to the stack.
-	 *
+	 * We are HW_MONITOR device. On beacon loss - queue
+	 * connection loss work. Cancel it on REGAINED event.
 	 */
 	if (vector & BSS_LOSE_EVENT_ID) {
 		/* TODO: check for multi-role */
+		int delay = wl->conf.conn.synch_fail_thold *
+					wl->conf.conn.bss_lose_timeout;
 		wl1271_info("Beacon loss detected.");
+		cancel_delayed_work_sync(&wl->connection_loss_work);
+		ieee80211_queue_delayed_work(wl->hw, &wl->connection_loss_work,
+		      msecs_to_jiffies(delay));
+	}
 
-		/* indicate to the stack, that beacons have been lost */
-		beacon_loss = true;
+	if (vector & REGAINED_BSS_EVENT_ID) {
+		/* TODO: check for multi-role */
+		wl1271_info("Beacon regained.");
+		cancel_delayed_work_sync(&wl->connection_loss_work);
 	}
 
 	if (vector & RSSI_SNR_TRIGGER_0_EVENT_ID) {
@@ -257,13 +259,6 @@ static int wl1271_event_process(struct wl1271 *wl)
 			rcu_read_unlock();
 		}
 	}
-
-	if (beacon_loss)
-		wl12xx_for_each_wlvif_sta(wl, wlvif) {
-			vif = wl12xx_wlvif_to_vif(wlvif);
-			ieee80211_connection_loss(vif);
-		}
-
 	return 0;
 }
 
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index 2b0f987660c6..acef93390d3d 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -1120,6 +1120,7 @@ int wl1271_plt_stop(struct wl1271 *wl)
 	cancel_work_sync(&wl->recovery_work);
 	cancel_delayed_work_sync(&wl->elp_work);
 	cancel_delayed_work_sync(&wl->tx_watchdog_work);
+	cancel_delayed_work_sync(&wl->connection_loss_work);
 
 	mutex_lock(&wl->mutex);
 	wl1271_power_off(wl);
@@ -1261,8 +1262,270 @@ static struct sk_buff *wl12xx_alloc_dummy_packet(struct wl1271 *wl)
 
 
 #ifdef CONFIG_PM
+static int
+wl1271_validate_wowlan_pattern(struct cfg80211_wowlan_trig_pkt_pattern *p)
+{
+	int num_fields = 0, in_field = 0, fields_size = 0;
+	int i, pattern_len = 0;
+
+	if (!p->mask) {
+		wl1271_warning("No mask in WoWLAN pattern");
+		return -EINVAL;
+	}
+
+	/*
+	 * The pattern is broken up into segments of bytes at different offsets
+	 * that need to be checked by the FW filter. Each segment is called
+	 * a field in the FW API. We verify that the total number of fields
+	 * required for this pattern won't exceed FW limits (8)
+	 * as well as the total fields buffer won't exceed the FW limit.
+	 * Note that if there's a pattern which crosses Ethernet/IP header
+	 * boundary a new field is required.
+	 */
+	for (i = 0; i < p->pattern_len; i++) {
+		if (test_bit(i, (unsigned long *)p->mask)) {
+			if (!in_field) {
+				in_field = 1;
+				pattern_len = 1;
+			} else {
+				if (i == WL1271_RX_FILTER_ETH_HEADER_SIZE) {
+					num_fields++;
+					fields_size += pattern_len +
+						RX_FILTER_FIELD_OVERHEAD;
+					pattern_len = 1;
+				} else
+					pattern_len++;
+			}
+		} else {
+			if (in_field) {
+				in_field = 0;
+				fields_size += pattern_len +
+					RX_FILTER_FIELD_OVERHEAD;
+				num_fields++;
+			}
+		}
+	}
+
+	if (in_field) {
+		fields_size += pattern_len + RX_FILTER_FIELD_OVERHEAD;
+		num_fields++;
+	}
+
+	if (num_fields > WL1271_RX_FILTER_MAX_FIELDS) {
+		wl1271_warning("RX Filter too complex. Too many segments");
+		return -EINVAL;
+	}
+
+	if (fields_size > WL1271_RX_FILTER_MAX_FIELDS_SIZE) {
+		wl1271_warning("RX filter pattern is too big");
+		return -E2BIG;
+	}
+
+	return 0;
+}
+
+struct wl12xx_rx_filter *wl1271_rx_filter_alloc(void)
+{
+	return kzalloc(sizeof(struct wl12xx_rx_filter), GFP_KERNEL);
+}
+
+void wl1271_rx_filter_free(struct wl12xx_rx_filter *filter)
+{
+	int i;
+
+	if (filter == NULL)
+		return;
+
+	for (i = 0; i < filter->num_fields; i++)
+		kfree(filter->fields[i].pattern);
+
+	kfree(filter);
+}
+
+int wl1271_rx_filter_alloc_field(struct wl12xx_rx_filter *filter,
+				 u16 offset, u8 flags,
+				 u8 *pattern, u8 len)
+{
+	struct wl12xx_rx_filter_field *field;
+
+	if (filter->num_fields == WL1271_RX_FILTER_MAX_FIELDS) {
+		wl1271_warning("Max fields per RX filter. can't alloc another");
+		return -EINVAL;
+	}
+
+	field = &filter->fields[filter->num_fields];
+
+	field->pattern = kzalloc(len, GFP_KERNEL);
+	if (!field->pattern) {
+		wl1271_warning("Failed to allocate RX filter pattern");
+		return -ENOMEM;
+	}
+
+	filter->num_fields++;
+
+	field->offset = cpu_to_le16(offset);
+	field->flags = flags;
+	field->len = len;
+	memcpy(field->pattern, pattern, len);
+
+	return 0;
+}
+
+int wl1271_rx_filter_get_fields_size(struct wl12xx_rx_filter *filter)
+{
+	int i, fields_size = 0;
+
+	for (i = 0; i < filter->num_fields; i++)
+		fields_size += filter->fields[i].len +
+			sizeof(struct wl12xx_rx_filter_field) -
+			sizeof(u8 *);
+
+	return fields_size;
+}
+
+void wl1271_rx_filter_flatten_fields(struct wl12xx_rx_filter *filter,
+				    u8 *buf)
+{
+	int i;
+	struct wl12xx_rx_filter_field *field;
+
+	for (i = 0; i < filter->num_fields; i++) {
+		field = (struct wl12xx_rx_filter_field *)buf;
+
+		field->offset = filter->fields[i].offset;
+		field->flags = filter->fields[i].flags;
+		field->len = filter->fields[i].len;
+
+		memcpy(&field->pattern, filter->fields[i].pattern, field->len);
+		buf += sizeof(struct wl12xx_rx_filter_field) -
+			sizeof(u8 *) + field->len;
+	}
+}
+
+/*
+ * Allocates an RX filter returned through f
+ * which needs to be freed using rx_filter_free()
+ */
+static int wl1271_convert_wowlan_pattern_to_rx_filter(
+	struct cfg80211_wowlan_trig_pkt_pattern *p,
+	struct wl12xx_rx_filter **f)
+{
+	int i, j, ret = 0;
+	struct wl12xx_rx_filter *filter;
+	u16 offset;
+	u8 flags, len;
+
+	filter = wl1271_rx_filter_alloc();
+	if (!filter) {
+		wl1271_warning("Failed to alloc rx filter");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	i = 0;
+	while (i < p->pattern_len) {
+		if (!test_bit(i, (unsigned long *)p->mask)) {
+			i++;
+			continue;
+		}
+
+		for (j = i; j < p->pattern_len; j++) {
+			if (!test_bit(j, (unsigned long *)p->mask))
+				break;
+
+			if (i < WL1271_RX_FILTER_ETH_HEADER_SIZE &&
+			    j >= WL1271_RX_FILTER_ETH_HEADER_SIZE)
+				break;
+		}
+
+		if (i < WL1271_RX_FILTER_ETH_HEADER_SIZE) {
+			offset = i;
+			flags = WL1271_RX_FILTER_FLAG_ETHERNET_HEADER;
+		} else {
+			offset = i - WL1271_RX_FILTER_ETH_HEADER_SIZE;
+			flags = WL1271_RX_FILTER_FLAG_IP_HEADER;
+		}
+
+		len = j - i;
+
+		ret = wl1271_rx_filter_alloc_field(filter,
+						   offset,
+						   flags,
+						   &p->pattern[i], len);
+		if (ret)
+			goto err;
+
+		i = j;
+	}
+
+	filter->action = FILTER_SIGNAL;
+
+	*f = filter;
+	return 0;
+
+err:
+	wl1271_rx_filter_free(filter);
+	*f = NULL;
+
+	return ret;
+}
+
+static int wl1271_configure_wowlan(struct wl1271 *wl,
+				   struct cfg80211_wowlan *wow)
+{
+	int i, ret;
+
+	if (!wow || wow->any || !wow->n_patterns) {
+		wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
+		wl1271_rx_filter_clear_all(wl);
+		return 0;
+	}
+
+	if (WARN_ON(wow->n_patterns > WL1271_MAX_RX_FILTERS))
+		return -EINVAL;
+
+	/* Validate all incoming patterns before clearing current FW state */
+	for (i = 0; i < wow->n_patterns; i++) {
+		ret = wl1271_validate_wowlan_pattern(&wow->patterns[i]);
+		if (ret) {
+			wl1271_warning("Bad wowlan pattern %d", i);
+			return ret;
+		}
+	}
+
+	wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
+	wl1271_rx_filter_clear_all(wl);
+
+	/* Translate WoWLAN patterns into filters */
+	for (i = 0; i < wow->n_patterns; i++) {
+		struct cfg80211_wowlan_trig_pkt_pattern *p;
+		struct wl12xx_rx_filter *filter = NULL;
+
+		p = &wow->patterns[i];
+
+		ret = wl1271_convert_wowlan_pattern_to_rx_filter(p, &filter);
+		if (ret) {
+			wl1271_warning("Failed to create an RX filter from "
+				       "wowlan pattern %d", i);
+			goto out;
+		}
+
+		ret = wl1271_rx_filter_enable(wl, i, 1, filter);
+
+		wl1271_rx_filter_free(filter);
+		if (ret)
+			goto out;
+	}
+
+	ret = wl1271_acx_default_rx_filter_enable(wl, 1, FILTER_DROP);
+
+out:
+	return ret;
+}
+
 static int wl1271_configure_suspend_sta(struct wl1271 *wl,
-					struct wl12xx_vif *wlvif)
+					struct wl12xx_vif *wlvif,
+					struct cfg80211_wowlan *wow)
 {
 	int ret = 0;
 
@@ -1273,6 +1536,7 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
 	if (ret < 0)
 		goto out;
 
+	wl1271_configure_wowlan(wl, wow);
 	ret = wl1271_acx_wake_up_conditions(wl, wlvif,
 				    wl->conf.conn.suspend_wake_up_event,
 				    wl->conf.conn.suspend_listen_interval);
@@ -1308,10 +1572,11 @@ out:
 }
 
 static int wl1271_configure_suspend(struct wl1271 *wl,
-				    struct wl12xx_vif *wlvif)
+				    struct wl12xx_vif *wlvif,
+				    struct cfg80211_wowlan *wow)
 {
 	if (wlvif->bss_type == BSS_TYPE_STA_BSS)
-		return wl1271_configure_suspend_sta(wl, wlvif);
+		return wl1271_configure_suspend_sta(wl, wlvif, wow);
 	if (wlvif->bss_type == BSS_TYPE_AP_BSS)
 		return wl1271_configure_suspend_ap(wl, wlvif);
 	return 0;
@@ -1332,6 +1597,8 @@ static void wl1271_configure_resume(struct wl1271 *wl,
 		return;
 
 	if (is_sta) {
+		wl1271_configure_wowlan(wl, NULL);
+
 		ret = wl1271_acx_wake_up_conditions(wl, wlvif,
 				    wl->conf.conn.wake_up_event,
 				    wl->conf.conn.listen_interval);
@@ -1355,15 +1622,16 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 	int ret;
 
 	wl1271_debug(DEBUG_MAC80211, "mac80211 suspend wow=%d", !!wow);
-	WARN_ON(!wow || !wow->any);
+	WARN_ON(!wow);
 
 	wl1271_tx_flush(wl);
 
 	mutex_lock(&wl->mutex);
 	wl->wow_enabled = true;
 	wl12xx_for_each_wlvif(wl, wlvif) {
-		ret = wl1271_configure_suspend(wl, wlvif);
+		ret = wl1271_configure_suspend(wl, wlvif, wow);
 		if (ret < 0) {
+			mutex_unlock(&wl->mutex);
 			wl1271_warning("couldn't prepare device to suspend");
 			return ret;
 		}
@@ -1487,6 +1755,7 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 	cancel_work_sync(&wl->tx_work);
 	cancel_delayed_work_sync(&wl->elp_work);
 	cancel_delayed_work_sync(&wl->tx_watchdog_work);
+	cancel_delayed_work_sync(&wl->connection_loss_work);
 
 	/* let's notify MAC80211 about the remaining pending TX frames */
 	wl12xx_tx_reset(wl, true);
@@ -3439,6 +3708,9 @@ sta_not_found:
 			do_join = true;
 			set_assoc = true;
 
+			/* Cancel connection_loss_work */
+			cancel_delayed_work_sync(&wl->connection_loss_work);
+
 			/*
 			 * use basic rates from AP, and determine lowest rate
 			 * to use with control frames.
@@ -4549,6 +4821,34 @@ static struct bin_attribute fwlog_attr = {
 	.read = wl1271_sysfs_read_fwlog,
 };
 
+static void wl1271_connection_loss_work(struct work_struct *work)
+{
+	struct delayed_work *dwork;
+	struct wl1271 *wl;
+	struct ieee80211_vif *vif;
+	struct wl12xx_vif *wlvif;
+
+	dwork = container_of(work, struct delayed_work, work);
+	wl = container_of(dwork, struct wl1271, connection_loss_work);
+
+	wl1271_info("Connection loss work.");
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state == WL1271_STATE_OFF))
+		goto out;
+
+	/* Call mac80211 connection loss */
+	wl12xx_for_each_wlvif_sta(wl, wlvif) {
+		if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
+			goto out;
+		vif = wl12xx_wlvif_to_vif(wlvif);
+		ieee80211_connection_loss(vif);
+	}
+out:
+	mutex_unlock(&wl->mutex);
+}
+
 static void wl12xx_derive_mac_addresses(struct wl1271 *wl,
 					u32 oui, u32 nic, int n)
 {
@@ -4804,6 +5104,8 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size)
 	INIT_WORK(&wl->recovery_work, wl1271_recovery_work);
 	INIT_DELAYED_WORK(&wl->scan_complete_work, wl1271_scan_complete_work);
 	INIT_DELAYED_WORK(&wl->tx_watchdog_work, wl12xx_tx_watchdog_work);
+	INIT_DELAYED_WORK(&wl->connection_loss_work,
+			  wl1271_connection_loss_work);
 
 	wl->freezable_wq = create_freezable_workqueue("wl12xx_wq");
 	if (!wl->freezable_wq) {
@@ -4861,7 +5163,7 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size)
 		goto err_dummy_packet;
 	}
 
-	wl->mbox = kmalloc(sizeof(*wl->mbox), GFP_DMA);
+	wl->mbox = kmalloc(sizeof(*wl->mbox), GFP_KERNEL | GFP_DMA);
 	if (!wl->mbox) {
 		ret = -ENOMEM;
 		goto err_fwlog;
@@ -5003,9 +5305,14 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
 	if (!ret) {
 		wl->irq_wake_enabled = true;
 		device_init_wakeup(wl->dev, 1);
-		if (pdata->pwr_in_suspend)
+		if (pdata->pwr_in_suspend) {
 			wl->hw->wiphy->wowlan.flags = WIPHY_WOWLAN_ANY;
-
+			wl->hw->wiphy->wowlan.n_patterns =
+				WL1271_MAX_RX_FILTERS;
+			wl->hw->wiphy->wowlan.pattern_min_len = 1;
+			wl->hw->wiphy->wowlan.pattern_max_len =
+				WL1271_RX_FILTER_MAX_PATTERN_SIZE;
+		}
 	}
 	disable_irq(wl->irq);
 
diff --git a/drivers/net/wireless/ti/wlcore/rx.c b/drivers/net/wireless/ti/wlcore/rx.c
index 89bd9385e90b..1f1d9488dfb6 100644
--- a/drivers/net/wireless/ti/wlcore/rx.c
+++ b/drivers/net/wireless/ti/wlcore/rx.c
@@ -278,3 +278,39 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status)
 
 	wl12xx_rearm_rx_streaming(wl, active_hlids);
 }
+
+int wl1271_rx_filter_enable(struct wl1271 *wl,
+			    int index, bool enable,
+			    struct wl12xx_rx_filter *filter)
+{
+	int ret;
+
+	if (wl->rx_filter_enabled[index] == enable) {
+		wl1271_warning("Request to enable an already "
+			     "enabled rx filter %d", index);
+		return 0;
+	}
+
+	ret = wl1271_acx_set_rx_filter(wl, index, enable, filter);
+
+	if (ret) {
+		wl1271_error("Failed to %s rx data filter %d (err=%d)",
+			     enable ? "enable" : "disable", index, ret);
+		return ret;
+	}
+
+	wl->rx_filter_enabled[index] = enable;
+
+	return 0;
+}
+
+void wl1271_rx_filter_clear_all(struct wl1271 *wl)
+{
+	int i;
+
+	for (i = 0; i < WL1271_MAX_RX_FILTERS; i++) {
+		if (!wl->rx_filter_enabled[i])
+			continue;
+		wl1271_rx_filter_enable(wl, i, 0, NULL);
+	}
+}
diff --git a/drivers/net/wireless/ti/wlcore/rx.h b/drivers/net/wireless/ti/wlcore/rx.h
index 6e129e2a8546..e9a162a864ca 100644
--- a/drivers/net/wireless/ti/wlcore/rx.h
+++ b/drivers/net/wireless/ti/wlcore/rx.h
@@ -138,5 +138,9 @@ struct wl1271_rx_descriptor {
 
 void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status);
 u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
+int wl1271_rx_filter_enable(struct wl1271 *wl,
+			    int index, bool enable,
+			    struct wl12xx_rx_filter *filter);
+void wl1271_rx_filter_clear_all(struct wl1271 *wl);
 
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/wl12xx.h b/drivers/net/wireless/ti/wlcore/wl12xx.h
index a9b220c43e54..f12bdf745180 100644
--- a/drivers/net/wireless/ti/wlcore/wl12xx.h
+++ b/drivers/net/wireless/ti/wlcore/wl12xx.h
@@ -279,6 +279,39 @@ struct wl1271_link {
 	u8 ba_bitmap;
 };
 
+#define WL1271_MAX_RX_FILTERS 5
+#define WL1271_RX_FILTER_MAX_FIELDS 8
+
+#define WL1271_RX_FILTER_ETH_HEADER_SIZE 14
+#define WL1271_RX_FILTER_MAX_FIELDS_SIZE 95
+#define RX_FILTER_FIELD_OVERHEAD				\
+	(sizeof(struct wl12xx_rx_filter_field) - sizeof(u8 *))
+#define WL1271_RX_FILTER_MAX_PATTERN_SIZE			\
+	(WL1271_RX_FILTER_MAX_FIELDS_SIZE - RX_FILTER_FIELD_OVERHEAD)
+
+#define WL1271_RX_FILTER_FLAG_MASK                BIT(0)
+#define WL1271_RX_FILTER_FLAG_IP_HEADER           0
+#define WL1271_RX_FILTER_FLAG_ETHERNET_HEADER     BIT(1)
+
+enum rx_filter_action {
+	FILTER_DROP = 0,
+	FILTER_SIGNAL = 1,
+	FILTER_FW_HANDLE = 2
+};
+
+struct wl12xx_rx_filter_field {
+	__le16 offset;
+	u8 len;
+	u8 flags;
+	u8 *pattern;
+} __packed;
+
+struct wl12xx_rx_filter {
+	u8 action;
+	int num_fields;
+	struct wl12xx_rx_filter_field fields[WL1271_RX_FILTER_MAX_FIELDS];
+};
+
 struct wl1271_station {
 	u8 hlid;
 };
@@ -439,6 +472,14 @@ int wl1271_plt_stop(struct wl1271 *wl);
 int wl1271_recalc_rx_streaming(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 void wl12xx_queue_recovery_work(struct wl1271 *wl);
 size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen);
+int wl1271_rx_filter_alloc_field(struct wl12xx_rx_filter *filter,
+					u16 offset, u8 flags,
+					u8 *pattern, u8 len);
+void wl1271_rx_filter_free(struct wl12xx_rx_filter *filter);
+struct wl12xx_rx_filter *wl1271_rx_filter_alloc(void);
+int wl1271_rx_filter_get_fields_size(struct wl12xx_rx_filter *filter);
+void wl1271_rx_filter_flatten_fields(struct wl12xx_rx_filter *filter,
+				     u8 *buf);
 
 #define JOIN_TIMEOUT 5000 /* 5000 milliseconds to join */
 
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
index 39f9fadfebd9..0b3f0b586f4b 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -243,6 +243,9 @@ struct wl1271 {
 	struct wl1271_scan scan;
 	struct delayed_work scan_complete_work;
 
+	/* Connection loss work */
+	struct delayed_work connection_loss_work;
+
 	bool sched_scanning;
 
 	/* The current band */
@@ -349,6 +352,9 @@ struct wl1271 {
 
 	/* size of the private FW status data */
 	size_t fw_status_priv_len;
+
+	/* RX Data filter rule state - enabled/disabled */
+	bool rx_filter_enabled[WL1271_MAX_RX_FILTERS];
 };
 
 int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev);
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index 0ebbb1906c30..2027afe405fe 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -1962,9 +1962,6 @@ static int __init netif_init(void)
 	if (!xen_domain())
 		return -ENODEV;
 
-	if (xen_initial_domain())
-		return 0;
-
 	if (xen_hvm_domain() && !xen_platform_pci_unplug)
 		return -ENODEV;
 
@@ -1977,9 +1974,6 @@ module_init(netif_init);
 
 static void __exit netif_exit(void)
 {
-	if (xen_initial_domain())
-		return;
-
 	xenbus_unregister_driver(&netfront_driver);
 }
 module_exit(netif_exit);
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index 5af959274d4e..3b20b73ee649 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -17,6 +17,19 @@ config PN544_NFC
 	  To compile this driver as a module, choose m here. The module will
 	  be called pn544.
 
+config PN544_HCI_NFC
+	tristate "HCI PN544 NFC driver"
+	depends on I2C && NFC_SHDLC
+	select CRC_CCITT
+	default n
+	---help---
+	  NXP PN544 i2c driver.
+	  This is a driver based on the SHDLC and HCI NFC kernel layers and
+	  will thus not work with NXP libnfc library.
+
+	  To compile this driver as a module, choose m here. The module will
+	  be called pn544_hci.
+
 config NFC_PN533
 	tristate "NXP PN533 USB driver"
 	depends on USB
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index ab99e8572f02..473e44cef612 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -3,6 +3,7 @@
 #
 
 obj-$(CONFIG_PN544_NFC)		+= pn544.o
+obj-$(CONFIG_PN544_HCI_NFC)	+= pn544_hci.o
 obj-$(CONFIG_NFC_PN533)		+= pn533.o
 obj-$(CONFIG_NFC_WILINK)	+= nfcwilink.o
 
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index e6ec16d92e65..19110f0eb15f 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -394,9 +394,6 @@ static void pn533_wq_cmd_complete(struct work_struct *work)
 	struct pn533_frame *in_frame;
 	int rc;
 
-	if (dev == NULL)
-		return;
-
 	in_frame = dev->wq_in_frame;
 
 	if (dev->wq_in_error)
@@ -1194,8 +1191,8 @@ static int pn533_activate_target_nfcdep(struct pn533 *dev)
 	return rc;
 }
 
-static int pn533_activate_target(struct nfc_dev *nfc_dev, u32 target_idx,
-								u32 protocol)
+static int pn533_activate_target(struct nfc_dev *nfc_dev,
+				 struct nfc_target *target, u32 protocol)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	int rc;
@@ -1243,7 +1240,8 @@ static int pn533_activate_target(struct nfc_dev *nfc_dev, u32 target_idx,
 	return 0;
 }
 
-static void pn533_deactivate_target(struct nfc_dev *nfc_dev, u32 target_idx)
+static void pn533_deactivate_target(struct nfc_dev *nfc_dev,
+				    struct nfc_target *target)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	u8 tg;
@@ -1351,7 +1349,7 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg,
 	return 0;
 }
 
-static int pn533_dep_link_up(struct nfc_dev *nfc_dev, int target_idx,
+static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 			     u8 comm_mode, u8* gb, size_t gb_len)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
@@ -1552,10 +1550,9 @@ error:
 	return 0;
 }
 
-static int pn533_data_exchange(struct nfc_dev *nfc_dev, u32 target_idx,
-						struct sk_buff *skb,
-						data_exchange_cb_t cb,
-						void *cb_context)
+static int pn533_data_exchange(struct nfc_dev *nfc_dev,
+			       struct nfc_target *target, struct sk_buff *skb,
+			       data_exchange_cb_t cb, void *cb_context)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	struct pn533_frame *out_frame, *in_frame;
diff --git a/drivers/nfc/pn544_hci.c b/drivers/nfc/pn544_hci.c
new file mode 100644
index 000000000000..46f4a9f9f5e4
--- /dev/null
+++ b/drivers/nfc/pn544_hci.c
@@ -0,0 +1,947 @@
+/*
+ * HCI based Driver for NXP PN544 NFC Chip
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/crc-ccitt.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+
+#include <linux/nfc.h>
+#include <net/nfc/hci.h>
+#include <net/nfc/shdlc.h>
+
+#include <linux/nfc/pn544.h>
+
+#define DRIVER_DESC "HCI NFC driver for PN544"
+
+#define PN544_HCI_DRIVER_NAME "pn544_hci"
+
+/* Timing restrictions (ms) */
+#define PN544_HCI_RESETVEN_TIME		30
+
+static struct i2c_device_id pn544_hci_id_table[] = {
+	{"pn544", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, pn544_hci_id_table);
+
+#define HCI_MODE 0
+#define FW_MODE 1
+
+/* framing in HCI mode */
+#define PN544_HCI_LLC_LEN		1
+#define PN544_HCI_LLC_CRC		2
+#define PN544_HCI_LLC_LEN_CRC		(PN544_HCI_LLC_LEN + PN544_HCI_LLC_CRC)
+#define PN544_HCI_LLC_MIN_SIZE		(1 + PN544_HCI_LLC_LEN_CRC)
+#define PN544_HCI_LLC_MAX_PAYLOAD	29
+#define PN544_HCI_LLC_MAX_SIZE		(PN544_HCI_LLC_LEN_CRC + 1 + \
+					 PN544_HCI_LLC_MAX_PAYLOAD)
+
+enum pn544_state {
+	PN544_ST_COLD,
+	PN544_ST_FW_READY,
+	PN544_ST_READY,
+};
+
+#define FULL_VERSION_LEN 11
+
+/* Proprietary commands */
+#define PN544_WRITE		0x3f
+
+/* Proprietary gates, events, commands and registers */
+
+/* NFC_HCI_RF_READER_A_GATE additional registers and commands */
+#define PN544_RF_READER_A_AUTO_ACTIVATION			0x10
+#define PN544_RF_READER_A_CMD_CONTINUE_ACTIVATION		0x12
+#define PN544_MIFARE_CMD					0x21
+
+/* Commands that apply to all RF readers */
+#define PN544_RF_READER_CMD_PRESENCE_CHECK	0x30
+#define PN544_RF_READER_CMD_ACTIVATE_NEXT	0x32
+
+/* NFC_HCI_ID_MGMT_GATE additional registers */
+#define PN544_ID_MGMT_FULL_VERSION_SW		0x10
+
+#define PN544_RF_READER_ISO15693_GATE		0x12
+
+#define PN544_RF_READER_F_GATE			0x14
+#define PN544_FELICA_ID				0x04
+#define PN544_FELICA_RAW			0x20
+
+#define PN544_RF_READER_JEWEL_GATE		0x15
+#define PN544_JEWEL_RAW_CMD			0x23
+
+#define PN544_RF_READER_NFCIP1_INITIATOR_GATE	0x30
+#define PN544_RF_READER_NFCIP1_TARGET_GATE	0x31
+
+#define PN544_SYS_MGMT_GATE			0x90
+#define PN544_SYS_MGMT_INFO_NOTIFICATION	0x02
+
+#define PN544_POLLING_LOOP_MGMT_GATE		0x94
+#define PN544_PL_RDPHASES			0x06
+#define PN544_PL_EMULATION			0x07
+#define PN544_PL_NFCT_DEACTIVATED		0x09
+
+#define PN544_SWP_MGMT_GATE			0xA0
+
+#define PN544_NFC_WI_MGMT_GATE			0xA1
+
+static u8 pn544_custom_gates[] = {
+	PN544_SYS_MGMT_GATE,
+	PN544_SWP_MGMT_GATE,
+	PN544_POLLING_LOOP_MGMT_GATE,
+	PN544_NFC_WI_MGMT_GATE,
+	PN544_RF_READER_F_GATE,
+	PN544_RF_READER_JEWEL_GATE,
+	PN544_RF_READER_ISO15693_GATE,
+	PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+	PN544_RF_READER_NFCIP1_TARGET_GATE
+};
+
+/* Largest headroom needed for outgoing custom commands */
+#define PN544_CMDS_HEADROOM	2
+
+struct pn544_hci_info {
+	struct i2c_client *i2c_dev;
+	struct nfc_shdlc *shdlc;
+
+	enum pn544_state state;
+
+	struct mutex info_lock;
+
+	unsigned int gpio_en;
+	unsigned int gpio_irq;
+	unsigned int gpio_fw;
+	unsigned int en_polarity;
+
+	int hard_fault;		/*
+				 * < 0 if hardware error occured (e.g. i2c err)
+				 * and prevents normal operation.
+				 */
+};
+
+static void pn544_hci_platform_init(struct pn544_hci_info *info)
+{
+	int polarity, retry, ret;
+	char rset_cmd[] = { 0x05, 0xF9, 0x04, 0x00, 0xC3, 0xE5 };
+	int count = sizeof(rset_cmd);
+
+	pr_info(DRIVER_DESC ": %s\n", __func__);
+	dev_info(&info->i2c_dev->dev, "Detecting nfc_en polarity\n");
+
+	/* Disable fw download */
+	gpio_set_value(info->gpio_fw, 0);
+
+	for (polarity = 0; polarity < 2; polarity++) {
+		info->en_polarity = polarity;
+		retry = 3;
+		while (retry--) {
+			/* power off */
+			gpio_set_value(info->gpio_en, !info->en_polarity);
+			usleep_range(10000, 15000);
+
+			/* power on */
+			gpio_set_value(info->gpio_en, info->en_polarity);
+			usleep_range(10000, 15000);
+
+			/* send reset */
+			dev_dbg(&info->i2c_dev->dev, "Sending reset cmd\n");
+			ret = i2c_master_send(info->i2c_dev, rset_cmd, count);
+			if (ret == count) {
+				dev_info(&info->i2c_dev->dev,
+					 "nfc_en polarity : active %s\n",
+					 (polarity == 0 ? "low" : "high"));
+				goto out;
+			}
+		}
+	}
+
+	dev_err(&info->i2c_dev->dev,
+		"Could not detect nfc_en polarity, fallback to active high\n");
+
+out:
+	gpio_set_value(info->gpio_en, !info->en_polarity);
+}
+
+static int pn544_hci_enable(struct pn544_hci_info *info, int mode)
+{
+	pr_info(DRIVER_DESC ": %s\n", __func__);
+
+	gpio_set_value(info->gpio_fw, 0);
+	gpio_set_value(info->gpio_en, info->en_polarity);
+	usleep_range(10000, 15000);
+
+	return 0;
+}
+
+static void pn544_hci_disable(struct pn544_hci_info *info)
+{
+	pr_info(DRIVER_DESC ": %s\n", __func__);
+
+	gpio_set_value(info->gpio_fw, 0);
+	gpio_set_value(info->gpio_en, !info->en_polarity);
+	usleep_range(10000, 15000);
+
+	gpio_set_value(info->gpio_en, info->en_polarity);
+	usleep_range(10000, 15000);
+
+	gpio_set_value(info->gpio_en, !info->en_polarity);
+	usleep_range(10000, 15000);
+}
+
+static int pn544_hci_i2c_write(struct i2c_client *client, u8 *buf, int len)
+{
+	int r;
+
+	usleep_range(3000, 6000);
+
+	r = i2c_master_send(client, buf, len);
+
+	if (r == -EREMOTEIO) {	/* Retry, chip was in standby */
+		usleep_range(6000, 10000);
+		r = i2c_master_send(client, buf, len);
+	}
+
+	if (r >= 0 && r != len)
+		r = -EREMOTEIO;
+
+	return r;
+}
+
+static int check_crc(u8 *buf, int buflen)
+{
+	u8 len;
+	u16 crc;
+
+	len = buf[0] + 1;
+	crc = crc_ccitt(0xffff, buf, len - 2);
+	crc = ~crc;
+
+	if (buf[len - 2] != (crc & 0xff) || buf[len - 1] != (crc >> 8)) {
+		pr_err(PN544_HCI_DRIVER_NAME ": CRC error 0x%x != 0x%x 0x%x\n",
+		       crc, buf[len - 1], buf[len - 2]);
+
+		pr_info(DRIVER_DESC ": %s : BAD CRC\n", __func__);
+		print_hex_dump(KERN_DEBUG, "crc: ", DUMP_PREFIX_NONE,
+			       16, 2, buf, buflen, false);
+		return -EPERM;
+	}
+	return 0;
+}
+
+/*
+ * Reads an shdlc frame and returns it in a newly allocated sk_buff. Guarantees
+ * that i2c bus will be flushed and that next read will start on a new frame.
+ * returned skb contains only LLC header and payload.
+ * returns:
+ * -EREMOTEIO : i2c read error (fatal)
+ * -EBADMSG : frame was incorrect and discarded
+ * -ENOMEM : cannot allocate skb, frame dropped
+ */
+static int pn544_hci_i2c_read(struct i2c_client *client, struct sk_buff **skb)
+{
+	int r;
+	u8 len;
+	u8 tmp[PN544_HCI_LLC_MAX_SIZE - 1];
+
+	r = i2c_master_recv(client, &len, 1);
+	if (r != 1) {
+		dev_err(&client->dev, "cannot read len byte\n");
+		return -EREMOTEIO;
+	}
+
+	if ((len < (PN544_HCI_LLC_MIN_SIZE - 1)) ||
+	    (len > (PN544_HCI_LLC_MAX_SIZE - 1))) {
+		dev_err(&client->dev, "invalid len byte\n");
+		r = -EBADMSG;
+		goto flush;
+	}
+
+	*skb = alloc_skb(1 + len, GFP_KERNEL);
+	if (*skb == NULL) {
+		r = -ENOMEM;
+		goto flush;
+	}
+
+	*skb_put(*skb, 1) = len;
+
+	r = i2c_master_recv(client, skb_put(*skb, len), len);
+	if (r != len) {
+		kfree_skb(*skb);
+		return -EREMOTEIO;
+	}
+
+	r = check_crc((*skb)->data, (*skb)->len);
+	if (r != 0) {
+		kfree_skb(*skb);
+		r = -EBADMSG;
+		goto flush;
+	}
+
+	skb_pull(*skb, 1);
+	skb_trim(*skb, (*skb)->len - 2);
+
+	usleep_range(3000, 6000);
+
+	return 0;
+
+flush:
+	if (i2c_master_recv(client, tmp, sizeof(tmp)) < 0)
+		r = -EREMOTEIO;
+
+	usleep_range(3000, 6000);
+
+	return r;
+}
+
+/*
+ * Reads an shdlc frame from the chip. This is not as straightforward as it
+ * seems. There are cases where we could loose the frame start synchronization.
+ * The frame format is len-data-crc, and corruption can occur anywhere while
+ * transiting on i2c bus, such that we could read an invalid len.
+ * In order to recover synchronization with the next frame, we must be sure
+ * to read the real amount of data without using the len byte. We do this by
+ * assuming the following:
+ * - the chip will always present only one single complete frame on the bus
+ *   before triggering the interrupt
+ * - the chip will not present a new frame until we have completely read
+ *   the previous one (or until we have handled the interrupt).
+ * The tricky case is when we read a corrupted len that is less than the real
+ * len. We must detect this here in order to determine that we need to flush
+ * the bus. This is the reason why we check the crc here.
+ */
+static irqreturn_t pn544_hci_irq_thread_fn(int irq, void *dev_id)
+{
+	struct pn544_hci_info *info = dev_id;
+	struct i2c_client *client = info->i2c_dev;
+	struct sk_buff *skb = NULL;
+	int r;
+
+	BUG_ON(!info);
+	BUG_ON(irq != info->i2c_dev->irq);
+
+	dev_dbg(&client->dev, "IRQ\n");
+
+	if (info->hard_fault != 0)
+		return IRQ_HANDLED;
+
+	r = pn544_hci_i2c_read(client, &skb);
+	if (r == -EREMOTEIO) {
+		info->hard_fault = r;
+
+		nfc_shdlc_recv_frame(info->shdlc, NULL);
+
+		return IRQ_HANDLED;
+	} else if ((r == -ENOMEM) || (r == -EBADMSG)) {
+		return IRQ_HANDLED;
+	}
+
+	nfc_shdlc_recv_frame(info->shdlc, skb);
+
+	return IRQ_HANDLED;
+}
+
+static int pn544_hci_open(struct nfc_shdlc *shdlc)
+{
+	struct pn544_hci_info *info = nfc_shdlc_get_clientdata(shdlc);
+	int r = 0;
+
+	mutex_lock(&info->info_lock);
+
+	if (info->state != PN544_ST_COLD) {
+		r = -EBUSY;
+		goto out;
+	}
+
+	r = pn544_hci_enable(info, HCI_MODE);
+
+out:
+	mutex_unlock(&info->info_lock);
+	return r;
+}
+
+static void pn544_hci_close(struct nfc_shdlc *shdlc)
+{
+	struct pn544_hci_info *info = nfc_shdlc_get_clientdata(shdlc);
+
+	mutex_lock(&info->info_lock);
+
+	if (info->state == PN544_ST_COLD)
+		goto out;
+
+	pn544_hci_disable(info);
+
+out:
+	mutex_unlock(&info->info_lock);
+}
+
+static int pn544_hci_ready(struct nfc_shdlc *shdlc)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	struct sk_buff *skb;
+	static struct hw_config {
+		u8 adr[2];
+		u8 value;
+	} hw_config[] = {
+		{{0x9f, 0x9a}, 0x00},
+
+		{{0x98, 0x10}, 0xbc},
+
+		{{0x9e, 0x71}, 0x00},
+
+		{{0x98, 0x09}, 0x00},
+
+		{{0x9e, 0xb4}, 0x00},
+
+		{{0x9e, 0xd9}, 0xff},
+		{{0x9e, 0xda}, 0xff},
+		{{0x9e, 0xdb}, 0x23},
+		{{0x9e, 0xdc}, 0x21},
+		{{0x9e, 0xdd}, 0x22},
+		{{0x9e, 0xde}, 0x24},
+
+		{{0x9c, 0x01}, 0x08},
+
+		{{0x9e, 0xaa}, 0x01},
+
+		{{0x9b, 0xd1}, 0x0d},
+		{{0x9b, 0xd2}, 0x24},
+		{{0x9b, 0xd3}, 0x0a},
+		{{0x9b, 0xd4}, 0x22},
+		{{0x9b, 0xd5}, 0x08},
+		{{0x9b, 0xd6}, 0x1e},
+		{{0x9b, 0xdd}, 0x1c},
+
+		{{0x9b, 0x84}, 0x13},
+		{{0x99, 0x81}, 0x7f},
+		{{0x99, 0x31}, 0x70},
+
+		{{0x98, 0x00}, 0x3f},
+
+		{{0x9f, 0x09}, 0x00},
+
+		{{0x9f, 0x0a}, 0x05},
+
+		{{0x9e, 0xd1}, 0xa1},
+		{{0x99, 0x23}, 0x00},
+
+		{{0x9e, 0x74}, 0x80},
+
+		{{0x9f, 0x28}, 0x10},
+
+		{{0x9f, 0x35}, 0x14},
+
+		{{0x9f, 0x36}, 0x60},
+
+		{{0x9c, 0x31}, 0x00},
+
+		{{0x9c, 0x32}, 0xc8},
+
+		{{0x9c, 0x19}, 0x40},
+
+		{{0x9c, 0x1a}, 0x40},
+
+		{{0x9c, 0x0c}, 0x00},
+
+		{{0x9c, 0x0d}, 0x00},
+
+		{{0x9c, 0x12}, 0x00},
+
+		{{0x9c, 0x13}, 0x00},
+
+		{{0x98, 0xa2}, 0x0e},
+
+		{{0x98, 0x93}, 0x40},
+
+		{{0x98, 0x7d}, 0x02},
+		{{0x98, 0x7e}, 0x00},
+		{{0x9f, 0xc8}, 0x01},
+	};
+	struct hw_config *p = hw_config;
+	int count = ARRAY_SIZE(hw_config);
+	struct sk_buff *res_skb;
+	u8 param[4];
+	int r;
+
+	param[0] = 0;
+	while (count--) {
+		param[1] = p->adr[0];
+		param[2] = p->adr[1];
+		param[3] = p->value;
+
+		r = nfc_hci_send_cmd(hdev, PN544_SYS_MGMT_GATE, PN544_WRITE,
+				     param, 4, &res_skb);
+		if (r < 0)
+			return r;
+
+		if (res_skb->len != 1) {
+			kfree_skb(res_skb);
+			return -EPROTO;
+		}
+
+		if (res_skb->data[0] != p->value) {
+			kfree_skb(res_skb);
+			return -EIO;
+		}
+
+		kfree_skb(res_skb);
+
+		p++;
+	}
+
+	param[0] = NFC_HCI_UICC_HOST_ID;
+	r = nfc_hci_set_param(hdev, NFC_HCI_ADMIN_GATE,
+			      NFC_HCI_ADMIN_WHITELIST, param, 1);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x3d;
+	r = nfc_hci_set_param(hdev, PN544_SYS_MGMT_GATE,
+			      PN544_SYS_MGMT_INFO_NOTIFICATION, param, 1);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x0;
+	r = nfc_hci_set_param(hdev, NFC_HCI_RF_READER_A_GATE,
+			      PN544_RF_READER_A_AUTO_ACTIVATION, param, 1);
+	if (r < 0)
+		return r;
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_END_OPERATION, NULL, 0);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x1;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_NFCT_DEACTIVATED, param, 1);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x0;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_RDPHASES, param, 1);
+	if (r < 0)
+		return r;
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
+			      PN544_ID_MGMT_FULL_VERSION_SW, &skb);
+	if (r < 0)
+		return r;
+
+	if (skb->len != FULL_VERSION_LEN) {
+		kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	print_hex_dump(KERN_DEBUG, "FULL VERSION SOFTWARE INFO: ",
+		       DUMP_PREFIX_NONE, 16, 1,
+		       skb->data, FULL_VERSION_LEN, false);
+
+	kfree_skb(skb);
+
+	return 0;
+}
+
+static int pn544_hci_xmit(struct nfc_shdlc *shdlc, struct sk_buff *skb)
+{
+	struct pn544_hci_info *info = nfc_shdlc_get_clientdata(shdlc);
+	struct i2c_client *client = info->i2c_dev;
+
+	if (info->hard_fault != 0)
+		return info->hard_fault;
+
+	return pn544_hci_i2c_write(client, skb->data, skb->len);
+}
+
+static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	u8 phases = 0;
+	int r;
+	u8 duration[2];
+	u8 activated;
+
+	pr_info(DRIVER_DESC ": %s protocols = %d\n", __func__, protocols);
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_END_OPERATION, NULL, 0);
+	if (r < 0)
+		return r;
+
+	duration[0] = 0x18;
+	duration[1] = 0x6a;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_EMULATION, duration, 2);
+	if (r < 0)
+		return r;
+
+	activated = 0;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_NFCT_DEACTIVATED, &activated, 1);
+	if (r < 0)
+		return r;
+
+	if (protocols & (NFC_PROTO_ISO14443_MASK | NFC_PROTO_MIFARE_MASK |
+			 NFC_PROTO_JEWEL_MASK))
+		phases |= 1;		/* Type A */
+	if (protocols & NFC_PROTO_FELICA_MASK) {
+		phases |= (1 << 2);	/* Type F 212 */
+		phases |= (1 << 3);	/* Type F 424 */
+	}
+
+	phases |= (1 << 5);		/* NFC active */
+
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_RDPHASES, &phases, 1);
+	if (r < 0)
+		return r;
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
+	if (r < 0)
+		nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+				   NFC_HCI_EVT_END_OPERATION, NULL, 0);
+
+	return r;
+}
+
+static int pn544_hci_target_from_gate(struct nfc_shdlc *shdlc, u8 gate,
+				      struct nfc_target *target)
+{
+	switch (gate) {
+	case PN544_RF_READER_F_GATE:
+		target->supported_protocols = NFC_PROTO_FELICA_MASK;
+		break;
+	case PN544_RF_READER_JEWEL_GATE:
+		target->supported_protocols = NFC_PROTO_JEWEL_MASK;
+		target->sens_res = 0x0c00;
+		break;
+	default:
+		return -EPROTO;
+	}
+
+	return 0;
+}
+
+static int pn544_hci_complete_target_discovered(struct nfc_shdlc *shdlc,
+						u8 gate,
+						struct nfc_target *target)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	struct sk_buff *uid_skb;
+	int r = 0;
+
+	if (target->supported_protocols & NFC_PROTO_MIFARE_MASK) {
+		if (target->nfcid1_len != 4 && target->nfcid1_len != 7 &&
+		    target->nfcid1_len != 10)
+			return -EPROTO;
+
+		r = nfc_hci_send_cmd(hdev, NFC_HCI_RF_READER_A_GATE,
+				     PN544_RF_READER_CMD_ACTIVATE_NEXT,
+				     target->nfcid1, target->nfcid1_len, NULL);
+	} else if (target->supported_protocols & NFC_PROTO_FELICA_MASK) {
+		r = nfc_hci_get_param(hdev, PN544_RF_READER_F_GATE,
+				      PN544_FELICA_ID, &uid_skb);
+		if (r < 0)
+			return r;
+
+		if (uid_skb->len != 8) {
+			kfree_skb(uid_skb);
+			return -EPROTO;
+		}
+
+		r = nfc_hci_send_cmd(hdev, PN544_RF_READER_F_GATE,
+				     PN544_RF_READER_CMD_ACTIVATE_NEXT,
+				     uid_skb->data, uid_skb->len, NULL);
+		kfree_skb(uid_skb);
+	} else if (target->supported_protocols & NFC_PROTO_ISO14443_MASK) {
+		/*
+		 * TODO: maybe other ISO 14443 require some kind of continue
+		 * activation, but for now we've seen only this one below.
+		 */
+		if (target->sens_res == 0x4403)	/* Type 4 Mifare DESFire */
+			r = nfc_hci_send_cmd(hdev, NFC_HCI_RF_READER_A_GATE,
+			      PN544_RF_READER_A_CMD_CONTINUE_ACTIVATION,
+			      NULL, 0, NULL);
+	}
+
+	return r;
+}
+
+#define MIFARE_CMD_AUTH_KEY_A	0x60
+#define MIFARE_CMD_AUTH_KEY_B	0x61
+#define MIFARE_CMD_HEADER	2
+#define MIFARE_UID_LEN		4
+#define MIFARE_KEY_LEN		6
+#define MIFARE_CMD_LEN		12
+/*
+ * Returns:
+ * <= 0: driver handled the data exchange
+ *    1: driver doesn't especially handle, please do standard processing
+ */
+static int pn544_hci_data_exchange(struct nfc_shdlc *shdlc,
+				   struct nfc_target *target,
+				   struct sk_buff *skb,
+				   struct sk_buff **res_skb)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	int r;
+
+	pr_info(DRIVER_DESC ": %s for gate=%d\n", __func__,
+		target->hci_reader_gate);
+
+	switch (target->hci_reader_gate) {
+	case NFC_HCI_RF_READER_A_GATE:
+		if (target->supported_protocols & NFC_PROTO_MIFARE_MASK) {
+			/*
+			 * It seems that pn544 is inverting key and UID for
+			 * MIFARE authentication commands.
+			 */
+			if (skb->len == MIFARE_CMD_LEN &&
+			    (skb->data[0] == MIFARE_CMD_AUTH_KEY_A ||
+			     skb->data[0] == MIFARE_CMD_AUTH_KEY_B)) {
+				u8 uid[MIFARE_UID_LEN];
+				u8 *data = skb->data + MIFARE_CMD_HEADER;
+
+				memcpy(uid, data + MIFARE_KEY_LEN,
+				       MIFARE_UID_LEN);
+				memmove(data + MIFARE_UID_LEN, data,
+					MIFARE_KEY_LEN);
+				memcpy(data, uid, MIFARE_UID_LEN);
+			}
+
+			return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+						PN544_MIFARE_CMD,
+						skb->data, skb->len, res_skb);
+		} else
+			return 1;
+	case PN544_RF_READER_F_GATE:
+		*skb_push(skb, 1) = 0;
+		*skb_push(skb, 1) = 0;
+
+		r = nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+				     PN544_FELICA_RAW,
+				     skb->data, skb->len, res_skb);
+		if (r == 0)
+			skb_pull(*res_skb, 1);
+		return r;
+	case PN544_RF_READER_JEWEL_GATE:
+		return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+					PN544_JEWEL_RAW_CMD,
+					skb->data, skb->len, res_skb);
+	default:
+		return 1;
+	}
+}
+
+static int pn544_hci_check_presence(struct nfc_shdlc *shdlc,
+				   struct nfc_target *target)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+
+	return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+				PN544_RF_READER_CMD_PRESENCE_CHECK,
+				NULL, 0, NULL);
+}
+
+static struct nfc_shdlc_ops pn544_shdlc_ops = {
+	.open = pn544_hci_open,
+	.close = pn544_hci_close,
+	.hci_ready = pn544_hci_ready,
+	.xmit = pn544_hci_xmit,
+	.start_poll = pn544_hci_start_poll,
+	.target_from_gate = pn544_hci_target_from_gate,
+	.complete_target_discovered = pn544_hci_complete_target_discovered,
+	.data_exchange = pn544_hci_data_exchange,
+	.check_presence = pn544_hci_check_presence,
+};
+
+static int __devinit pn544_hci_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct pn544_hci_info *info;
+	struct pn544_nfc_platform_data *pdata;
+	int r = 0;
+	u32 protocols;
+	struct nfc_hci_init_data init_data;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+	dev_dbg(&client->dev, "IRQ: %d\n", client->irq);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "Need I2C_FUNC_I2C\n");
+		return -ENODEV;
+	}
+
+	info = kzalloc(sizeof(struct pn544_hci_info), GFP_KERNEL);
+	if (!info) {
+		dev_err(&client->dev,
+			"Cannot allocate memory for pn544_hci_info.\n");
+		r = -ENOMEM;
+		goto err_info_alloc;
+	}
+
+	info->i2c_dev = client;
+	info->state = PN544_ST_COLD;
+	mutex_init(&info->info_lock);
+	i2c_set_clientdata(client, info);
+
+	pdata = client->dev.platform_data;
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		r = -EINVAL;
+		goto err_pdata;
+	}
+
+	if (pdata->request_resources == NULL) {
+		dev_err(&client->dev, "request_resources() missing\n");
+		r = -EINVAL;
+		goto err_pdata;
+	}
+
+	r = pdata->request_resources(client);
+	if (r) {
+		dev_err(&client->dev, "Cannot get platform resources\n");
+		goto err_pdata;
+	}
+
+	info->gpio_en = pdata->get_gpio(NFC_GPIO_ENABLE);
+	info->gpio_fw = pdata->get_gpio(NFC_GPIO_FW_RESET);
+	info->gpio_irq = pdata->get_gpio(NFC_GPIO_IRQ);
+
+	pn544_hci_platform_init(info);
+
+	r = request_threaded_irq(client->irq, NULL, pn544_hci_irq_thread_fn,
+				 IRQF_TRIGGER_RISING, PN544_HCI_DRIVER_NAME,
+				 info);
+	if (r < 0) {
+		dev_err(&client->dev, "Unable to register IRQ handler\n");
+		goto err_rti;
+	}
+
+	init_data.gate_count = ARRAY_SIZE(pn544_custom_gates);
+
+	memcpy(init_data.gates, pn544_custom_gates,
+	       ARRAY_SIZE(pn544_custom_gates));
+
+	/*
+	 * TODO: Session id must include the driver name + some bus addr
+	 * persistent info to discriminate 2 identical chips
+	 */
+	strcpy(init_data.session_id, "ID544HCI");
+
+	protocols = NFC_PROTO_JEWEL_MASK |
+		    NFC_PROTO_MIFARE_MASK |
+		    NFC_PROTO_FELICA_MASK |
+		    NFC_PROTO_ISO14443_MASK |
+		    NFC_PROTO_NFC_DEP_MASK;
+
+	info->shdlc = nfc_shdlc_allocate(&pn544_shdlc_ops,
+					 &init_data, protocols,
+					 PN544_CMDS_HEADROOM, 0,
+					 PN544_HCI_LLC_MAX_PAYLOAD,
+					 dev_name(&client->dev));
+	if (!info->shdlc) {
+		dev_err(&client->dev, "Cannot allocate nfc shdlc.\n");
+		r = -ENOMEM;
+		goto err_allocshdlc;
+	}
+
+	nfc_shdlc_set_clientdata(info->shdlc, info);
+
+	return 0;
+
+err_allocshdlc:
+	free_irq(client->irq, info);
+
+err_rti:
+	if (pdata->free_resources != NULL)
+		pdata->free_resources();
+
+err_pdata:
+	kfree(info);
+
+err_info_alloc:
+	return r;
+}
+
+static __devexit int pn544_hci_remove(struct i2c_client *client)
+{
+	struct pn544_hci_info *info = i2c_get_clientdata(client);
+	struct pn544_nfc_platform_data *pdata = client->dev.platform_data;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	nfc_shdlc_free(info->shdlc);
+
+	if (info->state != PN544_ST_COLD) {
+		if (pdata->disable)
+			pdata->disable();
+	}
+
+	free_irq(client->irq, info);
+	if (pdata->free_resources)
+		pdata->free_resources();
+
+	kfree(info);
+
+	return 0;
+}
+
+static struct i2c_driver pn544_hci_driver = {
+	.driver = {
+		   .name = PN544_HCI_DRIVER_NAME,
+		  },
+	.probe = pn544_hci_probe,
+	.id_table = pn544_hci_id_table,
+	.remove = __devexit_p(pn544_hci_remove),
+};
+
+static int __init pn544_hci_init(void)
+{
+	int r;
+
+	pr_debug(DRIVER_DESC ": %s\n", __func__);
+
+	r = i2c_add_driver(&pn544_hci_driver);
+	if (r) {
+		pr_err(PN544_HCI_DRIVER_NAME ": driver registration failed\n");
+		return r;
+	}
+
+	return 0;
+}
+
+static void __exit pn544_hci_exit(void)
+{
+	i2c_del_driver(&pn544_hci_driver);
+}
+
+module_init(pn544_hci_init);
+module_exit(pn544_hci_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index a55e248618cd..86c63fe45d11 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -27,6 +27,7 @@
 #include <linux/security.h>
 #include <linux/pci-aspm.h>
 #include <linux/slab.h>
+#include <linux/vgaarb.h>
 #include "pci.h"
 
 static int sysfs_initialized;	/* = 0 */
@@ -417,6 +418,10 @@ static ssize_t
 boot_vga_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_dev *vga_dev = vga_default_device();
+
+	if (vga_dev)
+		return sprintf(buf, "%u\n", (pdev == vga_dev));
 
 	return sprintf(buf, "%u\n",
 		!!(pdev->resource[PCI_ROM_RESOURCE].flags &
diff --git a/drivers/ssb/b43_pci_bridge.c b/drivers/ssb/b43_pci_bridge.c
index bad7ba517a1c..f551e5376147 100644
--- a/drivers/ssb/b43_pci_bridge.c
+++ b/drivers/ssb/b43_pci_bridge.c
@@ -29,6 +29,8 @@ static const struct pci_device_id b43_pci_bridge_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4319) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4320) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4321) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4322) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 43222) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4324) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4325) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4328) },
diff --git a/drivers/ssb/pci.c b/drivers/ssb/pci.c
index ed4124469a3a..e9d94968f394 100644
--- a/drivers/ssb/pci.c
+++ b/drivers/ssb/pci.c
@@ -178,6 +178,18 @@ err_pci:
 #define SPEX(_outvar, _offset, _mask, _shift) \
 	SPEX16(_outvar, _offset, _mask, _shift)
 
+#define SPEX_ARRAY8(_field, _offset, _mask, _shift)	\
+	do {	\
+		SPEX(_field[0], _offset +  0, _mask, _shift);	\
+		SPEX(_field[1], _offset +  2, _mask, _shift);	\
+		SPEX(_field[2], _offset +  4, _mask, _shift);	\
+		SPEX(_field[3], _offset +  6, _mask, _shift);	\
+		SPEX(_field[4], _offset +  8, _mask, _shift);	\
+		SPEX(_field[5], _offset + 10, _mask, _shift);	\
+		SPEX(_field[6], _offset + 12, _mask, _shift);	\
+		SPEX(_field[7], _offset + 14, _mask, _shift);	\
+	} while (0)
+
 
 static inline u8 ssb_crc8(u8 crc, u8 data)
 {
@@ -360,8 +372,9 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 	SPEX(et0mdcport, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET0M, 14);
 	SPEX(et1mdcport, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET1M, 15);
 	SPEX(board_rev, SSB_SPROM1_BINF, SSB_SPROM1_BINF_BREV, 0);
-	SPEX(country_code, SSB_SPROM1_BINF, SSB_SPROM1_BINF_CCODE,
-	     SSB_SPROM1_BINF_CCODE_SHIFT);
+	if (out->revision == 1)
+		SPEX(country_code, SSB_SPROM1_BINF, SSB_SPROM1_BINF_CCODE,
+		     SSB_SPROM1_BINF_CCODE_SHIFT);
 	SPEX(ant_available_a, SSB_SPROM1_BINF, SSB_SPROM1_BINF_ANTA,
 	     SSB_SPROM1_BINF_ANTA_SHIFT);
 	SPEX(ant_available_bg, SSB_SPROM1_BINF, SSB_SPROM1_BINF_ANTBG,
@@ -387,6 +400,8 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 	SPEX(boardflags_lo, SSB_SPROM1_BFLLO, 0xFFFF, 0);
 	if (out->revision >= 2)
 		SPEX(boardflags_hi, SSB_SPROM2_BFLHI, 0xFFFF, 0);
+	SPEX(alpha2[0], SSB_SPROM1_CCODE, 0xff00, 8);
+	SPEX(alpha2[1], SSB_SPROM1_CCODE, 0x00ff, 0);
 
 	/* Extract the antenna gain values. */
 	out->antenna_gain.a0 = r123_extract_antgain(out->revision, in,
@@ -455,14 +470,17 @@ static void sprom_extract_r45(struct ssb_sprom *out, const u16 *in)
 	SPEX(et0phyaddr, SSB_SPROM4_ETHPHY, SSB_SPROM4_ETHPHY_ET0A, 0);
 	SPEX(et1phyaddr, SSB_SPROM4_ETHPHY, SSB_SPROM4_ETHPHY_ET1A,
 	     SSB_SPROM4_ETHPHY_ET1A_SHIFT);
+	SPEX(board_rev, SSB_SPROM4_BOARDREV, 0xFFFF, 0);
 	if (out->revision == 4) {
-		SPEX(country_code, SSB_SPROM4_CCODE, 0xFFFF, 0);
+		SPEX(alpha2[0], SSB_SPROM4_CCODE, 0xff00, 8);
+		SPEX(alpha2[1], SSB_SPROM4_CCODE, 0x00ff, 0);
 		SPEX(boardflags_lo, SSB_SPROM4_BFLLO, 0xFFFF, 0);
 		SPEX(boardflags_hi, SSB_SPROM4_BFLHI, 0xFFFF, 0);
 		SPEX(boardflags2_lo, SSB_SPROM4_BFL2LO, 0xFFFF, 0);
 		SPEX(boardflags2_hi, SSB_SPROM4_BFL2HI, 0xFFFF, 0);
 	} else {
-		SPEX(country_code, SSB_SPROM5_CCODE, 0xFFFF, 0);
+		SPEX(alpha2[0], SSB_SPROM5_CCODE, 0xff00, 8);
+		SPEX(alpha2[1], SSB_SPROM5_CCODE, 0x00ff, 0);
 		SPEX(boardflags_lo, SSB_SPROM5_BFLLO, 0xFFFF, 0);
 		SPEX(boardflags_hi, SSB_SPROM5_BFLHI, 0xFFFF, 0);
 		SPEX(boardflags2_lo, SSB_SPROM5_BFL2LO, 0xFFFF, 0);
@@ -525,7 +543,9 @@ static void sprom_extract_r8(struct ssb_sprom *out, const u16 *in)
 		v = in[SPOFF(SSB_SPROM8_IL0MAC) + i];
 		*(((__be16 *)out->il0mac) + i) = cpu_to_be16(v);
 	}
-	SPEX(country_code, SSB_SPROM8_CCODE, 0xFFFF, 0);
+	SPEX(board_rev, SSB_SPROM8_BOARDREV, 0xFFFF, 0);
+	SPEX(alpha2[0], SSB_SPROM8_CCODE, 0xff00, 8);
+	SPEX(alpha2[1], SSB_SPROM8_CCODE, 0x00ff, 0);
 	SPEX(boardflags_lo, SSB_SPROM8_BFLLO, 0xFFFF, 0);
 	SPEX(boardflags_hi, SSB_SPROM8_BFLHI, 0xFFFF, 0);
 	SPEX(boardflags2_lo, SSB_SPROM8_BFL2LO, 0xFFFF, 0);
@@ -655,6 +675,63 @@ static void sprom_extract_r8(struct ssb_sprom *out, const u16 *in)
 	SPEX(fem.ghz5.antswlut, SSB_SPROM8_FEM5G,
 		SSB_SROM8_FEM_ANTSWLUT, SSB_SROM8_FEM_ANTSWLUT_SHIFT);
 
+	SPEX(leddc_on_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_ON,
+	     SSB_SPROM8_LEDDC_ON_SHIFT);
+	SPEX(leddc_off_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_OFF,
+	     SSB_SPROM8_LEDDC_OFF_SHIFT);
+
+	SPEX(txchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_TXCHAIN,
+	     SSB_SPROM8_TXRXC_TXCHAIN_SHIFT);
+	SPEX(rxchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_RXCHAIN,
+	     SSB_SPROM8_TXRXC_RXCHAIN_SHIFT);
+	SPEX(antswitch, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_SWITCH,
+	     SSB_SPROM8_TXRXC_SWITCH_SHIFT);
+
+	SPEX(opo, SSB_SPROM8_OFDM2GPO, 0x00ff, 0);
+
+	SPEX_ARRAY8(mcs2gpo, SSB_SPROM8_2G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5gpo, SSB_SPROM8_5G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5glpo, SSB_SPROM8_5GL_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5ghpo, SSB_SPROM8_5GH_MCSPO, ~0, 0);
+
+	SPEX(rawtempsense, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_RAWTEMP,
+	     SSB_SPROM8_RAWTS_RAWTEMP_SHIFT);
+	SPEX(measpower, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_MEASPOWER,
+	     SSB_SPROM8_RAWTS_MEASPOWER_SHIFT);
+	SPEX(tempsense_slope, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE_SHIFT);
+	SPEX(tempcorrx, SSB_SPROM8_OPT_CORRX, SSB_SPROM8_OPT_CORRX_TEMPCORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMPCORRX_SHIFT);
+	SPEX(tempsense_option, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION_SHIFT);
+	SPEX(freqoffset_corr, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR_SHIFT);
+	SPEX(iqcal_swp_dis, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP_SHIFT);
+	SPEX(hw_iqcal_en, SSB_SPROM8_HWIQ_IQSWP, SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL,
+	     SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL_SHIFT);
+
+	SPEX(bw40po, SSB_SPROM8_BW40PO, ~0, 0);
+	SPEX(cddpo, SSB_SPROM8_CDDPO, ~0, 0);
+	SPEX(stbcpo, SSB_SPROM8_STBCPO, ~0, 0);
+	SPEX(bwduppo, SSB_SPROM8_BWDUPPO, ~0, 0);
+
+	SPEX(tempthresh, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_TRESH,
+	     SSB_SPROM8_THERMAL_TRESH_SHIFT);
+	SPEX(tempoffset, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_OFFSET,
+	     SSB_SPROM8_THERMAL_OFFSET_SHIFT);
+	SPEX(phycal_tempdelta, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL_SHIFT);
+	SPEX(temps_period, SSB_SPROM8_TEMPDELTA, SSB_SPROM8_TEMPDELTA_PERIOD,
+	     SSB_SPROM8_TEMPDELTA_PERIOD_SHIFT);
+	SPEX(temps_hysteresis, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS_SHIFT);
 	sprom_extract_r458(out, in);
 
 	/* TODO - get remaining rev 8 stuff needed */
@@ -784,7 +861,6 @@ static void ssb_pci_get_boardinfo(struct ssb_bus *bus,
 {
 	bi->vendor = bus->host_pci->subsystem_vendor;
 	bi->type = bus->host_pci->subsystem_device;
-	bi->rev = bus->host_pci->revision;
 }
 
 int ssb_pci_get_invariants(struct ssb_bus *bus,
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index 9f1f27e7c86e..4511420849bc 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -269,7 +269,7 @@ out:
 	return ret;
 }
 
-static inline unsigned long calc_vm_may_flags(unsigned long prot)
+static inline vm_flags_t calc_vm_may_flags(unsigned long prot)
 {
 	return _calc_vm_trans(prot, PROT_READ,  VM_MAYREAD) |
 	       _calc_vm_trans(prot, PROT_WRITE, VM_MAYWRITE) |
diff --git a/drivers/staging/media/as102/as10x_cmd.c b/drivers/staging/media/as102/as10x_cmd.c
index 262bb94ad27e..a73df10982d0 100644
--- a/drivers/staging/media/as102/as10x_cmd.c
+++ b/drivers/staging/media/as102/as10x_cmd.c
@@ -31,7 +31,7 @@
  */
 int as10x_cmd_turn_on(struct as10x_bus_adapter_t *adap)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -54,8 +54,6 @@ int as10x_cmd_turn_on(struct as10x_bus_adapter_t *adap)
 					    (uint8_t *) prsp,
 					    sizeof(prsp->body.turn_on.rsp) +
 					    HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -77,7 +75,7 @@ out:
  */
 int as10x_cmd_turn_off(struct as10x_bus_adapter_t *adap)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -99,8 +97,6 @@ int as10x_cmd_turn_off(struct as10x_bus_adapter_t *adap)
 			sizeof(pcmd->body.turn_off.req) + HEADER_SIZE,
 			(uint8_t *) prsp,
 			sizeof(prsp->body.turn_off.rsp) + HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -124,7 +120,7 @@ out:
 int as10x_cmd_set_tune(struct as10x_bus_adapter_t *adap,
 		       struct as10x_tune_args *ptune)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *preq, *prsp;
 
 	ENTER();
@@ -159,8 +155,6 @@ int as10x_cmd_set_tune(struct as10x_bus_adapter_t *adap,
 					    (uint8_t *) prsp,
 					    sizeof(prsp->body.set_tune.rsp)
 					    + HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -184,7 +178,7 @@ out:
 int as10x_cmd_get_tune_status(struct as10x_bus_adapter_t *adap,
 			      struct as10x_tune_status *pstatus)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t  *preq, *prsp;
 
 	ENTER();
@@ -208,8 +202,6 @@ int as10x_cmd_get_tune_status(struct as10x_bus_adapter_t *adap,
 			sizeof(preq->body.get_tune_status.req) + HEADER_SIZE,
 			(uint8_t *) prsp,
 			sizeof(prsp->body.get_tune_status.rsp) + HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -241,7 +233,7 @@ out:
  */
 int as10x_cmd_get_tps(struct as10x_bus_adapter_t *adap, struct as10x_tps *ptps)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -266,8 +258,6 @@ int as10x_cmd_get_tps(struct as10x_bus_adapter_t *adap, struct as10x_tps *ptps)
 					    (uint8_t *) prsp,
 					    sizeof(prsp->body.get_tps.rsp) +
 					    HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -305,7 +295,7 @@ out:
 int as10x_cmd_get_demod_stats(struct as10x_bus_adapter_t *adap,
 			      struct as10x_demod_stats *pdemod_stats)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -330,8 +320,6 @@ int as10x_cmd_get_demod_stats(struct as10x_bus_adapter_t *adap,
 				(uint8_t *) prsp,
 				sizeof(prsp->body.get_demod_stats.rsp)
 				+ HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -370,7 +358,7 @@ out:
 int as10x_cmd_get_impulse_resp(struct as10x_bus_adapter_t *adap,
 			       uint8_t *is_ready)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -395,8 +383,6 @@ int as10x_cmd_get_impulse_resp(struct as10x_bus_adapter_t *adap,
 					(uint8_t *) prsp,
 					sizeof(prsp->body.get_impulse_rsp.rsp)
 					+ HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.c b/drivers/staging/media/dt3155v4l/dt3155v4l.c
index 280c84ec4cc2..c365cdf714ea 100644
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.c
+++ b/drivers/staging/media/dt3155v4l/dt3155v4l.c
@@ -898,6 +898,10 @@ dt3155_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	INIT_LIST_HEAD(&pd->dmaq);
 	mutex_init(&pd->mux);
 	pd->vdev->lock = &pd->mux; /* for locking v4l2_file_operations */
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &pd->vdev->flags);
 	spin_lock_init(&pd->lock);
 	pd->csr2 = csr2_init;
 	pd->config = config_init;
diff --git a/drivers/staging/media/easycap/easycap_main.c b/drivers/staging/media/easycap/easycap_main.c
index 6f83d362ab0d..a1c45e4dcdce 100644
--- a/drivers/staging/media/easycap/easycap_main.c
+++ b/drivers/staging/media/easycap/easycap_main.c
@@ -700,214 +700,7 @@ static int videodev_release(struct video_device *pvideo_device)
 	JOM(4, "ending successfully\n");
 	return 0;
 }
-/*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*/
-/*****************************************************************************/
-/*--------------------------------------------------------------------------*/
-/*
- *  THIS FUNCTION IS CALLED FROM WITHIN easycap_usb_disconnect() AND IS
- *  PROTECTED BY SEMAPHORES SET AND CLEARED BY easycap_usb_disconnect().
- *
- *  BY THIS STAGE THE DEVICE HAS ALREADY BEEN PHYSICALLY UNPLUGGED, SO
- *  peasycap->pusb_device IS NO LONGER VALID.
- */
-/*---------------------------------------------------------------------------*/
-static void easycap_delete(struct kref *pkref)
-{
-	struct easycap *peasycap;
-	struct data_urb *pdata_urb;
-	struct list_head *plist_head, *plist_next;
-	int k, m, gone, kd;
-	int allocation_video_urb;
-	int allocation_video_page;
-	int allocation_video_struct;
-	int allocation_audio_urb;
-	int allocation_audio_page;
-	int allocation_audio_struct;
-	int registered_video, registered_audio;
-
-	peasycap = container_of(pkref, struct easycap, kref);
-	if (!peasycap) {
-		SAM("ERROR: peasycap is NULL: cannot perform deletions\n");
-		return;
-	}
-	kd = easycap_isdongle(peasycap);
-/*---------------------------------------------------------------------------*/
-/*
- *  FREE VIDEO.
- */
-/*---------------------------------------------------------------------------*/
-	if (peasycap->purb_video_head) {
-		m = 0;
-		list_for_each(plist_head, peasycap->purb_video_head) {
-			pdata_urb = list_entry(plist_head,
-						struct data_urb, list_head);
-			if (pdata_urb && pdata_urb->purb) {
-				usb_free_urb(pdata_urb->purb);
-				pdata_urb->purb = NULL;
-				peasycap->allocation_video_urb--;
-				m++;
-			}
-		}
-
-		JOM(4, "%i video urbs freed\n", m);
-/*---------------------------------------------------------------------------*/
-		JOM(4, "freeing video data_urb structures.\n");
-		m = 0;
-		list_for_each_safe(plist_head, plist_next,
-					peasycap->purb_video_head) {
-			pdata_urb = list_entry(plist_head,
-						struct data_urb, list_head);
-			if (pdata_urb) {
-				peasycap->allocation_video_struct -=
-						sizeof(struct data_urb);
-				kfree(pdata_urb);
-				m++;
-			}
-		}
-		JOM(4, "%i video data_urb structures freed\n", m);
-		JOM(4, "setting peasycap->purb_video_head=NULL\n");
-		peasycap->purb_video_head = NULL;
-	}
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing video isoc buffers.\n");
-	m = 0;
-	for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY;  k++) {
-		if (peasycap->video_isoc_buffer[k].pgo) {
-			free_pages((unsigned long)
-				   peasycap->video_isoc_buffer[k].pgo,
-					VIDEO_ISOC_ORDER);
-			peasycap->video_isoc_buffer[k].pgo = NULL;
-			peasycap->allocation_video_page -=
-						BIT(VIDEO_ISOC_ORDER);
-			m++;
-		}
-	}
-	JOM(4, "isoc video buffers freed: %i pages\n",
-			m * (0x01 << VIDEO_ISOC_ORDER));
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing video field buffers.\n");
-	gone = 0;
-	for (k = 0;  k < FIELD_BUFFER_MANY;  k++) {
-		for (m = 0;  m < FIELD_BUFFER_SIZE/PAGE_SIZE;  m++) {
-			if (peasycap->field_buffer[k][m].pgo) {
-				free_page((unsigned long)
-					  peasycap->field_buffer[k][m].pgo);
-				peasycap->field_buffer[k][m].pgo = NULL;
-				peasycap->allocation_video_page -= 1;
-				gone++;
-			}
-		}
-	}
-	JOM(4, "video field buffers freed: %i pages\n", gone);
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing video frame buffers.\n");
-	gone = 0;
-	for (k = 0;  k < FRAME_BUFFER_MANY;  k++) {
-		for (m = 0;  m < FRAME_BUFFER_SIZE/PAGE_SIZE;  m++) {
-			if (peasycap->frame_buffer[k][m].pgo) {
-				free_page((unsigned long)
-					  peasycap->frame_buffer[k][m].pgo);
-				peasycap->frame_buffer[k][m].pgo = NULL;
-				peasycap->allocation_video_page -= 1;
-				gone++;
-			}
-		}
-	}
-	JOM(4, "video frame buffers freed: %i pages\n", gone);
-/*---------------------------------------------------------------------------*/
-/*
- *  FREE AUDIO.
- */
-/*---------------------------------------------------------------------------*/
-	if (peasycap->purb_audio_head) {
-		JOM(4, "freeing audio urbs\n");
-		m = 0;
-		list_for_each(plist_head, (peasycap->purb_audio_head)) {
-			pdata_urb = list_entry(plist_head,
-					struct data_urb, list_head);
-			if (pdata_urb && pdata_urb->purb) {
-				usb_free_urb(pdata_urb->purb);
-				pdata_urb->purb = NULL;
-				peasycap->allocation_audio_urb--;
-				m++;
-			}
-		}
-		JOM(4, "%i audio urbs freed\n", m);
-/*---------------------------------------------------------------------------*/
-		JOM(4, "freeing audio data_urb structures.\n");
-		m = 0;
-		list_for_each_safe(plist_head, plist_next,
-					peasycap->purb_audio_head) {
-			pdata_urb = list_entry(plist_head,
-					struct data_urb, list_head);
-			if (pdata_urb) {
-				peasycap->allocation_audio_struct -=
-							sizeof(struct data_urb);
-				kfree(pdata_urb);
-				m++;
-			}
-		}
-		JOM(4, "%i audio data_urb structures freed\n", m);
-		JOM(4, "setting peasycap->purb_audio_head=NULL\n");
-		peasycap->purb_audio_head = NULL;
-	}
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing audio isoc buffers.\n");
-	m = 0;
-	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
-		if (peasycap->audio_isoc_buffer[k].pgo) {
-			free_pages((unsigned long)
-					(peasycap->audio_isoc_buffer[k].pgo),
-					AUDIO_ISOC_ORDER);
-			peasycap->audio_isoc_buffer[k].pgo = NULL;
-			peasycap->allocation_audio_page -=
-					BIT(AUDIO_ISOC_ORDER);
-			m++;
-		}
-	}
-	JOM(4, "easyoss_delete(): isoc audio buffers freed: %i pages\n",
-					m * (0x01 << AUDIO_ISOC_ORDER));
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing easycap structure.\n");
-	allocation_video_urb    = peasycap->allocation_video_urb;
-	allocation_video_page   = peasycap->allocation_video_page;
-	allocation_video_struct = peasycap->allocation_video_struct;
-	registered_video        = peasycap->registered_video;
-	allocation_audio_urb    = peasycap->allocation_audio_urb;
-	allocation_audio_page   = peasycap->allocation_audio_page;
-	allocation_audio_struct = peasycap->allocation_audio_struct;
-	registered_audio        = peasycap->registered_audio;
-
-	if (0 <= kd && DONGLE_MANY > kd) {
-		if (mutex_lock_interruptible(&mutex_dongle)) {
-			SAY("ERROR: cannot down mutex_dongle\n");
-		} else {
-			JOM(4, "locked mutex_dongle\n");
-			easycapdc60_dongle[kd].peasycap = NULL;
-			mutex_unlock(&mutex_dongle);
-			JOM(4, "unlocked mutex_dongle\n");
-			JOT(4, "   null-->dongle[%i].peasycap\n", kd);
-			allocation_video_struct -= sizeof(struct easycap);
-		}
-	} else {
-		SAY("ERROR: cannot purge dongle[].peasycap");
-	}
 
-	kfree(peasycap);
-
-/*---------------------------------------------------------------------------*/
-	SAY("%8i=video urbs    after all deletions\n", allocation_video_urb);
-	SAY("%8i=video pages   after all deletions\n", allocation_video_page);
-	SAY("%8i=video structs after all deletions\n", allocation_video_struct);
-	SAY("%8i=video devices after all deletions\n", registered_video);
-	SAY("%8i=audio urbs    after all deletions\n", allocation_audio_urb);
-	SAY("%8i=audio pages   after all deletions\n", allocation_audio_page);
-	SAY("%8i=audio structs after all deletions\n", allocation_audio_struct);
-	SAY("%8i=audio devices after all deletions\n", registered_audio);
-
-	JOT(4, "ending.\n");
-	return;
-}
 /*****************************************************************************/
 static unsigned int easycap_poll(struct file *file, poll_table *wait)
 {
@@ -2842,6 +2635,813 @@ static void easycap_complete(struct urb *purb)
 	return;
 }
 
+static struct easycap *alloc_easycap(u8 bInterfaceNumber)
+{
+	struct easycap *peasycap;
+	int i;
+
+	peasycap = kzalloc(sizeof(struct easycap), GFP_KERNEL);
+	if (!peasycap) {
+		SAY("ERROR: Could not allocate peasycap\n");
+		return NULL;
+	}
+
+	if (mutex_lock_interruptible(&mutex_dongle)) {
+		SAY("ERROR: cannot lock mutex_dongle\n");
+		kfree(peasycap);
+		return NULL;
+	}
+
+	/* Find a free dongle in easycapdc60_dongle array */
+	for (i = 0; i < DONGLE_MANY; i++) {
+
+		if ((!easycapdc60_dongle[i].peasycap) &&
+		    (!mutex_is_locked(&easycapdc60_dongle[i].mutex_video)) &&
+		    (!mutex_is_locked(&easycapdc60_dongle[i].mutex_audio))) {
+
+			easycapdc60_dongle[i].peasycap = peasycap;
+			peasycap->isdongle = i;
+			JOM(8, "intf[%i]: peasycap-->easycap"
+				"_dongle[%i].peasycap\n",
+				bInterfaceNumber, i);
+			break;
+		}
+	}
+
+	mutex_unlock(&mutex_dongle);
+
+	if (i >= DONGLE_MANY) {
+		SAM("ERROR: too many dongles\n");
+		kfree(peasycap);
+		return NULL;
+	}
+
+	return peasycap;
+}
+
+static void free_easycap(struct easycap *peasycap)
+{
+	int allocation_video_urb;
+	int allocation_video_page;
+	int allocation_video_struct;
+	int allocation_audio_urb;
+	int allocation_audio_page;
+	int allocation_audio_struct;
+	int registered_video, registered_audio;
+	int kd;
+
+	JOM(4, "freeing easycap structure.\n");
+	allocation_video_urb    = peasycap->allocation_video_urb;
+	allocation_video_page   = peasycap->allocation_video_page;
+	allocation_video_struct = peasycap->allocation_video_struct;
+	registered_video        = peasycap->registered_video;
+	allocation_audio_urb    = peasycap->allocation_audio_urb;
+	allocation_audio_page   = peasycap->allocation_audio_page;
+	allocation_audio_struct = peasycap->allocation_audio_struct;
+	registered_audio        = peasycap->registered_audio;
+
+	kd = easycap_isdongle(peasycap);
+	if (0 <= kd && DONGLE_MANY > kd) {
+		if (mutex_lock_interruptible(&mutex_dongle)) {
+			SAY("ERROR: cannot down mutex_dongle\n");
+		} else {
+			JOM(4, "locked mutex_dongle\n");
+			easycapdc60_dongle[kd].peasycap = NULL;
+			mutex_unlock(&mutex_dongle);
+			JOM(4, "unlocked mutex_dongle\n");
+			JOT(4, "   null-->dongle[%i].peasycap\n", kd);
+			allocation_video_struct -= sizeof(struct easycap);
+		}
+	} else {
+		SAY("ERROR: cannot purge dongle[].peasycap");
+	}
+
+	/* Free device structure */
+	kfree(peasycap);
+
+	SAY("%8i=video urbs    after all deletions\n", allocation_video_urb);
+	SAY("%8i=video pages   after all deletions\n", allocation_video_page);
+	SAY("%8i=video structs after all deletions\n", allocation_video_struct);
+	SAY("%8i=video devices after all deletions\n", registered_video);
+	SAY("%8i=audio urbs    after all deletions\n", allocation_audio_urb);
+	SAY("%8i=audio pages   after all deletions\n", allocation_audio_page);
+	SAY("%8i=audio structs after all deletions\n", allocation_audio_struct);
+	SAY("%8i=audio devices after all deletions\n", registered_audio);
+}
+
+/*
+ * FIXME: Identify the appropriate pointer peasycap for interfaces
+ * 1 and 2. The address of peasycap->pusb_device is reluctantly used
+ * for this purpose.
+ */
+static struct easycap *get_easycap(struct usb_device *usbdev,
+				   u8 bInterfaceNumber)
+{
+	int i;
+	struct easycap *peasycap;
+
+	for (i = 0; i < DONGLE_MANY; i++) {
+		if (easycapdc60_dongle[i].peasycap->pusb_device == usbdev) {
+			peasycap = easycapdc60_dongle[i].peasycap;
+			JOT(8, "intf[%i]: dongle[%i].peasycap\n",
+					bInterfaceNumber, i);
+			break;
+		}
+	}
+	if (i >= DONGLE_MANY) {
+		SAY("ERROR: peasycap is unknown when probing interface %i\n",
+			bInterfaceNumber);
+		return NULL;
+	}
+	if (!peasycap) {
+		SAY("ERROR: peasycap is NULL when probing interface %i\n",
+			bInterfaceNumber);
+		return NULL;
+	}
+
+	return peasycap;
+}
+
+static void init_easycap(struct easycap *peasycap,
+			 struct usb_device *usbdev,
+			 struct usb_interface *intf,
+			 u8 bInterfaceNumber)
+{
+	/* Save usb_device and usb_interface */
+	peasycap->pusb_device = usbdev;
+	peasycap->pusb_interface = intf;
+
+	peasycap->minor = -1;
+	kref_init(&peasycap->kref);
+	JOM(8, "intf[%i]: after kref_init(..._video) "
+		"%i=peasycap->kref.refcount.counter\n",
+		bInterfaceNumber, peasycap->kref.refcount.counter);
+
+	/* module params */
+	peasycap->gain = (s8)clamp(easycap_gain, 0, 31);
+
+	init_waitqueue_head(&peasycap->wq_video);
+	init_waitqueue_head(&peasycap->wq_audio);
+	init_waitqueue_head(&peasycap->wq_trigger);
+
+	peasycap->allocation_video_struct = sizeof(struct easycap);
+
+	peasycap->microphone = false;
+
+	peasycap->video_interface = -1;
+	peasycap->video_altsetting_on = -1;
+	peasycap->video_altsetting_off = -1;
+	peasycap->video_endpointnumber = -1;
+	peasycap->video_isoc_maxframesize = -1;
+	peasycap->video_isoc_buffer_size = -1;
+
+	peasycap->audio_interface = -1;
+	peasycap->audio_altsetting_on = -1;
+	peasycap->audio_altsetting_off = -1;
+	peasycap->audio_endpointnumber = -1;
+	peasycap->audio_isoc_maxframesize = -1;
+	peasycap->audio_isoc_buffer_size = -1;
+
+	peasycap->frame_buffer_many = FRAME_BUFFER_MANY;
+
+	peasycap->ntsc = easycap_ntsc;
+	JOM(8, "defaulting initially to %s\n",
+		easycap_ntsc ? "NTSC" : "PAL");
+}
+
+static int populate_inputset(struct easycap *peasycap)
+{
+	struct inputset *inputset;
+	struct easycap_format *peasycap_format;
+	struct v4l2_pix_format *pix;
+	int m, i, k, mask, fmtidx;
+	s32 value;
+
+	inputset = peasycap->inputset;
+
+	fmtidx = peasycap->ntsc ? NTSC_M : PAL_BGHIN;
+
+	m = 0;
+	mask = 0;
+	for (i = 0; easycap_standard[i].mask != 0xffff; i++) {
+		if (fmtidx == easycap_standard[i].v4l2_standard.index) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].standard_offset = i;
+			mask = easycap_standard[i].mask;
+		}
+	}
+
+	if (m != 1) {
+		SAM("ERROR: inputset->standard_offset unpopulated, %i=m\n", m);
+		return -ENOENT;
+	}
+
+	peasycap_format = &easycap_format[0];
+	m = 0;
+	for (i = 0; peasycap_format->v4l2_format.fmt.pix.width; i++) {
+		pix = &peasycap_format->v4l2_format.fmt.pix;
+		if (((peasycap_format->mask & 0x0F) == (mask & 0x0F))
+			&& pix->field == V4L2_FIELD_NONE
+			&& pix->pixelformat == V4L2_PIX_FMT_UYVY
+			&& pix->width  == 640 && pix->height == 480) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].format_offset = i;
+			break;
+		}
+		peasycap_format++;
+	}
+	if (m != 1) {
+		SAM("ERROR: inputset[]->format_offset unpopulated\n");
+		return -ENOENT;
+	}
+
+	m = 0;
+	for (i = 0; easycap_control[i].id != 0xffffffff; i++) {
+		value = easycap_control[i].default_value;
+		if (V4L2_CID_BRIGHTNESS == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].brightness = value;
+		} else if (V4L2_CID_CONTRAST == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].contrast = value;
+		} else if (V4L2_CID_SATURATION == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].saturation = value;
+		} else if (V4L2_CID_HUE == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].hue = value;
+		}
+	}
+
+	if (m != 4) {
+		SAM("ERROR: inputset[]->brightness underpopulated\n");
+		return -ENOENT;
+	}
+
+	for (k = 0; k < INPUT_MANY; k++)
+		inputset[k].input = k;
+	JOM(4, "populated inputset[]\n");
+
+	return 0;
+}
+
+static int alloc_framebuffers(struct easycap *peasycap)
+{
+	int i, j;
+	void *pbuf;
+
+	JOM(4, "allocating %i frame buffers of size %li\n",
+			FRAME_BUFFER_MANY, (long int)FRAME_BUFFER_SIZE);
+	JOM(4, ".... each scattered over %li pages\n",
+			FRAME_BUFFER_SIZE/PAGE_SIZE);
+
+	for (i = 0; i < FRAME_BUFFER_MANY; i++) {
+		for (j = 0; j < FRAME_BUFFER_SIZE/PAGE_SIZE; j++) {
+			if (peasycap->frame_buffer[i][j].pgo)
+				SAM("attempting to reallocate framebuffers\n");
+			else {
+				pbuf = (void *)__get_free_page(GFP_KERNEL);
+				if (!pbuf) {
+					SAM("ERROR: Could not allocate "
+					"framebuffer %i page %i\n", i, j);
+					return -ENOMEM;
+				}
+				peasycap->allocation_video_page += 1;
+				peasycap->frame_buffer[i][j].pgo = pbuf;
+			}
+			peasycap->frame_buffer[i][j].pto =
+			    peasycap->frame_buffer[i][j].pgo;
+		}
+	}
+
+	peasycap->frame_fill = 0;
+	peasycap->frame_read = 0;
+	JOM(4, "allocation of frame buffers done: %i pages\n", i*j);
+
+	return 0;
+}
+
+static void free_framebuffers(struct easycap *peasycap)
+{
+	int k, m, gone;
+
+	JOM(4, "freeing video frame buffers.\n");
+	gone = 0;
+	for (k = 0;  k < FRAME_BUFFER_MANY;  k++) {
+		for (m = 0;  m < FRAME_BUFFER_SIZE/PAGE_SIZE;  m++) {
+			if (peasycap->frame_buffer[k][m].pgo) {
+				free_page((unsigned long)
+					peasycap->frame_buffer[k][m].pgo);
+				peasycap->frame_buffer[k][m].pgo = NULL;
+				peasycap->allocation_video_page -= 1;
+				gone++;
+			}
+		}
+	}
+	JOM(4, "video frame buffers freed: %i pages\n", gone);
+}
+
+static int alloc_fieldbuffers(struct easycap *peasycap)
+{
+	int i, j;
+	void *pbuf;
+
+	JOM(4, "allocating %i field buffers of size %li\n",
+			FIELD_BUFFER_MANY, (long int)FIELD_BUFFER_SIZE);
+	JOM(4, ".... each scattered over %li pages\n",
+			FIELD_BUFFER_SIZE/PAGE_SIZE);
+
+	for (i = 0; i < FIELD_BUFFER_MANY; i++) {
+		for (j = 0; j < FIELD_BUFFER_SIZE/PAGE_SIZE; j++) {
+			if (peasycap->field_buffer[i][j].pgo) {
+				SAM("ERROR: attempting to reallocate "
+					"fieldbuffers\n");
+			} else {
+				pbuf = (void *) __get_free_page(GFP_KERNEL);
+				if (!pbuf) {
+					SAM("ERROR: Could not allocate "
+					"fieldbuffer %i page %i\n", i, j);
+					return -ENOMEM;
+				}
+				peasycap->allocation_video_page += 1;
+				peasycap->field_buffer[i][j].pgo = pbuf;
+			}
+			peasycap->field_buffer[i][j].pto =
+				peasycap->field_buffer[i][j].pgo;
+		}
+		/* TODO: Hardcoded 0x0200 meaning? */
+		peasycap->field_buffer[i][0].kount = 0x0200;
+	}
+	peasycap->field_fill = 0;
+	peasycap->field_page = 0;
+	peasycap->field_read = 0;
+	JOM(4, "allocation of field buffers done:  %i pages\n", i*j);
+
+	return 0;
+}
+
+static void free_fieldbuffers(struct easycap *peasycap)
+{
+	int k, m, gone;
+
+	JOM(4, "freeing video field buffers.\n");
+	gone = 0;
+	for (k = 0;  k < FIELD_BUFFER_MANY;  k++) {
+		for (m = 0;  m < FIELD_BUFFER_SIZE/PAGE_SIZE;  m++) {
+			if (peasycap->field_buffer[k][m].pgo) {
+				free_page((unsigned long)
+					  peasycap->field_buffer[k][m].pgo);
+				peasycap->field_buffer[k][m].pgo = NULL;
+				peasycap->allocation_video_page -= 1;
+				gone++;
+			}
+		}
+	}
+	JOM(4, "video field buffers freed: %i pages\n", gone);
+}
+
+static int alloc_isocbuffers(struct easycap *peasycap)
+{
+	int i;
+	void *pbuf;
+
+	JOM(4, "allocating %i isoc video buffers of size %i\n",
+			VIDEO_ISOC_BUFFER_MANY,
+			peasycap->video_isoc_buffer_size);
+	JOM(4, ".... each occupying contiguous memory pages\n");
+
+	for (i = 0; i < VIDEO_ISOC_BUFFER_MANY; i++) {
+		pbuf = (void *)__get_free_pages(GFP_KERNEL,
+				VIDEO_ISOC_ORDER);
+		if (!pbuf) {
+			SAM("ERROR: Could not allocate isoc "
+				"video buffer %i\n", i);
+			return -ENOMEM;
+		}
+		peasycap->allocation_video_page += BIT(VIDEO_ISOC_ORDER);
+
+		peasycap->video_isoc_buffer[i].pgo = pbuf;
+		peasycap->video_isoc_buffer[i].pto =
+			pbuf + peasycap->video_isoc_buffer_size;
+		peasycap->video_isoc_buffer[i].kount = i;
+	}
+	JOM(4, "allocation of isoc video buffers done: %i pages\n",
+			i * (0x01 << VIDEO_ISOC_ORDER));
+	return 0;
+}
+
+static void free_isocbuffers(struct easycap *peasycap)
+{
+	int k, m;
+
+	JOM(4, "freeing video isoc buffers.\n");
+	m = 0;
+	for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY;  k++) {
+		if (peasycap->video_isoc_buffer[k].pgo) {
+			free_pages((unsigned long)
+				   peasycap->video_isoc_buffer[k].pgo,
+					VIDEO_ISOC_ORDER);
+			peasycap->video_isoc_buffer[k].pgo = NULL;
+			peasycap->allocation_video_page -=
+						BIT(VIDEO_ISOC_ORDER);
+			m++;
+		}
+	}
+	JOM(4, "isoc video buffers freed: %i pages\n",
+			m * (0x01 << VIDEO_ISOC_ORDER));
+}
+
+static int create_video_urbs(struct easycap *peasycap)
+{
+	struct urb *purb;
+	struct data_urb *pdata_urb;
+	int i, j;
+
+	JOM(4, "allocating %i struct urb.\n", VIDEO_ISOC_BUFFER_MANY);
+	JOM(4, "using %i=peasycap->video_isoc_framesperdesc\n",
+			peasycap->video_isoc_framesperdesc);
+	JOM(4, "using %i=peasycap->video_isoc_maxframesize\n",
+			peasycap->video_isoc_maxframesize);
+	JOM(4, "using %i=peasycap->video_isoc_buffer_sizen",
+			peasycap->video_isoc_buffer_size);
+
+	for (i = 0; i < VIDEO_ISOC_BUFFER_MANY; i++) {
+		purb = usb_alloc_urb(peasycap->video_isoc_framesperdesc,
+				GFP_KERNEL);
+		if (!purb) {
+			SAM("ERROR: usb_alloc_urb returned NULL for buffer "
+				"%i\n", i);
+			return -ENOMEM;
+		}
+
+		peasycap->allocation_video_urb += 1;
+		pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
+		if (!pdata_urb) {
+			SAM("ERROR: Could not allocate struct data_urb.\n");
+			return -ENOMEM;
+		}
+
+		peasycap->allocation_video_struct +=
+			sizeof(struct data_urb);
+
+		pdata_urb->purb = purb;
+		pdata_urb->isbuf = i;
+		pdata_urb->length = 0;
+		list_add_tail(&(pdata_urb->list_head),
+				peasycap->purb_video_head);
+
+		if (!i) {
+			JOM(4, "initializing video urbs thus:\n");
+			JOM(4, "  purb->interval = 1;\n");
+			JOM(4, "  purb->dev = peasycap->pusb_device;\n");
+			JOM(4, "  purb->pipe = usb_rcvisocpipe"
+					"(peasycap->pusb_device,%i);\n",
+					peasycap->video_endpointnumber);
+			JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
+			JOM(4, "  purb->transfer_buffer = peasycap->"
+					"video_isoc_buffer[.].pgo;\n");
+			JOM(4, "  purb->transfer_buffer_length = %i;\n",
+					peasycap->video_isoc_buffer_size);
+			JOM(4, "  purb->complete = easycap_complete;\n");
+			JOM(4, "  purb->context = peasycap;\n");
+			JOM(4, "  purb->start_frame = 0;\n");
+			JOM(4, "  purb->number_of_packets = %i;\n",
+					peasycap->video_isoc_framesperdesc);
+			JOM(4, "  for (j = 0; j < %i; j++)\n",
+					peasycap->video_isoc_framesperdesc);
+			JOM(4, "    {\n");
+			JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
+					peasycap->video_isoc_maxframesize);
+			JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
+					peasycap->video_isoc_maxframesize);
+			JOM(4, "    }\n");
+		}
+
+		purb->interval = 1;
+		purb->dev = peasycap->pusb_device;
+		purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
+				peasycap->video_endpointnumber);
+
+		purb->transfer_flags = URB_ISO_ASAP;
+		purb->transfer_buffer = peasycap->video_isoc_buffer[i].pgo;
+		purb->transfer_buffer_length =
+			peasycap->video_isoc_buffer_size;
+
+		purb->complete = easycap_complete;
+		purb->context = peasycap;
+		purb->start_frame = 0;
+		purb->number_of_packets = peasycap->video_isoc_framesperdesc;
+
+		for (j = 0; j < peasycap->video_isoc_framesperdesc; j++) {
+			purb->iso_frame_desc[j].offset =
+				j * peasycap->video_isoc_maxframesize;
+			purb->iso_frame_desc[j].length =
+				peasycap->video_isoc_maxframesize;
+		}
+	}
+	JOM(4, "allocation of %i struct urb done.\n", i);
+	return 0;
+}
+
+static void free_video_urbs(struct easycap *peasycap)
+{
+	struct list_head *plist_head, *plist_next;
+	struct data_urb *pdata_urb;
+	int m;
+
+	if (peasycap->purb_video_head) {
+		m = 0;
+		list_for_each(plist_head, peasycap->purb_video_head) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb && pdata_urb->purb) {
+				usb_free_urb(pdata_urb->purb);
+				pdata_urb->purb = NULL;
+				peasycap->allocation_video_urb--;
+				m++;
+			}
+		}
+
+		JOM(4, "%i video urbs freed\n", m);
+		JOM(4, "freeing video data_urb structures.\n");
+		m = 0;
+		list_for_each_safe(plist_head, plist_next,
+					peasycap->purb_video_head) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb) {
+				peasycap->allocation_video_struct -=
+					sizeof(struct data_urb);
+				kfree(pdata_urb);
+				m++;
+			}
+		}
+		JOM(4, "%i video data_urb structures freed\n", m);
+		JOM(4, "setting peasycap->purb_video_head=NULL\n");
+		peasycap->purb_video_head = NULL;
+	}
+}
+
+static int alloc_audio_buffers(struct easycap *peasycap)
+{
+	void *pbuf;
+	int k;
+
+	JOM(4, "allocating %i isoc audio buffers of size %i\n",
+		AUDIO_ISOC_BUFFER_MANY,
+		peasycap->audio_isoc_buffer_size);
+	JOM(4, ".... each occupying contiguous memory pages\n");
+
+	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
+		pbuf = (void *)__get_free_pages(GFP_KERNEL, AUDIO_ISOC_ORDER);
+		if (!pbuf) {
+			SAM("ERROR: Could not allocate isoc audio buffer %i\n",
+			    k);
+				return -ENOMEM;
+		}
+		peasycap->allocation_audio_page += BIT(AUDIO_ISOC_ORDER);
+
+		peasycap->audio_isoc_buffer[k].pgo = pbuf;
+		peasycap->audio_isoc_buffer[k].pto =
+			pbuf + peasycap->audio_isoc_buffer_size;
+		peasycap->audio_isoc_buffer[k].kount = k;
+	}
+
+	JOM(4, "allocation of isoc audio buffers done.\n");
+	return 0;
+}
+
+static void free_audio_buffers(struct easycap *peasycap)
+{
+	int k, m;
+
+	JOM(4, "freeing audio isoc buffers.\n");
+	m = 0;
+	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
+		if (peasycap->audio_isoc_buffer[k].pgo) {
+			free_pages((unsigned long)
+					(peasycap->audio_isoc_buffer[k].pgo),
+					AUDIO_ISOC_ORDER);
+			peasycap->audio_isoc_buffer[k].pgo = NULL;
+			peasycap->allocation_audio_page -=
+					BIT(AUDIO_ISOC_ORDER);
+			m++;
+		}
+	}
+	JOM(4, "easyoss_delete(): isoc audio buffers freed: %i pages\n",
+					m * (0x01 << AUDIO_ISOC_ORDER));
+}
+
+static int create_audio_urbs(struct easycap *peasycap)
+{
+	struct urb *purb;
+	struct data_urb *pdata_urb;
+	int k, j;
+
+	JOM(4, "allocating %i struct urb.\n", AUDIO_ISOC_BUFFER_MANY);
+	JOM(4, "using %i=peasycap->audio_isoc_framesperdesc\n",
+		peasycap->audio_isoc_framesperdesc);
+	JOM(4, "using %i=peasycap->audio_isoc_maxframesize\n",
+		peasycap->audio_isoc_maxframesize);
+	JOM(4, "using %i=peasycap->audio_isoc_buffer_size\n",
+		peasycap->audio_isoc_buffer_size);
+
+	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY; k++) {
+		purb = usb_alloc_urb(peasycap->audio_isoc_framesperdesc,
+				     GFP_KERNEL);
+		if (!purb) {
+			SAM("ERROR: usb_alloc_urb returned NULL for buffer "
+			     "%i\n", k);
+			return -ENOMEM;
+		}
+		peasycap->allocation_audio_urb += 1 ;
+		pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
+		if (!pdata_urb) {
+			usb_free_urb(purb);
+			SAM("ERROR: Could not allocate struct data_urb.\n");
+			return -ENOMEM;
+		}
+		peasycap->allocation_audio_struct +=
+			sizeof(struct data_urb);
+
+		pdata_urb->purb = purb;
+		pdata_urb->isbuf = k;
+		pdata_urb->length = 0;
+		list_add_tail(&(pdata_urb->list_head),
+				peasycap->purb_audio_head);
+
+		if (!k) {
+			JOM(4, "initializing audio urbs thus:\n");
+			JOM(4, "  purb->interval = 1;\n");
+			JOM(4, "  purb->dev = peasycap->pusb_device;\n");
+			JOM(4, "  purb->pipe = usb_rcvisocpipe(peasycap->"
+				"pusb_device,%i);\n",
+				peasycap->audio_endpointnumber);
+			JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
+			JOM(4, "  purb->transfer_buffer = "
+				"peasycap->audio_isoc_buffer[.].pgo;\n");
+			JOM(4, "  purb->transfer_buffer_length = %i;\n",
+				peasycap->audio_isoc_buffer_size);
+			JOM(4, "  purb->complete = easycap_alsa_complete;\n");
+			JOM(4, "  purb->context = peasycap;\n");
+			JOM(4, "  purb->start_frame = 0;\n");
+			JOM(4, "  purb->number_of_packets = %i;\n",
+				peasycap->audio_isoc_framesperdesc);
+			JOM(4, "  for (j = 0; j < %i; j++)\n",
+				peasycap->audio_isoc_framesperdesc);
+			JOM(4, "    {\n");
+			JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
+				peasycap->audio_isoc_maxframesize);
+			JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
+				peasycap->audio_isoc_maxframesize);
+			JOM(4, "    }\n");
+		}
+
+		purb->interval = 1;
+		purb->dev = peasycap->pusb_device;
+		purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
+					     peasycap->audio_endpointnumber);
+		purb->transfer_flags = URB_ISO_ASAP;
+		purb->transfer_buffer = peasycap->audio_isoc_buffer[k].pgo;
+		purb->transfer_buffer_length =
+			peasycap->audio_isoc_buffer_size;
+		purb->complete = easycap_alsa_complete;
+		purb->context = peasycap;
+		purb->start_frame = 0;
+		purb->number_of_packets = peasycap->audio_isoc_framesperdesc;
+		for (j = 0;  j < peasycap->audio_isoc_framesperdesc; j++) {
+			purb->iso_frame_desc[j].offset =
+				j * peasycap->audio_isoc_maxframesize;
+			purb->iso_frame_desc[j].length =
+				peasycap->audio_isoc_maxframesize;
+		}
+	}
+	JOM(4, "allocation of %i struct urb done.\n", k);
+	return 0;
+}
+
+static void free_audio_urbs(struct easycap *peasycap)
+{
+	struct list_head *plist_head, *plist_next;
+	struct data_urb *pdata_urb;
+	int m;
+
+	if (peasycap->purb_audio_head) {
+		JOM(4, "freeing audio urbs\n");
+		m = 0;
+		list_for_each(plist_head, (peasycap->purb_audio_head)) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb && pdata_urb->purb) {
+				usb_free_urb(pdata_urb->purb);
+				pdata_urb->purb = NULL;
+				peasycap->allocation_audio_urb--;
+				m++;
+			}
+		}
+		JOM(4, "%i audio urbs freed\n", m);
+		JOM(4, "freeing audio data_urb structures.\n");
+		m = 0;
+		list_for_each_safe(plist_head, plist_next,
+					peasycap->purb_audio_head) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb) {
+				peasycap->allocation_audio_struct -=
+							sizeof(struct data_urb);
+				kfree(pdata_urb);
+				m++;
+			}
+		}
+		JOM(4, "%i audio data_urb structures freed\n", m);
+		JOM(4, "setting peasycap->purb_audio_head=NULL\n");
+		peasycap->purb_audio_head = NULL;
+	}
+}
+
+static void config_easycap(struct easycap *peasycap,
+			   u8 bInterfaceNumber,
+			   u8 bInterfaceClass,
+			   u8 bInterfaceSubClass)
+{
+	if ((USB_CLASS_VIDEO == bInterfaceClass) ||
+	    (USB_CLASS_VENDOR_SPEC == bInterfaceClass)) {
+		if (-1 == peasycap->video_interface) {
+			peasycap->video_interface = bInterfaceNumber;
+			JOM(4, "setting peasycap->video_interface=%i\n",
+				peasycap->video_interface);
+		} else {
+			if (peasycap->video_interface != bInterfaceNumber) {
+				SAM("ERROR: attempting to reset "
+				    "peasycap->video_interface\n");
+				SAM("...... continuing with "
+				    "%i=peasycap->video_interface\n",
+				    peasycap->video_interface);
+			}
+		}
+	} else if ((USB_CLASS_AUDIO == bInterfaceClass) &&
+		   (USB_SUBCLASS_AUDIOSTREAMING == bInterfaceSubClass)) {
+		if (-1 == peasycap->audio_interface) {
+			peasycap->audio_interface = bInterfaceNumber;
+			JOM(4, "setting peasycap->audio_interface=%i\n",
+				peasycap->audio_interface);
+		} else {
+			if (peasycap->audio_interface != bInterfaceNumber) {
+				SAM("ERROR: attempting to reset "
+				    "peasycap->audio_interface\n");
+				SAM("...... continuing with "
+				    "%i=peasycap->audio_interface\n",
+				    peasycap->audio_interface);
+			}
+		}
+	}
+}
+
+/*
+ * This function is called from within easycap_usb_disconnect() and is
+ * protected by semaphores set and cleared by easycap_usb_disconnect().
+ * By this stage the device has already been physically unplugged,
+ * so peasycap->pusb_device is no longer valid.
+ */
+static void easycap_delete(struct kref *pkref)
+{
+	struct easycap *peasycap;
+
+	peasycap = container_of(pkref, struct easycap, kref);
+	if (!peasycap) {
+		SAM("ERROR: peasycap is NULL: cannot perform deletions\n");
+		return;
+	}
+
+	/* Free video urbs */
+	free_video_urbs(peasycap);
+
+	/* Free video isoc buffers */
+	free_isocbuffers(peasycap);
+
+	/* Free video field buffers */
+	free_fieldbuffers(peasycap);
+
+	/* Free video frame buffers */
+	free_framebuffers(peasycap);
+
+	/* Free audio urbs */
+	free_audio_urbs(peasycap);
+
+	/* Free audio isoc buffers */
+	free_audio_buffers(peasycap);
+
+	free_easycap(peasycap);
+
+	JOT(4, "ending.\n");
+}
+
 static const struct v4l2_file_operations v4l2_fops = {
 	.owner		= THIS_MODULE,
 	.open		= easycap_open_noinode,
@@ -2850,6 +3450,36 @@ static const struct v4l2_file_operations v4l2_fops = {
 	.mmap		= easycap_mmap,
 };
 
+static int easycap_register_video(struct easycap *peasycap)
+{
+	/*
+	 * FIXME: This is believed to be harmless,
+	 * but may well be unnecessary or wrong.
+	 */
+	peasycap->video_device.v4l2_dev = NULL;
+
+	strcpy(&peasycap->video_device.name[0], "easycapdc60");
+	peasycap->video_device.fops = &v4l2_fops;
+	peasycap->video_device.minor = -1;
+	peasycap->video_device.release = (void *)(&videodev_release);
+
+	video_set_drvdata(&(peasycap->video_device), (void *)peasycap);
+
+	if (0 != (video_register_device(&(peasycap->video_device),
+					VFL_TYPE_GRABBER, -1))) {
+		videodev_release(&(peasycap->video_device));
+		return -ENODEV;
+	}
+
+	peasycap->registered_video++;
+
+	SAM("registered with videodev: %i=minor\n",
+	    peasycap->video_device.minor);
+	    peasycap->minor = peasycap->video_device.minor;
+
+	return 0;
+}
+
 /*
  * When the device is plugged, this function is called three times,
  * one for each interface.
@@ -2861,24 +3491,15 @@ static int easycap_usb_probe(struct usb_interface *intf,
 	struct usb_host_interface *alt;
 	struct usb_endpoint_descriptor *ep;
 	struct usb_interface_descriptor *interface;
-	struct urb *purb;
 	struct easycap *peasycap;
-	int ndong;
-	struct data_urb *pdata_urb;
-	int i, j, k, m, rc;
+	int i, j, rc;
 	u8 bInterfaceNumber;
 	u8 bInterfaceClass;
 	u8 bInterfaceSubClass;
-	void *pbuf;
 	int okalt[8], isokalt;
 	int okepn[8];
 	int okmps[8];
 	int maxpacketsize;
-	u16 mask;
-	s32 value;
-	struct easycap_format *peasycap_format;
-	int fmtidx;
-	struct inputset *inputset;
 
 	usbdev = interface_to_usbdev(intf);
 
@@ -2916,76 +3537,16 @@ static int easycap_usb_probe(struct usb_interface *intf,
 	 * interfaces 1 and 2 are probed.
 	 */
 	if (0 == bInterfaceNumber) {
-		peasycap = kzalloc(sizeof(struct easycap), GFP_KERNEL);
-		if (!peasycap) {
-			SAY("ERROR: Could not allocate peasycap\n");
-			return -ENOMEM;
-		}
-
-		/* Perform urgent initializations */
-		peasycap->minor = -1;
-		kref_init(&peasycap->kref);
-		JOM(8, "intf[%i]: after kref_init(..._video) "
-				"%i=peasycap->kref.refcount.counter\n",
-				bInterfaceNumber, peasycap->kref.refcount.counter);
-
-		/* module params */
-		peasycap->gain = (s8)clamp(easycap_gain, 0, 31);
-
-		init_waitqueue_head(&peasycap->wq_video);
-		init_waitqueue_head(&peasycap->wq_audio);
-		init_waitqueue_head(&peasycap->wq_trigger);
-
-		if (mutex_lock_interruptible(&mutex_dongle)) {
-			SAY("ERROR: cannot down mutex_dongle\n");
-			return -ERESTARTSYS;
-		}
-
-		for (ndong = 0; ndong < DONGLE_MANY; ndong++) {
-			if ((!easycapdc60_dongle[ndong].peasycap) &&
-					(!mutex_is_locked(&easycapdc60_dongle
-						[ndong].mutex_video)) &&
-					(!mutex_is_locked(&easycapdc60_dongle
-						[ndong].mutex_audio))) {
-				easycapdc60_dongle[ndong].peasycap = peasycap;
-				peasycap->isdongle = ndong;
-				JOM(8, "intf[%i]: peasycap-->easycap"
-						"_dongle[%i].peasycap\n",
-						bInterfaceNumber, ndong);
-				break;
-			}
-		}
-
-		if (DONGLE_MANY <= ndong) {
-			SAM("ERROR: too many dongles\n");
-			mutex_unlock(&mutex_dongle);
+		/*
+		 * Alloc structure and save it in a free slot in
+		 * easycapdc60_dongle array
+		 */
+		peasycap = alloc_easycap(bInterfaceNumber);
+		if (!peasycap)
 			return -ENOMEM;
-		}
-		mutex_unlock(&mutex_dongle);
-
-		peasycap->allocation_video_struct = sizeof(struct easycap);
-
-		/* and further initialize the structure */
-		peasycap->pusb_device = usbdev;
-		peasycap->pusb_interface = intf;
 
-		peasycap->microphone = false;
-
-		peasycap->video_interface = -1;
-		peasycap->video_altsetting_on = -1;
-		peasycap->video_altsetting_off = -1;
-		peasycap->video_endpointnumber = -1;
-		peasycap->video_isoc_maxframesize = -1;
-		peasycap->video_isoc_buffer_size = -1;
-
-		peasycap->audio_interface = -1;
-		peasycap->audio_altsetting_on = -1;
-		peasycap->audio_altsetting_off = -1;
-		peasycap->audio_endpointnumber = -1;
-		peasycap->audio_isoc_maxframesize = -1;
-		peasycap->audio_isoc_buffer_size = -1;
-
-		peasycap->frame_buffer_many = FRAME_BUFFER_MANY;
+		/* Perform basic struct initialization */
+		init_easycap(peasycap, usbdev, intf, bInterfaceNumber);
 
 		/* Dynamically fill in the available formats */
 		rc = easycap_video_fillin_formats();
@@ -2996,136 +3557,19 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		JOM(4, "%i formats available\n", rc);
 
 		/* Populate easycap.inputset[] */
-		inputset = peasycap->inputset;
-		fmtidx = peasycap->ntsc ? NTSC_M : PAL_BGHIN;
-		m = 0;
-		mask = 0;
-		for (i = 0; 0xFFFF != easycap_standard[i].mask; i++) {
-			if (fmtidx == easycap_standard[i].v4l2_standard.index) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].standard_offset = i;
-
-				mask = easycap_standard[i].mask;
-			}
-		}
-		if (1 != m) {
-			SAM("ERROR: "
-			    "inputset->standard_offset unpopulated, %i=m\n", m);
-			return -ENOENT;
-		}
-
-		peasycap_format = &easycap_format[0];
-		m = 0;
-		for (i = 0; peasycap_format->v4l2_format.fmt.pix.width; i++) {
-			struct v4l2_pix_format *pix =
-				&peasycap_format->v4l2_format.fmt.pix;
-			if (((peasycap_format->mask & 0x0F) == (mask & 0x0F)) &&
-			    pix->field == V4L2_FIELD_NONE &&
-			    pix->pixelformat == V4L2_PIX_FMT_UYVY &&
-			    pix->width  == 640 && pix->height == 480) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].format_offset = i;
-				break;
-			}
-			peasycap_format++;
-		}
-		if (1 != m) {
-			SAM("ERROR: inputset[]->format_offset unpopulated\n");
-			return -ENOENT;
-		}
-
-		m = 0;
-		for (i = 0; 0xFFFFFFFF != easycap_control[i].id; i++) {
-			value = easycap_control[i].default_value;
-			if (V4L2_CID_BRIGHTNESS == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].brightness = value;
-			} else if (V4L2_CID_CONTRAST == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].contrast = value;
-			} else if (V4L2_CID_SATURATION == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].saturation = value;
-			} else if (V4L2_CID_HUE == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].hue = value;
-			}
-		}
-
-		if (4 != m) {
-			SAM("ERROR: inputset[]->brightness underpopulated\n");
-			return -ENOENT;
-		}
-		for (k = 0; k < INPUT_MANY; k++)
-			inputset[k].input = k;
-		JOM(4, "populated inputset[]\n");
+		rc = populate_inputset(peasycap);
+		if (rc < 0)
+			return rc;
 		JOM(4, "finished initialization\n");
 	} else {
-
-		/*
-		 * FIXME: Identify the appropriate pointer
-		 * peasycap for interfaces 1 and 2.
-		 * The address of peasycap->pusb_device
-		 * is reluctantly used for this purpose.
-		 */
-		for (ndong = 0; ndong < DONGLE_MANY; ndong++) {
-			if (usbdev == easycapdc60_dongle[ndong].peasycap->
-									pusb_device) {
-				peasycap = easycapdc60_dongle[ndong].peasycap;
-				JOT(8, "intf[%i]: dongle[%i].peasycap\n",
-						bInterfaceNumber, ndong);
-				break;
-			}
-		}
-		if (DONGLE_MANY <= ndong) {
-			SAY("ERROR: peasycap is unknown when probing interface %i\n",
-								bInterfaceNumber);
+		peasycap = get_easycap(usbdev, bInterfaceNumber);
+		if (!peasycap)
 			return -ENODEV;
-		}
-		if (!peasycap) {
-			SAY("ERROR: peasycap is NULL when probing interface %i\n",
-								bInterfaceNumber);
-			return -ENODEV;
-		}
 	}
 
-	if ((USB_CLASS_VIDEO == bInterfaceClass) ||
-	    (USB_CLASS_VENDOR_SPEC == bInterfaceClass)) {
-		if (-1 == peasycap->video_interface) {
-			peasycap->video_interface = bInterfaceNumber;
-			JOM(4, "setting peasycap->video_interface=%i\n",
-							peasycap->video_interface);
-		} else {
-			if (peasycap->video_interface != bInterfaceNumber) {
-				SAM("ERROR: attempting to reset "
-						"peasycap->video_interface\n");
-				SAM("...... continuing with "
-						"%i=peasycap->video_interface\n",
-						peasycap->video_interface);
-			}
-		}
-	} else if ((USB_CLASS_AUDIO == bInterfaceClass) &&
-		   (USB_SUBCLASS_AUDIOSTREAMING == bInterfaceSubClass)) {
-		if (-1 == peasycap->audio_interface) {
-			peasycap->audio_interface = bInterfaceNumber;
-			JOM(4, "setting peasycap->audio_interface=%i\n",
-							 peasycap->audio_interface);
-		} else {
-			if (peasycap->audio_interface != bInterfaceNumber) {
-				SAM("ERROR: attempting to reset "
-						"peasycap->audio_interface\n");
-				SAM("...... continuing with "
-						"%i=peasycap->audio_interface\n",
-						peasycap->audio_interface);
-			}
-		}
-	}
+	config_easycap(peasycap, bInterfaceNumber,
+				 bInterfaceClass,
+				 bInterfaceSubClass);
 
 	/*
 	 * Investigate all altsettings. This is done in detail
@@ -3368,173 +3812,23 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		 */
 		INIT_LIST_HEAD(&(peasycap->urb_video_head));
 		peasycap->purb_video_head = &(peasycap->urb_video_head);
-		JOM(4, "allocating %i frame buffers of size %li\n",
-				FRAME_BUFFER_MANY, (long int)FRAME_BUFFER_SIZE);
-		JOM(4, ".... each scattered over %li pages\n",
-							FRAME_BUFFER_SIZE/PAGE_SIZE);
-
-		for (k = 0;  k < FRAME_BUFFER_MANY;  k++) {
-			for (m = 0;  m < FRAME_BUFFER_SIZE/PAGE_SIZE;  m++) {
-				if (peasycap->frame_buffer[k][m].pgo)
-					SAM("attempting to reallocate frame "
-									" buffers\n");
-				else {
-					pbuf = (void *)__get_free_page(GFP_KERNEL);
-					if (!pbuf) {
-						SAM("ERROR: Could not allocate frame "
-							"buffer %i page %i\n", k, m);
-						return -ENOMEM;
-					}
-
-					peasycap->allocation_video_page += 1;
-					peasycap->frame_buffer[k][m].pgo = pbuf;
-				}
-				peasycap->frame_buffer[k][m].pto =
-						peasycap->frame_buffer[k][m].pgo;
-			}
-		}
-
-		peasycap->frame_fill = 0;
-		peasycap->frame_read = 0;
-		JOM(4, "allocation of frame buffers done:  %i pages\n", k *
-									m);
-		JOM(4, "allocating %i field buffers of size %li\n",
-				FIELD_BUFFER_MANY, (long int)FIELD_BUFFER_SIZE);
-		JOM(4, ".... each scattered over %li pages\n",
-						FIELD_BUFFER_SIZE/PAGE_SIZE);
-
-		for (k = 0;  k < FIELD_BUFFER_MANY;  k++) {
-			for (m = 0;  m < FIELD_BUFFER_SIZE/PAGE_SIZE;  m++) {
-				if (peasycap->field_buffer[k][m].pgo) {
-					SAM("ERROR: attempting to reallocate "
-								"field buffers\n");
-				} else {
-					pbuf = (void *) __get_free_page(GFP_KERNEL);
-					if (!pbuf) {
-						SAM("ERROR: Could not allocate field"
-							" buffer %i page %i\n", k, m);
-						return -ENOMEM;
-					}
-
-					peasycap->allocation_video_page += 1;
-					peasycap->field_buffer[k][m].pgo = pbuf;
-				}
-				peasycap->field_buffer[k][m].pto =
-						peasycap->field_buffer[k][m].pgo;
-			}
-			peasycap->field_buffer[k][0].kount = 0x0200;
-		}
-		peasycap->field_fill = 0;
-		peasycap->field_page = 0;
-		peasycap->field_read = 0;
-		JOM(4, "allocation of field buffers done:  %i pages\n", k *
-									m);
-		JOM(4, "allocating %i isoc video buffers of size %i\n",
-						VIDEO_ISOC_BUFFER_MANY,
-						peasycap->video_isoc_buffer_size);
-		JOM(4, ".... each occupying contiguous memory pages\n");
-
-		for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY; k++) {
-			pbuf = (void *)__get_free_pages(GFP_KERNEL,
-							VIDEO_ISOC_ORDER);
-			if (!pbuf) {
-				SAM("ERROR: Could not allocate isoc video buffer "
-									"%i\n", k);
-				return -ENOMEM;
-			}
-			peasycap->allocation_video_page +=
-						BIT(VIDEO_ISOC_ORDER);
 
-			peasycap->video_isoc_buffer[k].pgo = pbuf;
-			peasycap->video_isoc_buffer[k].pto =
-				pbuf + peasycap->video_isoc_buffer_size;
-			peasycap->video_isoc_buffer[k].kount = k;
-		}
-		JOM(4, "allocation of isoc video buffers done: %i pages\n",
-						k * (0x01 << VIDEO_ISOC_ORDER));
-
-		/* Allocate and initialize multiple struct usb */
-		JOM(4, "allocating %i struct urb.\n", VIDEO_ISOC_BUFFER_MANY);
-		JOM(4, "using %i=peasycap->video_isoc_framesperdesc\n",
-						peasycap->video_isoc_framesperdesc);
-		JOM(4, "using %i=peasycap->video_isoc_maxframesize\n",
-						peasycap->video_isoc_maxframesize);
-		JOM(4, "using %i=peasycap->video_isoc_buffer_sizen",
-						peasycap->video_isoc_buffer_size);
-
-		for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY; k++) {
-			purb = usb_alloc_urb(peasycap->video_isoc_framesperdesc,
-									GFP_KERNEL);
-			if (!purb) {
-				SAM("ERROR: usb_alloc_urb returned NULL for buffer "
-									"%i\n", k);
-				return -ENOMEM;
-			}
+		rc = alloc_framebuffers(peasycap);
+		if (rc < 0)
+			return rc;
 
-			peasycap->allocation_video_urb += 1;
-			pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
-			if (!pdata_urb) {
-				SAM("ERROR: Could not allocate struct data_urb.\n");
-				return -ENOMEM;
-			}
+		rc = alloc_fieldbuffers(peasycap);
+		if (rc < 0)
+			return rc;
 
-			peasycap->allocation_video_struct +=
-							sizeof(struct data_urb);
+		rc = alloc_isocbuffers(peasycap);
+		if (rc < 0)
+			return rc;
 
-			pdata_urb->purb = purb;
-			pdata_urb->isbuf = k;
-			pdata_urb->length = 0;
-			list_add_tail(&(pdata_urb->list_head),
-							peasycap->purb_video_head);
-
-			/* Initialize allocated urbs */
-			if (!k) {
-				JOM(4, "initializing video urbs thus:\n");
-				JOM(4, "  purb->interval = 1;\n");
-				JOM(4, "  purb->dev = peasycap->pusb_device;\n");
-				JOM(4, "  purb->pipe = usb_rcvisocpipe"
-						"(peasycap->pusb_device,%i);\n",
-						peasycap->video_endpointnumber);
-				JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
-				JOM(4, "  purb->transfer_buffer = peasycap->"
-						"video_isoc_buffer[.].pgo;\n");
-				JOM(4, "  purb->transfer_buffer_length = %i;\n",
-						peasycap->video_isoc_buffer_size);
-				JOM(4, "  purb->complete = easycap_complete;\n");
-				JOM(4, "  purb->context = peasycap;\n");
-				JOM(4, "  purb->start_frame = 0;\n");
-				JOM(4, "  purb->number_of_packets = %i;\n",
-						peasycap->video_isoc_framesperdesc);
-				JOM(4, "  for (j = 0; j < %i; j++)\n",
-						peasycap->video_isoc_framesperdesc);
-				JOM(4, "    {\n");
-				JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
-						peasycap->video_isoc_maxframesize);
-				JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
-						peasycap->video_isoc_maxframesize);
-				JOM(4, "    }\n");
-			}
-
-			purb->interval = 1;
-			purb->dev = peasycap->pusb_device;
-			purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
-						peasycap->video_endpointnumber);
-			purb->transfer_flags = URB_ISO_ASAP;
-			purb->transfer_buffer = peasycap->video_isoc_buffer[k].pgo;
-			purb->transfer_buffer_length =
-						peasycap->video_isoc_buffer_size;
-			purb->complete = easycap_complete;
-			purb->context = peasycap;
-			purb->start_frame = 0;
-			purb->number_of_packets = peasycap->video_isoc_framesperdesc;
-			for (j = 0;  j < peasycap->video_isoc_framesperdesc; j++) {
-				purb->iso_frame_desc[j].offset = j *
-						peasycap->video_isoc_maxframesize;
-				purb->iso_frame_desc[j].length =
-						peasycap->video_isoc_maxframesize;
-			}
-		}
-		JOM(4, "allocation of %i struct urb done.\n", k);
+		/* Allocate and initialize video urbs */
+		rc = create_video_urbs(peasycap);
+		if (rc < 0)
+			return rc;
 
 		/* Save pointer peasycap in this interface */
 		usb_set_intfdata(intf, peasycap);
@@ -3545,9 +3839,6 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		 * because some udev rules triggers easycap_open()
 		 * immediately after registration, causing a clash.
 		 */
-		peasycap->ntsc = easycap_ntsc;
-		JOM(8, "defaulting initially to %s\n",
-			easycap_ntsc ? "NTSC" : "PAL");
 		rc = reset(peasycap);
 		if (rc) {
 			SAM("ERROR: reset() rc = %i\n", rc);
@@ -3562,33 +3853,12 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		JOM(4, "registered device instance: %s\n",
 			peasycap->v4l2_device.name);
 
-		/*
-		 * FIXME: This is believed to be harmless,
-		 * but may well be unnecessary or wrong.
-		 */
-		peasycap->video_device.v4l2_dev = NULL;
-
-
-		strcpy(&peasycap->video_device.name[0], "easycapdc60");
-		peasycap->video_device.fops = &v4l2_fops;
-		peasycap->video_device.minor = -1;
-		peasycap->video_device.release = (void *)(&videodev_release);
-
-		video_set_drvdata(&(peasycap->video_device), (void *)peasycap);
-
-		if (0 != (video_register_device(&(peasycap->video_device),
-							VFL_TYPE_GRABBER, -1))) {
+		rc = easycap_register_video(peasycap);
+		if (rc < 0) {
 			dev_err(&intf->dev,
 				"Not able to register with videodev\n");
-			videodev_release(&(peasycap->video_device));
 			return -ENODEV;
 		}
-
-		peasycap->registered_video++;
-		SAM("registered with videodev: %i=minor\n",
-						peasycap->video_device.minor);
-		peasycap->minor = peasycap->video_device.minor;
-
 		break;
 	}
 	/* 1: Audio control */
@@ -3711,109 +3981,14 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		INIT_LIST_HEAD(&(peasycap->urb_audio_head));
 		peasycap->purb_audio_head = &(peasycap->urb_audio_head);
 
-		JOM(4, "allocating %i isoc audio buffers of size %i\n",
-			AUDIO_ISOC_BUFFER_MANY,
-			peasycap->audio_isoc_buffer_size);
-		JOM(4, ".... each occupying contiguous memory pages\n");
-
-		for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
-			pbuf = (void *)__get_free_pages(GFP_KERNEL,
-							AUDIO_ISOC_ORDER);
-			if (!pbuf) {
-				SAM("ERROR: Could not allocate isoc audio buffer "
-								"%i\n", k);
-				return -ENOMEM;
-			}
-			peasycap->allocation_audio_page +=
-						BIT(AUDIO_ISOC_ORDER);
-
-			peasycap->audio_isoc_buffer[k].pgo = pbuf;
-			peasycap->audio_isoc_buffer[k].pto = pbuf +
-			peasycap->audio_isoc_buffer_size;
-			peasycap->audio_isoc_buffer[k].kount = k;
-		}
-		JOM(4, "allocation of isoc audio buffers done.\n");
+		alloc_audio_buffers(peasycap);
+		if (rc < 0)
+			return rc;
 
 		/* Allocate and initialize urbs */
-		JOM(4, "allocating %i struct urb.\n", AUDIO_ISOC_BUFFER_MANY);
-		JOM(4, "using %i=peasycap->audio_isoc_framesperdesc\n",
-					peasycap->audio_isoc_framesperdesc);
-		JOM(4, "using %i=peasycap->audio_isoc_maxframesize\n",
-					peasycap->audio_isoc_maxframesize);
-		JOM(4, "using %i=peasycap->audio_isoc_buffer_size\n",
-					peasycap->audio_isoc_buffer_size);
-
-		for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY; k++) {
-			purb = usb_alloc_urb(peasycap->audio_isoc_framesperdesc,
-								GFP_KERNEL);
-			if (!purb) {
-				SAM("ERROR: usb_alloc_urb returned NULL for buffer "
-								"%i\n", k);
-				return -ENOMEM;
-			}
-			peasycap->allocation_audio_urb += 1 ;
-			pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
-			if (!pdata_urb) {
-				usb_free_urb(purb);
-				SAM("ERROR: Could not allocate struct data_urb.\n");
-				return -ENOMEM;
-			}
-			peasycap->allocation_audio_struct +=
-						sizeof(struct data_urb);
-
-			pdata_urb->purb = purb;
-			pdata_urb->isbuf = k;
-			pdata_urb->length = 0;
-			list_add_tail(&(pdata_urb->list_head),
-							peasycap->purb_audio_head);
-
-			if (!k) {
-				JOM(4, "initializing audio urbs thus:\n");
-				JOM(4, "  purb->interval = 1;\n");
-				JOM(4, "  purb->dev = peasycap->pusb_device;\n");
-				JOM(4, "  purb->pipe = usb_rcvisocpipe(peasycap->"
-						"pusb_device,%i);\n",
-						peasycap->audio_endpointnumber);
-				JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
-				JOM(4, "  purb->transfer_buffer = "
-					"peasycap->audio_isoc_buffer[.].pgo;\n");
-				JOM(4, "  purb->transfer_buffer_length = %i;\n",
-					peasycap->audio_isoc_buffer_size);
-				JOM(4, "  purb->complete = easycap_alsa_complete;\n");
-				JOM(4, "  purb->context = peasycap;\n");
-				JOM(4, "  purb->start_frame = 0;\n");
-				JOM(4, "  purb->number_of_packets = %i;\n",
-						peasycap->audio_isoc_framesperdesc);
-				JOM(4, "  for (j = 0; j < %i; j++)\n",
-						peasycap->audio_isoc_framesperdesc);
-				JOM(4, "    {\n");
-				JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
-					peasycap->audio_isoc_maxframesize);
-				JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
-					peasycap->audio_isoc_maxframesize);
-				JOM(4, "    }\n");
-			}
-
-			purb->interval = 1;
-			purb->dev = peasycap->pusb_device;
-			purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
-						peasycap->audio_endpointnumber);
-			purb->transfer_flags = URB_ISO_ASAP;
-			purb->transfer_buffer = peasycap->audio_isoc_buffer[k].pgo;
-			purb->transfer_buffer_length =
-						peasycap->audio_isoc_buffer_size;
-			purb->complete = easycap_alsa_complete;
-			purb->context = peasycap;
-			purb->start_frame = 0;
-			purb->number_of_packets = peasycap->audio_isoc_framesperdesc;
-			for (j = 0;  j < peasycap->audio_isoc_framesperdesc; j++) {
-				purb->iso_frame_desc[j].offset = j *
-						peasycap->audio_isoc_maxframesize;
-				purb->iso_frame_desc[j].length =
-						peasycap->audio_isoc_maxframesize;
-			}
-		}
-		JOM(4, "allocation of %i struct urb done.\n", k);
+		rc = create_audio_urbs(peasycap);
+		if (rc < 0)
+			return rc;
 
 		/* Save pointer peasycap in this interface */
 		usb_set_intfdata(intf, peasycap);
@@ -3843,15 +4018,13 @@ static int easycap_usb_probe(struct usb_interface *intf,
 	SAM("ends successfully for interface %i\n", bInterfaceNumber);
 	return 0;
 }
-/*****************************************************************************/
-/*---------------------------------------------------------------------------*/
+
 /*
- *  WHEN THIS FUNCTION IS CALLED THE EasyCAP HAS ALREADY BEEN PHYSICALLY
- *  UNPLUGGED.  HENCE peasycap->pusb_device IS NO LONGER VALID.
- *
- *  THIS FUNCTION AFFECTS ALSA.  BEWARE.
+ * When this function is called the device has already been
+ * physically unplugged.
+ * Hence, peasycap->pusb_device is no longer valid.
+ * This function affects alsa.
  */
-/*---------------------------------------------------------------------------*/
 static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 {
 	struct usb_host_interface *pusb_host_interface;
@@ -3876,6 +4049,7 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 	minor = pusb_interface->minor;
 	JOT(4, "intf[%i]: minor=%i\n", bInterfaceNumber, minor);
 
+	/* There is nothing to do for Interface Number 1 */
 	if (1 == bInterfaceNumber)
 		return;
 
@@ -3884,11 +4058,8 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		SAY("ERROR: peasycap is NULL\n");
 		return;
 	}
-/*---------------------------------------------------------------------------*/
-/*
- *  IF THE WAIT QUEUES ARE NOT CLEARED A DEADLOCK IS POSSIBLE.  BEWARE.
-*/
-/*---------------------------------------------------------------------------*/
+
+	/* If the waitqueues are not cleared a deadlock is possible */
 	peasycap->video_eof = 1;
 	peasycap->audio_eof = 1;
 	wake_up_interruptible(&(peasycap->wq_video));
@@ -3904,15 +4075,14 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 	default:
 		break;
 	}
-/*--------------------------------------------------------------------------*/
-/*
- *  DEREGISTER
- *
- *  THIS PROCEDURE WILL BLOCK UNTIL easycap_poll(), VIDEO IOCTL AND AUDIO
- *  IOCTL ARE ALL UNLOCKED.  IF THIS IS NOT DONE AN Oops CAN OCCUR WHEN
- *  AN EasyCAP IS UNPLUGGED WHILE THE URBS ARE RUNNING.  BEWARE.
- */
-/*--------------------------------------------------------------------------*/
+
+	/*
+	 * Deregister
+	 * This procedure will block until easycap_poll(),
+	 * video and audio ioctl are all unlocked.
+	 * If this is not done an oops can occur when an easycap
+	 * is unplugged while the urbs are running.
+	 */
 	kd = easycap_isdongle(peasycap);
 	switch (bInterfaceNumber) {
 	case 0: {
@@ -3929,7 +4099,6 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		} else {
 			SAY("ERROR: %i=kd is bad: cannot lock dongle\n", kd);
 		}
-/*---------------------------------------------------------------------------*/
 		if (!peasycap->v4l2_device.name[0]) {
 			SAM("ERROR: peasycap->v4l2_device.name is empty\n");
 			if (0 <= kd && DONGLE_MANY > kd)
@@ -3945,7 +4114,6 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		JOM(4, "intf[%i]: video_unregister_device() minor=%i\n",
 				bInterfaceNumber, minor);
 		peasycap->registered_video--;
-/*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*/
 
 		if (0 <= kd && DONGLE_MANY > kd) {
 			mutex_unlock(&easycapdc60_dongle[kd].mutex_video);
@@ -3981,12 +4149,12 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 	default:
 		break;
 	}
-/*---------------------------------------------------------------------------*/
-/*
- *  CALL easycap_delete() IF NO REMAINING REFERENCES TO peasycap
- *  (ALSO WHEN ALSA HAS BEEN IN USE)
- */
-/*---------------------------------------------------------------------------*/
+
+	/*
+	 * If no remaining references to peasycap,
+	 * call easycap_delete.
+	 * (Also when alsa has been in use)
+	 */
 	if (!peasycap->kref.refcount.counter) {
 		SAM("ERROR: peasycap->kref.refcount.counter is zero "
 							"so cannot call kref_put()\n");
@@ -4021,17 +4189,11 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		mutex_unlock(&easycapdc60_dongle[kd].mutex_video);
 		JOT(4, "unlocked dongle[%i].mutex_video\n", kd);
 	}
-/*---------------------------------------------------------------------------*/
 	JOM(4, "ends\n");
 	return;
 }
-/*****************************************************************************/
 
-/*---------------------------------------------------------------------------*/
-/*
- *  PARAMETERS APPLICABLE TO ENTIRE DRIVER, I.E. BOTH VIDEO AND AUDIO
- */
-/*---------------------------------------------------------------------------*/
+/* Devices supported by this driver */
 static struct usb_device_id easycap_usb_device_id_table[] = {
 	{USB_DEVICE(USB_EASYCAP_VENDOR_ID, USB_EASYCAP_PRODUCT_ID)},
 	{ }
@@ -4066,14 +4228,11 @@ static int __init easycap_module_init(void)
 
 	return rc;
 }
-/*****************************************************************************/
+
 static void __exit easycap_module_exit(void)
 {
 	usb_deregister(&easycap_usb_driver);
 }
-/*****************************************************************************/
 
 module_init(easycap_module_init);
 module_exit(easycap_module_exit);
-
-/*****************************************************************************/
diff --git a/drivers/staging/media/go7007/go7007-v4l2.c b/drivers/staging/media/go7007/go7007-v4l2.c
index 3ef4cd8b4de3..c184ad30fbd8 100644
--- a/drivers/staging/media/go7007/go7007-v4l2.c
+++ b/drivers/staging/media/go7007/go7007-v4l2.c
@@ -76,7 +76,6 @@ static void abort_queued(struct go7007 *go)
 
 static int go7007_streamoff(struct go7007 *go)
 {
-	int retval = -EINVAL;
 	unsigned long flags;
 
 	mutex_lock(&go->hw_lock);
@@ -87,7 +86,6 @@ static int go7007_streamoff(struct go7007 *go)
 		abort_queued(go);
 		spin_unlock_irqrestore(&go->spinlock, flags);
 		go7007_reset_encoder(go);
-		retval = 0;
 	}
 	mutex_unlock(&go->hw_lock);
 	return 0;
diff --git a/drivers/staging/media/go7007/s2250-loader.c b/drivers/staging/media/go7007/s2250-loader.c
index 7c5af4f289b6..f1bd159ac195 100644
--- a/drivers/staging/media/go7007/s2250-loader.c
+++ b/drivers/staging/media/go7007/s2250-loader.c
@@ -165,3 +165,5 @@ module_usb_driver(s2250loader_driver);
 MODULE_AUTHOR("");
 MODULE_DESCRIPTION("firmware loader for Sensoray 2250/2251");
 MODULE_LICENSE("GPL v2");
+MODULE_FIRMWARE(S2250_LOADER_FIRMWARE);
+MODULE_FIRMWARE(S2250_FIRMWARE);
diff --git a/drivers/staging/media/lirc/lirc_imon.c b/drivers/staging/media/lirc/lirc_imon.c
index d7cf5ef076a5..2944fde89f44 100644
--- a/drivers/staging/media/lirc/lirc_imon.c
+++ b/drivers/staging/media/lirc/lirc_imon.c
@@ -70,10 +70,6 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 static int ir_open(void *data);
 static void ir_close(void *data);
 
-/* Driver init/exit prototypes */
-static int __init imon_init(void);
-static void __exit imon_exit(void);
-
 /*** G L O B A L S ***/
 #define IMON_DATA_BUF_SZ	35
 
diff --git a/drivers/staging/media/lirc/lirc_sasem.c b/drivers/staging/media/lirc/lirc_sasem.c
index 352a20229ca2..f4e4d9003f38 100644
--- a/drivers/staging/media/lirc/lirc_sasem.c
+++ b/drivers/staging/media/lirc/lirc_sasem.c
@@ -80,10 +80,6 @@ static ssize_t vfd_write(struct file *file, const char *buf,
 static int ir_open(void *data);
 static void ir_close(void *data);
 
-/* Driver init/exit prototypes */
-static int __init sasem_init(void);
-static void __exit sasem_exit(void);
-
 /*** G L O B A L S ***/
 #define SASEM_DATA_BUF_SZ	32
 
diff --git a/drivers/staging/omapdrm/omap_crtc.c b/drivers/staging/omapdrm/omap_crtc.c
index 490a7f15604b..8b864afb40b6 100644
--- a/drivers/staging/omapdrm/omap_crtc.c
+++ b/drivers/staging/omapdrm/omap_crtc.c
@@ -36,12 +36,6 @@ struct omap_crtc {
 	struct drm_framebuffer *old_fb;
 };
 
-static void omap_crtc_gamma_set(struct drm_crtc *crtc,
-		u16 *red, u16 *green, u16 *blue, uint32_t start, uint32_t size)
-{
-	/* not supported.. at least not yet */
-}
-
 static void omap_crtc_destroy(struct drm_crtc *crtc)
 {
 	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
@@ -198,7 +192,6 @@ static int omap_crtc_page_flip_locked(struct drm_crtc *crtc,
 }
 
 static const struct drm_crtc_funcs omap_crtc_funcs = {
-	.gamma_set = omap_crtc_gamma_set,
 	.set_config = drm_crtc_helper_set_config,
 	.destroy = omap_crtc_destroy,
 	.page_flip = omap_crtc_page_flip_locked,
diff --git a/drivers/staging/omapdrm/omap_drv.c b/drivers/staging/omapdrm/omap_drv.c
index 0d2acca376ca..4beab9447ceb 100644
--- a/drivers/staging/omapdrm/omap_drv.c
+++ b/drivers/staging/omapdrm/omap_drv.c
@@ -58,7 +58,7 @@ static void omap_fb_output_poll_changed(struct drm_device *dev)
 	}
 }
 
-static struct drm_mode_config_funcs omap_mode_config_funcs = {
+static const struct drm_mode_config_funcs omap_mode_config_funcs = {
 	.fb_create = omap_framebuffer_create,
 	.output_poll_changed = omap_fb_output_poll_changed,
 };
@@ -726,7 +726,7 @@ static void dev_irq_uninstall(struct drm_device *dev)
 	DBG("irq_uninstall: dev=%p", dev);
 }
 
-static struct vm_operations_struct omap_gem_vm_ops = {
+static const struct vm_operations_struct omap_gem_vm_ops = {
 	.fault = omap_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
diff --git a/drivers/usb/gadget/uvc_queue.c b/drivers/usb/gadget/uvc_queue.c
index 0cdf89d32a15..104ae9c81251 100644
--- a/drivers/usb/gadget/uvc_queue.c
+++ b/drivers/usb/gadget/uvc_queue.c
@@ -543,7 +543,7 @@ done:
 	return ret;
 }
 
-/* called with queue->irqlock held.. */
+/* called with &queue_irqlock held.. */
 static struct uvc_buffer *
 uvc_queue_next_buffer(struct uvc_video_queue *queue, struct uvc_buffer *buf)
 {
diff --git a/drivers/usb/gadget/uvc_v4l2.c b/drivers/usb/gadget/uvc_v4l2.c
index 54d7ca559cb2..2ca9386d655b 100644
--- a/drivers/usb/gadget/uvc_v4l2.c
+++ b/drivers/usb/gadget/uvc_v4l2.c
@@ -296,7 +296,7 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		if (sub->type < UVC_EVENT_FIRST || sub->type > UVC_EVENT_LAST)
 			return -EINVAL;
 
-		return v4l2_event_subscribe(&handle->vfh, arg, 2);
+		return v4l2_event_subscribe(&handle->vfh, arg, 2, NULL);
 	}
 
 	case VIDIOC_UNSUBSCRIBE_EVENT:
diff --git a/drivers/video/efifb.c b/drivers/video/efifb.c
index 784139aed079..b4a632ada401 100644
--- a/drivers/video/efifb.c
+++ b/drivers/video/efifb.c
@@ -18,6 +18,8 @@
 
 static bool request_mem_succeeded = false;
 
+static struct pci_dev *default_vga;
+
 static struct fb_var_screeninfo efifb_defined __devinitdata = {
 	.activate		= FB_ACTIVATE_NOW,
 	.height			= -1,
@@ -298,35 +300,72 @@ static struct fb_ops efifb_ops = {
 	.fb_imageblit	= cfb_imageblit,
 };
 
+struct pci_dev *vga_default_device(void)
+{
+	return default_vga;
+}
+
+EXPORT_SYMBOL_GPL(vga_default_device);
+
+void vga_set_default_device(struct pci_dev *pdev)
+{
+	default_vga = pdev;
+}
+
 static int __init efifb_setup(char *options)
 {
 	char *this_opt;
 	int i;
+	struct pci_dev *dev = NULL;
+
+	if (options && *options) {
+		while ((this_opt = strsep(&options, ",")) != NULL) {
+			if (!*this_opt) continue;
+
+			for (i = 0; i < M_UNKNOWN; i++) {
+				if (!strcmp(this_opt, dmi_list[i].optname) &&
+				    dmi_list[i].base != 0) {
+					screen_info.lfb_base = dmi_list[i].base;
+					screen_info.lfb_linelength = dmi_list[i].stride;
+					screen_info.lfb_width = dmi_list[i].width;
+					screen_info.lfb_height = dmi_list[i].height;
+				}
+			}
+			if (!strncmp(this_opt, "base:", 5))
+				screen_info.lfb_base = simple_strtoul(this_opt+5, NULL, 0);
+			else if (!strncmp(this_opt, "stride:", 7))
+				screen_info.lfb_linelength = simple_strtoul(this_opt+7, NULL, 0) * 4;
+			else if (!strncmp(this_opt, "height:", 7))
+				screen_info.lfb_height = simple_strtoul(this_opt+7, NULL, 0);
+			else if (!strncmp(this_opt, "width:", 6))
+				screen_info.lfb_width = simple_strtoul(this_opt+6, NULL, 0);
+		}
+	}
 
-	if (!options || !*options)
-		return 0;
+	for_each_pci_dev(dev) {
+		int i;
 
-	while ((this_opt = strsep(&options, ",")) != NULL) {
-		if (!*this_opt) continue;
+		if ((dev->class >> 8) != PCI_CLASS_DISPLAY_VGA)
+			continue;
 
-		for (i = 0; i < M_UNKNOWN; i++) {
-			if (!strcmp(this_opt, dmi_list[i].optname) &&
-					dmi_list[i].base != 0) {
-				screen_info.lfb_base = dmi_list[i].base;
-				screen_info.lfb_linelength = dmi_list[i].stride;
-				screen_info.lfb_width = dmi_list[i].width;
-				screen_info.lfb_height = dmi_list[i].height;
-			}
+		for (i=0; i < DEVICE_COUNT_RESOURCE; i++) {
+			resource_size_t start, end;
+
+			if (!(pci_resource_flags(dev, i) & IORESOURCE_MEM))
+				continue;
+
+			start = pci_resource_start(dev, i);
+			end  = pci_resource_end(dev, i);
+
+			if (!start || !end)
+				continue;
+
+			if (screen_info.lfb_base >= start &&
+			    (screen_info.lfb_base + screen_info.lfb_size) < end)
+				default_vga = dev;
 		}
-		if (!strncmp(this_opt, "base:", 5))
-			screen_info.lfb_base = simple_strtoul(this_opt+5, NULL, 0);
-		else if (!strncmp(this_opt, "stride:", 7))
-			screen_info.lfb_linelength = simple_strtoul(this_opt+7, NULL, 0) * 4;
-		else if (!strncmp(this_opt, "height:", 7))
-			screen_info.lfb_height = simple_strtoul(this_opt+7, NULL, 0);
-		else if (!strncmp(this_opt, "width:", 6))
-			screen_info.lfb_width = simple_strtoul(this_opt+6, NULL, 0);
 	}
+
 	return 0;
 }
 
diff --git a/fs/dcache.c b/fs/dcache.c
index 8c1ab8fb5012..4435d8b32904 100644
--- a/fs/dcache.c
+++ b/fs/dcache.c
@@ -3093,6 +3093,7 @@ static void __init dcache_init_early(void)
 					HASH_EARLY,
 					&d_hash_shift,
 					&d_hash_mask,
+					0,
 					0);
 
 	for (loop = 0; loop < (1U << d_hash_shift); loop++)
@@ -3123,6 +3124,7 @@ static void __init dcache_init(void)
 					0,
 					&d_hash_shift,
 					&d_hash_mask,
+					0,
 					0);
 
 	for (loop = 0; loop < (1U << d_hash_shift); loop++)
diff --git a/fs/inode.c b/fs/inode.c
index deb72f6c2b4f..da93f7d160d4 100644
--- a/fs/inode.c
+++ b/fs/inode.c
@@ -1647,6 +1647,7 @@ void __init inode_init_early(void)
 					HASH_EARLY,
 					&i_hash_shift,
 					&i_hash_mask,
+					0,
 					0);
 
 	for (loop = 0; loop < (1U << i_hash_shift); loop++)
@@ -1677,6 +1678,7 @@ void __init inode_init(void)
 					0,
 					&i_hash_shift,
 					&i_hash_mask,
+					0,
 					0);
 
 	for (loop = 0; loop < (1U << i_hash_shift); loop++)
diff --git a/include/drm/drm.h b/include/drm/drm.h
index 64ff02d5b730..e51035a3757f 100644
--- a/include/drm/drm.h
+++ b/include/drm/drm.h
@@ -730,6 +730,8 @@ struct drm_prime_handle {
 #define DRM_IOCTL_MODE_GETPLANE	DRM_IOWR(0xB6, struct drm_mode_get_plane)
 #define DRM_IOCTL_MODE_SETPLANE	DRM_IOWR(0xB7, struct drm_mode_set_plane)
 #define DRM_IOCTL_MODE_ADDFB2		DRM_IOWR(0xB8, struct drm_mode_fb_cmd2)
+#define DRM_IOCTL_MODE_OBJ_GETPROPERTIES	DRM_IOWR(0xB9, struct drm_mode_obj_get_properties)
+#define DRM_IOCTL_MODE_OBJ_SETPROPERTY	DRM_IOWR(0xBA, struct drm_mode_obj_set_property)
 
 /**
  * Device specific ioctls should only be in their respective headers
@@ -775,6 +777,10 @@ struct drm_event_vblank {
 #define DRM_CAP_VBLANK_HIGH_CRTC 0x2
 #define DRM_CAP_DUMB_PREFERRED_DEPTH 0x3
 #define DRM_CAP_DUMB_PREFER_SHADOW 0x4
+#define DRM_CAP_PRIME 0x5
+
+#define DRM_PRIME_CAP_IMPORT 0x1
+#define DRM_PRIME_CAP_EXPORT 0x2
 
 /* typedef area */
 #ifndef __KERNEL__
diff --git a/include/drm/drmP.h b/include/drm/drmP.h
index dd731043fecd..31ad880ca2ef 100644
--- a/include/drm/drmP.h
+++ b/include/drm/drmP.h
@@ -755,11 +755,11 @@ struct drm_driver {
 	 * @dev: DRM device
 	 * @crtc: counter to fetch
 	 *
-	 * Driver callback for fetching a raw hardware vblank counter
-	 * for @crtc.  If a device doesn't have a hardware counter, the
-	 * driver can simply return the value of drm_vblank_count and
-	 * make the enable_vblank() and disable_vblank() hooks into no-ops,
-	 * leaving interrupts enabled at all times.
+	 * Driver callback for fetching a raw hardware vblank counter for @crtc.
+	 * If a device doesn't have a hardware counter, the driver can simply
+	 * return the value of drm_vblank_count. The DRM core will account for
+	 * missed vblank events while interrupts where disabled based on system
+	 * timestamps.
 	 *
 	 * Wraparound handling and loss of events due to modesetting is dealt
 	 * with in the DRM core code.
@@ -941,7 +941,7 @@ struct drm_driver {
 			    uint32_t handle);
 
 	/* Driver private ops for this object */
-	struct vm_operations_struct *gem_vm_ops;
+	const struct vm_operations_struct *gem_vm_ops;
 
 	int major;
 	int minor;
@@ -1309,8 +1309,8 @@ extern int drm_release(struct inode *inode, struct file *filp);
 				/* Mapping support (drm_vm.h) */
 extern int drm_mmap(struct file *filp, struct vm_area_struct *vma);
 extern int drm_mmap_locked(struct file *filp, struct vm_area_struct *vma);
-extern void drm_vm_open_locked(struct vm_area_struct *vma);
-extern void drm_vm_close_locked(struct vm_area_struct *vma);
+extern void drm_vm_open_locked(struct drm_device *dev, struct vm_area_struct *vma);
+extern void drm_vm_close_locked(struct drm_device *dev, struct vm_area_struct *vma);
 extern unsigned int drm_poll(struct file *filp, struct poll_table_struct *wait);
 
 				/* Memory management support (drm_memory.h) */
@@ -1378,6 +1378,7 @@ extern int drm_remove_magic(struct drm_master *master, drm_magic_t magic);
 
 /* Cache management (drm_cache.c) */
 void drm_clflush_pages(struct page *pages[], unsigned long num_pages);
+void drm_clflush_virt_range(char *addr, unsigned long length);
 
 				/* Locking IOCTL support (drm_lock.h) */
 extern int drm_lock(struct drm_device *dev, void *data,
@@ -1557,6 +1558,8 @@ extern int drm_prime_handle_to_fd_ioctl(struct drm_device *dev, void *data,
 extern int drm_prime_fd_to_handle_ioctl(struct drm_device *dev, void *data,
 					struct drm_file *file_priv);
 
+extern int drm_prime_sg_to_page_addr_arrays(struct sg_table *sgt, struct page **pages,
+					    dma_addr_t *addrs, int max_pages);
 extern struct sg_table *drm_prime_pages_to_sg(struct page **pages, int nr_pages);
 extern void drm_prime_gem_destroy(struct drm_gem_object *obj, struct sg_table *sg);
 
diff --git a/include/drm/drm_crtc.h b/include/drm/drm_crtc.h
index e250eda4e3a8..73e45600f95d 100644
--- a/include/drm/drm_crtc.h
+++ b/include/drm/drm_crtc.h
@@ -36,6 +36,7 @@
 struct drm_device;
 struct drm_mode_set;
 struct drm_framebuffer;
+struct drm_object_properties;
 
 
 #define DRM_MODE_OBJECT_CRTC 0xcccccccc
@@ -50,6 +51,14 @@ struct drm_framebuffer;
 struct drm_mode_object {
 	uint32_t id;
 	uint32_t type;
+	struct drm_object_properties *properties;
+};
+
+#define DRM_OBJECT_MAX_PROPERTY 16
+struct drm_object_properties {
+	int count;
+	uint32_t ids[DRM_OBJECT_MAX_PROPERTY];
+	uint64_t values[DRM_OBJECT_MAX_PROPERTY];
 };
 
 /*
@@ -285,19 +294,16 @@ struct drm_plane;
 
 /**
  * drm_crtc_funcs - control CRTCs for a given device
- * @reset: reset CRTC after state has been invalidate (e.g. resume)
- * @dpms: control display power levels
  * @save: save CRTC state
- * @resore: restore CRTC state
- * @lock: lock the CRTC
- * @unlock: unlock the CRTC
- * @shadow_allocate: allocate shadow pixmap
- * @shadow_create: create shadow pixmap for rotation support
- * @shadow_destroy: free shadow pixmap
- * @mode_fixup: fixup proposed mode
- * @mode_set: set the desired mode on the CRTC
+ * @restore: restore CRTC state
+ * @reset: reset CRTC after state has been invalidate (e.g. resume)
+ * @cursor_set: setup the cursor
+ * @cursor_move: move the cursor
  * @gamma_set: specify color ramp for CRTC
- * @destroy: deinit and free object.
+ * @destroy: deinit and free object
+ * @set_property: called when a property is changed
+ * @set_config: apply a new CRTC configuration
+ * @page_flip: initiate a page flip
  *
  * The drm_crtc_funcs structure is the central CRTC management structure
  * in the DRM.  Each CRTC controls one or more connectors (note that the name
@@ -341,6 +347,9 @@ struct drm_crtc_funcs {
 	int (*page_flip)(struct drm_crtc *crtc,
 			 struct drm_framebuffer *fb,
 			 struct drm_pending_vblank_event *event);
+
+	int (*set_property)(struct drm_crtc *crtc,
+			    struct drm_property *property, uint64_t val);
 };
 
 /**
@@ -360,6 +369,7 @@ struct drm_crtc_funcs {
  * @framedur_ns: precise line timing
  * @pixeldur_ns: precise pixel timing
  * @helper_private: mid-layer private data
+ * @properties: property tracking for this CRTC
  *
  * Each CRTC may have one or more connectors associated with it.  This structure
  * allows the CRTC to be controlled.
@@ -395,6 +405,8 @@ struct drm_crtc {
 
 	/* if you are using the helper */
 	void *helper_private;
+
+	struct drm_object_properties properties;
 };
 
 
@@ -404,11 +416,8 @@ struct drm_crtc {
  * @save: save connector state
  * @restore: restore connector state
  * @reset: reset connector after state has been invalidate (e.g. resume)
- * @mode_valid: is this mode valid on the given connector?
- * @mode_fixup: try to fixup proposed mode for this connector
- * @mode_set: set this mode
  * @detect: is this connector active?
- * @get_modes: get mode list for this connector
+ * @fill_modes: fill mode list for this connector
  * @set_property: property for this connector may need update
  * @destroy: make object go away
  * @force: notify the driver the connector is forced on
@@ -451,7 +460,6 @@ struct drm_encoder_funcs {
 };
 
 #define DRM_CONNECTOR_MAX_UMODES 16
-#define DRM_CONNECTOR_MAX_PROPERTY 16
 #define DRM_CONNECTOR_LEN 32
 #define DRM_CONNECTOR_MAX_ENCODER 3
 
@@ -520,8 +528,7 @@ enum drm_connector_force {
  * @funcs: connector control functions
  * @user_modes: user added mode list
  * @edid_blob_ptr: DRM property containing EDID if present
- * @property_ids: property tracking for this connector
- * @property_values: value pointers or data for properties
+ * @properties: property tracking for this connector
  * @polled: a %DRM_CONNECTOR_POLL_<foo> value for core driven polling
  * @dpms: current dpms state
  * @helper_private: mid-layer private data
@@ -565,8 +572,7 @@ struct drm_connector {
 
 	struct list_head user_modes;
 	struct drm_property_blob *edid_blob_ptr;
-	u32 property_ids[DRM_CONNECTOR_MAX_PROPERTY];
-	uint64_t property_values[DRM_CONNECTOR_MAX_PROPERTY];
+	struct drm_object_properties properties;
 
 	uint8_t polled; /* DRM_CONNECTOR_POLL_* */
 
@@ -595,6 +601,7 @@ struct drm_connector {
  * @update_plane: update the plane configuration
  * @disable_plane: shut down the plane
  * @destroy: clean up plane resources
+ * @set_property: called when a property is changed
  */
 struct drm_plane_funcs {
 	int (*update_plane)(struct drm_plane *plane,
@@ -605,6 +612,9 @@ struct drm_plane_funcs {
 			    uint32_t src_w, uint32_t src_h);
 	int (*disable_plane)(struct drm_plane *plane);
 	void (*destroy)(struct drm_plane *plane);
+
+	int (*set_property)(struct drm_plane *plane,
+			    struct drm_property *property, uint64_t val);
 };
 
 /**
@@ -622,6 +632,7 @@ struct drm_plane_funcs {
  * @enabled: enabled flag
  * @funcs: helper functions
  * @helper_private: storage for drver layer
+ * @properties: property tracking for this plane
  */
 struct drm_plane {
 	struct drm_device *dev;
@@ -644,6 +655,8 @@ struct drm_plane {
 
 	const struct drm_plane_funcs *funcs;
 	void *helper_private;
+
+	struct drm_object_properties properties;
 };
 
 /**
@@ -761,7 +774,7 @@ struct drm_mode_config {
 
 	int min_width, min_height;
 	int max_width, max_height;
-	struct drm_mode_config_funcs *funcs;
+	const struct drm_mode_config_funcs *funcs;
 	resource_size_t fb_base;
 
 	/* output poll support */
@@ -898,6 +911,12 @@ extern int drm_connector_property_set_value(struct drm_connector *connector,
 extern int drm_connector_property_get_value(struct drm_connector *connector,
 					 struct drm_property *property,
 					 uint64_t *value);
+extern int drm_object_property_set_value(struct drm_mode_object *obj,
+					 struct drm_property *property,
+					 uint64_t val);
+extern int drm_object_property_get_value(struct drm_mode_object *obj,
+					 struct drm_property *property,
+					 uint64_t *value);
 extern struct drm_display_mode *drm_crtc_mode_create(struct drm_device *dev);
 extern void drm_framebuffer_set_object(struct drm_device *dev,
 				       unsigned long handle);
@@ -910,14 +929,21 @@ extern int drmfb_remove(struct drm_device *dev, struct drm_framebuffer *fb);
 extern void drm_crtc_probe_connector_modes(struct drm_device *dev, int maxX, int maxY);
 extern bool drm_crtc_in_use(struct drm_crtc *crtc);
 
-extern int drm_connector_attach_property(struct drm_connector *connector,
-				      struct drm_property *property, uint64_t init_val);
+extern void drm_connector_attach_property(struct drm_connector *connector,
+					  struct drm_property *property, uint64_t init_val);
+extern void drm_object_attach_property(struct drm_mode_object *obj,
+				       struct drm_property *property,
+				       uint64_t init_val);
 extern struct drm_property *drm_property_create(struct drm_device *dev, int flags,
 						const char *name, int num_values);
 extern struct drm_property *drm_property_create_enum(struct drm_device *dev, int flags,
 					 const char *name,
 					 const struct drm_prop_enum_list *props,
 					 int num_values);
+struct drm_property *drm_property_create_bitmask(struct drm_device *dev,
+					 int flags, const char *name,
+					 const struct drm_prop_enum_list *props,
+					 int num_values);
 struct drm_property *drm_property_create_range(struct drm_device *dev, int flags,
 					 const char *name,
 					 uint64_t min, uint64_t max);
@@ -1012,10 +1038,11 @@ extern int drm_add_modes_noedid(struct drm_connector *connector,
 				int hdisplay, int vdisplay);
 
 extern int drm_edid_header_is_valid(const u8 *raw_edid);
-extern bool drm_edid_block_valid(u8 *raw_edid);
+extern bool drm_edid_block_valid(u8 *raw_edid, int block);
 extern bool drm_edid_is_valid(struct edid *edid);
 struct drm_display_mode *drm_mode_find_dmt(struct drm_device *dev,
-					   int hsize, int vsize, int fresh);
+					   int hsize, int vsize, int fresh,
+					   bool rb);
 
 extern int drm_mode_create_dumb_ioctl(struct drm_device *dev,
 				      void *data, struct drm_file *file_priv);
@@ -1023,7 +1050,16 @@ extern int drm_mode_mmap_dumb_ioctl(struct drm_device *dev,
 				    void *data, struct drm_file *file_priv);
 extern int drm_mode_destroy_dumb_ioctl(struct drm_device *dev,
 				      void *data, struct drm_file *file_priv);
+extern int drm_mode_obj_get_properties_ioctl(struct drm_device *dev, void *data,
+					     struct drm_file *file_priv);
+extern int drm_mode_obj_set_property_ioctl(struct drm_device *dev, void *data,
+					   struct drm_file *file_priv);
 
 extern void drm_fb_get_bpp_depth(uint32_t format, unsigned int *depth,
 				 int *bpp);
+extern int drm_format_num_planes(uint32_t format);
+extern int drm_format_plane_cpp(uint32_t format, int plane);
+extern int drm_format_horz_chroma_subsampling(uint32_t format);
+extern int drm_format_vert_chroma_subsampling(uint32_t format);
+
 #endif /* __DRM_CRTC_H__ */
diff --git a/include/drm/drm_crtc_helper.h b/include/drm/drm_crtc_helper.h
index 37515d1afab3..7988e55c98d0 100644
--- a/include/drm/drm_crtc_helper.h
+++ b/include/drm/drm_crtc_helper.h
@@ -44,6 +44,13 @@ enum mode_set_atomic {
 	ENTER_ATOMIC_MODE_SET,
 };
 
+/**
+ * drm_crtc_helper_funcs - helper operations for CRTCs
+ * @mode_fixup: try to fixup proposed mode for this connector
+ * @mode_set: set this mode
+ *
+ * The helper operations are called by the mid-layer CRTC helper.
+ */
 struct drm_crtc_helper_funcs {
 	/*
 	 * Control power levels on the CRTC.  If the mode passed in is
@@ -76,6 +83,13 @@ struct drm_crtc_helper_funcs {
 	void (*disable)(struct drm_crtc *crtc);
 };
 
+/**
+ * drm_encoder_helper_funcs - helper operations for encoders
+ * @mode_fixup: try to fixup proposed mode for this connector
+ * @mode_set: set this mode
+ *
+ * The helper operations are called by the mid-layer CRTC helper.
+ */
 struct drm_encoder_helper_funcs {
 	void (*dpms)(struct drm_encoder *encoder, int mode);
 	void (*save)(struct drm_encoder *encoder);
@@ -97,6 +111,13 @@ struct drm_encoder_helper_funcs {
 	void (*disable)(struct drm_encoder *encoder);
 };
 
+/**
+ * drm_connector_helper_funcs - helper operations for connectors
+ * @get_modes: get mode list for this connector
+ * @mode_valid: is this mode valid on the given connector?
+ *
+ * The helper operations are called by the mid-layer CRTC helper.
+ */
 struct drm_connector_helper_funcs {
 	int (*get_modes)(struct drm_connector *connector);
 	int (*mode_valid)(struct drm_connector *connector,
@@ -145,6 +166,4 @@ extern void drm_helper_hpd_irq_event(struct drm_device *dev);
 extern void drm_kms_helper_poll_disable(struct drm_device *dev);
 extern void drm_kms_helper_poll_enable(struct drm_device *dev);
 
-extern int drm_format_num_planes(uint32_t format);
-
 #endif
diff --git a/include/drm/drm_dp_helper.h b/include/drm/drm_dp_helper.h
index 93df2d72750b..1744b18c06b3 100644
--- a/include/drm/drm_dp_helper.h
+++ b/include/drm/drm_dp_helper.h
@@ -72,6 +72,10 @@
 
 #define DP_MAIN_LINK_CHANNEL_CODING         0x006
 
+#define DP_DOWN_STREAM_PORT_COUNT	    0x007
+#define  DP_PORT_COUNT_MASK		    0x0f
+#define  DP_OUI_SUPPORT			    (1 << 7)
+
 #define DP_EDP_CONFIGURATION_CAP            0x00d
 #define DP_TRAINING_AUX_RD_INTERVAL         0x00e
 
@@ -213,6 +217,10 @@
 # define DP_TEST_NAK			    (1 << 1)
 # define DP_TEST_EDID_CHECKSUM_WRITE	    (1 << 2)
 
+#define DP_SOURCE_OUI			    0x300
+#define DP_SINK_OUI			    0x400
+#define DP_BRANCH_OUI			    0x500
+
 #define DP_SET_POWER                        0x600
 # define DP_SET_POWER_D0                    0x1
 # define DP_SET_POWER_D3                    0x2
diff --git a/include/drm/drm_edid.h b/include/drm/drm_edid.h
index bcb9a66baa8c..0cac551c5347 100644
--- a/include/drm/drm_edid.h
+++ b/include/drm/drm_edid.h
@@ -90,12 +90,26 @@ struct detailed_data_monitor_range {
 	u8 min_hfreq_khz;
 	u8 max_hfreq_khz;
 	u8 pixel_clock_mhz; /* need to multiply by 10 */
-	__le16 sec_gtf_toggle; /* A000=use above, 20=use below */
-	u8 hfreq_start_khz; /* need to multiply by 2 */
-	u8 c; /* need to divide by 2 */
-	__le16 m;
-	u8 k;
-	u8 j; /* need to divide by 2 */
+	u8 flags;
+	union {
+		struct {
+			u8 reserved;
+			u8 hfreq_start_khz; /* need to multiply by 2 */
+			u8 c; /* need to divide by 2 */
+			__le16 m;
+			u8 k;
+			u8 j; /* need to divide by 2 */
+		} __attribute__((packed)) gtf2;
+		struct {
+			u8 version;
+			u8 data1; /* high 6 bits: extra clock resolution */
+			u8 data2; /* plus low 2 of above: max hactive */
+			u8 supported_aspects;
+			u8 flags; /* preferred aspect and blanking support */
+			u8 supported_scalings;
+			u8 preferred_refresh;
+		} __attribute__((packed)) cvt;
+	} formula;
 } __attribute__((packed));
 
 struct detailed_data_wpindex {
diff --git a/include/drm/drm_fixed.h b/include/drm/drm_fixed.h
index 4a08a664ff1f..0ead502e17d2 100644
--- a/include/drm/drm_fixed.h
+++ b/include/drm/drm_fixed.h
@@ -37,6 +37,7 @@ typedef union dfixed {
 #define dfixed_init(A) { .full = dfixed_const((A)) }
 #define dfixed_init_half(A) { .full = dfixed_const_half((A)) }
 #define dfixed_trunc(A) ((A).full >> 12)
+#define dfixed_frac(A) ((A).full & ((1 << 12) - 1))
 
 static inline u32 dfixed_floor(fixed20_12 A)
 {
diff --git a/include/drm/drm_mode.h b/include/drm/drm_mode.h
index 4a0aae38e160..5581980b14f6 100644
--- a/include/drm/drm_mode.h
+++ b/include/drm/drm_mode.h
@@ -230,6 +230,7 @@ struct drm_mode_get_connector {
 #define DRM_MODE_PROP_IMMUTABLE	(1<<2)
 #define DRM_MODE_PROP_ENUM	(1<<3) /* enumerated type with text strings */
 #define DRM_MODE_PROP_BLOB	(1<<4)
+#define DRM_MODE_PROP_BITMASK	(1<<5) /* bitmask of enumerated types */
 
 struct drm_mode_property_enum {
 	__u64 value;
@@ -254,6 +255,21 @@ struct drm_mode_connector_set_property {
 	__u32 connector_id;
 };
 
+struct drm_mode_obj_get_properties {
+	__u64 props_ptr;
+	__u64 prop_values_ptr;
+	__u32 count_props;
+	__u32 obj_id;
+	__u32 obj_type;
+};
+
+struct drm_mode_obj_set_property {
+	__u64 value;
+	__u32 prop_id;
+	__u32 obj_id;
+	__u32 obj_type;
+};
+
 struct drm_mode_get_blob {
 	__u32 blob_id;
 	__u32 length;
diff --git a/include/drm/exynos_drm.h b/include/drm/exynos_drm.h
index e478de4e5d56..b6d7ce92eadd 100644
--- a/include/drm/exynos_drm.h
+++ b/include/drm/exynos_drm.h
@@ -29,6 +29,8 @@
 #ifndef _EXYNOS_DRM_H_
 #define _EXYNOS_DRM_H_
 
+#include "drm.h"
+
 /**
  * User-desired buffer creation information structure.
  *
@@ -75,6 +77,21 @@ struct drm_exynos_gem_mmap {
 };
 
 /**
+ * A structure to gem information.
+ *
+ * @handle: a handle to gem object created.
+ * @flags: flag value including memory type and cache attribute and
+ *	this value would be set by driver.
+ * @size: size to memory region allocated by gem and this size would
+ *	be set by driver.
+ */
+struct drm_exynos_gem_info {
+	unsigned int handle;
+	unsigned int flags;
+	uint64_t size;
+};
+
+/**
  * A structure for user connection request of virtual display.
  *
  * @connection: indicate whether doing connetion or not by user.
@@ -95,18 +112,64 @@ struct drm_exynos_plane_set_zpos {
 
 /* memory type definitions. */
 enum e_drm_exynos_gem_mem_type {
+	/* Physically Continuous memory and used as default. */
+	EXYNOS_BO_CONTIG	= 0 << 0,
 	/* Physically Non-Continuous memory. */
 	EXYNOS_BO_NONCONTIG	= 1 << 0,
-	EXYNOS_BO_MASK		= EXYNOS_BO_NONCONTIG
+	/* non-cachable mapping and used as default. */
+	EXYNOS_BO_NONCACHABLE	= 0 << 1,
+	/* cachable mapping. */
+	EXYNOS_BO_CACHABLE	= 1 << 1,
+	/* write-combine mapping. */
+	EXYNOS_BO_WC		= 1 << 2,
+	EXYNOS_BO_MASK		= EXYNOS_BO_NONCONTIG | EXYNOS_BO_CACHABLE |
+					EXYNOS_BO_WC
+};
+
+struct drm_exynos_g2d_get_ver {
+	__u32	major;
+	__u32	minor;
+};
+
+struct drm_exynos_g2d_cmd {
+	__u32	offset;
+	__u32	data;
+};
+
+enum drm_exynos_g2d_event_type {
+	G2D_EVENT_NOT,
+	G2D_EVENT_NONSTOP,
+	G2D_EVENT_STOP,		/* not yet */
+};
+
+struct drm_exynos_g2d_set_cmdlist {
+	__u64					cmd;
+	__u64					cmd_gem;
+	__u32					cmd_nr;
+	__u32					cmd_gem_nr;
+
+	/* for g2d event */
+	__u64					event_type;
+	__u64					user_data;
+};
+
+struct drm_exynos_g2d_exec {
+	__u64					async;
 };
 
 #define DRM_EXYNOS_GEM_CREATE		0x00
 #define DRM_EXYNOS_GEM_MAP_OFFSET	0x01
 #define DRM_EXYNOS_GEM_MMAP		0x02
 /* Reserved 0x03 ~ 0x05 for exynos specific gem ioctl */
+#define DRM_EXYNOS_GEM_GET		0x04
 #define DRM_EXYNOS_PLANE_SET_ZPOS	0x06
 #define DRM_EXYNOS_VIDI_CONNECTION	0x07
 
+/* G2D */
+#define DRM_EXYNOS_G2D_GET_VER		0x20
+#define DRM_EXYNOS_G2D_SET_CMDLIST	0x21
+#define DRM_EXYNOS_G2D_EXEC		0x22
+
 #define DRM_IOCTL_EXYNOS_GEM_CREATE		DRM_IOWR(DRM_COMMAND_BASE + \
 		DRM_EXYNOS_GEM_CREATE, struct drm_exynos_gem_create)
 
@@ -116,12 +179,34 @@ enum e_drm_exynos_gem_mem_type {
 #define DRM_IOCTL_EXYNOS_GEM_MMAP	DRM_IOWR(DRM_COMMAND_BASE + \
 		DRM_EXYNOS_GEM_MMAP, struct drm_exynos_gem_mmap)
 
+#define DRM_IOCTL_EXYNOS_GEM_GET	DRM_IOWR(DRM_COMMAND_BASE + \
+		DRM_EXYNOS_GEM_GET,	struct drm_exynos_gem_info)
+
 #define DRM_IOCTL_EXYNOS_PLANE_SET_ZPOS	DRM_IOWR(DRM_COMMAND_BASE + \
 		DRM_EXYNOS_PLANE_SET_ZPOS, struct drm_exynos_plane_set_zpos)
 
 #define DRM_IOCTL_EXYNOS_VIDI_CONNECTION	DRM_IOWR(DRM_COMMAND_BASE + \
 		DRM_EXYNOS_VIDI_CONNECTION, struct drm_exynos_vidi_connection)
 
+#define DRM_IOCTL_EXYNOS_G2D_GET_VER		DRM_IOWR(DRM_COMMAND_BASE + \
+		DRM_EXYNOS_G2D_GET_VER, struct drm_exynos_g2d_get_ver)
+#define DRM_IOCTL_EXYNOS_G2D_SET_CMDLIST	DRM_IOWR(DRM_COMMAND_BASE + \
+		DRM_EXYNOS_G2D_SET_CMDLIST, struct drm_exynos_g2d_set_cmdlist)
+#define DRM_IOCTL_EXYNOS_G2D_EXEC		DRM_IOWR(DRM_COMMAND_BASE + \
+		DRM_EXYNOS_G2D_EXEC, struct drm_exynos_g2d_exec)
+
+/* EXYNOS specific events */
+#define DRM_EXYNOS_G2D_EVENT		0x80000000
+
+struct drm_exynos_g2d_event {
+	struct drm_event	base;
+	__u64			user_data;
+	__u32			tv_sec;
+	__u32			tv_usec;
+	__u32			cmdlist_no;
+	__u32			reserved;
+};
+
 #ifdef __KERNEL__
 
 /**
@@ -169,16 +254,14 @@ struct exynos_drm_common_hdmi_pd {
 /**
  * Platform Specific Structure for DRM based HDMI core.
  *
- * @timing: default video mode for initializing
- * @default_win: default window layer number to be used for UI.
- * @bpp: default bit per pixel.
  * @is_v13: set if hdmi version 13 is.
+ * @cfg_hpd: function pointer to configure hdmi hotplug detection pin
+ * @get_hpd: function pointer to get value of hdmi hotplug detection pin
  */
 struct exynos_drm_hdmi_pdata {
-	struct fb_videomode		timing;
-	unsigned int			default_win;
-	unsigned int			bpp;
-	unsigned int			is_v13:1;
+	bool is_v13;
+	void (*cfg_hpd)(bool external);
+	int (*get_hpd)(void);
 };
 
 #endif	/* __KERNEL__ */
diff --git a/include/drm/i915_drm.h b/include/drm/i915_drm.h
index da929bb5b788..f3f82242bf1d 100644
--- a/include/drm/i915_drm.h
+++ b/include/drm/i915_drm.h
@@ -296,7 +296,8 @@ typedef struct drm_i915_irq_wait {
 #define I915_PARAM_HAS_EXEC_CONSTANTS	 14
 #define I915_PARAM_HAS_RELAXED_DELTA	 15
 #define I915_PARAM_HAS_GEN7_SOL_RESET	 16
-#define I915_PARAM_HAS_LLC     	 17
+#define I915_PARAM_HAS_LLC     	 	 17
+#define I915_PARAM_HAS_ALIASING_PPGTT	 18
 
 typedef struct drm_i915_getparam {
 	int param;
diff --git a/include/drm/radeon_drm.h b/include/drm/radeon_drm.h
index 7c491b4bcf65..58056865b8e9 100644
--- a/include/drm/radeon_drm.h
+++ b/include/drm/radeon_drm.h
@@ -926,7 +926,6 @@ struct drm_radeon_cs_chunk {
 };
 
 /* drm_radeon_cs_reloc.flags */
-#define RADEON_RELOC_DONT_SYNC		0x01
 
 struct drm_radeon_cs_reloc {
 	uint32_t		handle;
diff --git a/include/drm/ttm/ttm_bo_api.h b/include/drm/ttm/ttm_bo_api.h
index 974c8f801c39..e15f2a89a270 100644
--- a/include/drm/ttm/ttm_bo_api.h
+++ b/include/drm/ttm/ttm_bo_api.h
@@ -124,11 +124,15 @@ struct ttm_mem_reg {
  *
  * @ttm_bo_type_kernel: These buffers are like ttm_bo_type_device buffers,
  * but they cannot be accessed from user-space. For kernel-only use.
+ *
+ * @ttm_bo_type_sg: Buffer made from dmabuf sg table shared with another
+ * driver.
  */
 
 enum ttm_bo_type {
 	ttm_bo_type_device,
-	ttm_bo_type_kernel
+	ttm_bo_type_kernel,
+	ttm_bo_type_sg
 };
 
 struct ttm_tt;
@@ -271,6 +275,8 @@ struct ttm_buffer_object {
 
 	unsigned long offset;
 	uint32_t cur_placement;
+
+	struct sg_table *sg;
 };
 
 /**
@@ -503,6 +509,7 @@ extern int ttm_bo_init(struct ttm_bo_device *bdev,
 			bool interrubtible,
 			struct file *persistent_swap_storage,
 			size_t acc_size,
+			struct sg_table *sg,
 			void (*destroy) (struct ttm_buffer_object *));
 
 /**
diff --git a/include/drm/ttm/ttm_bo_driver.h b/include/drm/ttm/ttm_bo_driver.h
index d43e892307ff..a05f1b55714d 100644
--- a/include/drm/ttm/ttm_bo_driver.h
+++ b/include/drm/ttm/ttm_bo_driver.h
@@ -81,6 +81,7 @@ struct ttm_backend_func {
 #define TTM_PAGE_FLAG_PERSISTENT_SWAP (1 << 5)
 #define TTM_PAGE_FLAG_ZERO_ALLOC      (1 << 6)
 #define TTM_PAGE_FLAG_DMA32           (1 << 7)
+#define TTM_PAGE_FLAG_SG              (1 << 8)
 
 enum ttm_caching_state {
 	tt_uncached,
@@ -116,6 +117,7 @@ struct ttm_tt {
 	struct page **pages;
 	uint32_t page_flags;
 	unsigned long num_pages;
+	struct sg_table *sg; /* for SG objects via dma-buf */
 	struct ttm_bo_global *glob;
 	struct ttm_backend *be;
 	struct file *swap_storage;
diff --git a/include/linux/Kbuild b/include/linux/Kbuild
index 39737839ce29..4cd59b95858f 100644
--- a/include/linux/Kbuild
+++ b/include/linux/Kbuild
@@ -270,6 +270,7 @@ header-y += netfilter_ipv4.h
 header-y += netfilter_ipv6.h
 header-y += netlink.h
 header-y += netrom.h
+header-y += nfc.h
 header-y += nfs.h
 header-y += nfs2.h
 header-y += nfs3.h
@@ -383,6 +384,7 @@ header-y += utime.h
 header-y += utsname.h
 header-y += uuid.h
 header-y += uvcvideo.h
+header-y += v4l2-dv-timings.h
 header-y += v4l2-mediabus.h
 header-y += v4l2-subdev.h
 header-y += veth.h
diff --git a/include/linux/bcma/bcma.h b/include/linux/bcma/bcma.h
index 98bb2901d7b7..8deaf6d050c3 100644
--- a/include/linux/bcma/bcma.h
+++ b/include/linux/bcma/bcma.h
@@ -26,6 +26,11 @@ struct bcma_chipinfo {
 	u8 pkg;
 };
 
+struct bcma_boardinfo {
+	u16 vendor;
+	u16 type;
+};
+
 enum bcma_clkmode {
 	BCMA_CLKMODE_FAST,
 	BCMA_CLKMODE_DYNAMIC,
@@ -199,6 +204,8 @@ struct bcma_bus {
 
 	struct bcma_chipinfo chipinfo;
 
+	struct bcma_boardinfo boardinfo;
+
 	struct bcma_device *mapped_core;
 	struct list_head cores;
 	u8 nr_cores;
diff --git a/include/linux/bcma/bcma_driver_pci.h b/include/linux/bcma/bcma_driver_pci.h
index 46c71e27d31f..41da581e1612 100644
--- a/include/linux/bcma/bcma_driver_pci.h
+++ b/include/linux/bcma/bcma_driver_pci.h
@@ -87,6 +87,13 @@ struct pci_dev;
 #define BCMA_CORE_PCI_PCICFG2			0x0600	/* PCI config space 2 (rev >= 8) */
 #define BCMA_CORE_PCI_PCICFG3			0x0700	/* PCI config space 3 (rev >= 8) */
 #define BCMA_CORE_PCI_SPROM(wordoffset)		(0x0800 + ((wordoffset) * 2)) /* SPROM shadow area (72 bytes) */
+#define  BCMA_CORE_PCI_SPROM_PI_OFFSET		0	/* first word */
+#define   BCMA_CORE_PCI_SPROM_PI_MASK		0xf000	/* bit 15:12 */
+#define   BCMA_CORE_PCI_SPROM_PI_SHIFT		12	/* bit 15:12 */
+#define  BCMA_CORE_PCI_SPROM_MISC_CONFIG	5	/* word 5 */
+#define   BCMA_CORE_PCI_SPROM_L23READY_EXIT_NOPERST	0x8000	/* bit 15 */
+#define   BCMA_CORE_PCI_SPROM_CLKREQ_OFFSET_REV5	20	/* word 20 for srom rev <= 5 */
+#define   BCMA_CORE_PCI_SPROM_CLKREQ_ENB	0x0800	/* bit 11 */
 
 /* SBtoPCIx */
 #define BCMA_CORE_PCI_SBTOPCI_MEM		0x00000000
@@ -133,6 +140,7 @@ struct pci_dev;
 #define BCMA_CORE_PCI_DLLP_LRREG		0x120	/* Link Replay */
 #define BCMA_CORE_PCI_DLLP_LACKTOREG		0x124	/* Link Ack Timeout */
 #define BCMA_CORE_PCI_DLLP_PMTHRESHREG		0x128	/* Power Management Threshold */
+#define  BCMA_CORE_PCI_ASPMTIMER_EXTEND		0x01000000 /* > rev7: enable extend ASPM timer */
 #define BCMA_CORE_PCI_DLLP_RTRYWPREG		0x12C	/* Retry buffer write ptr */
 #define BCMA_CORE_PCI_DLLP_RTRYRPREG		0x130	/* Retry buffer Read ptr */
 #define BCMA_CORE_PCI_DLLP_RTRYPPREG		0x134	/* Retry buffer Purged ptr */
@@ -201,12 +209,15 @@ struct bcma_drv_pci {
 };
 
 /* Register access */
+#define pcicore_read16(pc, offset)		bcma_read16((pc)->core, offset)
 #define pcicore_read32(pc, offset)		bcma_read32((pc)->core, offset)
+#define pcicore_write16(pc, offset, val)	bcma_write16((pc)->core, offset, val)
 #define pcicore_write32(pc, offset, val)	bcma_write32((pc)->core, offset, val)
 
 extern void __devinit bcma_core_pci_init(struct bcma_drv_pci *pc);
 extern int bcma_core_pci_irq_ctl(struct bcma_drv_pci *pc,
 				 struct bcma_device *core, bool enable);
+extern void bcma_core_pci_extend_L1timer(struct bcma_drv_pci *pc, bool extend);
 
 extern int bcma_core_pci_pcibios_map_irq(const struct pci_dev *dev);
 extern int bcma_core_pci_plat_dev_init(struct pci_dev *dev);
diff --git a/include/linux/bootmem.h b/include/linux/bootmem.h
index 66d3e954eb6c..1a0cd270bb7a 100644
--- a/include/linux/bootmem.h
+++ b/include/linux/bootmem.h
@@ -154,7 +154,8 @@ extern void *alloc_large_system_hash(const char *tablename,
 				     int flags,
 				     unsigned int *_hash_shift,
 				     unsigned int *_hash_mask,
-				     unsigned long limit);
+				     unsigned long low_limit,
+				     unsigned long high_limit);
 
 #define HASH_EARLY	0x00000001	/* Allocating during early boot? */
 #define HASH_SMALL	0x00000002	/* sub-page allocation allowed, min
diff --git a/include/linux/dvb/frontend.h b/include/linux/dvb/frontend.h
index cb4428ab81ed..f50d4058c5fb 100644
--- a/include/linux/dvb/frontend.h
+++ b/include/linux/dvb/frontend.h
@@ -320,7 +320,24 @@ struct dvb_frontend_event {
 
 #define DTV_ENUM_DELSYS		44
 
-#define DTV_MAX_COMMAND				DTV_ENUM_DELSYS
+/* ATSC-MH */
+#define DTV_ATSCMH_FIC_VER		45
+#define DTV_ATSCMH_PARADE_ID		46
+#define DTV_ATSCMH_NOG			47
+#define DTV_ATSCMH_TNOG			48
+#define DTV_ATSCMH_SGN			49
+#define DTV_ATSCMH_PRC			50
+#define DTV_ATSCMH_RS_FRAME_MODE	51
+#define DTV_ATSCMH_RS_FRAME_ENSEMBLE	52
+#define DTV_ATSCMH_RS_CODE_MODE_PRI	53
+#define DTV_ATSCMH_RS_CODE_MODE_SEC	54
+#define DTV_ATSCMH_SCCC_BLOCK_MODE	55
+#define DTV_ATSCMH_SCCC_CODE_MODE_A	56
+#define DTV_ATSCMH_SCCC_CODE_MODE_B	57
+#define DTV_ATSCMH_SCCC_CODE_MODE_C	58
+#define DTV_ATSCMH_SCCC_CODE_MODE_D	59
+
+#define DTV_MAX_COMMAND				DTV_ATSCMH_SCCC_CODE_MODE_D
 
 typedef enum fe_pilot {
 	PILOT_ON,
@@ -360,6 +377,38 @@ typedef enum fe_delivery_system {
 
 #define SYS_DVBC_ANNEX_AC	SYS_DVBC_ANNEX_A
 
+/* ATSC-MH */
+
+enum atscmh_sccc_block_mode {
+	ATSCMH_SCCC_BLK_SEP      = 0,
+	ATSCMH_SCCC_BLK_COMB     = 1,
+	ATSCMH_SCCC_BLK_RES      = 2,
+};
+
+enum atscmh_sccc_code_mode {
+	ATSCMH_SCCC_CODE_HLF     = 0,
+	ATSCMH_SCCC_CODE_QTR     = 1,
+	ATSCMH_SCCC_CODE_RES     = 2,
+};
+
+enum atscmh_rs_frame_ensemble {
+	ATSCMH_RSFRAME_ENS_PRI   = 0,
+	ATSCMH_RSFRAME_ENS_SEC   = 1,
+};
+
+enum atscmh_rs_frame_mode {
+	ATSCMH_RSFRAME_PRI_ONLY  = 0,
+	ATSCMH_RSFRAME_PRI_SEC   = 1,
+	ATSCMH_RSFRAME_RES       = 2,
+};
+
+enum atscmh_rs_code_mode {
+	ATSCMH_RSCODE_211_187    = 0,
+	ATSCMH_RSCODE_223_187    = 1,
+	ATSCMH_RSCODE_235_187    = 2,
+	ATSCMH_RSCODE_RES        = 3,
+};
+
 
 struct dtv_cmds_h {
 	char	*name;		/* A display name for debugging purposes */
diff --git a/include/linux/dvb/version.h b/include/linux/dvb/version.h
index 0559e2bd38f9..43d9e8d462d4 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 5
+#define DVB_API_VERSION_MINOR 6
 
 #endif /*_DVBVERSION_H_*/
diff --git a/include/linux/firewire.h b/include/linux/firewire.h
index e83c24af358a..7edcf1031718 100644
--- a/include/linux/firewire.h
+++ b/include/linux/firewire.h
@@ -349,6 +349,7 @@ int fw_cancel_transaction(struct fw_card *card,
 int fw_run_transaction(struct fw_card *card, int tcode, int destination_id,
 		       int generation, int speed, unsigned long long offset,
 		       void *payload, size_t length);
+const char *fw_rcode_string(int rcode);
 
 static inline int fw_stream_packet_destination_id(int tag, int channel, int sy)
 {
@@ -406,6 +407,7 @@ struct fw_iso_buffer {
 	enum dma_data_direction direction;
 	struct page **pages;
 	int page_count;
+	int page_count_mapped;
 };
 
 int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
diff --git a/drivers/input/fixp-arith.h b/include/linux/fixp-arith.h
index 3089d7382325..3089d7382325 100644
--- a/drivers/input/fixp-arith.h
+++ b/include/linux/fixp-arith.h
diff --git a/include/linux/gameport.h b/include/linux/gameport.h
index b456b08d70ed..b986be513406 100644
--- a/include/linux/gameport.h
+++ b/include/linux/gameport.h
@@ -153,6 +153,19 @@ int __must_check __gameport_register_driver(struct gameport_driver *drv,
 
 void gameport_unregister_driver(struct gameport_driver *drv);
 
+/**
+ * module_gameport_driver() - Helper macro for registering a gameport driver
+ * @__gameport_driver: gameport_driver struct
+ *
+ * Helper macro for gameport drivers which do not do anything special in
+ * module init/exit. This eliminates a lot of boilerplate. Each module may
+ * only use this macro once, and calling it replaces module_init() and
+ * module_exit().
+ */
+#define module_gameport_driver(__gameport_driver) \
+	module_driver(__gameport_driver, gameport_register_driver, \
+		       gameport_unregister_driver)
+
 #endif /* __KERNEL__ */
 
 #define GAMEPORT_MODE_DISABLED		0
diff --git a/include/linux/i2c/adp5588.h b/include/linux/i2c/adp5588.h
index cec17cf6cac2..d8341cb47b60 100644
--- a/include/linux/i2c/adp5588.h
+++ b/include/linux/i2c/adp5588.h
@@ -157,6 +157,7 @@ struct i2c_client; /* forward declaration */
 
 struct adp5588_gpio_platform_data {
 	int gpio_start;		/* GPIO Chip base # */
+	const char *const *names;
 	unsigned irq_base;	/* interrupt base # */
 	unsigned pullup_dis_mask; /* Pull-Up Disable Mask */
 	int	(*setup)(struct i2c_client *client,
diff --git a/include/linux/if_arp.h b/include/linux/if_arp.h
index 26cb3c2c5c71..f0e69c6e8208 100644
--- a/include/linux/if_arp.h
+++ b/include/linux/if_arp.h
@@ -82,7 +82,7 @@
 #define ARPHRD_FCPL	786		/* Fibrechannel public loop	*/
 #define ARPHRD_FCFABRIC	787		/* Fibrechannel fabric		*/
 	/* 787->799 reserved for fibrechannel media types */
-/* 800 used to be used for token ring */
+#define ARPHRD_IEEE802_TR 800		/* Magic type ident for TR	*/
 #define ARPHRD_IEEE80211 801		/* IEEE 802.11			*/
 #define ARPHRD_IEEE80211_PRISM 802	/* IEEE 802.11 + Prism2 header  */
 #define ARPHRD_IEEE80211_RADIOTAP 803	/* IEEE 802.11 + radiotap header */
diff --git a/include/linux/input/lm8333.h b/include/linux/input/lm8333.h
new file mode 100644
index 000000000000..79f918c6e8c5
--- /dev/null
+++ b/include/linux/input/lm8333.h
@@ -0,0 +1,24 @@
+/*
+ * public include for LM8333 keypad driver - same license as driver
+ * Copyright (C) 2012 Wolfram Sang, Pengutronix <w.sang@pengutronix.de>
+ */
+
+#ifndef _LM8333_H
+#define _LM8333_H
+
+struct lm8333;
+
+struct lm8333_platform_data {
+	/* Keymap data */
+	const struct matrix_keymap_data *matrix_data;
+	/* Active timeout before enter HALT mode in microseconds */
+	unsigned active_time;
+	/* Debounce interval in microseconds */
+	unsigned debounce_time;
+};
+
+extern int lm8333_read8(struct lm8333 *lm8333, u8 cmd);
+extern int lm8333_write8(struct lm8333 *lm8333, u8 cmd, u8 val);
+extern int lm8333_read_block(struct lm8333 *lm8333, u8 cmd, u8 len, u8 *buf);
+
+#endif /* _LM8333_H */
diff --git a/include/linux/input/matrix_keypad.h b/include/linux/input/matrix_keypad.h
index 6c07ced0af81..5f3aa6b11bfa 100644
--- a/include/linux/input/matrix_keypad.h
+++ b/include/linux/input/matrix_keypad.h
@@ -75,54 +75,10 @@ struct matrix_keypad_platform_data {
 	bool		no_autorepeat;
 };
 
-/**
- * matrix_keypad_build_keymap - convert platform keymap into matrix keymap
- * @keymap_data: keymap supplied by the platform code
- * @row_shift: number of bits to shift row value by to advance to the next
- * line in the keymap
- * @keymap: expanded version of keymap that is suitable for use by
- * matrix keyboad driver
- * @keybit: pointer to bitmap of keys supported by input device
- *
- * This function converts platform keymap (encoded with KEY() macro) into
- * an array of keycodes that is suitable for using in a standard matrix
- * keyboard driver that uses row and col as indices.
- */
-static inline void
-matrix_keypad_build_keymap(const struct matrix_keymap_data *keymap_data,
-			   unsigned int row_shift,
-			   unsigned short *keymap, unsigned long *keybit)
-{
-	int i;
-
-	for (i = 0; i < keymap_data->keymap_size; i++) {
-		unsigned int key = keymap_data->keymap[i];
-		unsigned int row = KEY_ROW(key);
-		unsigned int col = KEY_COL(key);
-		unsigned short code = KEY_VAL(key);
-
-		keymap[MATRIX_SCAN_CODE(row, col, row_shift)] = code;
-		__set_bit(code, keybit);
-	}
-	__clear_bit(KEY_RESERVED, keybit);
-}
-
-#ifdef CONFIG_INPUT_OF_MATRIX_KEYMAP
-struct matrix_keymap_data *
-matrix_keyboard_of_fill_keymap(struct device_node *np, const char *propname);
-
-void matrix_keyboard_of_free_keymap(const struct matrix_keymap_data *kd);
-#else
-static inline struct matrix_keymap_data *
-matrix_keyboard_of_fill_keymap(struct device_node *np, const char *propname)
-{
-	return NULL;
-}
-
-static inline void
-matrix_keyboard_of_free_keymap(const struct matrix_keymap_data *kd)
-{
-}
-#endif
+int matrix_keypad_build_keymap(const struct matrix_keymap_data *keymap_data,
+			       const char *keymap_name,
+			       unsigned int rows, unsigned int cols,
+			       unsigned short *keymap,
+			       struct input_dev *input_dev);
 
 #endif /* _MATRIX_KEYPAD_H */
diff --git a/include/linux/input/navpoint.h b/include/linux/input/navpoint.h
new file mode 100644
index 000000000000..45050eb34de3
--- /dev/null
+++ b/include/linux/input/navpoint.h
@@ -0,0 +1,12 @@
+/*
+ *  Copyright (C) 2012 Paul Parsons <lost.distance@yahoo.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.
+ */
+
+struct navpoint_platform_data {
+	int		port;		/* PXA SSP port for pxa_ssp_request() */
+	int		gpio;		/* GPIO for power on/off */
+};
diff --git a/include/linux/ipx.h b/include/linux/ipx.h
index 8f0243982eb6..3d48014cdd71 100644
--- a/include/linux/ipx.h
+++ b/include/linux/ipx.h
@@ -38,7 +38,7 @@ struct ipx_interface_definition {
 #define IPX_FRAME_8022		2
 #define IPX_FRAME_ETHERII	3
 #define IPX_FRAME_8023		4
-/* obsolete token ring was	5 */
+#define IPX_FRAME_TR_8022       5 /* obsolete */
 	unsigned char ipx_special;
 #define IPX_SPECIAL_NONE	0
 #define IPX_PRIMARY		1
diff --git a/include/linux/micrel_phy.h b/include/linux/micrel_phy.h
index dd8da342a991..61f0905bdc48 100644
--- a/include/linux/micrel_phy.h
+++ b/include/linux/micrel_phy.h
@@ -3,7 +3,7 @@
 
 #define MICREL_PHY_ID_MASK	0x00fffff0
 
-#define PHY_ID_KSZ9021		0x00221611
+#define PHY_ID_KSZ9021		0x00221610
 #define PHY_ID_KS8737		0x00221720
 #define PHY_ID_KS8041		0x00221510
 #define PHY_ID_KS8051		0x00221550
diff --git a/include/linux/mm_types.h b/include/linux/mm_types.h
index 3cc3062b3767..26574c726121 100644
--- a/include/linux/mm_types.h
+++ b/include/linux/mm_types.h
@@ -12,6 +12,7 @@
 #include <linux/completion.h>
 #include <linux/cpumask.h>
 #include <linux/page-debug-flags.h>
+#include <linux/uprobes.h>
 #include <asm/page.h>
 #include <asm/mmu.h>
 
@@ -388,6 +389,7 @@ struct mm_struct {
 #ifdef CONFIG_CPUMASK_OFFSTACK
 	struct cpumask cpumask_allocation;
 #endif
+	struct uprobes_state uprobes_state;
 };
 
 static inline void mm_init_cpumask(struct mm_struct *mm)
diff --git a/include/linux/nfc/pn544.h b/include/linux/nfc/pn544.h
index 7ab8521f2347..9890bbaf4328 100644
--- a/include/linux/nfc/pn544.h
+++ b/include/linux/nfc/pn544.h
@@ -84,6 +84,12 @@ struct pn544_fw_packet {
 };
 
 #ifdef __KERNEL__
+enum {
+	NFC_GPIO_ENABLE,
+	NFC_GPIO_FW_RESET,
+	NFC_GPIO_IRQ
+};
+
 /* board config */
 struct pn544_nfc_platform_data {
 	int (*request_resources) (struct i2c_client *client);
@@ -91,6 +97,7 @@ struct pn544_nfc_platform_data {
 	void (*enable) (int fw);
 	int (*test) (void);
 	void (*disable) (void);
+	int (*get_gpio)(int type);
 };
 #endif /* __KERNEL__ */
 
diff --git a/include/linux/nl80211.h b/include/linux/nl80211.h
index 2540e86d99ab..a6959f72745e 100644
--- a/include/linux/nl80211.h
+++ b/include/linux/nl80211.h
@@ -1594,6 +1594,8 @@ enum nl80211_sta_flags {
 	NL80211_STA_FLAG_MAX = __NL80211_STA_FLAG_AFTER_LAST - 1
 };
 
+#define NL80211_STA_FLAG_MAX_OLD_API	NL80211_STA_FLAG_TDLS_PEER
+
 /**
  * struct nl80211_sta_flag_update - station flags mask/set
  * @mask: mask of station flags to set
@@ -1994,9 +1996,9 @@ enum nl80211_reg_rule_flags {
  * enum nl80211_dfs_regions - regulatory DFS regions
  *
  * @NL80211_DFS_UNSET: Country has no DFS master region specified
- * @NL80211_DFS_FCC_: Country follows DFS master rules from FCC
- * @NL80211_DFS_FCC_: Country follows DFS master rules from ETSI
- * @NL80211_DFS_JP_: Country follows DFS master rules from JP/MKK/Telec
+ * @NL80211_DFS_FCC: Country follows DFS master rules from FCC
+ * @NL80211_DFS_ETSI: Country follows DFS master rules from ETSI
+ * @NL80211_DFS_JP: Country follows DFS master rules from JP/MKK/Telec
  */
 enum nl80211_dfs_regions {
 	NL80211_DFS_UNSET	= 0,
diff --git a/include/linux/pagemap.h b/include/linux/pagemap.h
index cfaaa6949b8b..efa26b4da8d2 100644
--- a/include/linux/pagemap.h
+++ b/include/linux/pagemap.h
@@ -426,7 +426,7 @@ static inline int fault_in_pages_writeable(char __user *uaddr, int size)
 		 */
 		if (((unsigned long)uaddr & PAGE_MASK) !=
 				((unsigned long)end & PAGE_MASK))
-		 	ret = __put_user(0, end);
+			ret = __put_user(0, end);
 	}
 	return ret;
 }
@@ -445,13 +445,73 @@ static inline int fault_in_pages_readable(const char __user *uaddr, int size)
 
 		if (((unsigned long)uaddr & PAGE_MASK) !=
 				((unsigned long)end & PAGE_MASK)) {
-		 	ret = __get_user(c, end);
+			ret = __get_user(c, end);
 			(void)c;
 		}
 	}
 	return ret;
 }
 
+/*
+ * Multipage variants of the above prefault helpers, useful if more than
+ * PAGE_SIZE of data needs to be prefaulted. These are separate from the above
+ * functions (which only handle up to PAGE_SIZE) to avoid clobbering the
+ * filemap.c hotpaths.
+ */
+static inline int fault_in_multipages_writeable(char __user *uaddr, int size)
+{
+	int ret;
+	char __user *end = uaddr + size - 1;
+
+	if (unlikely(size == 0))
+		return 0;
+
+	/*
+	 * Writing zeroes into userspace here is OK, because we know that if
+	 * the zero gets there, we'll be overwriting it.
+	 */
+	while (uaddr <= end) {
+		ret = __put_user(0, uaddr);
+		if (ret != 0)
+			return ret;
+		uaddr += PAGE_SIZE;
+	}
+
+	/* Check whether the range spilled into the next page. */
+	if (((unsigned long)uaddr & PAGE_MASK) ==
+			((unsigned long)end & PAGE_MASK))
+		ret = __put_user(0, end);
+
+	return ret;
+}
+
+static inline int fault_in_multipages_readable(const char __user *uaddr,
+					       int size)
+{
+	volatile char c;
+	int ret;
+	const char __user *end = uaddr + size - 1;
+
+	if (unlikely(size == 0))
+		return 0;
+
+	while (uaddr <= end) {
+		ret = __get_user(c, uaddr);
+		if (ret != 0)
+			return ret;
+		uaddr += PAGE_SIZE;
+	}
+
+	/* Check whether the range spilled into the next page. */
+	if (((unsigned long)uaddr & PAGE_MASK) ==
+			((unsigned long)end & PAGE_MASK)) {
+		ret = __get_user(c, end);
+		(void)c;
+	}
+
+	return ret;
+}
+
 int add_to_page_cache_locked(struct page *page, struct address_space *mapping,
 				pgoff_t index, gfp_t gfp_mask);
 int add_to_page_cache_lru(struct page *page, struct address_space *mapping,
diff --git a/include/linux/sched.h b/include/linux/sched.h
index 5ea8baea9387..f45c0b280b5d 100644
--- a/include/linux/sched.h
+++ b/include/linux/sched.h
@@ -1572,6 +1572,10 @@ struct task_struct {
 #ifdef CONFIG_HAVE_HW_BREAKPOINT
 	atomic_t ptrace_bp_refcnt;
 #endif
+#ifdef CONFIG_UPROBES
+	struct uprobe_task *utask;
+	int uprobe_srcu_id;
+#endif
 };
 
 /* Future-safe accessor for struct task_struct's cpus_allowed. */
diff --git a/include/linux/serio.h b/include/linux/serio.h
index ca82861b0e46..6d6cfd3e94a3 100644
--- a/include/linux/serio.h
+++ b/include/linux/serio.h
@@ -96,6 +96,19 @@ int __must_check __serio_register_driver(struct serio_driver *drv,
 
 void serio_unregister_driver(struct serio_driver *drv);
 
+/**
+ * module_serio_driver() - Helper macro for registering a serio driver
+ * @__serio_driver: serio_driver struct
+ *
+ * Helper macro for serio drivers which do not do anything special in
+ * module init/exit. This eliminates a lot of boilerplate. Each module
+ * may only use this macro once, and calling it replaces module_init()
+ * and module_exit().
+ */
+#define module_serio_driver(__serio_driver) \
+	module_driver(__serio_driver, serio_register_driver, \
+		       serio_unregister_driver)
+
 static inline int serio_write(struct serio *serio, unsigned char data)
 {
 	if (serio->write)
diff --git a/include/linux/ssb/ssb.h b/include/linux/ssb/ssb.h
index d27683180025..bc14bd738ade 100644
--- a/include/linux/ssb/ssb.h
+++ b/include/linux/ssb/ssb.h
@@ -188,7 +188,6 @@ struct ssb_sprom {
 struct ssb_boardinfo {
 	u16 vendor;
 	u16 type;
-	u8  rev;
 };
 
 
diff --git a/include/linux/ssb/ssb_regs.h b/include/linux/ssb/ssb_regs.h
index 40b1ef8595ee..a0525019e1d1 100644
--- a/include/linux/ssb/ssb_regs.h
+++ b/include/linux/ssb/ssb_regs.h
@@ -228,6 +228,7 @@
 #define  SSB_SPROM1_AGAIN_BG_SHIFT	0
 #define  SSB_SPROM1_AGAIN_A		0xFF00	/* A-PHY */
 #define  SSB_SPROM1_AGAIN_A_SHIFT	8
+#define SSB_SPROM1_CCODE		0x0076
 
 /* SPROM Revision 2 (inherits from rev 1) */
 #define SSB_SPROM2_BFLHI		0x0038	/* Boardflags (high 16 bits) */
@@ -267,6 +268,7 @@
 #define  SSB_SPROM3_OFDMGPO		0x107A	/* G-PHY OFDM Power Offset (4 bytes, BigEndian) */
 
 /* SPROM Revision 4 */
+#define SSB_SPROM4_BOARDREV		0x0042	/* Board revision */
 #define SSB_SPROM4_BFLLO		0x0044	/* Boardflags (low 16 bits) */
 #define SSB_SPROM4_BFLHI		0x0046  /* Board Flags Hi */
 #define SSB_SPROM4_BFL2LO		0x0048	/* Board flags 2 (low 16 bits) */
@@ -389,6 +391,11 @@
 #define  SSB_SPROM8_GPIOB_P2		0x00FF	/* Pin 2 */
 #define  SSB_SPROM8_GPIOB_P3		0xFF00	/* Pin 3 */
 #define  SSB_SPROM8_GPIOB_P3_SHIFT	8
+#define SSB_SPROM8_LEDDC		0x009A
+#define  SSB_SPROM8_LEDDC_ON		0xFF00	/* oncount */
+#define  SSB_SPROM8_LEDDC_ON_SHIFT	8
+#define  SSB_SPROM8_LEDDC_OFF		0x00FF	/* offcount */
+#define  SSB_SPROM8_LEDDC_OFF_SHIFT	0
 #define SSB_SPROM8_ANTAVAIL		0x009C  /* Antenna available bitfields*/
 #define  SSB_SPROM8_ANTAVAIL_A		0xFF00	/* A-PHY bitfield */
 #define  SSB_SPROM8_ANTAVAIL_A_SHIFT	8
@@ -404,6 +411,13 @@
 #define  SSB_SPROM8_AGAIN2_SHIFT	0
 #define  SSB_SPROM8_AGAIN3		0xFF00	/* Antenna 3 */
 #define  SSB_SPROM8_AGAIN3_SHIFT	8
+#define SSB_SPROM8_TXRXC		0x00A2
+#define  SSB_SPROM8_TXRXC_TXCHAIN	0x000f
+#define  SSB_SPROM8_TXRXC_TXCHAIN_SHIFT	0
+#define  SSB_SPROM8_TXRXC_RXCHAIN	0x00f0
+#define  SSB_SPROM8_TXRXC_RXCHAIN_SHIFT	4
+#define  SSB_SPROM8_TXRXC_SWITCH	0xff00
+#define  SSB_SPROM8_TXRXC_SWITCH_SHIFT	8
 #define SSB_SPROM8_RSSIPARM2G		0x00A4	/* RSSI params for 2GHz */
 #define  SSB_SPROM8_RSSISMF2G		0x000F
 #define  SSB_SPROM8_RSSISMC2G		0x00F0
@@ -430,6 +444,7 @@
 #define  SSB_SPROM8_TRI5GH_SHIFT	8
 #define SSB_SPROM8_RXPO			0x00AC  /* RX power offsets */
 #define  SSB_SPROM8_RXPO2G		0x00FF	/* 2GHz RX power offset */
+#define  SSB_SPROM8_RXPO2G_SHIFT	0
 #define  SSB_SPROM8_RXPO5G		0xFF00	/* 5GHz RX power offset */
 #define  SSB_SPROM8_RXPO5G_SHIFT	8
 #define SSB_SPROM8_FEM2G		0x00AE
@@ -445,10 +460,38 @@
 #define  SSB_SROM8_FEM_ANTSWLUT		0xF800
 #define  SSB_SROM8_FEM_ANTSWLUT_SHIFT	11
 #define SSB_SPROM8_THERMAL		0x00B2
-#define SSB_SPROM8_MPWR_RAWTS		0x00B4
-#define SSB_SPROM8_TS_SLP_OPT_CORRX	0x00B6
-#define SSB_SPROM8_FOC_HWIQ_IQSWP	0x00B8
-#define SSB_SPROM8_PHYCAL_TEMPDELTA	0x00BA
+#define  SSB_SPROM8_THERMAL_OFFSET	0x00ff
+#define  SSB_SPROM8_THERMAL_OFFSET_SHIFT	0
+#define  SSB_SPROM8_THERMAL_TRESH	0xff00
+#define  SSB_SPROM8_THERMAL_TRESH_SHIFT	8
+/* Temp sense related entries */
+#define SSB_SPROM8_RAWTS		0x00B4
+#define  SSB_SPROM8_RAWTS_RAWTEMP	0x01ff
+#define  SSB_SPROM8_RAWTS_RAWTEMP_SHIFT	0
+#define  SSB_SPROM8_RAWTS_MEASPOWER	0xfe00
+#define  SSB_SPROM8_RAWTS_MEASPOWER_SHIFT	9
+#define SSB_SPROM8_OPT_CORRX		0x00B6
+#define  SSB_SPROM8_OPT_CORRX_TEMP_SLOPE	0x00ff
+#define  SSB_SPROM8_OPT_CORRX_TEMP_SLOPE_SHIFT	0
+#define  SSB_SPROM8_OPT_CORRX_TEMPCORRX	0xfc00
+#define  SSB_SPROM8_OPT_CORRX_TEMPCORRX_SHIFT	10
+#define  SSB_SPROM8_OPT_CORRX_TEMP_OPTION	0x0300
+#define  SSB_SPROM8_OPT_CORRX_TEMP_OPTION_SHIFT	8
+/* FOC: freiquency offset correction, HWIQ: H/W IOCAL enable, IQSWP: IQ CAL swap disable */
+#define SSB_SPROM8_HWIQ_IQSWP		0x00B8
+#define  SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR	0x000f
+#define  SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR_SHIFT	0
+#define  SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP	0x0010
+#define  SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP_SHIFT	4
+#define  SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL	0x0020
+#define  SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL_SHIFT	5
+#define SSB_SPROM8_TEMPDELTA		0x00BA
+#define  SSB_SPROM8_TEMPDELTA_PHYCAL	0x00ff
+#define  SSB_SPROM8_TEMPDELTA_PHYCAL_SHIFT	0
+#define  SSB_SPROM8_TEMPDELTA_PERIOD	0x0f00
+#define  SSB_SPROM8_TEMPDELTA_PERIOD_SHIFT	8
+#define  SSB_SPROM8_TEMPDELTA_HYSTERESIS	0xf000
+#define  SSB_SPROM8_TEMPDELTA_HYSTERESIS_SHIFT	12
 
 /* There are 4 blocks with power info sharing the same layout */
 #define SSB_SROM8_PWR_INFO_CORE0	0x00C0
@@ -513,6 +556,16 @@
 #define SSB_SPROM8_OFDM5GLPO		0x014A	/* 5.2GHz OFDM power offset */
 #define SSB_SPROM8_OFDM5GHPO		0x014E	/* 5.8GHz OFDM power offset */
 
+#define SSB_SPROM8_2G_MCSPO		0x0152
+#define SSB_SPROM8_5G_MCSPO		0x0162
+#define SSB_SPROM8_5GL_MCSPO		0x0172
+#define SSB_SPROM8_5GH_MCSPO		0x0182
+
+#define SSB_SPROM8_CDDPO		0x0192
+#define SSB_SPROM8_STBCPO		0x0194
+#define SSB_SPROM8_BW40PO		0x0196
+#define SSB_SPROM8_BWDUPPO		0x0198
+
 /* Values for boardflags_lo read from SPROM */
 #define SSB_BFL_BTCOEXIST		0x0001	/* implements Bluetooth coexistance */
 #define SSB_BFL_PACTRL			0x0002	/* GPIO 9 controlling the PA */
diff --git a/include/linux/time.h b/include/linux/time.h
index 33a92ead4d88..179f4d6755fc 100644
--- a/include/linux/time.h
+++ b/include/linux/time.h
@@ -167,7 +167,6 @@ extern void get_monotonic_boottime(struct timespec *ts);
 extern struct timespec timespec_trunc(struct timespec t, unsigned gran);
 extern int timekeeping_valid_for_hres(void);
 extern u64 timekeeping_max_deferment(void);
-extern void timekeeping_leap_insert(int leapsecond);
 extern int timekeeping_inject_offset(struct timespec *ts);
 
 struct tms;
diff --git a/include/linux/uprobes.h b/include/linux/uprobes.h
new file mode 100644
index 000000000000..efe4b3308c74
--- /dev/null
+++ b/include/linux/uprobes.h
@@ -0,0 +1,165 @@
+#ifndef _LINUX_UPROBES_H
+#define _LINUX_UPROBES_H
+/*
+ * User-space Probes (UProbes)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Copyright (C) IBM Corporation, 2008-2012
+ * Authors:
+ *	Srikar Dronamraju
+ *	Jim Keniston
+ * Copyright (C) 2011-2012 Red Hat, Inc., Peter Zijlstra <pzijlstr@redhat.com>
+ */
+
+#include <linux/errno.h>
+#include <linux/rbtree.h>
+
+struct vm_area_struct;
+struct mm_struct;
+struct inode;
+
+#ifdef CONFIG_ARCH_SUPPORTS_UPROBES
+# include <asm/uprobes.h>
+#endif
+
+/* flags that denote/change uprobes behaviour */
+
+/* Have a copy of original instruction */
+#define UPROBE_COPY_INSN	0x1
+
+/* Dont run handlers when first register/ last unregister in progress*/
+#define UPROBE_RUN_HANDLER	0x2
+/* Can skip singlestep */
+#define UPROBE_SKIP_SSTEP	0x4
+
+struct uprobe_consumer {
+	int (*handler)(struct uprobe_consumer *self, struct pt_regs *regs);
+	/*
+	 * filter is optional; If a filter exists, handler is run
+	 * if and only if filter returns true.
+	 */
+	bool (*filter)(struct uprobe_consumer *self, struct task_struct *task);
+
+	struct uprobe_consumer *next;
+};
+
+#ifdef CONFIG_UPROBES
+enum uprobe_task_state {
+	UTASK_RUNNING,
+	UTASK_BP_HIT,
+	UTASK_SSTEP,
+	UTASK_SSTEP_ACK,
+	UTASK_SSTEP_TRAPPED,
+};
+
+/*
+ * uprobe_task: Metadata of a task while it singlesteps.
+ */
+struct uprobe_task {
+	enum uprobe_task_state		state;
+	struct arch_uprobe_task		autask;
+
+	struct uprobe			*active_uprobe;
+
+	unsigned long			xol_vaddr;
+	unsigned long			vaddr;
+};
+
+/*
+ * On a breakpoint hit, thread contests for a slot.  It frees the
+ * slot after singlestep. Currently a fixed number of slots are
+ * allocated.
+ */
+struct xol_area {
+	wait_queue_head_t 	wq;		/* if all slots are busy */
+	atomic_t 		slot_count;	/* number of in-use slots */
+	unsigned long 		*bitmap;	/* 0 = free slot */
+	struct page 		*page;
+
+	/*
+	 * We keep the vma's vm_start rather than a pointer to the vma
+	 * itself.  The probed process or a naughty kernel module could make
+	 * the vma go away, and we must handle that reasonably gracefully.
+	 */
+	unsigned long 		vaddr;		/* Page(s) of instruction slots */
+};
+
+struct uprobes_state {
+	struct xol_area		*xol_area;
+	atomic_t		count;
+};
+extern int __weak set_swbp(struct arch_uprobe *aup, struct mm_struct *mm, unsigned long vaddr);
+extern int __weak set_orig_insn(struct arch_uprobe *aup, struct mm_struct *mm,  unsigned long vaddr, bool verify);
+extern bool __weak is_swbp_insn(uprobe_opcode_t *insn);
+extern int uprobe_register(struct inode *inode, loff_t offset, struct uprobe_consumer *uc);
+extern void uprobe_unregister(struct inode *inode, loff_t offset, struct uprobe_consumer *uc);
+extern int uprobe_mmap(struct vm_area_struct *vma);
+extern void uprobe_munmap(struct vm_area_struct *vma, unsigned long start, unsigned long end);
+extern void uprobe_free_utask(struct task_struct *t);
+extern void uprobe_copy_process(struct task_struct *t);
+extern unsigned long __weak uprobe_get_swbp_addr(struct pt_regs *regs);
+extern int uprobe_post_sstep_notifier(struct pt_regs *regs);
+extern int uprobe_pre_sstep_notifier(struct pt_regs *regs);
+extern void uprobe_notify_resume(struct pt_regs *regs);
+extern bool uprobe_deny_signal(void);
+extern bool __weak arch_uprobe_skip_sstep(struct arch_uprobe *aup, struct pt_regs *regs);
+extern void uprobe_clear_state(struct mm_struct *mm);
+extern void uprobe_reset_state(struct mm_struct *mm);
+#else /* !CONFIG_UPROBES */
+struct uprobes_state {
+};
+static inline int
+uprobe_register(struct inode *inode, loff_t offset, struct uprobe_consumer *uc)
+{
+	return -ENOSYS;
+}
+static inline void
+uprobe_unregister(struct inode *inode, loff_t offset, struct uprobe_consumer *uc)
+{
+}
+static inline int uprobe_mmap(struct vm_area_struct *vma)
+{
+	return 0;
+}
+static inline void
+uprobe_munmap(struct vm_area_struct *vma, unsigned long start, unsigned long end)
+{
+}
+static inline void uprobe_notify_resume(struct pt_regs *regs)
+{
+}
+static inline bool uprobe_deny_signal(void)
+{
+	return false;
+}
+static inline unsigned long uprobe_get_swbp_addr(struct pt_regs *regs)
+{
+	return 0;
+}
+static inline void uprobe_free_utask(struct task_struct *t)
+{
+}
+static inline void uprobe_copy_process(struct task_struct *t)
+{
+}
+static inline void uprobe_clear_state(struct mm_struct *mm)
+{
+}
+static inline void uprobe_reset_state(struct mm_struct *mm)
+{
+}
+#endif /* !CONFIG_UPROBES */
+#endif	/* _LINUX_UPROBES_H */
diff --git a/include/linux/v4l2-dv-timings.h b/include/linux/v4l2-dv-timings.h
new file mode 100644
index 000000000000..9ef8172e5ed0
--- /dev/null
+++ b/include/linux/v4l2-dv-timings.h
@@ -0,0 +1,816 @@
+/*
+ * V4L2 DV timings header.
+ *
+ * Copyright (C) 2012  Hans Verkuil <hans.verkuil@cisco.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#ifndef _V4L2_DV_TIMINGS_H
+#define _V4L2_DV_TIMINGS_H
+
+#if __GNUC__ < 4 || (__GNUC__ == 4 && (__GNUC_MINOR__ < 6))
+/* Sadly gcc versions older than 4.6 have a bug in how they initialize
+   anonymous unions where they require additional curly brackets.
+   This violates the C1x standard. This workaround adds the curly brackets
+   if needed. */
+#define V4L2_INIT_BT_TIMINGS(_width, args...) \
+	{ .bt = { _width , ## args } }
+#else
+#define V4L2_INIT_BT_TIMINGS(_width, args...) \
+	.bt = { _width , ## args }
+#endif
+
+/* CEA-861-E timings (i.e. standard HDTV timings) */
+
+#define V4L2_DV_BT_CEA_640X480P59_94 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(640, 480, 0, 0, \
+		25175000, 16, 96, 48, 10, 2, 33, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CEA861, 0) \
+}
+
+#define V4L2_DV_BT_CEA_720X480P59_94 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(720, 480, 0, 0, \
+		27000000, 16, 62, 60, 9, 6, 30, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, 0) \
+}
+
+#define V4L2_DV_BT_CEA_720X576P50 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(720, 576, 0, 0, \
+		27000000, 12, 64, 68, 5, 5, 39, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, 0) \
+}
+
+#define V4L2_DV_BT_CEA_1280X720P24 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 720, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		59400000, 1760, 40, 220, 5, 5, 20, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CEA861, \
+		V4L2_DV_FL_CAN_REDUCE_FPS) \
+}
+
+#define V4L2_DV_BT_CEA_1280X720P25 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 720, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 2420, 40, 220, 5, 5, 20, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, 0) \
+}
+
+#define V4L2_DV_BT_CEA_1280X720P30 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 720, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 1760, 40, 220, 5, 5, 20, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_FL_CAN_REDUCE_FPS) \
+}
+
+#define V4L2_DV_BT_CEA_1280X720P50 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 720, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 440, 40, 220, 5, 5, 20, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, 0) \
+}
+
+#define V4L2_DV_BT_CEA_1280X720P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 720, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 110, 40, 220, 5, 5, 20, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_FL_CAN_REDUCE_FPS) \
+}
+
+#define V4L2_DV_BT_CEA_1920X1080P24 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1080, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 638, 44, 148, 4, 5, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_FL_CAN_REDUCE_FPS) \
+}
+
+#define V4L2_DV_BT_CEA_1920X1080P25 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1080, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 528, 44, 148, 4, 5, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, 0) \
+}
+
+#define V4L2_DV_BT_CEA_1920X1080P30 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1080, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 88, 44, 148, 4, 5, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_FL_CAN_REDUCE_FPS) \
+}
+
+#define V4L2_DV_BT_CEA_1920X1080I50 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1080, 1, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 528, 44, 148, 2, 5, 15, 2, 5, 16, \
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_FL_HALF_LINE) \
+}
+
+#define V4L2_DV_BT_CEA_1920X1080P50 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1080, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		148500000, 528, 44, 148, 4, 5, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_CEA861, 0) \
+}
+
+#define V4L2_DV_BT_CEA_1920X1080I60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1080, 1, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		74250000, 88, 44, 148, 2, 5, 15, 2, 5, 16, \
+		V4L2_DV_BT_STD_CEA861, \
+		V4L2_DV_FL_CAN_REDUCE_FPS | V4L2_DV_FL_HALF_LINE) \
+}
+
+#define V4L2_DV_BT_CEA_1920X1080P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1080, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		148500000, 88, 44, 148, 4, 5, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CEA861, \
+		V4L2_DV_FL_CAN_REDUCE_FPS) \
+}
+
+
+/* VESA Discrete Monitor Timings as per version 1.0, revision 12 */
+
+#define V4L2_DV_BT_DMT_640X350P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(640, 350, 0, V4L2_DV_HSYNC_POS_POL, \
+		31500000, 32, 64, 96, 32, 3, 60, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_640X400P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(640, 400, 0, V4L2_DV_VSYNC_POS_POL, \
+		31500000, 32, 64, 96, 1, 3, 41, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_720X400P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(720, 400, 0, V4L2_DV_VSYNC_POS_POL, \
+		35500000, 36, 72, 108, 1, 3, 42, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+/* VGA resolutions */
+#define V4L2_DV_BT_DMT_640X480P60 V4L2_DV_BT_CEA_640X480P59_94
+
+#define V4L2_DV_BT_DMT_640X480P72 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(640, 480, 0, 0, \
+		31500000, 24, 40, 128, 9, 3, 28, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_640X480P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(640, 480, 0, 0, \
+		31500000, 16, 64, 120, 1, 3, 16, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_640X480P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(640, 480, 0, 0, \
+		36000000, 56, 56, 80, 1, 3, 25, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+/* SVGA resolutions */
+#define V4L2_DV_BT_DMT_800X600P56 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(800, 600, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		36000000, 24, 72, 128, 1, 2, 22, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_800X600P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(800, 600, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		40000000, 40, 128, 88, 1, 4, 23, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_800X600P72 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(800, 600, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		50000000, 56, 120, 64, 37, 6, 23, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_800X600P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(800, 600, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		49500000, 16, 80, 160, 1, 3, 21, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_800X600P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(800, 600, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		56250000, 32, 64, 152, 1, 3, 27, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_800X600P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(800, 600, 0, V4L2_DV_HSYNC_POS_POL, \
+		73250000, 48, 32, 80, 3, 4, 29, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_848X480P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(848, 480, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		33750000, 16, 112, 112, 6, 8, 23, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1024X768I43 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1024, 768, 1, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		44900000, 8, 176, 56, 0, 4, 20, 0, 4, 21, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+/* XGA resolutions */
+#define V4L2_DV_BT_DMT_1024X768P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1024, 768, 0, 0, \
+		65000000, 24, 136, 160, 3, 6, 29, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1024X768P70 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1024, 768, 0, 0, \
+		75000000, 24, 136, 144, 3, 6, 29, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1024X768P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1024, 768, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		78750000, 16, 96, 176, 1, 3, 28, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1024X768P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1024, 768, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		94500000, 48, 96, 208, 1, 3, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1024X768P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1024, 768, 0, V4L2_DV_HSYNC_POS_POL, \
+		115500000, 48, 32, 80, 3, 4, 38, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+/* XGA+ resolution */
+#define V4L2_DV_BT_DMT_1152X864P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1152, 864, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		108000000, 64, 128, 256, 1, 3, 32, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X720P60 V4L2_DV_BT_CEA_1280X720P60
+
+/* WXGA resolutions */
+#define V4L2_DV_BT_DMT_1280X768P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 768, 0, V4L2_DV_HSYNC_POS_POL, \
+		68250000, 48, 32, 80, 3, 7, 12, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1280X768P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 768, 0, V4L2_DV_VSYNC_POS_POL, \
+		79500000, 64, 128, 192, 3, 7, 20, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X768P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 768, 0, V4L2_DV_VSYNC_POS_POL, \
+		102250000, 80, 128, 208, 3, 7, 27, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X768P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 768, 0, V4L2_DV_VSYNC_POS_POL, \
+		117500000, 80, 136, 216, 3, 7, 31, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X768P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 768, 0, V4L2_DV_HSYNC_POS_POL, \
+		140250000, 48, 32, 80, 3, 7, 35, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1280X800P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 800, 0, V4L2_DV_HSYNC_POS_POL, \
+		71000000, 48, 32, 80, 3, 6, 14, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1280X800P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 800, 0, V4L2_DV_VSYNC_POS_POL, \
+		83500000, 72, 128, 200, 3, 6, 22, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X800P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 800, 0, V4L2_DV_VSYNC_POS_POL, \
+		106500000, 80, 128, 208, 3, 6, 29, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X800P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 800, 0, V4L2_DV_VSYNC_POS_POL, \
+		122500000, 80, 136, 216, 3, 6, 34, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X800P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 800, 0, V4L2_DV_HSYNC_POS_POL, \
+		146250000, 48, 32, 80, 3, 6, 38, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1280X960P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 960, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		108000000, 96, 112, 312, 1, 3, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X960P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 960, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		148500000, 64, 160, 224, 1, 3, 47, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X960P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 960, 0, V4L2_DV_HSYNC_POS_POL, \
+		175500000, 48, 32, 80, 3, 4, 50, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+/* SXGA resolutions */
+#define V4L2_DV_BT_DMT_1280X1024P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 1024, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		108000000, 48, 112, 248, 1, 3, 38, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X1024P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 1024, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		135000000, 16, 144, 248, 1, 3, 38, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X1024P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 1024, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		157500000, 64, 160, 224, 1, 3, 44, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1280X1024P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1280, 1024, 0, V4L2_DV_HSYNC_POS_POL, \
+		187250000, 48, 32, 80, 3, 7, 50, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1360X768P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1360, 768, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		85500000, 64, 112, 256, 3, 6, 18, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1360X768P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1360, 768, 0, V4L2_DV_HSYNC_POS_POL, \
+		148250000, 48, 32, 80, 3, 5, 37, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1366X768P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1366, 768, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		85500000, 70, 143, 213, 3, 3, 24, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1366X768P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1366, 768, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		72000000, 14, 56, 64, 1, 3, 28, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+/* SXGA+ resolutions */
+#define V4L2_DV_BT_DMT_1400X1050P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1400, 1050, 0, V4L2_DV_HSYNC_POS_POL, \
+		101000000, 48, 32, 80, 3, 4, 23, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1400X1050P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1400, 1050, 0, V4L2_DV_VSYNC_POS_POL, \
+		121750000, 88, 144, 232, 3, 4, 32, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1400X1050P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1400, 1050, 0, V4L2_DV_VSYNC_POS_POL, \
+		156000000, 104, 144, 248, 3, 4, 42, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1400X1050P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1400, 1050, 0, V4L2_DV_VSYNC_POS_POL, \
+		179500000, 104, 152, 256, 3, 4, 48, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1400X1050P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1400, 1050, 0, V4L2_DV_HSYNC_POS_POL, \
+		208000000, 48, 32, 80, 3, 4, 55, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+/* WXGA+ resolutions */
+#define V4L2_DV_BT_DMT_1440X900P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1440, 900, 0, V4L2_DV_HSYNC_POS_POL, \
+		88750000, 48, 32, 80, 3, 6, 17, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1440X900P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1440, 900, 0, V4L2_DV_VSYNC_POS_POL, \
+		106500000, 80, 152, 232, 3, 6, 25, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1440X900P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1440, 900, 0, V4L2_DV_VSYNC_POS_POL, \
+		136750000, 96, 152, 248, 3, 6, 33, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1440X900P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1440, 900, 0, V4L2_DV_VSYNC_POS_POL, \
+		157000000, 104, 152, 256, 3, 6, 39, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1440X900P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1440, 900, 0, V4L2_DV_HSYNC_POS_POL, \
+		182750000, 48, 32, 80, 3, 6, 44, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1600X900P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1600, 900, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		108000000, 24, 80, 96, 1, 3, 96, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+/* UXGA resolutions */
+#define V4L2_DV_BT_DMT_1600X1200P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1600, 1200, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		162000000, 64, 192, 304, 1, 3, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1600X1200P65 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1600, 1200, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		175500000, 64, 192, 304, 1, 3, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1600X1200P70 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1600, 1200, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		189000000, 64, 192, 304, 1, 3, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1600X1200P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1600, 1200, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		202500000, 64, 192, 304, 1, 3, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1600X1200P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1600, 1200, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		229500000, 64, 192, 304, 1, 3, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1600X1200P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1600, 1200, 0, V4L2_DV_HSYNC_POS_POL, \
+		268250000, 48, 32, 80, 3, 4, 64, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+/* WSXGA+ resolutions */
+#define V4L2_DV_BT_DMT_1680X1050P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1680, 1050, 0, V4L2_DV_HSYNC_POS_POL, \
+		119000000, 48, 32, 80, 3, 6, 21, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1680X1050P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1680, 1050, 0, V4L2_DV_VSYNC_POS_POL, \
+		146250000, 104, 176, 280, 3, 6, 30, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1680X1050P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1680, 1050, 0, V4L2_DV_VSYNC_POS_POL, \
+		187000000, 120, 176, 296, 3, 6, 40, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1680X1050P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1680, 1050, 0, V4L2_DV_VSYNC_POS_POL, \
+		214750000, 128, 176, 304, 3, 6, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1680X1050P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1680, 1050, 0, V4L2_DV_HSYNC_POS_POL, \
+		245500000, 48, 32, 80, 3, 6, 53, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1792X1344P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1792, 1344, 0, V4L2_DV_VSYNC_POS_POL, \
+		204750000, 128, 200, 328, 1, 3, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1792X1344P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1792, 1344, 0, V4L2_DV_VSYNC_POS_POL, \
+		261000000, 96, 216, 352, 1, 3, 69, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1792X1344P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1792, 1344, 0, V4L2_DV_HSYNC_POS_POL, \
+		333250000, 48, 32, 80, 3, 4, 72, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1856X1392P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1856, 1392, 0, V4L2_DV_VSYNC_POS_POL, \
+		218250000, 96, 224, 352, 1, 3, 43, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1856X1392P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1856, 1392, 0, V4L2_DV_VSYNC_POS_POL, \
+		288000000, 128, 224, 352, 1, 3, 104, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1856X1392P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1856, 1392, 0, V4L2_DV_HSYNC_POS_POL, \
+		356500000, 48, 32, 80, 3, 4, 75, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1080P60 V4L2_DV_BT_CEA_1920X1080P60
+
+/* WUXGA resolutions */
+#define V4L2_DV_BT_DMT_1920X1200P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1200, 0, V4L2_DV_HSYNC_POS_POL, \
+		154000000, 48, 32, 80, 3, 6, 26, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1200P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1200, 0, V4L2_DV_VSYNC_POS_POL, \
+		193250000, 136, 200, 336, 3, 6, 36, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1200P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1200, 0, V4L2_DV_VSYNC_POS_POL, \
+		245250000, 136, 208, 344, 3, 6, 46, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1200P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1200, 0, V4L2_DV_VSYNC_POS_POL, \
+		281250000, 144, 208, 352, 3, 6, 53, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1200P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1200, 0, V4L2_DV_HSYNC_POS_POL, \
+		317000000, 48, 32, 80, 3, 6, 62, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1440P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1440, 0, V4L2_DV_VSYNC_POS_POL, \
+		234000000, 128, 208, 344, 1, 3, 56, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1440P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1440, 0, V4L2_DV_VSYNC_POS_POL, \
+		297000000, 144, 224, 352, 1, 3, 56, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_1920X1440P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1920, 1440, 0, V4L2_DV_HSYNC_POS_POL, \
+		380500000, 48, 32, 80, 3, 4, 78, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_2048X1152P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(2048, 1152, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		162000000, 26, 80, 96, 1, 3, 44, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+/* WQXGA resolutions */
+#define V4L2_DV_BT_DMT_2560X1600P60_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(2560, 1600, 0, V4L2_DV_HSYNC_POS_POL, \
+		268500000, 48, 32, 80, 3, 6, 37, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_2560X1600P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(2560, 1600, 0, V4L2_DV_VSYNC_POS_POL, \
+		348500000, 192, 280, 472, 3, 6, 49, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_2560X1600P75 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(2560, 1600, 0, V4L2_DV_VSYNC_POS_POL, \
+		443250000, 208, 280, 488, 3, 6, 63, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_2560X1600P85 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(2560, 1600, 0, V4L2_DV_VSYNC_POS_POL, \
+		505250000, 208, 280, 488, 3, 6, 73, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, 0) \
+}
+
+#define V4L2_DV_BT_DMT_2560X1600P120_RB { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(2560, 1600, 0, V4L2_DV_HSYNC_POS_POL, \
+		552750000, 48, 32, 80, 3, 6, 85, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CVT, \
+		V4L2_DV_FL_REDUCED_BLANKING) \
+}
+
+#define V4L2_DV_BT_DMT_1366X768P60 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(1366, 768, 0, \
+		V4L2_DV_HSYNC_POS_POL | V4L2_DV_VSYNC_POS_POL, \
+		85500000, 70, 143, 213, 3, 3, 24, 0, 0, 0, \
+		V4L2_DV_BT_STD_DMT, 0) \
+}
+
+#endif
diff --git a/include/linux/v4l2-subdev.h b/include/linux/v4l2-subdev.h
index ed29cbbebfef..812019ee1e06 100644
--- a/include/linux/v4l2-subdev.h
+++ b/include/linux/v4l2-subdev.h
@@ -123,6 +123,43 @@ struct v4l2_subdev_frame_interval_enum {
 	__u32 reserved[9];
 };
 
+#define V4L2_SUBDEV_SEL_FLAG_SIZE_GE			(1 << 0)
+#define V4L2_SUBDEV_SEL_FLAG_SIZE_LE			(1 << 1)
+#define V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG		(1 << 2)
+
+/* active cropping area */
+#define V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL			0x0000
+/* cropping bounds */
+#define V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS			0x0002
+/* current composing area */
+#define V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL		0x0100
+/* composing bounds */
+#define V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS		0x0102
+
+
+/**
+ * struct v4l2_subdev_selection - selection info
+ *
+ * @which: either V4L2_SUBDEV_FORMAT_ACTIVE or V4L2_SUBDEV_FORMAT_TRY
+ * @pad: pad number, as reported by the media API
+ * @target: selection target, used to choose one of possible rectangles
+ * @flags: constraint flags
+ * @r: coordinates of the selection window
+ * @reserved: for future use, set to zero for now
+ *
+ * Hardware may use multiple helper windows to process a video stream.
+ * The structure is used to exchange this selection areas between
+ * an application and a driver.
+ */
+struct v4l2_subdev_selection {
+	__u32 which;
+	__u32 pad;
+	__u32 target;
+	__u32 flags;
+	struct v4l2_rect r;
+	__u32 reserved[8];
+};
+
 #define VIDIOC_SUBDEV_G_FMT	_IOWR('V',  4, struct v4l2_subdev_format)
 #define VIDIOC_SUBDEV_S_FMT	_IOWR('V',  5, struct v4l2_subdev_format)
 #define VIDIOC_SUBDEV_G_FRAME_INTERVAL \
@@ -137,5 +174,9 @@ struct v4l2_subdev_frame_interval_enum {
 			_IOWR('V', 75, struct v4l2_subdev_frame_interval_enum)
 #define VIDIOC_SUBDEV_G_CROP	_IOWR('V', 59, struct v4l2_subdev_crop)
 #define VIDIOC_SUBDEV_S_CROP	_IOWR('V', 60, struct v4l2_subdev_crop)
+#define VIDIOC_SUBDEV_G_SELECTION \
+	_IOWR('V', 61, struct v4l2_subdev_selection)
+#define VIDIOC_SUBDEV_S_SELECTION \
+	_IOWR('V', 62, struct v4l2_subdev_selection)
 
 #endif
diff --git a/include/linux/vga_switcheroo.h b/include/linux/vga_switcheroo.h
index 4b9a7f596f92..b455c7c212eb 100644
--- a/include/linux/vga_switcheroo.h
+++ b/include/linux/vga_switcheroo.h
@@ -28,13 +28,19 @@ struct vga_switcheroo_handler {
 	int (*get_client_id)(struct pci_dev *pdev);
 };
 
+struct vga_switcheroo_client_ops {
+	void (*set_gpu_state)(struct pci_dev *dev, enum vga_switcheroo_state);
+	void (*reprobe)(struct pci_dev *dev);
+	bool (*can_switch)(struct pci_dev *dev);
+};
 
 #if defined(CONFIG_VGA_SWITCHEROO)
 void vga_switcheroo_unregister_client(struct pci_dev *dev);
 int vga_switcheroo_register_client(struct pci_dev *dev,
-				   void (*set_gpu_state)(struct pci_dev *dev, enum vga_switcheroo_state),
-				   void (*reprobe)(struct pci_dev *dev),
-				   bool (*can_switch)(struct pci_dev *dev));
+				   const struct vga_switcheroo_client_ops *ops);
+int vga_switcheroo_register_audio_client(struct pci_dev *pdev,
+					 const struct vga_switcheroo_client_ops *ops,
+					 int id, bool active);
 
 void vga_switcheroo_client_fb_set(struct pci_dev *dev,
 				  struct fb_info *info);
@@ -48,11 +54,12 @@ int vga_switcheroo_process_delayed_switch(void);
 
 static inline void vga_switcheroo_unregister_client(struct pci_dev *dev) {}
 static inline int vga_switcheroo_register_client(struct pci_dev *dev,
-					  void (*set_gpu_state)(struct pci_dev *dev, enum vga_switcheroo_state),
-					  void (*reprobe)(struct pci_dev *dev),
-					  bool (*can_switch)(struct pci_dev *dev)) { return 0; }
+		const struct vga_switcheroo_client_ops *ops) { return 0; }
 static inline void vga_switcheroo_client_fb_set(struct pci_dev *dev, struct fb_info *info) {}
 static inline int vga_switcheroo_register_handler(struct vga_switcheroo_handler *handler) { return 0; }
+static inline int vga_switcheroo_register_audio_client(struct pci_dev *pdev,
+	const struct vga_switcheroo_client_ops *ops,
+	int id, bool active) { return 0; }
 static inline void vga_switcheroo_unregister_handler(void) {}
 static inline int vga_switcheroo_process_delayed_switch(void) { return 0; }
 
diff --git a/include/linux/vgaarb.h b/include/linux/vgaarb.h
index b572f80bdfd5..0ee42d9acdc0 100644
--- a/include/linux/vgaarb.h
+++ b/include/linux/vgaarb.h
@@ -31,6 +31,7 @@
 #ifndef LINUX_VGA_H
 #define LINUX_VGA_H
 
+#include <video/vga.h>
 
 /* Legacy VGA regions */
 #define VGA_RSRC_NONE	       0x00
@@ -182,7 +183,13 @@ extern void vga_put(struct pci_dev *pdev, unsigned int rsrc);
  */
 
 #ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
+#ifdef CONFIG_VGA_ARB
 extern struct pci_dev *vga_default_device(void);
+extern void vga_set_default_device(struct pci_dev *pdev);
+#else
+static inline struct pci_dev *vga_default_device(void) { return NULL; };
+static inline void vga_set_default_device(struct pci_dev *pdev) { };
+#endif
 #endif
 
 /**
diff --git a/include/linux/videodev2.h b/include/linux/videodev2.h
index c9c9a4680cc5..370d11106c11 100644
--- a/include/linux/videodev2.h
+++ b/include/linux/videodev2.h
@@ -292,10 +292,10 @@ struct v4l2_pix_format {
 	__u32         		width;
 	__u32			height;
 	__u32			pixelformat;
-	enum v4l2_field  	field;
+	__u32			field;		/* enum v4l2_field */
 	__u32            	bytesperline;	/* for padding, zero if unused */
 	__u32          		sizeimage;
-	enum v4l2_colorspace	colorspace;
+	__u32			colorspace;	/* enum v4l2_colorspace */
 	__u32			priv;		/* private data, depends on pixelformat */
 };
 
@@ -378,7 +378,10 @@ struct v4l2_pix_format {
 #define V4L2_PIX_FMT_SGRBG12 v4l2_fourcc('B', 'A', '1', '2') /* 12  GRGR.. BGBG.. */
 #define V4L2_PIX_FMT_SRGGB12 v4l2_fourcc('R', 'G', '1', '2') /* 12  RGRG.. GBGB.. */
 	/* 10bit raw bayer DPCM compressed to 8 bits */
+#define V4L2_PIX_FMT_SBGGR10DPCM8 v4l2_fourcc('b', 'B', 'A', '8')
+#define V4L2_PIX_FMT_SGBRG10DPCM8 v4l2_fourcc('b', 'G', 'A', '8')
 #define V4L2_PIX_FMT_SGRBG10DPCM8 v4l2_fourcc('B', 'D', '1', '0')
+#define V4L2_PIX_FMT_SRGGB10DPCM8 v4l2_fourcc('b', 'R', 'A', '8')
 	/*
 	 * 10bit raw bayer, expanded to 16 bits
 	 * xxxxrrrrrrrrrrxxxxgggggggggg xxxxggggggggggxxxxbbbbbbbbbb...
@@ -432,7 +435,7 @@ struct v4l2_pix_format {
  */
 struct v4l2_fmtdesc {
 	__u32		    index;             /* Format number      */
-	enum v4l2_buf_type  type;              /* buffer type        */
+	__u32		    type;              /* enum v4l2_buf_type */
 	__u32               flags;
 	__u8		    description[32];   /* Description string */
 	__u32		    pixelformat;       /* Format fourcc      */
@@ -573,8 +576,8 @@ struct v4l2_jpegcompression {
  */
 struct v4l2_requestbuffers {
 	__u32			count;
-	enum v4l2_buf_type      type;
-	enum v4l2_memory        memory;
+	__u32			type;		/* enum v4l2_buf_type */
+	__u32			memory;		/* enum v4l2_memory */
 	__u32			reserved[2];
 };
 
@@ -610,15 +613,17 @@ struct v4l2_plane {
 /**
  * struct v4l2_buffer - video buffer info
  * @index:	id number of the buffer
- * @type:	buffer type (type == *_MPLANE for multiplanar buffers)
+ * @type:	enum v4l2_buf_type; buffer type (type == *_MPLANE for
+ *		multiplanar buffers);
  * @bytesused:	number of bytes occupied by data in the buffer (payload);
  *		unused (set to 0) for multiplanar buffers
  * @flags:	buffer informational flags
- * @field:	field order of the image in the buffer
+ * @field:	enum v4l2_field; field order of the image in the buffer
  * @timestamp:	frame timestamp
  * @timecode:	frame timecode
  * @sequence:	sequence count of this frame
- * @memory:	the method, in which the actual video data is passed
+ * @memory:	enum v4l2_memory; the method, in which the actual video data is
+ *		passed
  * @offset:	for non-multiplanar buffers with memory == V4L2_MEMORY_MMAP;
  *		offset from the start of the device memory for this plane,
  *		(or a "cookie" that should be passed to mmap() as offset)
@@ -636,16 +641,16 @@ struct v4l2_plane {
  */
 struct v4l2_buffer {
 	__u32			index;
-	enum v4l2_buf_type      type;
+	__u32			type;
 	__u32			bytesused;
 	__u32			flags;
-	enum v4l2_field		field;
+	__u32			field;
 	struct timeval		timestamp;
 	struct v4l2_timecode	timecode;
 	__u32			sequence;
 
 	/* memory location */
-	enum v4l2_memory        memory;
+	__u32			memory;
 	union {
 		__u32           offset;
 		unsigned long   userptr;
@@ -708,7 +713,7 @@ struct v4l2_clip {
 
 struct v4l2_window {
 	struct v4l2_rect        w;
-	enum v4l2_field  	field;
+	__u32			field;	 /* enum v4l2_field */
 	__u32			chromakey;
 	struct v4l2_clip	__user *clips;
 	__u32			clipcount;
@@ -745,14 +750,14 @@ struct v4l2_outputparm {
  *	I N P U T   I M A G E   C R O P P I N G
  */
 struct v4l2_cropcap {
-	enum v4l2_buf_type      type;
+	__u32			type;	/* enum v4l2_buf_type */
 	struct v4l2_rect        bounds;
 	struct v4l2_rect        defrect;
 	struct v4l2_fract       pixelaspect;
 };
 
 struct v4l2_crop {
-	enum v4l2_buf_type      type;
+	__u32			type;	/* enum v4l2_buf_type */
 	struct v4l2_rect        c;
 };
 
@@ -939,6 +944,9 @@ struct v4l2_standard {
 	__u32		     reserved[4];
 };
 
+/* The DV Preset API is deprecated in favor of the DV Timings API.
+   New drivers shouldn't use this anymore! */
+
 /*
  *	V I D E O	T I M I N G S	D V	P R E S E T
  */
@@ -986,29 +994,56 @@ struct v4l2_dv_enum_preset {
  *	D V 	B T	T I M I N G S
  */
 
-/* BT.656/BT.1120 timing data */
+/** struct v4l2_bt_timings - BT.656/BT.1120 timing data
+ * @width:	total width of the active video in pixels
+ * @height:	total height of the active video in lines
+ * @interlaced:	Interlaced or progressive
+ * @polarities:	Positive or negative polarities
+ * @pixelclock:	Pixel clock in HZ. Ex. 74.25MHz->74250000
+ * @hfrontporch:Horizontal front porch in pixels
+ * @hsync:	Horizontal Sync length in pixels
+ * @hbackporch:	Horizontal back porch in pixels
+ * @vfrontporch:Vertical front porch in lines
+ * @vsync:	Vertical Sync length in lines
+ * @vbackporch:	Vertical back porch in lines
+ * @il_vfrontporch:Vertical front porch for the even field
+ *		(aka field 2) of interlaced field formats
+ * @il_vsync:	Vertical Sync length for the even field
+ *		(aka field 2) of interlaced field formats
+ * @il_vbackporch:Vertical back porch for the even field
+ *		(aka field 2) of interlaced field formats
+ * @standards:	Standards the timing belongs to
+ * @flags:	Flags
+ * @reserved:	Reserved fields, must be zeroed.
+ *
+ * A note regarding vertical interlaced timings: height refers to the total
+ * height of the active video frame (= two fields). The blanking timings refer
+ * to the blanking of each field. So the height of the total frame is
+ * calculated as follows:
+ *
+ * tot_height = height + vfrontporch + vsync + vbackporch +
+ *                       il_vfrontporch + il_vsync + il_vbackporch
+ *
+ * The active height of each field is height / 2.
+ */
 struct v4l2_bt_timings {
-	__u32	width;		/* width in pixels */
-	__u32	height;		/* height in lines */
-	__u32	interlaced;	/* Interlaced or progressive */
-	__u32	polarities;	/* Positive or negative polarity */
-	__u64	pixelclock;	/* Pixel clock in HZ. Ex. 74.25MHz->74250000 */
-	__u32	hfrontporch;	/* Horizpontal front porch in pixels */
-	__u32	hsync;		/* Horizontal Sync length in pixels */
-	__u32	hbackporch;	/* Horizontal back porch in pixels */
-	__u32	vfrontporch;	/* Vertical front porch in pixels */
-	__u32	vsync;		/* Vertical Sync length in lines */
-	__u32	vbackporch;	/* Vertical back porch in lines */
-	__u32	il_vfrontporch;	/* Vertical front porch for bottom field of
-				 * interlaced field formats
-				 */
-	__u32	il_vsync;	/* Vertical sync length for bottom field of
-				 * interlaced field formats
-				 */
-	__u32	il_vbackporch;	/* Vertical back porch for bottom field of
-				 * interlaced field formats
-				 */
-	__u32	reserved[16];
+	__u32	width;
+	__u32	height;
+	__u32	interlaced;
+	__u32	polarities;
+	__u64	pixelclock;
+	__u32	hfrontporch;
+	__u32	hsync;
+	__u32	hbackporch;
+	__u32	vfrontporch;
+	__u32	vsync;
+	__u32	vbackporch;
+	__u32	il_vfrontporch;
+	__u32	il_vsync;
+	__u32	il_vbackporch;
+	__u32	standards;
+	__u32	flags;
+	__u32	reserved[14];
 } __attribute__ ((packed));
 
 /* Interlaced or progressive format */
@@ -1019,8 +1054,42 @@ struct v4l2_bt_timings {
 #define V4L2_DV_VSYNC_POS_POL	0x00000001
 #define V4L2_DV_HSYNC_POS_POL	0x00000002
 
-
-/* DV timings */
+/* Timings standards */
+#define V4L2_DV_BT_STD_CEA861	(1 << 0)  /* CEA-861 Digital TV Profile */
+#define V4L2_DV_BT_STD_DMT	(1 << 1)  /* VESA Discrete Monitor Timings */
+#define V4L2_DV_BT_STD_CVT	(1 << 2)  /* VESA Coordinated Video Timings */
+#define V4L2_DV_BT_STD_GTF	(1 << 3)  /* VESA Generalized Timings Formula */
+
+/* Flags */
+
+/* CVT/GTF specific: timing uses reduced blanking (CVT) or the 'Secondary
+   GTF' curve (GTF). In both cases the horizontal and/or vertical blanking
+   intervals are reduced, allowing a higher resolution over the same
+   bandwidth. This is a read-only flag. */
+#define V4L2_DV_FL_REDUCED_BLANKING		(1 << 0)
+/* CEA-861 specific: set for CEA-861 formats with a framerate of a multiple
+   of six. These formats can be optionally played at 1 / 1.001 speed.
+   This is a read-only flag. */
+#define V4L2_DV_FL_CAN_REDUCE_FPS		(1 << 1)
+/* CEA-861 specific: only valid for video transmitters, the flag is cleared
+   by receivers.
+   If the framerate of the format is a multiple of six, then the pixelclock
+   used to set up the transmitter is divided by 1.001 to make it compatible
+   with 60 Hz based standards such as NTSC and PAL-M that use a framerate of
+   29.97 Hz. Otherwise this flag is cleared. If the transmitter can't generate
+   such frequencies, then the flag will also be cleared. */
+#define V4L2_DV_FL_REDUCED_FPS			(1 << 2)
+/* Specific to interlaced formats: if set, then field 1 is really one half-line
+   longer and field 2 is really one half-line shorter, so each field has
+   exactly the same number of half-lines. Whether half-lines can be detected
+   or used depends on the hardware. */
+#define V4L2_DV_FL_HALF_LINE			(1 << 0)
+
+
+/** struct v4l2_dv_timings - DV timings
+ * @type:	the type of the timings
+ * @bt:	BT656/1120 timings
+ */
 struct v4l2_dv_timings {
 	__u32 type;
 	union {
@@ -1032,6 +1101,64 @@ struct v4l2_dv_timings {
 /* Values for the type field */
 #define V4L2_DV_BT_656_1120	0	/* BT.656/1120 timing type */
 
+
+/** struct v4l2_enum_dv_timings - DV timings enumeration
+ * @index:	enumeration index
+ * @reserved:	must be zeroed
+ * @timings:	the timings for the given index
+ */
+struct v4l2_enum_dv_timings {
+	__u32 index;
+	__u32 reserved[3];
+	struct v4l2_dv_timings timings;
+};
+
+/** struct v4l2_bt_timings_cap - BT.656/BT.1120 timing capabilities
+ * @min_width:		width in pixels
+ * @max_width:		width in pixels
+ * @min_height:		height in lines
+ * @max_height:		height in lines
+ * @min_pixelclock:	Pixel clock in HZ. Ex. 74.25MHz->74250000
+ * @max_pixelclock:	Pixel clock in HZ. Ex. 74.25MHz->74250000
+ * @standards:		Supported standards
+ * @capabilities:	Supported capabilities
+ * @reserved:		Must be zeroed
+ */
+struct v4l2_bt_timings_cap {
+	__u32	min_width;
+	__u32	max_width;
+	__u32	min_height;
+	__u32	max_height;
+	__u64	min_pixelclock;
+	__u64	max_pixelclock;
+	__u32	standards;
+	__u32	capabilities;
+	__u32	reserved[16];
+} __attribute__ ((packed));
+
+/* Supports interlaced formats */
+#define V4L2_DV_BT_CAP_INTERLACED	(1 << 0)
+/* Supports progressive formats */
+#define V4L2_DV_BT_CAP_PROGRESSIVE	(1 << 1)
+/* Supports CVT/GTF reduced blanking */
+#define V4L2_DV_BT_CAP_REDUCED_BLANKING	(1 << 2)
+/* Supports custom formats */
+#define V4L2_DV_BT_CAP_CUSTOM		(1 << 3)
+
+/** struct v4l2_dv_timings_cap - DV timings capabilities
+ * @type:	the type of the timings (same as in struct v4l2_dv_timings)
+ * @bt:		the BT656/1120 timings capabilities
+ */
+struct v4l2_dv_timings_cap {
+	__u32 type;
+	__u32 reserved[3];
+	union {
+		struct v4l2_bt_timings_cap bt;
+		__u32 raw_data[32];
+	};
+};
+
+
 /*
  *	V I D E O   I N P U T S
  */
@@ -1040,7 +1167,7 @@ struct v4l2_input {
 	__u8	     name[32];		/*  Label */
 	__u32	     type;		/*  Type of input */
 	__u32	     audioset;		/*  Associated audios (bitfield) */
-	__u32        tuner;             /*  Associated tuner */
+	__u32        tuner;             /*  enum v4l2_tuner_type */
 	v4l2_std_id  std;
 	__u32	     status;
 	__u32	     capabilities;
@@ -1137,6 +1264,8 @@ struct v4l2_ext_controls {
 #define V4L2_CTRL_CLASS_FM_TX 0x009b0000	/* FM Modulator control class */
 #define V4L2_CTRL_CLASS_FLASH 0x009c0000	/* Camera flash controls */
 #define V4L2_CTRL_CLASS_JPEG 0x009d0000		/* JPEG-compression controls */
+#define V4L2_CTRL_CLASS_IMAGE_SOURCE 0x009e0000	/* Image source controls */
+#define V4L2_CTRL_CLASS_IMAGE_PROC 0x009f0000	/* Image processing controls */
 
 #define V4L2_CTRL_ID_MASK      	  (0x0fffffff)
 #define V4L2_CTRL_ID2CLASS(id)    ((id) & 0x0fff0000UL)
@@ -1151,12 +1280,13 @@ enum v4l2_ctrl_type {
 	V4L2_CTRL_TYPE_CTRL_CLASS    = 6,
 	V4L2_CTRL_TYPE_STRING        = 7,
 	V4L2_CTRL_TYPE_BITMASK       = 8,
+	V4L2_CTRL_TYPE_INTEGER_MENU = 9,
 };
 
 /*  Used in the VIDIOC_QUERYCTRL ioctl for querying controls */
 struct v4l2_queryctrl {
 	__u32		     id;
-	enum v4l2_ctrl_type  type;
+	__u32		     type;	/* enum v4l2_ctrl_type */
 	__u8		     name[32];	/* Whatever */
 	__s32		     minimum;	/* Note signedness */
 	__s32		     maximum;
@@ -1170,9 +1300,12 @@ struct v4l2_queryctrl {
 struct v4l2_querymenu {
 	__u32		id;
 	__u32		index;
-	__u8		name[32];	/* Whatever */
+	union {
+		__u8	name[32];	/* Whatever */
+		__s64	value;
+	};
 	__u32		reserved;
-};
+} __attribute__ ((packed));
 
 /*  Control flags  */
 #define V4L2_CTRL_FLAG_DISABLED		0x0001
@@ -1237,16 +1370,22 @@ enum v4l2_power_line_frequency {
 #define V4L2_CID_COLOR_KILLER                   (V4L2_CID_BASE+30)
 #define V4L2_CID_COLORFX			(V4L2_CID_BASE+31)
 enum v4l2_colorfx {
-	V4L2_COLORFX_NONE	= 0,
-	V4L2_COLORFX_BW		= 1,
-	V4L2_COLORFX_SEPIA	= 2,
-	V4L2_COLORFX_NEGATIVE = 3,
-	V4L2_COLORFX_EMBOSS = 4,
-	V4L2_COLORFX_SKETCH = 5,
-	V4L2_COLORFX_SKY_BLUE = 6,
-	V4L2_COLORFX_GRASS_GREEN = 7,
-	V4L2_COLORFX_SKIN_WHITEN = 8,
-	V4L2_COLORFX_VIVID = 9,
+	V4L2_COLORFX_NONE			= 0,
+	V4L2_COLORFX_BW				= 1,
+	V4L2_COLORFX_SEPIA			= 2,
+	V4L2_COLORFX_NEGATIVE			= 3,
+	V4L2_COLORFX_EMBOSS			= 4,
+	V4L2_COLORFX_SKETCH			= 5,
+	V4L2_COLORFX_SKY_BLUE			= 6,
+	V4L2_COLORFX_GRASS_GREEN		= 7,
+	V4L2_COLORFX_SKIN_WHITEN		= 8,
+	V4L2_COLORFX_VIVID			= 9,
+	V4L2_COLORFX_AQUA			= 10,
+	V4L2_COLORFX_ART_FREEZE			= 11,
+	V4L2_COLORFX_SILHOUETTE			= 12,
+	V4L2_COLORFX_SOLARIZATION		= 13,
+	V4L2_COLORFX_ANTIQUE			= 14,
+	V4L2_COLORFX_SET_CBCR			= 15,
 };
 #define V4L2_CID_AUTOBRIGHTNESS			(V4L2_CID_BASE+32)
 #define V4L2_CID_BAND_STOP_FILTER		(V4L2_CID_BASE+33)
@@ -1263,9 +1402,10 @@ enum v4l2_colorfx {
 #define V4L2_CID_MIN_BUFFERS_FOR_OUTPUT		(V4L2_CID_BASE+40)
 
 #define V4L2_CID_ALPHA_COMPONENT		(V4L2_CID_BASE+41)
+#define V4L2_CID_COLORFX_CBCR			(V4L2_CID_BASE+42)
 
 /* last CID + 1 */
-#define V4L2_CID_LASTP1                         (V4L2_CID_BASE+42)
+#define V4L2_CID_LASTP1                         (V4L2_CID_BASE+43)
 
 /*  MPEG-class control IDs defined by V4L2 */
 #define V4L2_CID_MPEG_BASE 			(V4L2_CTRL_CLASS_MPEG | 0x900)
@@ -1689,6 +1829,78 @@ enum  v4l2_exposure_auto_type {
 #define V4L2_CID_IRIS_ABSOLUTE			(V4L2_CID_CAMERA_CLASS_BASE+17)
 #define V4L2_CID_IRIS_RELATIVE			(V4L2_CID_CAMERA_CLASS_BASE+18)
 
+#define V4L2_CID_AUTO_EXPOSURE_BIAS		(V4L2_CID_CAMERA_CLASS_BASE+19)
+
+#define V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE	(V4L2_CID_CAMERA_CLASS_BASE+20)
+enum v4l2_auto_n_preset_white_balance {
+	V4L2_WHITE_BALANCE_MANUAL		= 0,
+	V4L2_WHITE_BALANCE_AUTO			= 1,
+	V4L2_WHITE_BALANCE_INCANDESCENT		= 2,
+	V4L2_WHITE_BALANCE_FLUORESCENT		= 3,
+	V4L2_WHITE_BALANCE_FLUORESCENT_H	= 4,
+	V4L2_WHITE_BALANCE_HORIZON		= 5,
+	V4L2_WHITE_BALANCE_DAYLIGHT		= 6,
+	V4L2_WHITE_BALANCE_FLASH		= 7,
+	V4L2_WHITE_BALANCE_CLOUDY		= 8,
+	V4L2_WHITE_BALANCE_SHADE		= 9,
+};
+
+#define V4L2_CID_WIDE_DYNAMIC_RANGE		(V4L2_CID_CAMERA_CLASS_BASE+21)
+#define V4L2_CID_IMAGE_STABILIZATION		(V4L2_CID_CAMERA_CLASS_BASE+22)
+
+#define V4L2_CID_ISO_SENSITIVITY		(V4L2_CID_CAMERA_CLASS_BASE+23)
+#define V4L2_CID_ISO_SENSITIVITY_AUTO		(V4L2_CID_CAMERA_CLASS_BASE+24)
+enum v4l2_iso_sensitivity_auto_type {
+	V4L2_ISO_SENSITIVITY_MANUAL		= 0,
+	V4L2_ISO_SENSITIVITY_AUTO		= 1,
+};
+
+#define V4L2_CID_EXPOSURE_METERING		(V4L2_CID_CAMERA_CLASS_BASE+25)
+enum v4l2_exposure_metering {
+	V4L2_EXPOSURE_METERING_AVERAGE		= 0,
+	V4L2_EXPOSURE_METERING_CENTER_WEIGHTED	= 1,
+	V4L2_EXPOSURE_METERING_SPOT		= 2,
+};
+
+#define V4L2_CID_SCENE_MODE			(V4L2_CID_CAMERA_CLASS_BASE+26)
+enum v4l2_scene_mode {
+	V4L2_SCENE_MODE_NONE			= 0,
+	V4L2_SCENE_MODE_BACKLIGHT		= 1,
+	V4L2_SCENE_MODE_BEACH_SNOW		= 2,
+	V4L2_SCENE_MODE_CANDLE_LIGHT		= 3,
+	V4L2_SCENE_MODE_DAWN_DUSK		= 4,
+	V4L2_SCENE_MODE_FALL_COLORS		= 5,
+	V4L2_SCENE_MODE_FIREWORKS		= 6,
+	V4L2_SCENE_MODE_LANDSCAPE		= 7,
+	V4L2_SCENE_MODE_NIGHT			= 8,
+	V4L2_SCENE_MODE_PARTY_INDOOR		= 9,
+	V4L2_SCENE_MODE_PORTRAIT		= 10,
+	V4L2_SCENE_MODE_SPORTS			= 11,
+	V4L2_SCENE_MODE_SUNSET			= 12,
+	V4L2_SCENE_MODE_TEXT			= 13,
+};
+
+#define V4L2_CID_3A_LOCK			(V4L2_CID_CAMERA_CLASS_BASE+27)
+#define V4L2_LOCK_EXPOSURE			(1 << 0)
+#define V4L2_LOCK_WHITE_BALANCE			(1 << 1)
+#define V4L2_LOCK_FOCUS				(1 << 2)
+
+#define V4L2_CID_AUTO_FOCUS_START		(V4L2_CID_CAMERA_CLASS_BASE+28)
+#define V4L2_CID_AUTO_FOCUS_STOP		(V4L2_CID_CAMERA_CLASS_BASE+29)
+#define V4L2_CID_AUTO_FOCUS_STATUS		(V4L2_CID_CAMERA_CLASS_BASE+30)
+#define V4L2_AUTO_FOCUS_STATUS_IDLE		(0 << 0)
+#define V4L2_AUTO_FOCUS_STATUS_BUSY		(1 << 0)
+#define V4L2_AUTO_FOCUS_STATUS_REACHED		(1 << 1)
+#define V4L2_AUTO_FOCUS_STATUS_FAILED		(1 << 2)
+
+#define V4L2_CID_AUTO_FOCUS_RANGE		(V4L2_CID_CAMERA_CLASS_BASE+31)
+enum v4l2_auto_focus_range {
+	V4L2_AUTO_FOCUS_RANGE_AUTO		= 0,
+	V4L2_AUTO_FOCUS_RANGE_NORMAL		= 1,
+	V4L2_AUTO_FOCUS_RANGE_MACRO		= 2,
+	V4L2_AUTO_FOCUS_RANGE_INFINITY		= 3,
+};
+
 /* FM Modulator class control IDs */
 #define V4L2_CID_FM_TX_CLASS_BASE		(V4L2_CTRL_CLASS_FM_TX | 0x900)
 #define V4L2_CID_FM_TX_CLASS			(V4L2_CTRL_CLASS_FM_TX | 1)
@@ -1782,13 +1994,28 @@ enum v4l2_jpeg_chroma_subsampling {
 #define	V4L2_JPEG_ACTIVE_MARKER_DQT		(1 << 17)
 #define	V4L2_JPEG_ACTIVE_MARKER_DHT		(1 << 18)
 
+/* Image source controls */
+#define V4L2_CID_IMAGE_SOURCE_CLASS_BASE	(V4L2_CTRL_CLASS_IMAGE_SOURCE | 0x900)
+#define V4L2_CID_IMAGE_SOURCE_CLASS		(V4L2_CTRL_CLASS_IMAGE_SOURCE | 1)
+
+#define V4L2_CID_VBLANK				(V4L2_CID_IMAGE_SOURCE_CLASS_BASE + 1)
+#define V4L2_CID_HBLANK				(V4L2_CID_IMAGE_SOURCE_CLASS_BASE + 2)
+#define V4L2_CID_ANALOGUE_GAIN			(V4L2_CID_IMAGE_SOURCE_CLASS_BASE + 3)
+
+/* Image processing controls */
+#define V4L2_CID_IMAGE_PROC_CLASS_BASE		(V4L2_CTRL_CLASS_IMAGE_PROC | 0x900)
+#define V4L2_CID_IMAGE_PROC_CLASS		(V4L2_CTRL_CLASS_IMAGE_PROC | 1)
+
+#define V4L2_CID_LINK_FREQ			(V4L2_CID_IMAGE_PROC_CLASS_BASE + 1)
+#define V4L2_CID_PIXEL_RATE			(V4L2_CID_IMAGE_PROC_CLASS_BASE + 2)
+
 /*
  *	T U N I N G
  */
 struct v4l2_tuner {
 	__u32                   index;
 	__u8			name[32];
-	enum v4l2_tuner_type    type;
+	__u32			type;	/* enum v4l2_tuner_type */
 	__u32			capability;
 	__u32			rangelow;
 	__u32			rangehigh;
@@ -1838,14 +2065,14 @@ struct v4l2_modulator {
 
 struct v4l2_frequency {
 	__u32		      tuner;
-	enum v4l2_tuner_type  type;
+	__u32		      type;	/* enum v4l2_tuner_type */
 	__u32		      frequency;
 	__u32		      reserved[8];
 };
 
 struct v4l2_hw_freq_seek {
 	__u32		      tuner;
-	enum v4l2_tuner_type  type;
+	__u32		      type;	/* enum v4l2_tuner_type */
 	__u32		      seek_upward;
 	__u32		      wrap_around;
 	__u32		      spacing;
@@ -2056,7 +2283,7 @@ struct v4l2_sliced_vbi_cap {
 				 (equals frame lines 313-336 for 625 line video
 				  standards, 263-286 for 525 line standards) */
 	__u16   service_lines[2][24];
-	enum v4l2_buf_type type;
+	__u32	type;		/* enum v4l2_buf_type */
 	__u32   reserved[3];    /* must be 0 */
 };
 
@@ -2137,8 +2364,8 @@ struct v4l2_plane_pix_format {
  * @width:		image width in pixels
  * @height:		image height in pixels
  * @pixelformat:	little endian four character code (fourcc)
- * @field:		field order (for interlaced video)
- * @colorspace:		supplemental to pixelformat
+ * @field:		enum v4l2_field; field order (for interlaced video)
+ * @colorspace:		enum v4l2_colorspace; supplemental to pixelformat
  * @plane_fmt:		per-plane information
  * @num_planes:		number of planes for this format
  */
@@ -2146,8 +2373,8 @@ struct v4l2_pix_format_mplane {
 	__u32				width;
 	__u32				height;
 	__u32				pixelformat;
-	enum v4l2_field			field;
-	enum v4l2_colorspace		colorspace;
+	__u32				field;
+	__u32				colorspace;
 
 	struct v4l2_plane_pix_format	plane_fmt[VIDEO_MAX_PLANES];
 	__u8				num_planes;
@@ -2156,7 +2383,7 @@ struct v4l2_pix_format_mplane {
 
 /**
  * struct v4l2_format - stream data format
- * @type:	type of the data stream
+ * @type:	enum v4l2_buf_type; type of the data stream
  * @pix:	definition of an image format
  * @pix_mp:	definition of a multiplanar image format
  * @win:	definition of an overlaid image
@@ -2165,7 +2392,7 @@ struct v4l2_pix_format_mplane {
  * @raw_data:	placeholder for future extensions and custom formats
  */
 struct v4l2_format {
-	enum v4l2_buf_type type;
+	__u32	 type;
 	union {
 		struct v4l2_pix_format		pix;     /* V4L2_BUF_TYPE_VIDEO_CAPTURE */
 		struct v4l2_pix_format_mplane	pix_mp;  /* V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE */
@@ -2179,7 +2406,7 @@ struct v4l2_format {
 /*	Stream type-dependent parameters
  */
 struct v4l2_streamparm {
-	enum v4l2_buf_type type;
+	__u32	 type;			/* enum v4l2_buf_type */
 	union {
 		struct v4l2_captureparm	capture;
 		struct v4l2_outputparm	output;
@@ -2292,14 +2519,14 @@ struct v4l2_dbg_chip_ident {
  * @index:	on return, index of the first created buffer
  * @count:	entry: number of requested buffers,
  *		return: number of created buffers
- * @memory:	buffer memory type
+ * @memory:	enum v4l2_memory; buffer memory type
  * @format:	frame format, for which buffers are requested
  * @reserved:	future extensions
  */
 struct v4l2_create_buffers {
 	__u32			index;
 	__u32			count;
-	enum v4l2_memory        memory;
+	__u32			memory;
 	struct v4l2_format	format;
 	__u32			reserved[8];
 };
@@ -2356,8 +2583,8 @@ struct v4l2_create_buffers {
 #define VIDIOC_TRY_FMT      	_IOWR('V', 64, struct v4l2_format)
 #define VIDIOC_ENUMAUDIO	_IOWR('V', 65, struct v4l2_audio)
 #define VIDIOC_ENUMAUDOUT	_IOWR('V', 66, struct v4l2_audioout)
-#define VIDIOC_G_PRIORITY        _IOR('V', 67, enum v4l2_priority)
-#define VIDIOC_S_PRIORITY        _IOW('V', 68, enum v4l2_priority)
+#define VIDIOC_G_PRIORITY	 _IOR('V', 67, __u32) /* enum v4l2_priority */
+#define VIDIOC_S_PRIORITY	 _IOW('V', 68, __u32) /* enum v4l2_priority */
 #define VIDIOC_G_SLICED_VBI_CAP _IOWR('V', 69, struct v4l2_sliced_vbi_cap)
 #define VIDIOC_LOG_STATUS         _IO('V', 70)
 #define VIDIOC_G_EXT_CTRLS	_IOWR('V', 71, struct v4l2_ext_controls)
@@ -2384,6 +2611,9 @@ struct v4l2_create_buffers {
 #endif
 
 #define VIDIOC_S_HW_FREQ_SEEK	 _IOW('V', 82, struct v4l2_hw_freq_seek)
+
+/* These four DV Preset ioctls are deprecated in favor of the DV Timings
+   ioctls. */
 #define	VIDIOC_ENUM_DV_PRESETS	_IOWR('V', 83, struct v4l2_dv_enum_preset)
 #define	VIDIOC_S_DV_PRESET	_IOWR('V', 84, struct v4l2_dv_preset)
 #define	VIDIOC_G_DV_PRESET	_IOWR('V', 85, struct v4l2_dv_preset)
@@ -2408,6 +2638,12 @@ struct v4l2_create_buffers {
 #define VIDIOC_DECODER_CMD	_IOWR('V', 96, struct v4l2_decoder_cmd)
 #define VIDIOC_TRY_DECODER_CMD	_IOWR('V', 97, struct v4l2_decoder_cmd)
 
+/* Experimental, these three ioctls may change over the next couple of kernel
+   versions. */
+#define VIDIOC_ENUM_DV_TIMINGS  _IOWR('V', 96, struct v4l2_enum_dv_timings)
+#define VIDIOC_QUERY_DV_TIMINGS  _IOR('V', 97, struct v4l2_dv_timings)
+#define VIDIOC_DV_TIMINGS_CAP   _IOWR('V', 98, struct v4l2_dv_timings_cap)
+
 /* Reminder: when adding new ioctls please add support for them to
    drivers/media/video/v4l2-compat-ioctl32.c as well! */
 
diff --git a/include/media/media-entity.h b/include/media/media-entity.h
index 29e7bba78ffe..0c16f518ee09 100644
--- a/include/media/media-entity.h
+++ b/include/media/media-entity.h
@@ -46,6 +46,7 @@ struct media_entity_operations {
 	int (*link_setup)(struct media_entity *entity,
 			  const struct media_pad *local,
 			  const struct media_pad *remote, u32 flags);
+	int (*link_validate)(struct media_link *link);
 };
 
 struct media_entity {
@@ -140,8 +141,8 @@ void media_entity_graph_walk_start(struct media_entity_graph *graph,
 		struct media_entity *entity);
 struct media_entity *
 media_entity_graph_walk_next(struct media_entity_graph *graph);
-void media_entity_pipeline_start(struct media_entity *entity,
-		struct media_pipeline *pipe);
+__must_check int media_entity_pipeline_start(struct media_entity *entity,
+					     struct media_pipeline *pipe);
 void media_entity_pipeline_stop(struct media_entity *entity);
 
 #define media_entity_call(entity, operation, args...)			\
diff --git a/include/media/mt9p031.h b/include/media/mt9p031.h
index 96448c7a318b..0c97b19af293 100644
--- a/include/media/mt9p031.h
+++ b/include/media/mt9p031.h
@@ -3,17 +3,18 @@
 
 struct v4l2_subdev;
 
-enum {
-	MT9P031_COLOR_VERSION,
-	MT9P031_MONOCHROME_VERSION,
-};
-
+/*
+ * struct mt9p031_platform_data - MT9P031 platform data
+ * @set_xclk: Clock frequency set callback
+ * @reset: Chip reset GPIO (set to -1 if not used)
+ * @ext_freq: Input clock frequency
+ * @target_freq: Pixel clock frequency
+ */
 struct mt9p031_platform_data {
 	int (*set_xclk)(struct v4l2_subdev *subdev, int hz);
-	int (*reset)(struct v4l2_subdev *subdev, int active);
-	int ext_freq; /* input frequency to the mt9p031 for PLL dividers */
-	int target_freq; /* frequency target for the PLL */
-	int version; /* MT9P031_COLOR_VERSION or MT9P031_MONOCHROME_VERSION */
+	int reset;
+	int ext_freq;
+	int target_freq;
 };
 
 #endif
diff --git a/include/media/omap3isp.h b/include/media/omap3isp.h
index 042849a34640..4d94be5226af 100644
--- a/include/media/omap3isp.h
+++ b/include/media/omap3isp.h
@@ -29,6 +29,10 @@
 struct i2c_board_info;
 struct isp_device;
 
+#define ISP_XCLK_NONE			0
+#define ISP_XCLK_A			1
+#define ISP_XCLK_B			2
+
 enum isp_interface_type {
 	ISP_INTERFACE_PARALLEL,
 	ISP_INTERFACE_CSI2A_PHY2,
@@ -87,6 +91,29 @@ enum {
 };
 
 /**
+ * struct isp_csiphy_lane: CCP2/CSI2 lane position and polarity
+ * @pos: position of the lane
+ * @pol: polarity of the lane
+ */
+struct isp_csiphy_lane {
+	u8 pos;
+	u8 pol;
+};
+
+#define ISP_CSIPHY1_NUM_DATA_LANES	1
+#define ISP_CSIPHY2_NUM_DATA_LANES	2
+
+/**
+ * struct isp_csiphy_lanes_cfg - CCP2/CSI2 lane configuration
+ * @data: Configuration of one or two data lanes
+ * @clk: Clock lane configuration
+ */
+struct isp_csiphy_lanes_cfg {
+	struct isp_csiphy_lane data[ISP_CSIPHY2_NUM_DATA_LANES];
+	struct isp_csiphy_lane clk;
+};
+
+/**
  * struct isp_ccp2_platform_data - CCP2 interface platform data
  * @strobe_clk_pol: Strobe/clock polarity
  *		0 - Non Inverted, 1 - Inverted
@@ -105,6 +132,7 @@ struct isp_ccp2_platform_data {
 	unsigned int ccp2_mode:1;
 	unsigned int phy_layer:1;
 	unsigned int vpclk_div:2;
+	struct isp_csiphy_lanes_cfg lanecfg;
 };
 
 /**
@@ -115,6 +143,7 @@ struct isp_ccp2_platform_data {
 struct isp_csi2_platform_data {
 	unsigned crc:1;
 	unsigned vpclk_div:2;
+	struct isp_csiphy_lanes_cfg lanecfg;
 };
 
 struct isp_subdev_i2c_board_info {
diff --git a/include/media/rc-map.h b/include/media/rc-map.h
index 8db6741c1256..cfd5163ff7f3 100644
--- a/include/media/rc-map.h
+++ b/include/media/rc-map.h
@@ -62,6 +62,7 @@ void rc_map_init(void);
 #define RC_MAP_ANYSEE                    "rc-anysee"
 #define RC_MAP_APAC_VIEWCOMP             "rc-apac-viewcomp"
 #define RC_MAP_ASUS_PC39                 "rc-asus-pc39"
+#define RC_MAP_ASUS_PS3_100              "rc-asus-ps3-100"
 #define RC_MAP_ATI_TV_WONDER_HD_600      "rc-ati-tv-wonder-hd-600"
 #define RC_MAP_ATI_X10                   "rc-ati-x10"
 #define RC_MAP_AVERMEDIA_A16D            "rc-avermedia-a16d"
@@ -113,6 +114,8 @@ void rc_map_init(void);
 #define RC_MAP_LME2510                   "rc-lme2510"
 #define RC_MAP_MANLI                     "rc-manli"
 #define RC_MAP_MEDION_X10                "rc-medion-x10"
+#define RC_MAP_MEDION_X10_DIGITAINER     "rc-medion-x10-digitainer"
+#define RC_MAP_MEDION_X10_OR2X           "rc-medion-x10-or2x"
 #define RC_MAP_MSI_DIGIVOX_II            "rc-msi-digivox-ii"
 #define RC_MAP_MSI_DIGIVOX_III           "rc-msi-digivox-iii"
 #define RC_MAP_MSI_TVANYWHERE_PLUS       "rc-msi-tvanywhere-plus"
diff --git a/include/media/s5p_fimc.h b/include/media/s5p_fimc.h
index 688fb3f1dc35..8587aaf73646 100644
--- a/include/media/s5p_fimc.h
+++ b/include/media/s5p_fimc.h
@@ -64,4 +64,20 @@ struct s5p_platform_fimc {
  */
 #define S5P_FIMC_TX_END_NOTIFY _IO('e', 0)
 
+enum fimc_subdev_index {
+	IDX_SENSOR,
+	IDX_CSIS,
+	IDX_FLITE,
+	IDX_FIMC,
+	IDX_MAX,
+};
+
+struct media_pipeline;
+struct v4l2_subdev;
+
+struct fimc_pipeline {
+	struct v4l2_subdev *subdevs[IDX_MAX];
+	struct media_pipeline *m_pipeline;
+};
+
 #endif /* S5P_FIMC_H_ */
diff --git a/include/media/saa7146.h b/include/media/saa7146.h
index 0f037e8edf9a..773e527deabe 100644
--- a/include/media/saa7146.h
+++ b/include/media/saa7146.h
@@ -13,12 +13,11 @@
 #include <linux/mutex.h>
 #include <linux/scatterlist.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 
 #include <linux/vmalloc.h>	/* for vmalloc() */
 #include <linux/mm.h>		/* for vmalloc_to_page() */
 
-#define SAA7146_VERSION_CODE 0x000600	/* 0.6.0 */
-
 #define saa7146_write(sxy,adr,dat)    writel((dat),(sxy->mem+(adr)))
 #define saa7146_read(sxy,adr)         readl(sxy->mem+(adr))
 
@@ -121,6 +120,7 @@ struct saa7146_dev
 	struct list_head		item;
 
 	struct v4l2_device 		v4l2_dev;
+	struct v4l2_ctrl_handler	ctrl_handler;
 
 	/* different device locks */
 	spinlock_t			slock;
diff --git a/include/media/saa7146_vv.h b/include/media/saa7146_vv.h
index 4aeff96ff7d8..944ecdf3530f 100644
--- a/include/media/saa7146_vv.h
+++ b/include/media/saa7146_vv.h
@@ -3,6 +3,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
 #include <media/saa7146.h>
 #include <media/videobuf-dma-sg.h>
 
@@ -84,21 +85,15 @@ struct saa7146_overlay {
 
 /* per open data */
 struct saa7146_fh {
+	/* Must be the first field! */
+	struct v4l2_fh		fh;
 	struct saa7146_dev	*dev;
-	/* if this is a vbi or capture open */
-	enum v4l2_buf_type	type;
-
-	/* video overlay */
-	struct saa7146_overlay	ov;
 
 	/* video capture */
 	struct videobuf_queue	video_q;
-	struct v4l2_pix_format	video_fmt;
 
 	/* vbi capture */
 	struct videobuf_queue	vbi_q;
-	struct v4l2_vbi_format	vbi_fmt;
-	struct timer_list	vbi_read_timeout;
 
 	unsigned int resources;	/* resource management for device open */
 };
@@ -109,7 +104,9 @@ struct saa7146_fh {
 struct saa7146_vv
 {
 	/* vbi capture */
-	struct saa7146_dmaqueue		vbi_q;
+	struct saa7146_dmaqueue		vbi_dmaq;
+	struct v4l2_vbi_format		vbi_fmt;
+	struct timer_list		vbi_read_timeout;
 	/* vbi workaround interrupt queue */
 	wait_queue_head_t		vbi_wq;
 	int				vbi_fieldcount;
@@ -119,13 +116,14 @@ struct saa7146_vv
 	struct saa7146_fh		*video_fh;
 
 	/* video overlay */
+	struct saa7146_overlay		ov;
 	struct v4l2_framebuffer		ov_fb;
 	struct saa7146_format		*ov_fmt;
-	struct saa7146_overlay		*ov_data;
 	struct saa7146_fh		*ov_suspend;
 
 	/* video capture */
-	struct saa7146_dmaqueue		video_q;
+	struct saa7146_dmaqueue		video_dmaq;
+	struct v4l2_pix_format		video_fmt;
 	enum v4l2_field			last_field;
 
 	/* common: fixme? shouldn't this be in saa7146_fh?
@@ -163,7 +161,8 @@ struct saa7146_ext_vv
 	int (*std_callback)(struct saa7146_dev*, struct saa7146_standard *);
 
 	/* the extension can override this */
-	struct v4l2_ioctl_ops ops;
+	struct v4l2_ioctl_ops vid_ops;
+	struct v4l2_ioctl_ops vbi_ops;
 	/* pointer to the saa7146 core ops */
 	const struct v4l2_ioctl_ops *core_ops;
 
@@ -202,10 +201,12 @@ void saa7146_set_gpio(struct saa7146_dev *saa, u8 pin, u8 data);
 
 /* from saa7146_video.c */
 extern const struct v4l2_ioctl_ops saa7146_video_ioctl_ops;
+extern const struct v4l2_ioctl_ops saa7146_vbi_ioctl_ops;
 extern struct saa7146_use_ops saa7146_video_uops;
 int saa7146_start_preview(struct saa7146_fh *fh);
 int saa7146_stop_preview(struct saa7146_fh *fh);
 long saa7146_video_do_ioctl(struct file *file, unsigned int cmd, void *arg);
+int saa7146_s_ctrl(struct v4l2_ctrl *ctrl);
 
 /* from saa7146_vbi.c */
 extern struct saa7146_use_ops saa7146_vbi_uops;
diff --git a/include/media/sh_mobile_ceu.h b/include/media/sh_mobile_ceu.h
index a90a765f18da..6fdb6adf6b2b 100644
--- a/include/media/sh_mobile_ceu.h
+++ b/include/media/sh_mobile_ceu.h
@@ -5,6 +5,7 @@
 #define SH_CEU_FLAG_USE_16BIT_BUS	(1 << 1) /* use 16bit bus width */
 #define SH_CEU_FLAG_HSYNC_LOW		(1 << 2) /* default High if possible */
 #define SH_CEU_FLAG_VSYNC_LOW		(1 << 3) /* default High if possible */
+#define SH_CEU_FLAG_LOWER_8BIT		(1 << 4) /* default upper 8bit */
 
 struct device;
 struct resource;
diff --git a/include/media/smiapp.h b/include/media/smiapp.h
new file mode 100644
index 000000000000..9ab07fd45d5c
--- /dev/null
+++ b/include/media/smiapp.h
@@ -0,0 +1,84 @@
+/*
+ * include/media/smiapp.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_H_
+#define __SMIAPP_H_
+
+#include <media/v4l2-subdev.h>
+
+#define SMIAPP_NAME		"smiapp"
+
+#define SMIAPP_DFL_I2C_ADDR	(0x20 >> 1) /* Default I2C Address */
+#define SMIAPP_ALT_I2C_ADDR	(0x6e >> 1) /* Alternate I2C Address */
+
+#define SMIAPP_CSI_SIGNALLING_MODE_CCP2_DATA_CLOCK	0
+#define SMIAPP_CSI_SIGNALLING_MODE_CCP2_DATA_STROBE	1
+#define SMIAPP_CSI_SIGNALLING_MODE_CSI2			2
+
+#define SMIAPP_NO_XSHUTDOWN	-1
+
+/*
+ * Sometimes due to board layout considerations the camera module can be
+ * mounted rotated. The typical rotation used is 180 degrees which can be
+ * corrected by giving a default H-FLIP and V-FLIP in the sensor readout.
+ * FIXME: rotation also changes the bayer pattern.
+ */
+enum smiapp_module_board_orient {
+	SMIAPP_MODULE_BOARD_ORIENT_0 = 0,
+	SMIAPP_MODULE_BOARD_ORIENT_180,
+};
+
+struct smiapp_flash_strobe_parms {
+	u8 mode;
+	u32 strobe_width_high_us;
+	u16 strobe_delay;
+	u16 stobe_start_point;
+	u8 trigger;
+};
+
+struct smiapp_platform_data {
+	/*
+	 * Change the cci address if i2c_addr_alt is set.
+	 * Both default and alternate cci addr need to be present
+	 */
+	unsigned short i2c_addr_dfl;	/* Default i2c addr */
+	unsigned short i2c_addr_alt;	/* Alternate i2c addr */
+
+	unsigned int nvm_size;			/* bytes */
+	unsigned int ext_clk;			/* sensor external clk */
+
+	unsigned int lanes;		/* Number of CSI-2 lanes */
+	u8 csi_signalling_mode;		/* SMIAPP_CSI_SIGNALLING_MODE_* */
+	const s64 *op_sys_clock;
+
+	enum smiapp_module_board_orient module_board_orient;
+
+	struct smiapp_flash_strobe_parms *strobe_setup;
+
+	int (*set_xclk)(struct v4l2_subdev *sd, int hz);
+	char *ext_clk_name;
+	int xshutdown;			/* gpio or SMIAPP_NO_XSHUTDOWN */
+};
+
+#endif /* __SMIAPP_H_  */
diff --git a/include/media/soc_camera.h b/include/media/soc_camera.h
index cad374bdcf4b..d865dcf9879f 100644
--- a/include/media/soc_camera.h
+++ b/include/media/soc_camera.h
@@ -56,11 +56,15 @@ struct soc_camera_device {
 	};
 };
 
+/* Host supports programmable stride */
+#define SOCAM_HOST_CAP_STRIDE		(1 << 0)
+
 struct soc_camera_host {
 	struct v4l2_device v4l2_dev;
 	struct list_head list;
 	struct mutex host_lock;		/* Protect during probing */
 	unsigned char nr;		/* Host number */
+	u32 capabilities;
 	void *priv;
 	const char *drv_name;
 	struct soc_camera_host_ops *ops;
@@ -98,7 +102,7 @@ struct soc_camera_host_ops {
 	int (*set_bus_param)(struct soc_camera_device *);
 	int (*get_parm)(struct soc_camera_device *, struct v4l2_streamparm *);
 	int (*set_parm)(struct soc_camera_device *, struct v4l2_streamparm *);
-	int (*enum_fsizes)(struct soc_camera_device *, struct v4l2_frmsizeenum *);
+	int (*enum_framesizes)(struct soc_camera_device *, struct v4l2_frmsizeenum *);
 	unsigned int (*poll)(struct file *, poll_table *);
 };
 
diff --git a/include/media/soc_mediabus.h b/include/media/soc_mediabus.h
index 73f1e7eb60f3..0dc6f4625b92 100644
--- a/include/media/soc_mediabus.h
+++ b/include/media/soc_mediabus.h
@@ -47,6 +47,24 @@ enum soc_mbus_order {
 };
 
 /**
+ * enum soc_mbus_layout - planes layout in memory
+ * @SOC_MBUS_LAYOUT_PACKED:		color components packed
+ * @SOC_MBUS_LAYOUT_PLANAR_2Y_U_V:	YUV components stored in 3 planes (4:2:2)
+ * @SOC_MBUS_LAYOUT_PLANAR_2Y_C:	YUV components stored in a luma and a
+ *					chroma plane (C plane is half the size
+ *					of Y plane)
+ * @SOC_MBUS_LAYOUT_PLANAR_Y_C:		YUV components stored in a luma and a
+ *					chroma plane (C plane is the same size
+ *					as Y plane)
+ */
+enum soc_mbus_layout {
+	SOC_MBUS_LAYOUT_PACKED = 0,
+	SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
+	SOC_MBUS_LAYOUT_PLANAR_2Y_C,
+	SOC_MBUS_LAYOUT_PLANAR_Y_C,
+};
+
+/**
  * struct soc_mbus_pixelfmt - Data format on the media bus
  * @name:		Name of the format
  * @fourcc:		Fourcc code, that will be obtained if the data is
@@ -60,6 +78,7 @@ struct soc_mbus_pixelfmt {
 	u32			fourcc;
 	enum soc_mbus_packing	packing;
 	enum soc_mbus_order	order;
+	enum soc_mbus_layout	layout;
 	u8			bits_per_sample;
 };
 
@@ -80,6 +99,8 @@ const struct soc_mbus_pixelfmt *soc_mbus_find_fmtdesc(
 const struct soc_mbus_pixelfmt *soc_mbus_get_fmtdesc(
 	enum v4l2_mbus_pixelcode code);
 s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf);
+s32 soc_mbus_image_size(const struct soc_mbus_pixelfmt *mf,
+			u32 bytes_per_line, u32 height);
 int soc_mbus_samples_per_pixel(const struct soc_mbus_pixelfmt *mf,
 			unsigned int *numerator, unsigned int *denominator);
 unsigned int soc_mbus_config_compatible(const struct v4l2_mbus_config *cfg,
diff --git a/include/media/v4l2-ctrls.h b/include/media/v4l2-ctrls.h
index 11e67562b3ac..776605f1cbe2 100644
--- a/include/media/v4l2-ctrls.h
+++ b/include/media/v4l2-ctrls.h
@@ -25,6 +25,7 @@
 #include <linux/videodev2.h>
 
 /* forward references */
+struct file;
 struct v4l2_ctrl_handler;
 struct v4l2_ctrl_helper;
 struct v4l2_ctrl;
@@ -129,7 +130,10 @@ struct v4l2_ctrl {
 		u32 step;
 		u32 menu_skip_mask;
 	};
-	const char * const *qmenu;
+	union {
+		const char * const *qmenu;
+		const s64 *qmenu_int;
+	};
 	unsigned long flags;
 	union {
 		s32 val;
@@ -164,7 +168,9 @@ struct v4l2_ctrl_ref {
 /** struct v4l2_ctrl_handler - The control handler keeps track of all the
   * controls: both the controls owned by the handler and those inherited
   * from other handlers.
+  * @_lock:	Default for "lock".
   * @lock:	Lock to control access to this handler and its controls.
+  *		May be replaced by the user right after init.
   * @ctrls:	The list of controls owned by this handler.
   * @ctrl_refs:	The list of control references.
   * @cached:	The last found control reference. It is common that the same
@@ -175,7 +181,8 @@ struct v4l2_ctrl_ref {
   * @error:	The error code of the first failed control addition.
   */
 struct v4l2_ctrl_handler {
-	struct mutex lock;
+	struct mutex _lock;
+	struct mutex *lock;
 	struct list_head ctrls;
 	struct list_head ctrl_refs;
 	struct v4l2_ctrl_ref *cached;
@@ -219,6 +226,7 @@ struct v4l2_ctrl_config {
 	u32 flags;
 	u32 menu_skip_mask;
 	const char * const *qmenu;
+	const s64 *qmenu_int;
 	unsigned int is_private:1;
 };
 
@@ -343,6 +351,23 @@ 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_int_menu() - Create a new standard V4L2 integer menu control.
+  * @hdl:	The control handler.
+  * @ops:	The control ops.
+  * @id:	The control ID.
+  * @max:	The control's maximum value.
+  * @def:	The control's default value.
+  * @qmenu_int:	The control's menu entries.
+  *
+  * Same as v4l2_ctrl_new_std_menu(), but @mask is set to 0 and it additionaly
+  * takes as an argument an array of integers determining the menu items.
+  *
+  * If @id refers to a non-integer-menu control, then this function will return NULL.
+  */
+struct v4l2_ctrl *v4l2_ctrl_new_int_menu(struct v4l2_ctrl_handler *hdl,
+			const struct v4l2_ctrl_ops *ops,
+			u32 id, s32 max, s32 def, const s64 *qmenu_int);
+
 /** v4l2_ctrl_add_ctrl() - Add a control from another handler to this handler.
   * @hdl:	The control handler.
   * @ctrl:	The control to add.
@@ -451,7 +476,7 @@ void v4l2_ctrl_grab(struct v4l2_ctrl *ctrl, bool grabbed);
   */
 static inline void v4l2_ctrl_lock(struct v4l2_ctrl *ctrl)
 {
-	mutex_lock(&ctrl->handler->lock);
+	mutex_lock(ctrl->handler->lock);
 }
 
 /** v4l2_ctrl_lock() - Helper function to unlock the handler
@@ -460,7 +485,7 @@ static inline void v4l2_ctrl_lock(struct v4l2_ctrl *ctrl)
   */
 static inline void v4l2_ctrl_unlock(struct v4l2_ctrl *ctrl)
 {
-	mutex_unlock(&ctrl->handler->lock);
+	mutex_unlock(ctrl->handler->lock);
 }
 
 /** v4l2_ctrl_g_ctrl() - Helper function to get the control's value from within a driver.
@@ -487,10 +512,9 @@ s32 v4l2_ctrl_g_ctrl(struct v4l2_ctrl *ctrl);
 int v4l2_ctrl_s_ctrl(struct v4l2_ctrl *ctrl, s32 val);
 
 /* Internal helper functions that deal with control events. */
-void v4l2_ctrl_add_event(struct v4l2_ctrl *ctrl,
-		struct v4l2_subscribed_event *sev);
-void v4l2_ctrl_del_event(struct v4l2_ctrl *ctrl,
-		struct v4l2_subscribed_event *sev);
+extern const struct v4l2_subscribed_event_ops v4l2_ctrl_sub_ev_ops;
+void v4l2_ctrl_replace(struct v4l2_event *old, const struct v4l2_event *new);
+void v4l2_ctrl_merge(const struct v4l2_event *old, struct v4l2_event *new);
 
 /* Can be used as a vidioc_log_status function that just dumps all controls
    associated with the filehandle. */
diff --git a/include/media/v4l2-dev.h b/include/media/v4l2-dev.h
index 96d22215cc88..a056e6ee1b68 100644
--- a/include/media/v4l2-dev.h
+++ b/include/media/v4l2-dev.h
@@ -39,6 +39,9 @@ struct v4l2_ctrl_handler;
 #define V4L2_FL_USES_V4L2_FH	(1)
 /* Use the prio field of v4l2_fh for core priority checking */
 #define V4L2_FL_USE_FH_PRIO	(2)
+/* If ioctl core locking is in use, then apply that also to all
+   file operations. Don't use this flag in new drivers! */
+#define V4L2_FL_LOCK_ALL_FOPS	(3)
 
 /* Priority helper functions */
 
@@ -126,8 +129,10 @@ struct video_device
 
 	/* ioctl callbacks */
 	const struct v4l2_ioctl_ops *ioctl_ops;
+	DECLARE_BITMAP(valid_ioctls, BASE_VIDIOC_PRIVATE);
 
 	/* serialization lock */
+	DECLARE_BITMAP(disable_locking, BASE_VIDIOC_PRIVATE);
 	struct mutex *lock;
 };
 
@@ -173,6 +178,26 @@ void video_device_release(struct video_device *vdev);
    a dubious construction at best. */
 void video_device_release_empty(struct video_device *vdev);
 
+/* returns true if cmd is a known V4L2 ioctl */
+bool v4l2_is_known_ioctl(unsigned int cmd);
+
+/* mark that this command shouldn't use core locking */
+static inline void v4l2_disable_ioctl_locking(struct video_device *vdev, unsigned int cmd)
+{
+	if (_IOC_NR(cmd) < BASE_VIDIOC_PRIVATE)
+		set_bit(_IOC_NR(cmd), vdev->disable_locking);
+}
+
+/* Mark that this command isn't implemented. This must be called before
+   video_device_register. See also the comments in determine_valid_ioctls().
+   This function allows drivers to provide just one v4l2_ioctl_ops struct, but
+   disable ioctls based on the specific card that is actually found. */
+static inline void v4l2_disable_ioctl(struct video_device *vdev, unsigned int cmd)
+{
+	if (_IOC_NR(cmd) < BASE_VIDIOC_PRIVATE)
+		set_bit(_IOC_NR(cmd), vdev->valid_ioctls);
+}
+
 /* helper functions to access driver private data. */
 static inline void *video_get_drvdata(struct video_device *vdev)
 {
diff --git a/include/media/v4l2-event.h b/include/media/v4l2-event.h
index 5f14e8895ce2..2885a810a128 100644
--- a/include/media/v4l2-event.h
+++ b/include/media/v4l2-event.h
@@ -78,6 +78,19 @@ struct v4l2_kevent {
 	struct v4l2_event	event;
 };
 
+/** struct v4l2_subscribed_event_ops - Subscribed event operations.
+  * @add:	Optional callback, called when a new listener is added
+  * @del:	Optional callback, called when a listener stops listening
+  * @replace:	Optional callback that can replace event 'old' with event 'new'.
+  * @merge:	Optional callback that can merge event 'old' into event 'new'.
+  */
+struct v4l2_subscribed_event_ops {
+	int  (*add)(struct v4l2_subscribed_event *sev, unsigned elems);
+	void (*del)(struct v4l2_subscribed_event *sev);
+	void (*replace)(struct v4l2_event *old, const struct v4l2_event *new);
+	void (*merge)(const struct v4l2_event *old, struct v4l2_event *new);
+};
+
 /** struct v4l2_subscribed_event - Internal struct representing a subscribed event.
   * @list:	List node for the v4l2_fh->subscribed list.
   * @type:	Event type.
@@ -85,8 +98,7 @@ struct v4l2_kevent {
   * @flags:	Copy of v4l2_event_subscription->flags.
   * @fh:	Filehandle that subscribed to this event.
   * @node:	List node that hooks into the object's event list (if there is one).
-  * @replace:	Optional callback that can replace event 'old' with event 'new'.
-  * @merge:	Optional callback that can merge event 'old' into event 'new'.
+  * @ops:	v4l2_subscribed_event_ops
   * @elems:	The number of elements in the events array.
   * @first:	The index of the events containing the oldest available event.
   * @in_use:	The number of queued events.
@@ -99,10 +111,7 @@ struct v4l2_subscribed_event {
 	u32			flags;
 	struct v4l2_fh		*fh;
 	struct list_head	node;
-	void			(*replace)(struct v4l2_event *old,
-					   const struct v4l2_event *new);
-	void			(*merge)(const struct v4l2_event *old,
-					 struct v4l2_event *new);
+	const struct v4l2_subscribed_event_ops *ops;
 	unsigned		elems;
 	unsigned		first;
 	unsigned		in_use;
@@ -115,7 +124,8 @@ void v4l2_event_queue(struct video_device *vdev, const struct v4l2_event *ev);
 void v4l2_event_queue_fh(struct v4l2_fh *fh, const struct v4l2_event *ev);
 int v4l2_event_pending(struct v4l2_fh *fh);
 int v4l2_event_subscribe(struct v4l2_fh *fh,
-			 struct v4l2_event_subscription *sub, unsigned elems);
+			 struct v4l2_event_subscription *sub, unsigned elems,
+			 const struct v4l2_subscribed_event_ops *ops);
 int v4l2_event_unsubscribe(struct v4l2_fh *fh,
 			   struct v4l2_event_subscription *sub);
 void v4l2_event_unsubscribe_all(struct v4l2_fh *fh);
diff --git a/include/media/v4l2-ioctl.h b/include/media/v4l2-ioctl.h
index 3cb939cd03f9..d8b76f7392f8 100644
--- a/include/media/v4l2-ioctl.h
+++ b/include/media/v4l2-ioctl.h
@@ -271,6 +271,12 @@ struct v4l2_ioctl_ops {
 				    struct v4l2_dv_timings *timings);
 	int (*vidioc_g_dv_timings) (struct file *file, void *fh,
 				    struct v4l2_dv_timings *timings);
+	int (*vidioc_query_dv_timings) (struct file *file, void *fh,
+				    struct v4l2_dv_timings *timings);
+	int (*vidioc_enum_dv_timings) (struct file *file, void *fh,
+				    struct v4l2_enum_dv_timings *timings);
+	int (*vidioc_dv_timings_cap) (struct file *file, void *fh,
+				    struct v4l2_dv_timings_cap *cap);
 
 	int (*vidioc_subscribe_event)  (struct v4l2_fh *fh,
 					struct v4l2_event_subscription *sub);
diff --git a/include/media/v4l2-subdev.h b/include/media/v4l2-subdev.h
index f0f3358d1b1b..c35a3545e273 100644
--- a/include/media/v4l2-subdev.h
+++ b/include/media/v4l2-subdev.h
@@ -307,6 +307,12 @@ struct v4l2_subdev_video_ops {
 			struct v4l2_dv_timings *timings);
 	int (*g_dv_timings)(struct v4l2_subdev *sd,
 			struct v4l2_dv_timings *timings);
+	int (*enum_dv_timings)(struct v4l2_subdev *sd,
+			struct v4l2_enum_dv_timings *timings);
+	int (*query_dv_timings)(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings *timings);
+	int (*dv_timings_cap)(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings_cap *cap);
 	int (*enum_mbus_fmt)(struct v4l2_subdev *sd, unsigned int index,
 			     enum v4l2_mbus_pixelcode *code);
 	int (*enum_mbus_fsizes)(struct v4l2_subdev *sd,
@@ -466,6 +472,15 @@ struct v4l2_subdev_pad_ops {
 		       struct v4l2_subdev_crop *crop);
 	int (*get_crop)(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 		       struct v4l2_subdev_crop *crop);
+	int (*get_selection)(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_selection *sel);
+	int (*set_selection)(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_selection *sel);
+#ifdef CONFIG_MEDIA_CONTROLLER
+	int (*link_validate)(struct v4l2_subdev *sd, struct media_link *link,
+			     struct v4l2_subdev_format *source_fmt,
+			     struct v4l2_subdev_format *sink_fmt);
+#endif /* CONFIG_MEDIA_CONTROLLER */
 };
 
 struct v4l2_subdev_ops {
@@ -541,7 +556,7 @@ struct v4l2_subdev {
 #define media_entity_to_v4l2_subdev(ent) \
 	container_of(ent, struct v4l2_subdev, entity)
 #define vdev_to_v4l2_subdev(vdev) \
-	video_get_drvdata(vdev)
+	((struct v4l2_subdev *)video_get_drvdata(vdev))
 
 /*
  * Used for storing subdev information per file handle
@@ -549,8 +564,11 @@ struct v4l2_subdev {
 struct v4l2_subdev_fh {
 	struct v4l2_fh vfh;
 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
-	struct v4l2_mbus_framefmt *try_fmt;
-	struct v4l2_rect *try_crop;
+	struct {
+		struct v4l2_mbus_framefmt try_fmt;
+		struct v4l2_rect try_crop;
+		struct v4l2_rect try_compose;
+	} *pad;
 #endif
 };
 
@@ -558,17 +576,19 @@ struct v4l2_subdev_fh {
 	container_of(fh, struct v4l2_subdev_fh, vfh)
 
 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
-static inline struct v4l2_mbus_framefmt *
-v4l2_subdev_get_try_format(struct v4l2_subdev_fh *fh, unsigned int pad)
-{
-	return &fh->try_fmt[pad];
-}
-
-static inline struct v4l2_rect *
-v4l2_subdev_get_try_crop(struct v4l2_subdev_fh *fh, unsigned int pad)
-{
-	return &fh->try_crop[pad];
-}
+#define __V4L2_SUBDEV_MK_GET_TRY(rtype, fun_name, field_name)		\
+	static inline struct rtype *					\
+	v4l2_subdev_get_try_##fun_name(struct v4l2_subdev_fh *fh,	\
+				       unsigned int pad)		\
+	{								\
+		BUG_ON(unlikely(pad >= vdev_to_v4l2_subdev(		\
+					fh->vfh.vdev)->entity.num_pads)); \
+		return &fh->pad[pad].field_name;			\
+	}
+
+__V4L2_SUBDEV_MK_GET_TRY(v4l2_mbus_framefmt, format, try_fmt)
+__V4L2_SUBDEV_MK_GET_TRY(v4l2_rect, crop, try_compose)
+__V4L2_SUBDEV_MK_GET_TRY(v4l2_rect, compose, try_compose)
 #endif
 
 extern const struct v4l2_file_operations v4l2_subdev_fops;
@@ -593,6 +613,13 @@ static inline void *v4l2_get_subdev_hostdata(const struct v4l2_subdev *sd)
 	return sd->host_priv;
 }
 
+#ifdef CONFIG_MEDIA_CONTROLLER
+int v4l2_subdev_link_validate_default(struct v4l2_subdev *sd,
+				      struct media_link *link,
+				      struct v4l2_subdev_format *source_fmt,
+				      struct v4l2_subdev_format *sink_fmt);
+int v4l2_subdev_link_validate(struct media_link *link);
+#endif /* CONFIG_MEDIA_CONTROLLER */
 void v4l2_subdev_init(struct v4l2_subdev *sd,
 		      const struct v4l2_subdev_ops *ops);
 
diff --git a/include/media/videobuf-dma-contig.h b/include/media/videobuf-dma-contig.h
index f0ed82543d9f..f473aeb86d3f 100644
--- a/include/media/videobuf-dma-contig.h
+++ b/include/media/videobuf-dma-contig.h
@@ -26,6 +26,16 @@ void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
 				    void *priv,
 				    struct mutex *ext_lock);
 
+void videobuf_queue_dma_contig_init_cached(struct videobuf_queue *q,
+					   const struct videobuf_queue_ops *ops,
+					   struct device *dev,
+					   spinlock_t *irqlock,
+					   enum v4l2_buf_type type,
+					   enum v4l2_field field,
+					   unsigned int msize,
+					   void *priv,
+					   struct mutex *ext_lock);
+
 dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
 void videobuf_dma_contig_free(struct videobuf_queue *q,
 			      struct videobuf_buffer *buf);
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index a65910bda381..961669b648fd 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -163,6 +163,11 @@ typedef struct {
 	__u8 b[6];
 } __packed bdaddr_t;
 
+/* BD Address type */
+#define BDADDR_BREDR		0x00
+#define BDADDR_LE_PUBLIC	0x01
+#define BDADDR_LE_RANDOM	0x02
+
 #define BDADDR_ANY   (&(bdaddr_t) {{0, 0, 0, 0, 0, 0}})
 #define BDADDR_LOCAL (&(bdaddr_t) {{0, 0, 0, 0xff, 0xff, 0xff}})
 
@@ -178,7 +183,6 @@ static inline void bacpy(bdaddr_t *dst, bdaddr_t *src)
 
 void baswap(bdaddr_t *dst, bdaddr_t *src);
 char *batostr(bdaddr_t *ba);
-bdaddr_t *strtoba(char *str);
 
 /* Common socket structures and functions */
 
@@ -190,8 +194,12 @@ struct bt_sock {
 	bdaddr_t    dst;
 	struct list_head accept_q;
 	struct sock *parent;
-	u32 defer_setup;
-	bool suspended;
+	unsigned long flags;
+};
+
+enum {
+	BT_SK_DEFER_SETUP,
+	BT_SK_SUSPEND,
 };
 
 struct bt_sock_list {
@@ -216,14 +224,24 @@ void bt_accept_unlink(struct sock *sk);
 struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock);
 
 /* Skb helpers */
+struct l2cap_ctrl {
+	unsigned int	sframe	: 1,
+			poll	: 1,
+			final	: 1,
+			fcs	: 1,
+			sar	: 2,
+			super	: 2;
+	__u16		reqseq;
+	__u16		txseq;
+	__u8		retries;
+};
+
 struct bt_skb_cb {
 	__u8 pkt_type;
 	__u8 incoming;
 	__u16 expect;
-	__u16 tx_seq;
-	__u8 retries;
-	__u8 sar;
 	__u8 force_active;
+	struct l2cap_ctrl control;
 };
 #define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
 
@@ -243,12 +261,10 @@ static inline struct sk_buff *bt_skb_send_alloc(struct sock *sk,
 {
 	struct sk_buff *skb;
 
-	release_sock(sk);
 	if ((skb = sock_alloc_send_skb(sk, len + BT_SKB_RESERVE, nb, err))) {
 		skb_reserve(skb, BT_SKB_RESERVE);
 		bt_cb(skb)->incoming  = 0;
 	}
-	lock_sock(sk);
 
 	if (!skb && *err)
 		return NULL;
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index d47e523c9d83..66a7b579e31c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -102,6 +102,7 @@ enum {
 	HCI_DISCOVERABLE,
 	HCI_LINK_SECURITY,
 	HCI_PENDING_CLASS,
+	HCI_PERIODIC_INQ,
 };
 
 /* HCI ioctl defines */
@@ -324,6 +325,8 @@ struct hci_cp_inquiry {
 
 #define HCI_OP_INQUIRY_CANCEL		0x0402
 
+#define HCI_OP_PERIODIC_INQ		0x0403
+
 #define HCI_OP_EXIT_PERIODIC_INQ	0x0404
 
 #define HCI_OP_CREATE_CONN		0x0405
@@ -717,6 +720,10 @@ struct hci_rp_read_local_oob_data {
 } __packed;
 
 #define HCI_OP_READ_INQ_RSP_TX_POWER	0x0c58
+struct hci_rp_read_inq_rsp_tx_power {
+	__u8     status;
+	__s8     tx_power;
+} __packed;
 
 #define HCI_OP_READ_FLOW_CONTROL_MODE	0x0c66
 struct hci_rp_read_flow_control_mode {
@@ -1431,6 +1438,5 @@ struct hci_inquiry_req {
 #define IREQ_CACHE_FLUSH 0x0001
 
 extern bool enable_hs;
-extern bool enable_le;
 
 #endif /* __HCI_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index db1c5df45224..9fc7728f94e4 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -155,9 +155,14 @@ struct hci_dev {
 	__u16		hci_rev;
 	__u8		lmp_ver;
 	__u16		manufacturer;
-	__le16		lmp_subver;
+	__u16		lmp_subver;
 	__u16		voice_setting;
 	__u8		io_capability;
+	__s8		inq_tx_power;
+	__u16		devid_source;
+	__u16		devid_vendor;
+	__u16		devid_product;
+	__u16		devid_version;
 
 	__u16		pkt_type;
 	__u16		esco_type;
@@ -250,9 +255,6 @@ struct hci_dev {
 
 	struct list_head	remote_oob_data;
 
-	struct list_head	adv_entries;
-	struct delayed_work	adv_work;
-
 	struct hci_dev_stats	stat;
 
 	struct sk_buff_head	driver_init;
@@ -263,7 +265,6 @@ struct hci_dev {
 
 	struct dentry		*debugfs;
 
-	struct device		*parent;
 	struct device		dev;
 
 	struct rfkill		*rfkill;
@@ -571,7 +572,7 @@ int hci_chan_del(struct hci_chan *chan);
 void hci_chan_list_flush(struct hci_conn *conn);
 
 struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
-						__u8 sec_level, __u8 auth_type);
+			     __u8 dst_type, __u8 sec_level, __u8 auth_type);
 int hci_conn_check_link_mode(struct hci_conn *conn);
 int hci_conn_check_secure(struct hci_conn *conn, __u8 sec_level);
 int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type);
@@ -673,8 +674,8 @@ int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key,
 		     bdaddr_t *bdaddr, u8 *val, u8 type, u8 pin_len);
 struct smp_ltk *hci_find_ltk(struct hci_dev *hdev, __le16 ediv, u8 rand[8]);
 int hci_add_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 addr_type, u8 type,
-		int new_key, u8 authenticated, u8 tk[16], u8 enc_size, u16 ediv,
-		u8 rand[8]);
+		int new_key, u8 authenticated, u8 tk[16], u8 enc_size,
+		__le16 ediv, u8 rand[8]);
 struct smp_ltk *hci_find_ltk_by_addr(struct hci_dev *hdev, bdaddr_t *bdaddr,
 				     u8 addr_type);
 int hci_remove_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr);
@@ -688,14 +689,6 @@ int hci_add_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 *hash,
 								u8 *randomizer);
 int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr);
 
-#define ADV_CLEAR_TIMEOUT (3*60*HZ) /* Three minutes */
-int hci_adv_entries_clear(struct hci_dev *hdev);
-struct adv_entry *hci_find_adv_entry(struct hci_dev *hdev, bdaddr_t *bdaddr);
-int hci_add_adv_entry(struct hci_dev *hdev,
-					struct hci_ev_le_advertising_info *ev);
-
-void hci_del_off_timer(struct hci_dev *hdev);
-
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb);
 
 int hci_recv_frame(struct sk_buff *skb);
@@ -709,7 +702,7 @@ void hci_conn_init_sysfs(struct hci_conn *conn);
 void hci_conn_add_sysfs(struct hci_conn *conn);
 void hci_conn_del_sysfs(struct hci_conn *conn);
 
-#define SET_HCIDEV_DEV(hdev, pdev) ((hdev)->parent = (pdev))
+#define SET_HCIDEV_DEV(hdev, pdev) ((hdev)->dev.parent = (pdev))
 
 /* ----- LMP capabilities ----- */
 #define lmp_rswitch_capable(dev)   ((dev)->features[0] & LMP_RSWITCH)
@@ -933,6 +926,23 @@ static inline bool eir_has_data_type(u8 *data, size_t data_len, u8 type)
 	return false;
 }
 
+static inline size_t eir_get_length(u8 *eir, size_t eir_len)
+{
+	size_t parsed = 0;
+
+	while (parsed < eir_len) {
+		u8 field_len = eir[0];
+
+		if (field_len == 0)
+			return parsed;
+
+		parsed += field_len + 1;
+		eir += field_len + 1;
+	}
+
+	return eir_len;
+}
+
 static inline u16 eir_append_data(u8 *eir, u16 eir_len, u8 type, u8 *data,
 				  u8 data_len)
 {
@@ -961,17 +971,12 @@ void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb);
 void hci_sock_dev_event(struct hci_dev *hdev, int event);
 
 /* Management interface */
-#define MGMT_ADDR_BREDR			0x00
-#define MGMT_ADDR_LE_PUBLIC		0x01
-#define MGMT_ADDR_LE_RANDOM		0x02
-#define MGMT_ADDR_INVALID		0xff
-
-#define DISCOV_TYPE_BREDR		(BIT(MGMT_ADDR_BREDR))
-#define DISCOV_TYPE_LE			(BIT(MGMT_ADDR_LE_PUBLIC) | \
-						BIT(MGMT_ADDR_LE_RANDOM))
-#define DISCOV_TYPE_INTERLEAVED		(BIT(MGMT_ADDR_BREDR) | \
-						BIT(MGMT_ADDR_LE_PUBLIC) | \
-						BIT(MGMT_ADDR_LE_RANDOM))
+#define DISCOV_TYPE_BREDR		(BIT(BDADDR_BREDR))
+#define DISCOV_TYPE_LE			(BIT(BDADDR_LE_PUBLIC) | \
+					 BIT(BDADDR_LE_RANDOM))
+#define DISCOV_TYPE_INTERLEAVED		(BIT(BDADDR_BREDR) | \
+					 BIT(BDADDR_LE_PUBLIC) | \
+					 BIT(BDADDR_LE_RANDOM))
 
 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t len);
 int mgmt_index_added(struct hci_dev *hdev);
@@ -1067,12 +1072,12 @@ void hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max,
 					u16 latency, u16 to_multiplier);
 void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __u8 rand[8],
 							__u8 ltk[16]);
-void hci_le_ltk_reply(struct hci_conn *conn, u8 ltk[16]);
-void hci_le_ltk_neg_reply(struct hci_conn *conn);
-
 int hci_do_inquiry(struct hci_dev *hdev, u8 length);
 int hci_cancel_inquiry(struct hci_dev *hdev);
 int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 		int timeout);
+int hci_cancel_le_scan(struct hci_dev *hdev);
+
+u8 bdaddr_to_le(u8 bdaddr_type);
 
 #endif /* __HCI_CORE_H */
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 9b242c6bf55b..1c7d1cd5e679 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -44,6 +44,7 @@
 #define L2CAP_DEFAULT_MAX_SDU_SIZE	0xFFFF
 #define L2CAP_DEFAULT_SDU_ITIME		0xFFFFFFFF
 #define L2CAP_DEFAULT_ACC_LAT		0xFFFFFFFF
+#define L2CAP_BREDR_MAX_PAYLOAD		1019    /* 3-DH5 packet */
 
 #define L2CAP_DISC_TIMEOUT		msecs_to_jiffies(100)
 #define L2CAP_DISC_REJ_TIMEOUT		msecs_to_jiffies(5000)
@@ -57,6 +58,7 @@ struct sockaddr_l2 {
 	__le16		l2_psm;
 	bdaddr_t	l2_bdaddr;
 	__le16		l2_cid;
+	__u8		l2_bdaddr_type;
 };
 
 /* L2CAP socket options */
@@ -139,6 +141,8 @@ struct l2cap_conninfo {
 
 #define L2CAP_CTRL_TXSEQ_SHIFT		1
 #define L2CAP_CTRL_SUPER_SHIFT		2
+#define L2CAP_CTRL_POLL_SHIFT		4
+#define L2CAP_CTRL_FINAL_SHIFT		7
 #define L2CAP_CTRL_REQSEQ_SHIFT		8
 #define L2CAP_CTRL_SAR_SHIFT		14
 
@@ -152,9 +156,11 @@ struct l2cap_conninfo {
 #define L2CAP_EXT_CTRL_FINAL		0x00000002
 #define L2CAP_EXT_CTRL_FRAME_TYPE	0x00000001 /* I- or S-Frame */
 
+#define L2CAP_EXT_CTRL_FINAL_SHIFT	1
 #define L2CAP_EXT_CTRL_REQSEQ_SHIFT	2
 #define L2CAP_EXT_CTRL_SAR_SHIFT	16
 #define L2CAP_EXT_CTRL_SUPER_SHIFT	16
+#define L2CAP_EXT_CTRL_POLL_SHIFT	18
 #define L2CAP_EXT_CTRL_TXSEQ_SHIFT	18
 
 /* L2CAP Supervisory Function */
@@ -186,6 +192,8 @@ struct l2cap_hdr {
 #define L2CAP_FCS_SIZE		2
 #define L2CAP_SDULEN_SIZE	2
 #define L2CAP_PSMLEN_SIZE	2
+#define L2CAP_ENH_CTRL_SIZE	2
+#define L2CAP_EXT_CTRL_SIZE	4
 
 struct l2cap_cmd_hdr {
 	__u8       code;
@@ -401,6 +409,16 @@ struct l2cap_conn_param_update_rsp {
 #define L2CAP_CONN_PARAM_REJECTED	0x0001
 
 /* ----- L2CAP channels and connections ----- */
+struct l2cap_seq_list {
+	__u16	head;
+	__u16	tail;
+	__u16	mask;
+	__u16	*list;
+};
+
+#define L2CAP_SEQ_LIST_CLEAR	0xFFFF
+#define L2CAP_SEQ_LIST_TAIL	0x8000
+
 struct srej_list {
 	__u16	tx_seq;
 	struct list_head list;
@@ -446,6 +464,9 @@ struct l2cap_chan {
 	__u16		monitor_timeout;
 	__u16		mps;
 
+	__u8		tx_state;
+	__u8		rx_state;
+
 	unsigned long	conf_state;
 	unsigned long	conn_state;
 	unsigned long	flags;
@@ -456,9 +477,11 @@ struct l2cap_chan {
 	__u16		buffer_seq;
 	__u16		buffer_seq_srej;
 	__u16		srej_save_reqseq;
+	__u16		last_acked_seq;
 	__u16		frames_sent;
 	__u16		unacked_frames;
 	__u8		retry_count;
+	__u16		srej_queue_next;
 	__u8		num_acked;
 	__u16		sdu_len;
 	struct sk_buff	*sdu;
@@ -490,6 +513,8 @@ struct l2cap_chan {
 	struct sk_buff		*tx_send_head;
 	struct sk_buff_head	tx_q;
 	struct sk_buff_head	srej_q;
+	struct l2cap_seq_list	srej_list;
+	struct l2cap_seq_list	retrans_list;
 	struct list_head	srej_l;
 
 	struct list_head	list;
@@ -508,8 +533,7 @@ struct l2cap_ops {
 	void			(*close) (void *data);
 	void			(*state_change) (void *data, int state);
 	struct sk_buff		*(*alloc_skb) (struct l2cap_chan *chan,
-					unsigned long len, int nb, int *err);
-
+					       unsigned long len, int nb);
 };
 
 struct l2cap_conn {
@@ -600,6 +624,44 @@ enum {
 	FLAG_EFS_ENABLE,
 };
 
+enum {
+	L2CAP_TX_STATE_XMIT,
+	L2CAP_TX_STATE_WAIT_F,
+};
+
+enum {
+	L2CAP_RX_STATE_RECV,
+	L2CAP_RX_STATE_SREJ_SENT,
+};
+
+enum {
+	L2CAP_TXSEQ_EXPECTED,
+	L2CAP_TXSEQ_EXPECTED_SREJ,
+	L2CAP_TXSEQ_UNEXPECTED,
+	L2CAP_TXSEQ_UNEXPECTED_SREJ,
+	L2CAP_TXSEQ_DUPLICATE,
+	L2CAP_TXSEQ_DUPLICATE_SREJ,
+	L2CAP_TXSEQ_INVALID,
+	L2CAP_TXSEQ_INVALID_IGNORE,
+};
+
+enum {
+	L2CAP_EV_DATA_REQUEST,
+	L2CAP_EV_LOCAL_BUSY_DETECTED,
+	L2CAP_EV_LOCAL_BUSY_CLEAR,
+	L2CAP_EV_RECV_REQSEQ_AND_FBIT,
+	L2CAP_EV_RECV_FBIT,
+	L2CAP_EV_RETRANS_TO,
+	L2CAP_EV_MONITOR_TO,
+	L2CAP_EV_EXPLICIT_POLL,
+	L2CAP_EV_RECV_IFRAME,
+	L2CAP_EV_RECV_RR,
+	L2CAP_EV_RECV_REJ,
+	L2CAP_EV_RECV_RNR,
+	L2CAP_EV_RECV_SREJ,
+	L2CAP_EV_RECV_FRAME,
+};
+
 static inline void l2cap_chan_hold(struct l2cap_chan *c)
 {
 	atomic_inc(&c->refcnt);
@@ -622,21 +684,26 @@ static inline void l2cap_chan_unlock(struct l2cap_chan *chan)
 }
 
 static inline void l2cap_set_timer(struct l2cap_chan *chan,
-					struct delayed_work *work, long timeout)
+				   struct delayed_work *work, long timeout)
 {
 	BT_DBG("chan %p state %s timeout %ld", chan,
-					state_to_string(chan->state), timeout);
+	       state_to_string(chan->state), timeout);
 
+	/* If delayed work cancelled do not hold(chan)
+	   since it is already done with previous set_timer */
 	if (!cancel_delayed_work(work))
 		l2cap_chan_hold(chan);
+
 	schedule_delayed_work(work, timeout);
 }
 
 static inline bool l2cap_clear_timer(struct l2cap_chan *chan,
-					struct delayed_work *work)
+				     struct delayed_work *work)
 {
 	bool ret;
 
+	/* put(chan) if delayed work cancelled otherwise it
+	   is done in delayed work function */
 	ret = cancel_delayed_work(work);
 	if (ret)
 		l2cap_chan_put(chan);
@@ -658,13 +725,10 @@ static inline bool l2cap_clear_timer(struct l2cap_chan *chan,
 
 static inline int __seq_offset(struct l2cap_chan *chan, __u16 seq1, __u16 seq2)
 {
-	int offset;
-
-	offset = (seq1 - seq2) % (chan->tx_win_max + 1);
-	if (offset < 0)
-		offset += (chan->tx_win_max + 1);
-
-	return offset;
+	if (seq1 >= seq2)
+		return seq1 - seq2;
+	else
+		return chan->tx_win_max + 1 - seq2 + seq1;
 }
 
 static inline __u16 __next_seq(struct l2cap_chan *chan, __u16 seq)
@@ -852,14 +916,15 @@ int __l2cap_wait_ack(struct sock *sk);
 int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm);
 int l2cap_add_scid(struct l2cap_chan *chan,  __u16 scid);
 
-struct l2cap_chan *l2cap_chan_create(struct sock *sk);
+struct l2cap_chan *l2cap_chan_create(void);
 void l2cap_chan_close(struct l2cap_chan *chan, int reason);
 void l2cap_chan_destroy(struct l2cap_chan *chan);
 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
-								bdaddr_t *dst);
+		       bdaddr_t *dst, u8 dst_type);
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 								u32 priority);
 void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
 int l2cap_chan_check_security(struct l2cap_chan *chan);
+void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h
index ebfd91fc20f8..23fd0546fccb 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -341,6 +341,15 @@ struct mgmt_cp_unblock_device {
 } __packed;
 #define MGMT_UNBLOCK_DEVICE_SIZE	MGMT_ADDR_INFO_SIZE
 
+#define MGMT_OP_SET_DEVICE_ID		0x0028
+struct mgmt_cp_set_device_id {
+	__le16	source;
+	__le16	vendor;
+	__le16	product;
+	__le16	version;
+} __packed;
+#define MGMT_SET_DEVICE_ID_SIZE		8
+
 #define MGMT_EV_CMD_COMPLETE		0x0001
 struct mgmt_ev_cmd_complete {
 	__le16	opcode;
diff --git a/include/net/bluetooth/smp.h b/include/net/bluetooth/smp.h
index 7b3acdd29134..ca356a734920 100644
--- a/include/net/bluetooth/smp.h
+++ b/include/net/bluetooth/smp.h
@@ -77,7 +77,7 @@ struct smp_cmd_encrypt_info {
 
 #define SMP_CMD_MASTER_IDENT	0x07
 struct smp_cmd_master_ident {
-	__u16	ediv;
+	__le16	ediv;
 	__u8	rand[8];
 } __packed;
 
diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index adb2320bccdf..0289d4ce7070 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -3365,9 +3365,9 @@ void cfg80211_report_obss_beacon(struct wiphy *wiphy,
  * @chan: main channel
  * @channel_type: HT mode
  */
-int cfg80211_can_beacon_sec_chan(struct wiphy *wiphy,
-				 struct ieee80211_channel *chan,
-				 enum nl80211_channel_type channel_type);
+bool cfg80211_can_beacon_sec_chan(struct wiphy *wiphy,
+				  struct ieee80211_channel *chan,
+				  enum nl80211_channel_type channel_type);
 
 /*
  * cfg80211_ch_switch_notify - update wdev channel and notify userspace
diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index 4d6e6c6818d0..1937c7d98304 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -667,6 +667,9 @@ ieee80211_tx_info_clear_status(struct ieee80211_tx_info *info)
  * @RX_FLAG_SHORT_GI: Short guard interval was used
  * @RX_FLAG_NO_SIGNAL_VAL: The signal strength value is not present.
  *	Valid only for data frames (mainly A-MPDU)
+ * @RX_FLAG_HT_GF: This frame was received in a HT-greenfield transmission, if
+ *	the driver fills this value it should add %IEEE80211_RADIOTAP_MCS_HAVE_FMT
+ *	to hw.radiotap_mcs_details to advertise that fact
  */
 enum mac80211_rx_flags {
 	RX_FLAG_MMIC_ERROR	= 1<<0,
@@ -681,6 +684,7 @@ enum mac80211_rx_flags {
 	RX_FLAG_40MHZ		= 1<<10,
 	RX_FLAG_SHORT_GI	= 1<<11,
 	RX_FLAG_NO_SIGNAL_VAL	= 1<<12,
+	RX_FLAG_HT_GF		= 1<<13,
 };
 
 /**
@@ -939,7 +943,7 @@ static inline bool ieee80211_vif_is_mesh(struct ieee80211_vif *vif)
  *	CCMP key if it requires CCMP encryption of management frames (MFP) to
  *	be done in software.
  * @IEEE80211_KEY_FLAG_PUT_IV_SPACE: This flag should be set by the driver
- *	for a CCMP key if space should be prepared for the IV, but the IV
+ *	if space should be prepared for the IV, but the IV
  *	itself should not be generated. Do not set together with
  *	@IEEE80211_KEY_FLAG_GENERATE_IV on the same key.
  */
@@ -1288,6 +1292,11 @@ enum ieee80211_hw_flags {
  *
  * @offchannel_tx_hw_queue: HW queue ID to use for offchannel TX
  *	(if %IEEE80211_HW_QUEUE_CONTROL is set)
+ *
+ * @radiotap_mcs_details: lists which MCS information can the HW
+ *	reports, by default it is set to _MCS, _GI and _BW but doesn't
+ *	include _FMT. Use %IEEE80211_RADIOTAP_MCS_HAVE_* values, only
+ *	adding _BW is supported today.
  */
 struct ieee80211_hw {
 	struct ieee80211_conf conf;
@@ -1309,6 +1318,7 @@ struct ieee80211_hw {
 	u8 max_rx_aggregation_subframes;
 	u8 max_tx_aggregation_subframes;
 	u8 offchannel_tx_hw_queue;
+	u8 radiotap_mcs_details;
 };
 
 /**
diff --git a/include/net/nfc/hci.h b/include/net/nfc/hci.h
index aca65a5a9d0d..4467c9460857 100644
--- a/include/net/nfc/hci.h
+++ b/include/net/nfc/hci.h
@@ -39,6 +39,8 @@ struct nfc_hci_ops {
 	int (*data_exchange) (struct nfc_hci_dev *hdev,
 			      struct nfc_target *target,
 			      struct sk_buff *skb, struct sk_buff **res_skb);
+	int (*check_presence)(struct nfc_hci_dev *hdev,
+			      struct nfc_target *target);
 };
 
 #define NFC_HCI_MAX_CUSTOM_GATES	15
@@ -82,10 +84,6 @@ struct nfc_hci_dev {
 
 	u8 gate2pipe[NFC_HCI_MAX_GATES];
 
-	bool poll_started;
-	struct nfc_target *targets;
-	int target_count;
-
 	u8 sw_romlib;
 	u8 sw_patch;
 	u8 sw_flashlib_major;
diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h
index 9a2505a5b8de..b7ca4a2a1d72 100644
--- a/include/net/nfc/nfc.h
+++ b/include/net/nfc/nfc.h
@@ -48,26 +48,28 @@ struct nfc_dev;
 typedef void (*data_exchange_cb_t)(void *context, struct sk_buff *skb,
 								int err);
 
+struct nfc_target;
+
 struct nfc_ops {
 	int (*dev_up)(struct nfc_dev *dev);
 	int (*dev_down)(struct nfc_dev *dev);
 	int (*start_poll)(struct nfc_dev *dev, u32 protocols);
 	void (*stop_poll)(struct nfc_dev *dev);
-	int (*dep_link_up)(struct nfc_dev *dev, int target_idx, u8 comm_mode,
-			   u8 *gb, size_t gb_len);
+	int (*dep_link_up)(struct nfc_dev *dev, struct nfc_target *target,
+			   u8 comm_mode, u8 *gb, size_t gb_len);
 	int (*dep_link_down)(struct nfc_dev *dev);
-	int (*activate_target)(struct nfc_dev *dev, u32 target_idx,
+	int (*activate_target)(struct nfc_dev *dev, struct nfc_target *target,
 			       u32 protocol);
-	void (*deactivate_target)(struct nfc_dev *dev, u32 target_idx);
-	int (*data_exchange)(struct nfc_dev *dev, u32 target_idx,
+	void (*deactivate_target)(struct nfc_dev *dev,
+				  struct nfc_target *target);
+	int (*data_exchange)(struct nfc_dev *dev, struct nfc_target *target,
 			     struct sk_buff *skb, data_exchange_cb_t cb,
 			     void *cb_context);
-	int (*check_presence)(struct nfc_dev *dev, u32 target_idx);
+	int (*check_presence)(struct nfc_dev *dev, struct nfc_target *target);
 };
 
 #define NFC_TARGET_IDX_ANY -1
 #define NFC_MAX_GT_LEN 48
-#define NFC_TARGET_IDX_NONE 0xffffffff
 
 struct nfc_target {
 	u32 idx;
@@ -95,11 +97,10 @@ struct nfc_dev {
 	struct nfc_target *targets;
 	int n_targets;
 	int targets_generation;
-	spinlock_t targets_lock;
 	struct device dev;
 	bool dev_up;
 	bool polling;
-	u32 activated_target_idx;
+	struct nfc_target *active_target;
 	bool dep_link_up;
 	u32 dep_rf_mode;
 	struct nfc_genl_data genl_data;
diff --git a/include/net/nfc/shdlc.h b/include/net/nfc/shdlc.h
index 1071987d0408..ab06afd462da 100644
--- a/include/net/nfc/shdlc.h
+++ b/include/net/nfc/shdlc.h
@@ -35,6 +35,8 @@ struct nfc_shdlc_ops {
 	int (*data_exchange) (struct nfc_shdlc *shdlc,
 			      struct nfc_target *target,
 			      struct sk_buff *skb, struct sk_buff **res_skb);
+	int (*check_presence)(struct nfc_shdlc *shdlc,
+			      struct nfc_target *target);
 };
 
 enum shdlc_state {
diff --git a/init/Kconfig b/init/Kconfig
index ccb5248474c2..81816b82860b 100644
--- a/init/Kconfig
+++ b/init/Kconfig
@@ -390,6 +390,7 @@ config AUDIT_LOGINUID_IMMUTABLE
 	  but may not be backwards compatible with older init systems.
 
 source "kernel/irq/Kconfig"
+source "kernel/time/Kconfig"
 
 menu "RCU Subsystem"
 
diff --git a/kernel/events/Makefile b/kernel/events/Makefile
index 22d901f9caf4..103f5d147b2f 100644
--- a/kernel/events/Makefile
+++ b/kernel/events/Makefile
@@ -3,4 +3,7 @@ CFLAGS_REMOVE_core.o = -pg
 endif
 
 obj-y := core.o ring_buffer.o callchain.o
+
 obj-$(CONFIG_HAVE_HW_BREAKPOINT) += hw_breakpoint.o
+obj-$(CONFIG_UPROBES) += uprobes.o
+
diff --git a/kernel/events/uprobes.c b/kernel/events/uprobes.c
new file mode 100644
index 000000000000..985be4d80fe8
--- /dev/null
+++ b/kernel/events/uprobes.c
@@ -0,0 +1,1667 @@
+/*
+ * User-space Probes (UProbes)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Copyright (C) IBM Corporation, 2008-2012
+ * Authors:
+ *	Srikar Dronamraju
+ *	Jim Keniston
+ * Copyright (C) 2011-2012 Red Hat, Inc., Peter Zijlstra <pzijlstr@redhat.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/highmem.h>
+#include <linux/pagemap.h>	/* read_mapping_page */
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/rmap.h>		/* anon_vma_prepare */
+#include <linux/mmu_notifier.h>	/* set_pte_at_notify */
+#include <linux/swap.h>		/* try_to_free_swap */
+#include <linux/ptrace.h>	/* user_enable_single_step */
+#include <linux/kdebug.h>	/* notifier mechanism */
+
+#include <linux/uprobes.h>
+
+#define UINSNS_PER_PAGE			(PAGE_SIZE/UPROBE_XOL_SLOT_BYTES)
+#define MAX_UPROBE_XOL_SLOTS		UINSNS_PER_PAGE
+
+static struct srcu_struct uprobes_srcu;
+static struct rb_root uprobes_tree = RB_ROOT;
+
+static DEFINE_SPINLOCK(uprobes_treelock);	/* serialize rbtree access */
+
+#define UPROBES_HASH_SZ	13
+
+/* serialize (un)register */
+static struct mutex uprobes_mutex[UPROBES_HASH_SZ];
+
+#define uprobes_hash(v)		(&uprobes_mutex[((unsigned long)(v)) % UPROBES_HASH_SZ])
+
+/* serialize uprobe->pending_list */
+static struct mutex uprobes_mmap_mutex[UPROBES_HASH_SZ];
+#define uprobes_mmap_hash(v)	(&uprobes_mmap_mutex[((unsigned long)(v)) % UPROBES_HASH_SZ])
+
+/*
+ * uprobe_events allows us to skip the uprobe_mmap if there are no uprobe
+ * events active at this time.  Probably a fine grained per inode count is
+ * better?
+ */
+static atomic_t uprobe_events = ATOMIC_INIT(0);
+
+/*
+ * Maintain a temporary per vma info that can be used to search if a vma
+ * has already been handled. This structure is introduced since extending
+ * vm_area_struct wasnt recommended.
+ */
+struct vma_info {
+	struct list_head	probe_list;
+	struct mm_struct	*mm;
+	loff_t			vaddr;
+};
+
+struct uprobe {
+	struct rb_node		rb_node;	/* node in the rb tree */
+	atomic_t		ref;
+	struct rw_semaphore	consumer_rwsem;
+	struct list_head	pending_list;
+	struct uprobe_consumer	*consumers;
+	struct inode		*inode;		/* Also hold a ref to inode */
+	loff_t			offset;
+	int			flags;
+	struct arch_uprobe	arch;
+};
+
+/*
+ * valid_vma: Verify if the specified vma is an executable vma
+ * Relax restrictions while unregistering: vm_flags might have
+ * changed after breakpoint was inserted.
+ *	- is_register: indicates if we are in register context.
+ *	- Return 1 if the specified virtual address is in an
+ *	  executable vma.
+ */
+static bool valid_vma(struct vm_area_struct *vma, bool is_register)
+{
+	if (!vma->vm_file)
+		return false;
+
+	if (!is_register)
+		return true;
+
+	if ((vma->vm_flags & (VM_READ|VM_WRITE|VM_EXEC|VM_SHARED)) == (VM_READ|VM_EXEC))
+		return true;
+
+	return false;
+}
+
+static loff_t vma_address(struct vm_area_struct *vma, loff_t offset)
+{
+	loff_t vaddr;
+
+	vaddr = vma->vm_start + offset;
+	vaddr -= vma->vm_pgoff << PAGE_SHIFT;
+
+	return vaddr;
+}
+
+/**
+ * __replace_page - replace page in vma by new page.
+ * based on replace_page in mm/ksm.c
+ *
+ * @vma:      vma that holds the pte pointing to page
+ * @page:     the cowed page we are replacing by kpage
+ * @kpage:    the modified page we replace page by
+ *
+ * Returns 0 on success, -EFAULT on failure.
+ */
+static int __replace_page(struct vm_area_struct *vma, struct page *page, struct page *kpage)
+{
+	struct mm_struct *mm = vma->vm_mm;
+	pgd_t *pgd;
+	pud_t *pud;
+	pmd_t *pmd;
+	pte_t *ptep;
+	spinlock_t *ptl;
+	unsigned long addr;
+	int err = -EFAULT;
+
+	addr = page_address_in_vma(page, vma);
+	if (addr == -EFAULT)
+		goto out;
+
+	pgd = pgd_offset(mm, addr);
+	if (!pgd_present(*pgd))
+		goto out;
+
+	pud = pud_offset(pgd, addr);
+	if (!pud_present(*pud))
+		goto out;
+
+	pmd = pmd_offset(pud, addr);
+	if (!pmd_present(*pmd))
+		goto out;
+
+	ptep = pte_offset_map_lock(mm, pmd, addr, &ptl);
+	if (!ptep)
+		goto out;
+
+	get_page(kpage);
+	page_add_new_anon_rmap(kpage, vma, addr);
+
+	if (!PageAnon(page)) {
+		dec_mm_counter(mm, MM_FILEPAGES);
+		inc_mm_counter(mm, MM_ANONPAGES);
+	}
+
+	flush_cache_page(vma, addr, pte_pfn(*ptep));
+	ptep_clear_flush(vma, addr, ptep);
+	set_pte_at_notify(mm, addr, ptep, mk_pte(kpage, vma->vm_page_prot));
+
+	page_remove_rmap(page);
+	if (!page_mapped(page))
+		try_to_free_swap(page);
+	put_page(page);
+	pte_unmap_unlock(ptep, ptl);
+	err = 0;
+
+out:
+	return err;
+}
+
+/**
+ * is_swbp_insn - check if instruction is breakpoint instruction.
+ * @insn: instruction to be checked.
+ * Default implementation of is_swbp_insn
+ * Returns true if @insn is a breakpoint instruction.
+ */
+bool __weak is_swbp_insn(uprobe_opcode_t *insn)
+{
+	return *insn == UPROBE_SWBP_INSN;
+}
+
+/*
+ * NOTE:
+ * Expect the breakpoint instruction to be the smallest size instruction for
+ * the architecture. If an arch has variable length instruction and the
+ * breakpoint instruction is not of the smallest length instruction
+ * supported by that architecture then we need to modify read_opcode /
+ * write_opcode accordingly. This would never be a problem for archs that
+ * have fixed length instructions.
+ */
+
+/*
+ * write_opcode - write the opcode at a given virtual address.
+ * @auprobe: arch breakpointing information.
+ * @mm: the probed process address space.
+ * @vaddr: the virtual address to store the opcode.
+ * @opcode: opcode to be written at @vaddr.
+ *
+ * Called with mm->mmap_sem held (for read and with a reference to
+ * mm).
+ *
+ * For mm @mm, write the opcode at @vaddr.
+ * Return 0 (success) or a negative errno.
+ */
+static int write_opcode(struct arch_uprobe *auprobe, struct mm_struct *mm,
+			unsigned long vaddr, uprobe_opcode_t opcode)
+{
+	struct page *old_page, *new_page;
+	struct address_space *mapping;
+	void *vaddr_old, *vaddr_new;
+	struct vm_area_struct *vma;
+	struct uprobe *uprobe;
+	loff_t addr;
+	int ret;
+
+	/* Read the page with vaddr into memory */
+	ret = get_user_pages(NULL, mm, vaddr, 1, 0, 0, &old_page, &vma);
+	if (ret <= 0)
+		return ret;
+
+	ret = -EINVAL;
+
+	/*
+	 * We are interested in text pages only. Our pages of interest
+	 * should be mapped for read and execute only. We desist from
+	 * adding probes in write mapped pages since the breakpoints
+	 * might end up in the file copy.
+	 */
+	if (!valid_vma(vma, is_swbp_insn(&opcode)))
+		goto put_out;
+
+	uprobe = container_of(auprobe, struct uprobe, arch);
+	mapping = uprobe->inode->i_mapping;
+	if (mapping != vma->vm_file->f_mapping)
+		goto put_out;
+
+	addr = vma_address(vma, uprobe->offset);
+	if (vaddr != (unsigned long)addr)
+		goto put_out;
+
+	ret = -ENOMEM;
+	new_page = alloc_page_vma(GFP_HIGHUSER_MOVABLE, vma, vaddr);
+	if (!new_page)
+		goto put_out;
+
+	__SetPageUptodate(new_page);
+
+	/*
+	 * lock page will serialize against do_wp_page()'s
+	 * PageAnon() handling
+	 */
+	lock_page(old_page);
+	/* copy the page now that we've got it stable */
+	vaddr_old = kmap_atomic(old_page);
+	vaddr_new = kmap_atomic(new_page);
+
+	memcpy(vaddr_new, vaddr_old, PAGE_SIZE);
+
+	/* poke the new insn in, ASSUMES we don't cross page boundary */
+	vaddr &= ~PAGE_MASK;
+	BUG_ON(vaddr + UPROBE_SWBP_INSN_SIZE > PAGE_SIZE);
+	memcpy(vaddr_new + vaddr, &opcode, UPROBE_SWBP_INSN_SIZE);
+
+	kunmap_atomic(vaddr_new);
+	kunmap_atomic(vaddr_old);
+
+	ret = anon_vma_prepare(vma);
+	if (ret)
+		goto unlock_out;
+
+	lock_page(new_page);
+	ret = __replace_page(vma, old_page, new_page);
+	unlock_page(new_page);
+
+unlock_out:
+	unlock_page(old_page);
+	page_cache_release(new_page);
+
+put_out:
+	put_page(old_page);
+
+	return ret;
+}
+
+/**
+ * read_opcode - read the opcode at a given virtual address.
+ * @mm: the probed process address space.
+ * @vaddr: the virtual address to read the opcode.
+ * @opcode: location to store the read opcode.
+ *
+ * Called with mm->mmap_sem held (for read and with a reference to
+ * mm.
+ *
+ * For mm @mm, read the opcode at @vaddr and store it in @opcode.
+ * Return 0 (success) or a negative errno.
+ */
+static int read_opcode(struct mm_struct *mm, unsigned long vaddr, uprobe_opcode_t *opcode)
+{
+	struct page *page;
+	void *vaddr_new;
+	int ret;
+
+	ret = get_user_pages(NULL, mm, vaddr, 1, 0, 0, &page, NULL);
+	if (ret <= 0)
+		return ret;
+
+	lock_page(page);
+	vaddr_new = kmap_atomic(page);
+	vaddr &= ~PAGE_MASK;
+	memcpy(opcode, vaddr_new + vaddr, UPROBE_SWBP_INSN_SIZE);
+	kunmap_atomic(vaddr_new);
+	unlock_page(page);
+
+	put_page(page);
+
+	return 0;
+}
+
+static int is_swbp_at_addr(struct mm_struct *mm, unsigned long vaddr)
+{
+	uprobe_opcode_t opcode;
+	int result;
+
+	result = read_opcode(mm, vaddr, &opcode);
+	if (result)
+		return result;
+
+	if (is_swbp_insn(&opcode))
+		return 1;
+
+	return 0;
+}
+
+/**
+ * set_swbp - store breakpoint at a given address.
+ * @auprobe: arch specific probepoint information.
+ * @mm: the probed process address space.
+ * @vaddr: the virtual address to insert the opcode.
+ *
+ * For mm @mm, store the breakpoint instruction at @vaddr.
+ * Return 0 (success) or a negative errno.
+ */
+int __weak set_swbp(struct arch_uprobe *auprobe, struct mm_struct *mm, unsigned long vaddr)
+{
+	int result;
+
+	result = is_swbp_at_addr(mm, vaddr);
+	if (result == 1)
+		return -EEXIST;
+
+	if (result)
+		return result;
+
+	return write_opcode(auprobe, mm, vaddr, UPROBE_SWBP_INSN);
+}
+
+/**
+ * set_orig_insn - Restore the original instruction.
+ * @mm: the probed process address space.
+ * @auprobe: arch specific probepoint information.
+ * @vaddr: the virtual address to insert the opcode.
+ * @verify: if true, verify existance of breakpoint instruction.
+ *
+ * For mm @mm, restore the original opcode (opcode) at @vaddr.
+ * Return 0 (success) or a negative errno.
+ */
+int __weak
+set_orig_insn(struct arch_uprobe *auprobe, struct mm_struct *mm, unsigned long vaddr, bool verify)
+{
+	if (verify) {
+		int result;
+
+		result = is_swbp_at_addr(mm, vaddr);
+		if (!result)
+			return -EINVAL;
+
+		if (result != 1)
+			return result;
+	}
+	return write_opcode(auprobe, mm, vaddr, *(uprobe_opcode_t *)auprobe->insn);
+}
+
+static int match_uprobe(struct uprobe *l, struct uprobe *r)
+{
+	if (l->inode < r->inode)
+		return -1;
+
+	if (l->inode > r->inode)
+		return 1;
+
+	if (l->offset < r->offset)
+		return -1;
+
+	if (l->offset > r->offset)
+		return 1;
+
+	return 0;
+}
+
+static struct uprobe *__find_uprobe(struct inode *inode, loff_t offset)
+{
+	struct uprobe u = { .inode = inode, .offset = offset };
+	struct rb_node *n = uprobes_tree.rb_node;
+	struct uprobe *uprobe;
+	int match;
+
+	while (n) {
+		uprobe = rb_entry(n, struct uprobe, rb_node);
+		match = match_uprobe(&u, uprobe);
+		if (!match) {
+			atomic_inc(&uprobe->ref);
+			return uprobe;
+		}
+
+		if (match < 0)
+			n = n->rb_left;
+		else
+			n = n->rb_right;
+	}
+	return NULL;
+}
+
+/*
+ * Find a uprobe corresponding to a given inode:offset
+ * Acquires uprobes_treelock
+ */
+static struct uprobe *find_uprobe(struct inode *inode, loff_t offset)
+{
+	struct uprobe *uprobe;
+	unsigned long flags;
+
+	spin_lock_irqsave(&uprobes_treelock, flags);
+	uprobe = __find_uprobe(inode, offset);
+	spin_unlock_irqrestore(&uprobes_treelock, flags);
+
+	return uprobe;
+}
+
+static struct uprobe *__insert_uprobe(struct uprobe *uprobe)
+{
+	struct rb_node **p = &uprobes_tree.rb_node;
+	struct rb_node *parent = NULL;
+	struct uprobe *u;
+	int match;
+
+	while (*p) {
+		parent = *p;
+		u = rb_entry(parent, struct uprobe, rb_node);
+		match = match_uprobe(uprobe, u);
+		if (!match) {
+			atomic_inc(&u->ref);
+			return u;
+		}
+
+		if (match < 0)
+			p = &parent->rb_left;
+		else
+			p = &parent->rb_right;
+
+	}
+
+	u = NULL;
+	rb_link_node(&uprobe->rb_node, parent, p);
+	rb_insert_color(&uprobe->rb_node, &uprobes_tree);
+	/* get access + creation ref */
+	atomic_set(&uprobe->ref, 2);
+
+	return u;
+}
+
+/*
+ * Acquire uprobes_treelock.
+ * Matching uprobe already exists in rbtree;
+ *	increment (access refcount) and return the matching uprobe.
+ *
+ * No matching uprobe; insert the uprobe in rb_tree;
+ *	get a double refcount (access + creation) and return NULL.
+ */
+static struct uprobe *insert_uprobe(struct uprobe *uprobe)
+{
+	unsigned long flags;
+	struct uprobe *u;
+
+	spin_lock_irqsave(&uprobes_treelock, flags);
+	u = __insert_uprobe(uprobe);
+	spin_unlock_irqrestore(&uprobes_treelock, flags);
+
+	/* For now assume that the instruction need not be single-stepped */
+	uprobe->flags |= UPROBE_SKIP_SSTEP;
+
+	return u;
+}
+
+static void put_uprobe(struct uprobe *uprobe)
+{
+	if (atomic_dec_and_test(&uprobe->ref))
+		kfree(uprobe);
+}
+
+static struct uprobe *alloc_uprobe(struct inode *inode, loff_t offset)
+{
+	struct uprobe *uprobe, *cur_uprobe;
+
+	uprobe = kzalloc(sizeof(struct uprobe), GFP_KERNEL);
+	if (!uprobe)
+		return NULL;
+
+	uprobe->inode = igrab(inode);
+	uprobe->offset = offset;
+	init_rwsem(&uprobe->consumer_rwsem);
+	INIT_LIST_HEAD(&uprobe->pending_list);
+
+	/* add to uprobes_tree, sorted on inode:offset */
+	cur_uprobe = insert_uprobe(uprobe);
+
+	/* a uprobe exists for this inode:offset combination */
+	if (cur_uprobe) {
+		kfree(uprobe);
+		uprobe = cur_uprobe;
+		iput(inode);
+	} else {
+		atomic_inc(&uprobe_events);
+	}
+
+	return uprobe;
+}
+
+static void handler_chain(struct uprobe *uprobe, struct pt_regs *regs)
+{
+	struct uprobe_consumer *uc;
+
+	if (!(uprobe->flags & UPROBE_RUN_HANDLER))
+		return;
+
+	down_read(&uprobe->consumer_rwsem);
+	for (uc = uprobe->consumers; uc; uc = uc->next) {
+		if (!uc->filter || uc->filter(uc, current))
+			uc->handler(uc, regs);
+	}
+	up_read(&uprobe->consumer_rwsem);
+}
+
+/* Returns the previous consumer */
+static struct uprobe_consumer *
+consumer_add(struct uprobe *uprobe, struct uprobe_consumer *uc)
+{
+	down_write(&uprobe->consumer_rwsem);
+	uc->next = uprobe->consumers;
+	uprobe->consumers = uc;
+	up_write(&uprobe->consumer_rwsem);
+
+	return uc->next;
+}
+
+/*
+ * For uprobe @uprobe, delete the consumer @uc.
+ * Return true if the @uc is deleted successfully
+ * or return false.
+ */
+static bool consumer_del(struct uprobe *uprobe, struct uprobe_consumer *uc)
+{
+	struct uprobe_consumer **con;
+	bool ret = false;
+
+	down_write(&uprobe->consumer_rwsem);
+	for (con = &uprobe->consumers; *con; con = &(*con)->next) {
+		if (*con == uc) {
+			*con = uc->next;
+			ret = true;
+			break;
+		}
+	}
+	up_write(&uprobe->consumer_rwsem);
+
+	return ret;
+}
+
+static int
+__copy_insn(struct address_space *mapping, struct vm_area_struct *vma, char *insn,
+			unsigned long nbytes, unsigned long offset)
+{
+	struct file *filp = vma->vm_file;
+	struct page *page;
+	void *vaddr;
+	unsigned long off1;
+	unsigned long idx;
+
+	if (!filp)
+		return -EINVAL;
+
+	idx = (unsigned long)(offset >> PAGE_CACHE_SHIFT);
+	off1 = offset &= ~PAGE_MASK;
+
+	/*
+	 * Ensure that the page that has the original instruction is
+	 * populated and in page-cache.
+	 */
+	page = read_mapping_page(mapping, idx, filp);
+	if (IS_ERR(page))
+		return PTR_ERR(page);
+
+	vaddr = kmap_atomic(page);
+	memcpy(insn, vaddr + off1, nbytes);
+	kunmap_atomic(vaddr);
+	page_cache_release(page);
+
+	return 0;
+}
+
+static int
+copy_insn(struct uprobe *uprobe, struct vm_area_struct *vma, unsigned long addr)
+{
+	struct address_space *mapping;
+	unsigned long nbytes;
+	int bytes;
+
+	addr &= ~PAGE_MASK;
+	nbytes = PAGE_SIZE - addr;
+	mapping = uprobe->inode->i_mapping;
+
+	/* Instruction at end of binary; copy only available bytes */
+	if (uprobe->offset + MAX_UINSN_BYTES > uprobe->inode->i_size)
+		bytes = uprobe->inode->i_size - uprobe->offset;
+	else
+		bytes = MAX_UINSN_BYTES;
+
+	/* Instruction at the page-boundary; copy bytes in second page */
+	if (nbytes < bytes) {
+		if (__copy_insn(mapping, vma, uprobe->arch.insn + nbytes,
+				bytes - nbytes, uprobe->offset + nbytes))
+			return -ENOMEM;
+
+		bytes = nbytes;
+	}
+	return __copy_insn(mapping, vma, uprobe->arch.insn, bytes, uprobe->offset);
+}
+
+/*
+ * How mm->uprobes_state.count gets updated
+ * uprobe_mmap() increments the count if
+ * 	- it successfully adds a breakpoint.
+ * 	- it cannot add a breakpoint, but sees that there is a underlying
+ * 	  breakpoint (via a is_swbp_at_addr()).
+ *
+ * uprobe_munmap() decrements the count if
+ * 	- it sees a underlying breakpoint, (via is_swbp_at_addr)
+ * 	  (Subsequent uprobe_unregister wouldnt find the breakpoint
+ * 	  unless a uprobe_mmap kicks in, since the old vma would be
+ * 	  dropped just after uprobe_munmap.)
+ *
+ * uprobe_register increments the count if:
+ * 	- it successfully adds a breakpoint.
+ *
+ * uprobe_unregister decrements the count if:
+ * 	- it sees a underlying breakpoint and removes successfully.
+ * 	  (via is_swbp_at_addr)
+ * 	  (Subsequent uprobe_munmap wouldnt find the breakpoint
+ * 	  since there is no underlying breakpoint after the
+ * 	  breakpoint removal.)
+ */
+static int
+install_breakpoint(struct uprobe *uprobe, struct mm_struct *mm,
+			struct vm_area_struct *vma, loff_t vaddr)
+{
+	unsigned long addr;
+	int ret;
+
+	/*
+	 * If probe is being deleted, unregister thread could be done with
+	 * the vma-rmap-walk through. Adding a probe now can be fatal since
+	 * nobody will be able to cleanup. Also we could be from fork or
+	 * mremap path, where the probe might have already been inserted.
+	 * Hence behave as if probe already existed.
+	 */
+	if (!uprobe->consumers)
+		return -EEXIST;
+
+	addr = (unsigned long)vaddr;
+
+	if (!(uprobe->flags & UPROBE_COPY_INSN)) {
+		ret = copy_insn(uprobe, vma, addr);
+		if (ret)
+			return ret;
+
+		if (is_swbp_insn((uprobe_opcode_t *)uprobe->arch.insn))
+			return -EEXIST;
+
+		ret = arch_uprobe_analyze_insn(&uprobe->arch, mm);
+		if (ret)
+			return ret;
+
+		uprobe->flags |= UPROBE_COPY_INSN;
+	}
+
+	/*
+	 * Ideally, should be updating the probe count after the breakpoint
+	 * has been successfully inserted. However a thread could hit the
+	 * breakpoint we just inserted even before the probe count is
+	 * incremented. If this is the first breakpoint placed, breakpoint
+	 * notifier might ignore uprobes and pass the trap to the thread.
+	 * Hence increment before and decrement on failure.
+	 */
+	atomic_inc(&mm->uprobes_state.count);
+	ret = set_swbp(&uprobe->arch, mm, addr);
+	if (ret)
+		atomic_dec(&mm->uprobes_state.count);
+
+	return ret;
+}
+
+static void
+remove_breakpoint(struct uprobe *uprobe, struct mm_struct *mm, loff_t vaddr)
+{
+	if (!set_orig_insn(&uprobe->arch, mm, (unsigned long)vaddr, true))
+		atomic_dec(&mm->uprobes_state.count);
+}
+
+/*
+ * There could be threads that have hit the breakpoint and are entering the
+ * notifier code and trying to acquire the uprobes_treelock. The thread
+ * calling delete_uprobe() that is removing the uprobe from the rb_tree can
+ * race with these threads and might acquire the uprobes_treelock compared
+ * to some of the breakpoint hit threads. In such a case, the breakpoint
+ * hit threads will not find the uprobe. The current unregistering thread
+ * waits till all other threads have hit a breakpoint, to acquire the
+ * uprobes_treelock before the uprobe is removed from the rbtree.
+ */
+static void delete_uprobe(struct uprobe *uprobe)
+{
+	unsigned long flags;
+
+	synchronize_srcu(&uprobes_srcu);
+	spin_lock_irqsave(&uprobes_treelock, flags);
+	rb_erase(&uprobe->rb_node, &uprobes_tree);
+	spin_unlock_irqrestore(&uprobes_treelock, flags);
+	iput(uprobe->inode);
+	put_uprobe(uprobe);
+	atomic_dec(&uprobe_events);
+}
+
+static struct vma_info *
+__find_next_vma_info(struct address_space *mapping, struct list_head *head,
+			struct vma_info *vi, loff_t offset, bool is_register)
+{
+	struct prio_tree_iter iter;
+	struct vm_area_struct *vma;
+	struct vma_info *tmpvi;
+	unsigned long pgoff;
+	int existing_vma;
+	loff_t vaddr;
+
+	pgoff = offset >> PAGE_SHIFT;
+
+	vma_prio_tree_foreach(vma, &iter, &mapping->i_mmap, pgoff, pgoff) {
+		if (!valid_vma(vma, is_register))
+			continue;
+
+		existing_vma = 0;
+		vaddr = vma_address(vma, offset);
+
+		list_for_each_entry(tmpvi, head, probe_list) {
+			if (tmpvi->mm == vma->vm_mm && tmpvi->vaddr == vaddr) {
+				existing_vma = 1;
+				break;
+			}
+		}
+
+		/*
+		 * Another vma needs a probe to be installed. However skip
+		 * installing the probe if the vma is about to be unlinked.
+		 */
+		if (!existing_vma && atomic_inc_not_zero(&vma->vm_mm->mm_users)) {
+			vi->mm = vma->vm_mm;
+			vi->vaddr = vaddr;
+			list_add(&vi->probe_list, head);
+
+			return vi;
+		}
+	}
+
+	return NULL;
+}
+
+/*
+ * Iterate in the rmap prio tree  and find a vma where a probe has not
+ * yet been inserted.
+ */
+static struct vma_info *
+find_next_vma_info(struct address_space *mapping, struct list_head *head,
+		loff_t offset, bool is_register)
+{
+	struct vma_info *vi, *retvi;
+
+	vi = kzalloc(sizeof(struct vma_info), GFP_KERNEL);
+	if (!vi)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&mapping->i_mmap_mutex);
+	retvi = __find_next_vma_info(mapping, head, vi, offset, is_register);
+	mutex_unlock(&mapping->i_mmap_mutex);
+
+	if (!retvi)
+		kfree(vi);
+
+	return retvi;
+}
+
+static int register_for_each_vma(struct uprobe *uprobe, bool is_register)
+{
+	struct list_head try_list;
+	struct vm_area_struct *vma;
+	struct address_space *mapping;
+	struct vma_info *vi, *tmpvi;
+	struct mm_struct *mm;
+	loff_t vaddr;
+	int ret;
+
+	mapping = uprobe->inode->i_mapping;
+	INIT_LIST_HEAD(&try_list);
+
+	ret = 0;
+
+	for (;;) {
+		vi = find_next_vma_info(mapping, &try_list, uprobe->offset, is_register);
+		if (!vi)
+			break;
+
+		if (IS_ERR(vi)) {
+			ret = PTR_ERR(vi);
+			break;
+		}
+
+		mm = vi->mm;
+		down_read(&mm->mmap_sem);
+		vma = find_vma(mm, (unsigned long)vi->vaddr);
+		if (!vma || !valid_vma(vma, is_register)) {
+			list_del(&vi->probe_list);
+			kfree(vi);
+			up_read(&mm->mmap_sem);
+			mmput(mm);
+			continue;
+		}
+		vaddr = vma_address(vma, uprobe->offset);
+		if (vma->vm_file->f_mapping->host != uprobe->inode ||
+						vaddr != vi->vaddr) {
+			list_del(&vi->probe_list);
+			kfree(vi);
+			up_read(&mm->mmap_sem);
+			mmput(mm);
+			continue;
+		}
+
+		if (is_register)
+			ret = install_breakpoint(uprobe, mm, vma, vi->vaddr);
+		else
+			remove_breakpoint(uprobe, mm, vi->vaddr);
+
+		up_read(&mm->mmap_sem);
+		mmput(mm);
+		if (is_register) {
+			if (ret && ret == -EEXIST)
+				ret = 0;
+			if (ret)
+				break;
+		}
+	}
+
+	list_for_each_entry_safe(vi, tmpvi, &try_list, probe_list) {
+		list_del(&vi->probe_list);
+		kfree(vi);
+	}
+
+	return ret;
+}
+
+static int __uprobe_register(struct uprobe *uprobe)
+{
+	return register_for_each_vma(uprobe, true);
+}
+
+static void __uprobe_unregister(struct uprobe *uprobe)
+{
+	if (!register_for_each_vma(uprobe, false))
+		delete_uprobe(uprobe);
+
+	/* TODO : cant unregister? schedule a worker thread */
+}
+
+/*
+ * uprobe_register - register a probe
+ * @inode: the file in which the probe has to be placed.
+ * @offset: offset from the start of the file.
+ * @uc: information on howto handle the probe..
+ *
+ * Apart from the access refcount, uprobe_register() takes a creation
+ * refcount (thro alloc_uprobe) if and only if this @uprobe is getting
+ * inserted into the rbtree (i.e first consumer for a @inode:@offset
+ * tuple).  Creation refcount stops uprobe_unregister from freeing the
+ * @uprobe even before the register operation is complete. Creation
+ * refcount is released when the last @uc for the @uprobe
+ * unregisters.
+ *
+ * Return errno if it cannot successully install probes
+ * else return 0 (success)
+ */
+int uprobe_register(struct inode *inode, loff_t offset, struct uprobe_consumer *uc)
+{
+	struct uprobe *uprobe;
+	int ret;
+
+	if (!inode || !uc || uc->next)
+		return -EINVAL;
+
+	if (offset > i_size_read(inode))
+		return -EINVAL;
+
+	ret = 0;
+	mutex_lock(uprobes_hash(inode));
+	uprobe = alloc_uprobe(inode, offset);
+
+	if (uprobe && !consumer_add(uprobe, uc)) {
+		ret = __uprobe_register(uprobe);
+		if (ret) {
+			uprobe->consumers = NULL;
+			__uprobe_unregister(uprobe);
+		} else {
+			uprobe->flags |= UPROBE_RUN_HANDLER;
+		}
+	}
+
+	mutex_unlock(uprobes_hash(inode));
+	put_uprobe(uprobe);
+
+	return ret;
+}
+
+/*
+ * uprobe_unregister - unregister a already registered probe.
+ * @inode: the file in which the probe has to be removed.
+ * @offset: offset from the start of the file.
+ * @uc: identify which probe if multiple probes are colocated.
+ */
+void uprobe_unregister(struct inode *inode, loff_t offset, struct uprobe_consumer *uc)
+{
+	struct uprobe *uprobe;
+
+	if (!inode || !uc)
+		return;
+
+	uprobe = find_uprobe(inode, offset);
+	if (!uprobe)
+		return;
+
+	mutex_lock(uprobes_hash(inode));
+
+	if (consumer_del(uprobe, uc)) {
+		if (!uprobe->consumers) {
+			__uprobe_unregister(uprobe);
+			uprobe->flags &= ~UPROBE_RUN_HANDLER;
+		}
+	}
+
+	mutex_unlock(uprobes_hash(inode));
+	if (uprobe)
+		put_uprobe(uprobe);
+}
+
+/*
+ * Of all the nodes that correspond to the given inode, return the node
+ * with the least offset.
+ */
+static struct rb_node *find_least_offset_node(struct inode *inode)
+{
+	struct uprobe u = { .inode = inode, .offset = 0};
+	struct rb_node *n = uprobes_tree.rb_node;
+	struct rb_node *close_node = NULL;
+	struct uprobe *uprobe;
+	int match;
+
+	while (n) {
+		uprobe = rb_entry(n, struct uprobe, rb_node);
+		match = match_uprobe(&u, uprobe);
+
+		if (uprobe->inode == inode)
+			close_node = n;
+
+		if (!match)
+			return close_node;
+
+		if (match < 0)
+			n = n->rb_left;
+		else
+			n = n->rb_right;
+	}
+
+	return close_node;
+}
+
+/*
+ * For a given inode, build a list of probes that need to be inserted.
+ */
+static void build_probe_list(struct inode *inode, struct list_head *head)
+{
+	struct uprobe *uprobe;
+	unsigned long flags;
+	struct rb_node *n;
+
+	spin_lock_irqsave(&uprobes_treelock, flags);
+
+	n = find_least_offset_node(inode);
+
+	for (; n; n = rb_next(n)) {
+		uprobe = rb_entry(n, struct uprobe, rb_node);
+		if (uprobe->inode != inode)
+			break;
+
+		list_add(&uprobe->pending_list, head);
+		atomic_inc(&uprobe->ref);
+	}
+
+	spin_unlock_irqrestore(&uprobes_treelock, flags);
+}
+
+/*
+ * Called from mmap_region.
+ * called with mm->mmap_sem acquired.
+ *
+ * Return -ve no if we fail to insert probes and we cannot
+ * bail-out.
+ * Return 0 otherwise. i.e:
+ *
+ *	- successful insertion of probes
+ *	- (or) no possible probes to be inserted.
+ *	- (or) insertion of probes failed but we can bail-out.
+ */
+int uprobe_mmap(struct vm_area_struct *vma)
+{
+	struct list_head tmp_list;
+	struct uprobe *uprobe, *u;
+	struct inode *inode;
+	int ret, count;
+
+	if (!atomic_read(&uprobe_events) || !valid_vma(vma, true))
+		return 0;
+
+	inode = vma->vm_file->f_mapping->host;
+	if (!inode)
+		return 0;
+
+	INIT_LIST_HEAD(&tmp_list);
+	mutex_lock(uprobes_mmap_hash(inode));
+	build_probe_list(inode, &tmp_list);
+
+	ret = 0;
+	count = 0;
+
+	list_for_each_entry_safe(uprobe, u, &tmp_list, pending_list) {
+		loff_t vaddr;
+
+		list_del(&uprobe->pending_list);
+		if (!ret) {
+			vaddr = vma_address(vma, uprobe->offset);
+
+			if (vaddr < vma->vm_start || vaddr >= vma->vm_end) {
+				put_uprobe(uprobe);
+				continue;
+			}
+
+			ret = install_breakpoint(uprobe, vma->vm_mm, vma, vaddr);
+
+			/* Ignore double add: */
+			if (ret == -EEXIST) {
+				ret = 0;
+
+				if (!is_swbp_at_addr(vma->vm_mm, vaddr))
+					continue;
+
+				/*
+				 * Unable to insert a breakpoint, but
+				 * breakpoint lies underneath. Increment the
+				 * probe count.
+				 */
+				atomic_inc(&vma->vm_mm->uprobes_state.count);
+			}
+
+			if (!ret)
+				count++;
+		}
+		put_uprobe(uprobe);
+	}
+
+	mutex_unlock(uprobes_mmap_hash(inode));
+
+	if (ret)
+		atomic_sub(count, &vma->vm_mm->uprobes_state.count);
+
+	return ret;
+}
+
+/*
+ * Called in context of a munmap of a vma.
+ */
+void uprobe_munmap(struct vm_area_struct *vma, unsigned long start, unsigned long end)
+{
+	struct list_head tmp_list;
+	struct uprobe *uprobe, *u;
+	struct inode *inode;
+
+	if (!atomic_read(&uprobe_events) || !valid_vma(vma, false))
+		return;
+
+	if (!atomic_read(&vma->vm_mm->uprobes_state.count))
+		return;
+
+	inode = vma->vm_file->f_mapping->host;
+	if (!inode)
+		return;
+
+	INIT_LIST_HEAD(&tmp_list);
+	mutex_lock(uprobes_mmap_hash(inode));
+	build_probe_list(inode, &tmp_list);
+
+	list_for_each_entry_safe(uprobe, u, &tmp_list, pending_list) {
+		loff_t vaddr;
+
+		list_del(&uprobe->pending_list);
+		vaddr = vma_address(vma, uprobe->offset);
+
+		if (vaddr >= start && vaddr < end) {
+			/*
+			 * An unregister could have removed the probe before
+			 * unmap. So check before we decrement the count.
+			 */
+			if (is_swbp_at_addr(vma->vm_mm, vaddr) == 1)
+				atomic_dec(&vma->vm_mm->uprobes_state.count);
+		}
+		put_uprobe(uprobe);
+	}
+	mutex_unlock(uprobes_mmap_hash(inode));
+}
+
+/* Slot allocation for XOL */
+static int xol_add_vma(struct xol_area *area)
+{
+	struct mm_struct *mm;
+	int ret;
+
+	area->page = alloc_page(GFP_HIGHUSER);
+	if (!area->page)
+		return -ENOMEM;
+
+	ret = -EALREADY;
+	mm = current->mm;
+
+	down_write(&mm->mmap_sem);
+	if (mm->uprobes_state.xol_area)
+		goto fail;
+
+	ret = -ENOMEM;
+
+	/* Try to map as high as possible, this is only a hint. */
+	area->vaddr = get_unmapped_area(NULL, TASK_SIZE - PAGE_SIZE, PAGE_SIZE, 0, 0);
+	if (area->vaddr & ~PAGE_MASK) {
+		ret = area->vaddr;
+		goto fail;
+	}
+
+	ret = install_special_mapping(mm, area->vaddr, PAGE_SIZE,
+				VM_EXEC|VM_MAYEXEC|VM_DONTCOPY|VM_IO, &area->page);
+	if (ret)
+		goto fail;
+
+	smp_wmb();	/* pairs with get_xol_area() */
+	mm->uprobes_state.xol_area = area;
+	ret = 0;
+
+fail:
+	up_write(&mm->mmap_sem);
+	if (ret)
+		__free_page(area->page);
+
+	return ret;
+}
+
+static struct xol_area *get_xol_area(struct mm_struct *mm)
+{
+	struct xol_area *area;
+
+	area = mm->uprobes_state.xol_area;
+	smp_read_barrier_depends();	/* pairs with wmb in xol_add_vma() */
+
+	return area;
+}
+
+/*
+ * xol_alloc_area - Allocate process's xol_area.
+ * This area will be used for storing instructions for execution out of
+ * line.
+ *
+ * Returns the allocated area or NULL.
+ */
+static struct xol_area *xol_alloc_area(void)
+{
+	struct xol_area *area;
+
+	area = kzalloc(sizeof(*area), GFP_KERNEL);
+	if (unlikely(!area))
+		return NULL;
+
+	area->bitmap = kzalloc(BITS_TO_LONGS(UINSNS_PER_PAGE) * sizeof(long), GFP_KERNEL);
+
+	if (!area->bitmap)
+		goto fail;
+
+	init_waitqueue_head(&area->wq);
+	if (!xol_add_vma(area))
+		return area;
+
+fail:
+	kfree(area->bitmap);
+	kfree(area);
+
+	return get_xol_area(current->mm);
+}
+
+/*
+ * uprobe_clear_state - Free the area allocated for slots.
+ */
+void uprobe_clear_state(struct mm_struct *mm)
+{
+	struct xol_area *area = mm->uprobes_state.xol_area;
+
+	if (!area)
+		return;
+
+	put_page(area->page);
+	kfree(area->bitmap);
+	kfree(area);
+}
+
+/*
+ * uprobe_reset_state - Free the area allocated for slots.
+ */
+void uprobe_reset_state(struct mm_struct *mm)
+{
+	mm->uprobes_state.xol_area = NULL;
+	atomic_set(&mm->uprobes_state.count, 0);
+}
+
+/*
+ *  - search for a free slot.
+ */
+static unsigned long xol_take_insn_slot(struct xol_area *area)
+{
+	unsigned long slot_addr;
+	int slot_nr;
+
+	do {
+		slot_nr = find_first_zero_bit(area->bitmap, UINSNS_PER_PAGE);
+		if (slot_nr < UINSNS_PER_PAGE) {
+			if (!test_and_set_bit(slot_nr, area->bitmap))
+				break;
+
+			slot_nr = UINSNS_PER_PAGE;
+			continue;
+		}
+		wait_event(area->wq, (atomic_read(&area->slot_count) < UINSNS_PER_PAGE));
+	} while (slot_nr >= UINSNS_PER_PAGE);
+
+	slot_addr = area->vaddr + (slot_nr * UPROBE_XOL_SLOT_BYTES);
+	atomic_inc(&area->slot_count);
+
+	return slot_addr;
+}
+
+/*
+ * xol_get_insn_slot - If was not allocated a slot, then
+ * allocate a slot.
+ * Returns the allocated slot address or 0.
+ */
+static unsigned long xol_get_insn_slot(struct uprobe *uprobe, unsigned long slot_addr)
+{
+	struct xol_area *area;
+	unsigned long offset;
+	void *vaddr;
+
+	area = get_xol_area(current->mm);
+	if (!area) {
+		area = xol_alloc_area();
+		if (!area)
+			return 0;
+	}
+	current->utask->xol_vaddr = xol_take_insn_slot(area);
+
+	/*
+	 * Initialize the slot if xol_vaddr points to valid
+	 * instruction slot.
+	 */
+	if (unlikely(!current->utask->xol_vaddr))
+		return 0;
+
+	current->utask->vaddr = slot_addr;
+	offset = current->utask->xol_vaddr & ~PAGE_MASK;
+	vaddr = kmap_atomic(area->page);
+	memcpy(vaddr + offset, uprobe->arch.insn, MAX_UINSN_BYTES);
+	kunmap_atomic(vaddr);
+
+	return current->utask->xol_vaddr;
+}
+
+/*
+ * xol_free_insn_slot - If slot was earlier allocated by
+ * @xol_get_insn_slot(), make the slot available for
+ * subsequent requests.
+ */
+static void xol_free_insn_slot(struct task_struct *tsk)
+{
+	struct xol_area *area;
+	unsigned long vma_end;
+	unsigned long slot_addr;
+
+	if (!tsk->mm || !tsk->mm->uprobes_state.xol_area || !tsk->utask)
+		return;
+
+	slot_addr = tsk->utask->xol_vaddr;
+
+	if (unlikely(!slot_addr || IS_ERR_VALUE(slot_addr)))
+		return;
+
+	area = tsk->mm->uprobes_state.xol_area;
+	vma_end = area->vaddr + PAGE_SIZE;
+	if (area->vaddr <= slot_addr && slot_addr < vma_end) {
+		unsigned long offset;
+		int slot_nr;
+
+		offset = slot_addr - area->vaddr;
+		slot_nr = offset / UPROBE_XOL_SLOT_BYTES;
+		if (slot_nr >= UINSNS_PER_PAGE)
+			return;
+
+		clear_bit(slot_nr, area->bitmap);
+		atomic_dec(&area->slot_count);
+		if (waitqueue_active(&area->wq))
+			wake_up(&area->wq);
+
+		tsk->utask->xol_vaddr = 0;
+	}
+}
+
+/**
+ * uprobe_get_swbp_addr - compute address of swbp given post-swbp regs
+ * @regs: Reflects the saved state of the task after it has hit a breakpoint
+ * instruction.
+ * Return the address of the breakpoint instruction.
+ */
+unsigned long __weak uprobe_get_swbp_addr(struct pt_regs *regs)
+{
+	return instruction_pointer(regs) - UPROBE_SWBP_INSN_SIZE;
+}
+
+/*
+ * Called with no locks held.
+ * Called in context of a exiting or a exec-ing thread.
+ */
+void uprobe_free_utask(struct task_struct *t)
+{
+	struct uprobe_task *utask = t->utask;
+
+	if (t->uprobe_srcu_id != -1)
+		srcu_read_unlock_raw(&uprobes_srcu, t->uprobe_srcu_id);
+
+	if (!utask)
+		return;
+
+	if (utask->active_uprobe)
+		put_uprobe(utask->active_uprobe);
+
+	xol_free_insn_slot(t);
+	kfree(utask);
+	t->utask = NULL;
+}
+
+/*
+ * Called in context of a new clone/fork from copy_process.
+ */
+void uprobe_copy_process(struct task_struct *t)
+{
+	t->utask = NULL;
+	t->uprobe_srcu_id = -1;
+}
+
+/*
+ * Allocate a uprobe_task object for the task.
+ * Called when the thread hits a breakpoint for the first time.
+ *
+ * Returns:
+ * - pointer to new uprobe_task on success
+ * - NULL otherwise
+ */
+static struct uprobe_task *add_utask(void)
+{
+	struct uprobe_task *utask;
+
+	utask = kzalloc(sizeof *utask, GFP_KERNEL);
+	if (unlikely(!utask))
+		return NULL;
+
+	utask->active_uprobe = NULL;
+	current->utask = utask;
+	return utask;
+}
+
+/* Prepare to single-step probed instruction out of line. */
+static int
+pre_ssout(struct uprobe *uprobe, struct pt_regs *regs, unsigned long vaddr)
+{
+	if (xol_get_insn_slot(uprobe, vaddr) && !arch_uprobe_pre_xol(&uprobe->arch, regs))
+		return 0;
+
+	return -EFAULT;
+}
+
+/*
+ * If we are singlestepping, then ensure this thread is not connected to
+ * non-fatal signals until completion of singlestep.  When xol insn itself
+ * triggers the signal,  restart the original insn even if the task is
+ * already SIGKILL'ed (since coredump should report the correct ip).  This
+ * is even more important if the task has a handler for SIGSEGV/etc, The
+ * _same_ instruction should be repeated again after return from the signal
+ * handler, and SSTEP can never finish in this case.
+ */
+bool uprobe_deny_signal(void)
+{
+	struct task_struct *t = current;
+	struct uprobe_task *utask = t->utask;
+
+	if (likely(!utask || !utask->active_uprobe))
+		return false;
+
+	WARN_ON_ONCE(utask->state != UTASK_SSTEP);
+
+	if (signal_pending(t)) {
+		spin_lock_irq(&t->sighand->siglock);
+		clear_tsk_thread_flag(t, TIF_SIGPENDING);
+		spin_unlock_irq(&t->sighand->siglock);
+
+		if (__fatal_signal_pending(t) || arch_uprobe_xol_was_trapped(t)) {
+			utask->state = UTASK_SSTEP_TRAPPED;
+			set_tsk_thread_flag(t, TIF_UPROBE);
+			set_tsk_thread_flag(t, TIF_NOTIFY_RESUME);
+		}
+	}
+
+	return true;
+}
+
+/*
+ * Avoid singlestepping the original instruction if the original instruction
+ * is a NOP or can be emulated.
+ */
+static bool can_skip_sstep(struct uprobe *uprobe, struct pt_regs *regs)
+{
+	if (arch_uprobe_skip_sstep(&uprobe->arch, regs))
+		return true;
+
+	uprobe->flags &= ~UPROBE_SKIP_SSTEP;
+	return false;
+}
+
+/*
+ * Run handler and ask thread to singlestep.
+ * Ensure all non-fatal signals cannot interrupt thread while it singlesteps.
+ */
+static void handle_swbp(struct pt_regs *regs)
+{
+	struct vm_area_struct *vma;
+	struct uprobe_task *utask;
+	struct uprobe *uprobe;
+	struct mm_struct *mm;
+	unsigned long bp_vaddr;
+
+	uprobe = NULL;
+	bp_vaddr = uprobe_get_swbp_addr(regs);
+	mm = current->mm;
+	down_read(&mm->mmap_sem);
+	vma = find_vma(mm, bp_vaddr);
+
+	if (vma && vma->vm_start <= bp_vaddr && valid_vma(vma, false)) {
+		struct inode *inode;
+		loff_t offset;
+
+		inode = vma->vm_file->f_mapping->host;
+		offset = bp_vaddr - vma->vm_start;
+		offset += (vma->vm_pgoff << PAGE_SHIFT);
+		uprobe = find_uprobe(inode, offset);
+	}
+
+	srcu_read_unlock_raw(&uprobes_srcu, current->uprobe_srcu_id);
+	current->uprobe_srcu_id = -1;
+	up_read(&mm->mmap_sem);
+
+	if (!uprobe) {
+		/* No matching uprobe; signal SIGTRAP. */
+		send_sig(SIGTRAP, current, 0);
+		return;
+	}
+
+	utask = current->utask;
+	if (!utask) {
+		utask = add_utask();
+		/* Cannot allocate; re-execute the instruction. */
+		if (!utask)
+			goto cleanup_ret;
+	}
+	utask->active_uprobe = uprobe;
+	handler_chain(uprobe, regs);
+	if (uprobe->flags & UPROBE_SKIP_SSTEP && can_skip_sstep(uprobe, regs))
+		goto cleanup_ret;
+
+	utask->state = UTASK_SSTEP;
+	if (!pre_ssout(uprobe, regs, bp_vaddr)) {
+		user_enable_single_step(current);
+		return;
+	}
+
+cleanup_ret:
+	if (utask) {
+		utask->active_uprobe = NULL;
+		utask->state = UTASK_RUNNING;
+	}
+	if (uprobe) {
+		if (!(uprobe->flags & UPROBE_SKIP_SSTEP))
+
+			/*
+			 * cannot singlestep; cannot skip instruction;
+			 * re-execute the instruction.
+			 */
+			instruction_pointer_set(regs, bp_vaddr);
+
+		put_uprobe(uprobe);
+	}
+}
+
+/*
+ * Perform required fix-ups and disable singlestep.
+ * Allow pending signals to take effect.
+ */
+static void handle_singlestep(struct uprobe_task *utask, struct pt_regs *regs)
+{
+	struct uprobe *uprobe;
+
+	uprobe = utask->active_uprobe;
+	if (utask->state == UTASK_SSTEP_ACK)
+		arch_uprobe_post_xol(&uprobe->arch, regs);
+	else if (utask->state == UTASK_SSTEP_TRAPPED)
+		arch_uprobe_abort_xol(&uprobe->arch, regs);
+	else
+		WARN_ON_ONCE(1);
+
+	put_uprobe(uprobe);
+	utask->active_uprobe = NULL;
+	utask->state = UTASK_RUNNING;
+	user_disable_single_step(current);
+	xol_free_insn_slot(current);
+
+	spin_lock_irq(&current->sighand->siglock);
+	recalc_sigpending(); /* see uprobe_deny_signal() */
+	spin_unlock_irq(&current->sighand->siglock);
+}
+
+/*
+ * On breakpoint hit, breakpoint notifier sets the TIF_UPROBE flag.  (and on
+ * subsequent probe hits on the thread sets the state to UTASK_BP_HIT) and
+ * allows the thread to return from interrupt.
+ *
+ * On singlestep exception, singlestep notifier sets the TIF_UPROBE flag and
+ * also sets the state to UTASK_SSTEP_ACK and allows the thread to return from
+ * interrupt.
+ *
+ * While returning to userspace, thread notices the TIF_UPROBE flag and calls
+ * uprobe_notify_resume().
+ */
+void uprobe_notify_resume(struct pt_regs *regs)
+{
+	struct uprobe_task *utask;
+
+	utask = current->utask;
+	if (!utask || utask->state == UTASK_BP_HIT)
+		handle_swbp(regs);
+	else
+		handle_singlestep(utask, regs);
+}
+
+/*
+ * uprobe_pre_sstep_notifier gets called from interrupt context as part of
+ * notifier mechanism. Set TIF_UPROBE flag and indicate breakpoint hit.
+ */
+int uprobe_pre_sstep_notifier(struct pt_regs *regs)
+{
+	struct uprobe_task *utask;
+
+	if (!current->mm || !atomic_read(&current->mm->uprobes_state.count))
+		/* task is currently not uprobed */
+		return 0;
+
+	utask = current->utask;
+	if (utask)
+		utask->state = UTASK_BP_HIT;
+
+	set_thread_flag(TIF_UPROBE);
+	current->uprobe_srcu_id = srcu_read_lock_raw(&uprobes_srcu);
+
+	return 1;
+}
+
+/*
+ * uprobe_post_sstep_notifier gets called in interrupt context as part of notifier
+ * mechanism. Set TIF_UPROBE flag and indicate completion of singlestep.
+ */
+int uprobe_post_sstep_notifier(struct pt_regs *regs)
+{
+	struct uprobe_task *utask = current->utask;
+
+	if (!current->mm || !utask || !utask->active_uprobe)
+		/* task is currently not uprobed */
+		return 0;
+
+	utask->state = UTASK_SSTEP_ACK;
+	set_thread_flag(TIF_UPROBE);
+	return 1;
+}
+
+static struct notifier_block uprobe_exception_nb = {
+	.notifier_call		= arch_uprobe_exception_notify,
+	.priority		= INT_MAX-1,	/* notified after kprobes, kgdb */
+};
+
+static int __init init_uprobes(void)
+{
+	int i;
+
+	for (i = 0; i < UPROBES_HASH_SZ; i++) {
+		mutex_init(&uprobes_mutex[i]);
+		mutex_init(&uprobes_mmap_mutex[i]);
+	}
+	init_srcu_struct(&uprobes_srcu);
+
+	return register_die_notifier(&uprobe_exception_nb);
+}
+module_init(init_uprobes);
+
+static void __exit exit_uprobes(void)
+{
+}
+module_exit(exit_uprobes);
diff --git a/kernel/fork.c b/kernel/fork.c
index 05c813dc9ecc..47b4e4f379f9 100644
--- a/kernel/fork.c
+++ b/kernel/fork.c
@@ -69,6 +69,7 @@
 #include <linux/oom.h>
 #include <linux/khugepaged.h>
 #include <linux/signalfd.h>
+#include <linux/uprobes.h>
 
 #include <asm/pgtable.h>
 #include <asm/pgalloc.h>
@@ -451,6 +452,9 @@ static int dup_mmap(struct mm_struct *mm, struct mm_struct *oldmm)
 
 		if (retval)
 			goto out;
+
+		if (file && uprobe_mmap(tmp))
+			goto out;
 	}
 	/* a new mm has just been created */
 	arch_dup_mmap(oldmm, mm);
@@ -599,6 +603,7 @@ void mmput(struct mm_struct *mm)
 	might_sleep();
 
 	if (atomic_dec_and_test(&mm->mm_users)) {
+		uprobe_clear_state(mm);
 		exit_aio(mm);
 		ksm_exit(mm);
 		khugepaged_exit(mm); /* must run before exit_mmap */
@@ -777,6 +782,8 @@ void mm_release(struct task_struct *tsk, struct mm_struct *mm)
 		exit_pi_state_list(tsk);
 #endif
 
+	uprobe_free_utask(tsk);
+
 	/* Get rid of any cached register state */
 	deactivate_mm(tsk, mm);
 
@@ -831,6 +838,7 @@ struct mm_struct *dup_mm(struct task_struct *tsk)
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
 	mm->pmd_huge_pte = NULL;
 #endif
+	uprobe_reset_state(mm);
 
 	if (!mm_init(mm, tsk))
 		goto fail_nomem;
@@ -1373,6 +1381,7 @@ static struct task_struct *copy_process(unsigned long clone_flags,
 	INIT_LIST_HEAD(&p->pi_state_list);
 	p->pi_state_cache = NULL;
 #endif
+	uprobe_copy_process(p);
 	/*
 	 * sigaltstack should be cleared when sharing the same VM
 	 */
diff --git a/kernel/kfifo.c b/kernel/kfifo.c
index c744b88c44e2..59dcf5b81d24 100644
--- a/kernel/kfifo.c
+++ b/kernel/kfifo.c
@@ -402,6 +402,7 @@ unsigned int __kfifo_max_r(unsigned int len, size_t recsize)
 		return max;
 	return len;
 }
+EXPORT_SYMBOL(__kfifo_max_r);
 
 #define	__KFIFO_PEEK(data, out, mask) \
 	((data)[(out) & (mask)])
diff --git a/kernel/pid.c b/kernel/pid.c
index 9f08dfabaf13..e86b291ad834 100644
--- a/kernel/pid.c
+++ b/kernel/pid.c
@@ -547,7 +547,8 @@ void __init pidhash_init(void)
 
 	pid_hash = alloc_large_system_hash("PID", sizeof(*pid_hash), 0, 18,
 					   HASH_EARLY | HASH_SMALL,
-					   &pidhash_shift, NULL, 4096);
+					   &pidhash_shift, NULL,
+					   0, 4096);
 	pidhash_size = 1U << pidhash_shift;
 
 	for (i = 0; i < pidhash_size; i++)
diff --git a/kernel/signal.c b/kernel/signal.c
index 4dbf00dfb359..f7b418217633 100644
--- a/kernel/signal.c
+++ b/kernel/signal.c
@@ -29,6 +29,7 @@
 #include <linux/pid_namespace.h>
 #include <linux/nsproxy.h>
 #include <linux/user_namespace.h>
+#include <linux/uprobes.h>
 #define CREATE_TRACE_POINTS
 #include <trace/events/signal.h>
 
@@ -2191,6 +2192,9 @@ int get_signal_to_deliver(siginfo_t *info, struct k_sigaction *return_ka,
 	struct signal_struct *signal = current->signal;
 	int signr;
 
+	if (unlikely(uprobe_deny_signal()))
+		return 0;
+
 relock:
 	/*
 	 * We'll jump back here after any time we were stopped in TASK_STOPPED.
diff --git a/kernel/time/Kconfig b/kernel/time/Kconfig
index a20dc8a3c949..fd42bd452b75 100644
--- a/kernel/time/Kconfig
+++ b/kernel/time/Kconfig
@@ -2,6 +2,55 @@
 # Timer subsystem related configuration options
 #
 
+# Options selectable by arch Kconfig
+
+# Watchdog function for clocksources to detect instabilities
+config CLOCKSOURCE_WATCHDOG
+	bool
+
+# Architecture has extra clocksource data
+config ARCH_CLOCKSOURCE_DATA
+	bool
+
+# Timekeeping vsyscall support
+config GENERIC_TIME_VSYSCALL
+	bool
+
+# ktime_t scalar 64bit nsec representation
+config KTIME_SCALAR
+	bool
+
+# Old style timekeeping
+config ARCH_USES_GETTIMEOFFSET
+	bool
+
+# The generic clock events infrastructure
+config GENERIC_CLOCKEVENTS
+	bool
+
+# Migration helper. Builds, but does not invoke
+config GENERIC_CLOCKEVENTS_BUILD
+	bool
+	default y
+	depends on GENERIC_CLOCKEVENTS
+
+# Clockevents broadcasting infrastructure
+config GENERIC_CLOCKEVENTS_BROADCAST
+	bool
+	depends on GENERIC_CLOCKEVENTS
+
+# Automatically adjust the min. reprogramming time for
+# clock event device
+config GENERIC_CLOCKEVENTS_MIN_ADJUST
+	bool
+
+# Generic update of CMOS clock
+config GENERIC_CMOS_UPDATE
+	bool
+
+if GENERIC_CLOCKEVENTS
+menu "Timers subsystem"
+
 # Core internal switch. Selected by NO_HZ / HIGH_RES_TIMERS. This is
 # only related to the tick functionality. Oneshot clockevent devices
 # are supported independ of this.
@@ -26,10 +75,5 @@ config HIGH_RES_TIMERS
 	  hardware is not capable then this option only increases
 	  the size of the kernel image.
 
-config GENERIC_CLOCKEVENTS_BUILD
-	bool
-	default y
-	depends on GENERIC_CLOCKEVENTS
-
-config GENERIC_CLOCKEVENTS_MIN_ADJUST
-	bool
+endmenu
+endif
diff --git a/kernel/time/ntp.c b/kernel/time/ntp.c
index f03fd83b170b..70b33abcc7bb 100644
--- a/kernel/time/ntp.c
+++ b/kernel/time/ntp.c
@@ -412,6 +412,7 @@ int second_overflow(unsigned long secs)
 		if (secs % 86400 == 0) {
 			leap = -1;
 			time_state = TIME_OOP;
+			time_tai++;
 			printk(KERN_NOTICE
 				"Clock: inserting leap second 23:59:60 UTC\n");
 		}
@@ -426,7 +427,6 @@ int second_overflow(unsigned long secs)
 		}
 		break;
 	case TIME_OOP:
-		time_tai++;
 		time_state = TIME_WAIT;
 		break;
 
@@ -473,8 +473,6 @@ int second_overflow(unsigned long secs)
 							 << NTP_SCALE_SHIFT;
 	time_adjust = 0;
 
-
-
 out:
 	spin_unlock_irqrestore(&ntp_lock, flags);
 
@@ -559,10 +557,10 @@ static inline void process_adj_status(struct timex *txc, struct timespec *ts)
 	/* only set allowed bits */
 	time_status &= STA_RONLY;
 	time_status |= txc->status & ~STA_RONLY;
-
 }
+
 /*
- * Called with the xtime lock held, so we can access and modify
+ * Called with ntp_lock held, so we can access and modify
  * all the global NTP state:
  */
 static inline void process_adjtimex_modes(struct timex *txc, struct timespec *ts)
diff --git a/kernel/time/timekeeping.c b/kernel/time/timekeeping.c
index d66b21308f7c..6e46cacf5969 100644
--- a/kernel/time/timekeeping.c
+++ b/kernel/time/timekeeping.c
@@ -240,7 +240,6 @@ void getnstimeofday(struct timespec *ts)
 
 	timespec_add_ns(ts, nsecs);
 }
-
 EXPORT_SYMBOL(getnstimeofday);
 
 ktime_t ktime_get(void)
@@ -357,8 +356,8 @@ void do_gettimeofday(struct timeval *tv)
 	tv->tv_sec = now.tv_sec;
 	tv->tv_usec = now.tv_nsec/1000;
 }
-
 EXPORT_SYMBOL(do_gettimeofday);
+
 /**
  * do_settimeofday - Sets the time of day
  * @tv:		pointer to the timespec variable containing the new time
@@ -392,7 +391,6 @@ int do_settimeofday(const struct timespec *tv)
 
 	return 0;
 }
-
 EXPORT_SYMBOL(do_settimeofday);
 
 
diff --git a/kernel/trace/Kconfig b/kernel/trace/Kconfig
index f347ac91292d..8c4c07071cc5 100644
--- a/kernel/trace/Kconfig
+++ b/kernel/trace/Kconfig
@@ -372,6 +372,7 @@ config KPROBE_EVENT
 	depends on HAVE_REGS_AND_STACK_ACCESS_API
 	bool "Enable kprobes-based dynamic events"
 	select TRACING
+	select PROBE_EVENTS
 	default y
 	help
 	  This allows the user to add tracing events (similar to tracepoints)
@@ -384,6 +385,25 @@ config KPROBE_EVENT
 	  This option is also required by perf-probe subcommand of perf tools.
 	  If you want to use perf tools, this option is strongly recommended.
 
+config UPROBE_EVENT
+	bool "Enable uprobes-based dynamic events"
+	depends on ARCH_SUPPORTS_UPROBES
+	depends on MMU
+	select UPROBES
+	select PROBE_EVENTS
+	select TRACING
+	default n
+	help
+	  This allows the user to add tracing events on top of userspace
+	  dynamic events (similar to tracepoints) on the fly via the trace
+	  events interface. Those events can be inserted wherever uprobes
+	  can probe, and record various registers.
+	  This option is required if you plan to use perf-probe subcommand
+	  of perf tools on user space applications.
+
+config PROBE_EVENTS
+	def_bool n
+
 config DYNAMIC_FTRACE
 	bool "enable/disable ftrace tracepoints dynamically"
 	depends on FUNCTION_TRACER
diff --git a/kernel/trace/Makefile b/kernel/trace/Makefile
index b3afe0e76f79..b831087c8200 100644
--- a/kernel/trace/Makefile
+++ b/kernel/trace/Makefile
@@ -60,5 +60,7 @@ endif
 ifeq ($(CONFIG_TRACING),y)
 obj-$(CONFIG_KGDB_KDB) += trace_kdb.o
 endif
+obj-$(CONFIG_PROBE_EVENTS) += trace_probe.o
+obj-$(CONFIG_UPROBE_EVENT) += trace_uprobe.o
 
 libftrace-y := ftrace.o
diff --git a/kernel/trace/trace.h b/kernel/trace/trace.h
index 6c6f7933eede..5aec220d2de0 100644
--- a/kernel/trace/trace.h
+++ b/kernel/trace/trace.h
@@ -103,6 +103,11 @@ struct kretprobe_trace_entry_head {
 	unsigned long		ret_ip;
 };
 
+struct uprobe_trace_entry_head {
+	struct trace_entry	ent;
+	unsigned long		ip;
+};
+
 /*
  * trace_flag_type is an enumeration that holds different
  * states when a trace occurs. These are:
diff --git a/kernel/trace/trace_kprobe.c b/kernel/trace/trace_kprobe.c
index 580a05ec926b..b31d3d5699fe 100644
--- a/kernel/trace/trace_kprobe.c
+++ b/kernel/trace/trace_kprobe.c
@@ -19,547 +19,15 @@
 
 #include <linux/module.h>
 #include <linux/uaccess.h>
-#include <linux/kprobes.h>
-#include <linux/seq_file.h>
-#include <linux/slab.h>
-#include <linux/smp.h>
-#include <linux/debugfs.h>
-#include <linux/types.h>
-#include <linux/string.h>
-#include <linux/ctype.h>
-#include <linux/ptrace.h>
-#include <linux/perf_event.h>
-#include <linux/stringify.h>
-#include <linux/limits.h>
-#include <asm/bitsperlong.h>
-
-#include "trace.h"
-#include "trace_output.h"
-
-#define MAX_TRACE_ARGS 128
-#define MAX_ARGSTR_LEN 63
-#define MAX_EVENT_NAME_LEN 64
-#define MAX_STRING_SIZE PATH_MAX
-#define KPROBE_EVENT_SYSTEM "kprobes"
-
-/* Reserved field names */
-#define FIELD_STRING_IP "__probe_ip"
-#define FIELD_STRING_RETIP "__probe_ret_ip"
-#define FIELD_STRING_FUNC "__probe_func"
-
-const char *reserved_field_names[] = {
-	"common_type",
-	"common_flags",
-	"common_preempt_count",
-	"common_pid",
-	"common_tgid",
-	FIELD_STRING_IP,
-	FIELD_STRING_RETIP,
-	FIELD_STRING_FUNC,
-};
-
-/* Printing function type */
-typedef int (*print_type_func_t)(struct trace_seq *, const char *, void *,
-				 void *);
-#define PRINT_TYPE_FUNC_NAME(type)	print_type_##type
-#define PRINT_TYPE_FMT_NAME(type)	print_type_format_##type
-
-/* Printing  in basic type function template */
-#define DEFINE_BASIC_PRINT_TYPE_FUNC(type, fmt, cast)			\
-static __kprobes int PRINT_TYPE_FUNC_NAME(type)(struct trace_seq *s,	\
-						const char *name,	\
-						void *data, void *ent)\
-{									\
-	return trace_seq_printf(s, " %s=" fmt, name, (cast)*(type *)data);\
-}									\
-static const char PRINT_TYPE_FMT_NAME(type)[] = fmt;
-
-DEFINE_BASIC_PRINT_TYPE_FUNC(u8, "%x", unsigned int)
-DEFINE_BASIC_PRINT_TYPE_FUNC(u16, "%x", unsigned int)
-DEFINE_BASIC_PRINT_TYPE_FUNC(u32, "%lx", unsigned long)
-DEFINE_BASIC_PRINT_TYPE_FUNC(u64, "%llx", unsigned long long)
-DEFINE_BASIC_PRINT_TYPE_FUNC(s8, "%d", int)
-DEFINE_BASIC_PRINT_TYPE_FUNC(s16, "%d", int)
-DEFINE_BASIC_PRINT_TYPE_FUNC(s32, "%ld", long)
-DEFINE_BASIC_PRINT_TYPE_FUNC(s64, "%lld", long long)
-
-/* data_rloc: data relative location, compatible with u32 */
-#define make_data_rloc(len, roffs)	\
-	(((u32)(len) << 16) | ((u32)(roffs) & 0xffff))
-#define get_rloc_len(dl)	((u32)(dl) >> 16)
-#define get_rloc_offs(dl)	((u32)(dl) & 0xffff)
-
-static inline void *get_rloc_data(u32 *dl)
-{
-	return (u8 *)dl + get_rloc_offs(*dl);
-}
-
-/* For data_loc conversion */
-static inline void *get_loc_data(u32 *dl, void *ent)
-{
-	return (u8 *)ent + get_rloc_offs(*dl);
-}
-
-/*
- * Convert data_rloc to data_loc:
- *  data_rloc stores the offset from data_rloc itself, but data_loc
- *  stores the offset from event entry.
- */
-#define convert_rloc_to_loc(dl, offs)	((u32)(dl) + (offs))
-
-/* For defining macros, define string/string_size types */
-typedef u32 string;
-typedef u32 string_size;
-
-/* Print type function for string type */
-static __kprobes int PRINT_TYPE_FUNC_NAME(string)(struct trace_seq *s,
-						  const char *name,
-						  void *data, void *ent)
-{
-	int len = *(u32 *)data >> 16;
-
-	if (!len)
-		return trace_seq_printf(s, " %s=(fault)", name);
-	else
-		return trace_seq_printf(s, " %s=\"%s\"", name,
-					(const char *)get_loc_data(data, ent));
-}
-static const char PRINT_TYPE_FMT_NAME(string)[] = "\\\"%s\\\"";
-
-/* Data fetch function type */
-typedef	void (*fetch_func_t)(struct pt_regs *, void *, void *);
-
-struct fetch_param {
-	fetch_func_t	fn;
-	void *data;
-};
-
-static __kprobes void call_fetch(struct fetch_param *fprm,
-				 struct pt_regs *regs, void *dest)
-{
-	return fprm->fn(regs, fprm->data, dest);
-}
-
-#define FETCH_FUNC_NAME(method, type)	fetch_##method##_##type
-/*
- * Define macro for basic types - we don't need to define s* types, because
- * we have to care only about bitwidth at recording time.
- */
-#define DEFINE_BASIC_FETCH_FUNCS(method) \
-DEFINE_FETCH_##method(u8)		\
-DEFINE_FETCH_##method(u16)		\
-DEFINE_FETCH_##method(u32)		\
-DEFINE_FETCH_##method(u64)
-
-#define CHECK_FETCH_FUNCS(method, fn)			\
-	(((FETCH_FUNC_NAME(method, u8) == fn) ||	\
-	  (FETCH_FUNC_NAME(method, u16) == fn) ||	\
-	  (FETCH_FUNC_NAME(method, u32) == fn) ||	\
-	  (FETCH_FUNC_NAME(method, u64) == fn) ||	\
-	  (FETCH_FUNC_NAME(method, string) == fn) ||	\
-	  (FETCH_FUNC_NAME(method, string_size) == fn)) \
-	 && (fn != NULL))
-
-/* Data fetch function templates */
-#define DEFINE_FETCH_reg(type)						\
-static __kprobes void FETCH_FUNC_NAME(reg, type)(struct pt_regs *regs,	\
-					void *offset, void *dest)	\
-{									\
-	*(type *)dest = (type)regs_get_register(regs,			\
-				(unsigned int)((unsigned long)offset));	\
-}
-DEFINE_BASIC_FETCH_FUNCS(reg)
-/* No string on the register */
-#define fetch_reg_string NULL
-#define fetch_reg_string_size NULL
-
-#define DEFINE_FETCH_stack(type)					\
-static __kprobes void FETCH_FUNC_NAME(stack, type)(struct pt_regs *regs,\
-					  void *offset, void *dest)	\
-{									\
-	*(type *)dest = (type)regs_get_kernel_stack_nth(regs,		\
-				(unsigned int)((unsigned long)offset));	\
-}
-DEFINE_BASIC_FETCH_FUNCS(stack)
-/* No string on the stack entry */
-#define fetch_stack_string NULL
-#define fetch_stack_string_size NULL
-
-#define DEFINE_FETCH_retval(type)					\
-static __kprobes void FETCH_FUNC_NAME(retval, type)(struct pt_regs *regs,\
-					  void *dummy, void *dest)	\
-{									\
-	*(type *)dest = (type)regs_return_value(regs);			\
-}
-DEFINE_BASIC_FETCH_FUNCS(retval)
-/* No string on the retval */
-#define fetch_retval_string NULL
-#define fetch_retval_string_size NULL
-
-#define DEFINE_FETCH_memory(type)					\
-static __kprobes void FETCH_FUNC_NAME(memory, type)(struct pt_regs *regs,\
-					  void *addr, void *dest)	\
-{									\
-	type retval;							\
-	if (probe_kernel_address(addr, retval))				\
-		*(type *)dest = 0;					\
-	else								\
-		*(type *)dest = retval;					\
-}
-DEFINE_BASIC_FETCH_FUNCS(memory)
-/*
- * Fetch a null-terminated string. Caller MUST set *(u32 *)dest with max
- * length and relative data location.
- */
-static __kprobes void FETCH_FUNC_NAME(memory, string)(struct pt_regs *regs,
-						      void *addr, void *dest)
-{
-	long ret;
-	int maxlen = get_rloc_len(*(u32 *)dest);
-	u8 *dst = get_rloc_data(dest);
-	u8 *src = addr;
-	mm_segment_t old_fs = get_fs();
-	if (!maxlen)
-		return;
-	/*
-	 * Try to get string again, since the string can be changed while
-	 * probing.
-	 */
-	set_fs(KERNEL_DS);
-	pagefault_disable();
-	do
-		ret = __copy_from_user_inatomic(dst++, src++, 1);
-	while (dst[-1] && ret == 0 && src - (u8 *)addr < maxlen);
-	dst[-1] = '\0';
-	pagefault_enable();
-	set_fs(old_fs);
-
-	if (ret < 0) {	/* Failed to fetch string */
-		((u8 *)get_rloc_data(dest))[0] = '\0';
-		*(u32 *)dest = make_data_rloc(0, get_rloc_offs(*(u32 *)dest));
-	} else
-		*(u32 *)dest = make_data_rloc(src - (u8 *)addr,
-					      get_rloc_offs(*(u32 *)dest));
-}
-/* Return the length of string -- including null terminal byte */
-static __kprobes void FETCH_FUNC_NAME(memory, string_size)(struct pt_regs *regs,
-							void *addr, void *dest)
-{
-	int ret, len = 0;
-	u8 c;
-	mm_segment_t old_fs = get_fs();
-
-	set_fs(KERNEL_DS);
-	pagefault_disable();
-	do {
-		ret = __copy_from_user_inatomic(&c, (u8 *)addr + len, 1);
-		len++;
-	} while (c && ret == 0 && len < MAX_STRING_SIZE);
-	pagefault_enable();
-	set_fs(old_fs);
-
-	if (ret < 0)	/* Failed to check the length */
-		*(u32 *)dest = 0;
-	else
-		*(u32 *)dest = len;
-}
-
-/* Memory fetching by symbol */
-struct symbol_cache {
-	char *symbol;
-	long offset;
-	unsigned long addr;
-};
-
-static unsigned long update_symbol_cache(struct symbol_cache *sc)
-{
-	sc->addr = (unsigned long)kallsyms_lookup_name(sc->symbol);
-	if (sc->addr)
-		sc->addr += sc->offset;
-	return sc->addr;
-}
-
-static void free_symbol_cache(struct symbol_cache *sc)
-{
-	kfree(sc->symbol);
-	kfree(sc);
-}
-
-static struct symbol_cache *alloc_symbol_cache(const char *sym, long offset)
-{
-	struct symbol_cache *sc;
-
-	if (!sym || strlen(sym) == 0)
-		return NULL;
-	sc = kzalloc(sizeof(struct symbol_cache), GFP_KERNEL);
-	if (!sc)
-		return NULL;
-
-	sc->symbol = kstrdup(sym, GFP_KERNEL);
-	if (!sc->symbol) {
-		kfree(sc);
-		return NULL;
-	}
-	sc->offset = offset;
 
-	update_symbol_cache(sc);
-	return sc;
-}
-
-#define DEFINE_FETCH_symbol(type)					\
-static __kprobes void FETCH_FUNC_NAME(symbol, type)(struct pt_regs *regs,\
-					  void *data, void *dest)	\
-{									\
-	struct symbol_cache *sc = data;					\
-	if (sc->addr)							\
-		fetch_memory_##type(regs, (void *)sc->addr, dest);	\
-	else								\
-		*(type *)dest = 0;					\
-}
-DEFINE_BASIC_FETCH_FUNCS(symbol)
-DEFINE_FETCH_symbol(string)
-DEFINE_FETCH_symbol(string_size)
-
-/* Dereference memory access function */
-struct deref_fetch_param {
-	struct fetch_param orig;
-	long offset;
-};
-
-#define DEFINE_FETCH_deref(type)					\
-static __kprobes void FETCH_FUNC_NAME(deref, type)(struct pt_regs *regs,\
-					    void *data, void *dest)	\
-{									\
-	struct deref_fetch_param *dprm = data;				\
-	unsigned long addr;						\
-	call_fetch(&dprm->orig, regs, &addr);				\
-	if (addr) {							\
-		addr += dprm->offset;					\
-		fetch_memory_##type(regs, (void *)addr, dest);		\
-	} else								\
-		*(type *)dest = 0;					\
-}
-DEFINE_BASIC_FETCH_FUNCS(deref)
-DEFINE_FETCH_deref(string)
-DEFINE_FETCH_deref(string_size)
-
-static __kprobes void update_deref_fetch_param(struct deref_fetch_param *data)
-{
-	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
-		update_deref_fetch_param(data->orig.data);
-	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
-		update_symbol_cache(data->orig.data);
-}
-
-static __kprobes void free_deref_fetch_param(struct deref_fetch_param *data)
-{
-	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
-		free_deref_fetch_param(data->orig.data);
-	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
-		free_symbol_cache(data->orig.data);
-	kfree(data);
-}
-
-/* Bitfield fetch function */
-struct bitfield_fetch_param {
-	struct fetch_param orig;
-	unsigned char hi_shift;
-	unsigned char low_shift;
-};
+#include "trace_probe.h"
 
-#define DEFINE_FETCH_bitfield(type)					\
-static __kprobes void FETCH_FUNC_NAME(bitfield, type)(struct pt_regs *regs,\
-					    void *data, void *dest)	\
-{									\
-	struct bitfield_fetch_param *bprm = data;			\
-	type buf = 0;							\
-	call_fetch(&bprm->orig, regs, &buf);				\
-	if (buf) {							\
-		buf <<= bprm->hi_shift;					\
-		buf >>= bprm->low_shift;				\
-	}								\
-	*(type *)dest = buf;						\
-}
-DEFINE_BASIC_FETCH_FUNCS(bitfield)
-#define fetch_bitfield_string NULL
-#define fetch_bitfield_string_size NULL
-
-static __kprobes void
-update_bitfield_fetch_param(struct bitfield_fetch_param *data)
-{
-	/*
-	 * Don't check the bitfield itself, because this must be the
-	 * last fetch function.
-	 */
-	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
-		update_deref_fetch_param(data->orig.data);
-	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
-		update_symbol_cache(data->orig.data);
-}
-
-static __kprobes void
-free_bitfield_fetch_param(struct bitfield_fetch_param *data)
-{
-	/*
-	 * Don't check the bitfield itself, because this must be the
-	 * last fetch function.
-	 */
-	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
-		free_deref_fetch_param(data->orig.data);
-	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
-		free_symbol_cache(data->orig.data);
-	kfree(data);
-}
-
-/* Default (unsigned long) fetch type */
-#define __DEFAULT_FETCH_TYPE(t) u##t
-#define _DEFAULT_FETCH_TYPE(t) __DEFAULT_FETCH_TYPE(t)
-#define DEFAULT_FETCH_TYPE _DEFAULT_FETCH_TYPE(BITS_PER_LONG)
-#define DEFAULT_FETCH_TYPE_STR __stringify(DEFAULT_FETCH_TYPE)
-
-/* Fetch types */
-enum {
-	FETCH_MTD_reg = 0,
-	FETCH_MTD_stack,
-	FETCH_MTD_retval,
-	FETCH_MTD_memory,
-	FETCH_MTD_symbol,
-	FETCH_MTD_deref,
-	FETCH_MTD_bitfield,
-	FETCH_MTD_END,
-};
-
-#define ASSIGN_FETCH_FUNC(method, type)	\
-	[FETCH_MTD_##method] = FETCH_FUNC_NAME(method, type)
-
-#define __ASSIGN_FETCH_TYPE(_name, ptype, ftype, _size, sign, _fmttype)	\
-	{.name = _name,				\
-	 .size = _size,					\
-	 .is_signed = sign,				\
-	 .print = PRINT_TYPE_FUNC_NAME(ptype),		\
-	 .fmt = PRINT_TYPE_FMT_NAME(ptype),		\
-	 .fmttype = _fmttype,				\
-	 .fetch = {					\
-ASSIGN_FETCH_FUNC(reg, ftype),				\
-ASSIGN_FETCH_FUNC(stack, ftype),			\
-ASSIGN_FETCH_FUNC(retval, ftype),			\
-ASSIGN_FETCH_FUNC(memory, ftype),			\
-ASSIGN_FETCH_FUNC(symbol, ftype),			\
-ASSIGN_FETCH_FUNC(deref, ftype),			\
-ASSIGN_FETCH_FUNC(bitfield, ftype),			\
-	  }						\
-	}
-
-#define ASSIGN_FETCH_TYPE(ptype, ftype, sign)			\
-	__ASSIGN_FETCH_TYPE(#ptype, ptype, ftype, sizeof(ftype), sign, #ptype)
-
-#define FETCH_TYPE_STRING 0
-#define FETCH_TYPE_STRSIZE 1
-
-/* Fetch type information table */
-static const struct fetch_type {
-	const char	*name;		/* Name of type */
-	size_t		size;		/* Byte size of type */
-	int		is_signed;	/* Signed flag */
-	print_type_func_t	print;	/* Print functions */
-	const char	*fmt;		/* Fromat string */
-	const char	*fmttype;	/* Name in format file */
-	/* Fetch functions */
-	fetch_func_t	fetch[FETCH_MTD_END];
-} fetch_type_table[] = {
-	/* Special types */
-	[FETCH_TYPE_STRING] = __ASSIGN_FETCH_TYPE("string", string, string,
-					sizeof(u32), 1, "__data_loc char[]"),
-	[FETCH_TYPE_STRSIZE] = __ASSIGN_FETCH_TYPE("string_size", u32,
-					string_size, sizeof(u32), 0, "u32"),
-	/* Basic types */
-	ASSIGN_FETCH_TYPE(u8,  u8,  0),
-	ASSIGN_FETCH_TYPE(u16, u16, 0),
-	ASSIGN_FETCH_TYPE(u32, u32, 0),
-	ASSIGN_FETCH_TYPE(u64, u64, 0),
-	ASSIGN_FETCH_TYPE(s8,  u8,  1),
-	ASSIGN_FETCH_TYPE(s16, u16, 1),
-	ASSIGN_FETCH_TYPE(s32, u32, 1),
-	ASSIGN_FETCH_TYPE(s64, u64, 1),
-};
-
-static const struct fetch_type *find_fetch_type(const char *type)
-{
-	int i;
-
-	if (!type)
-		type = DEFAULT_FETCH_TYPE_STR;
-
-	/* Special case: bitfield */
-	if (*type == 'b') {
-		unsigned long bs;
-		type = strchr(type, '/');
-		if (!type)
-			goto fail;
-		type++;
-		if (strict_strtoul(type, 0, &bs))
-			goto fail;
-		switch (bs) {
-		case 8:
-			return find_fetch_type("u8");
-		case 16:
-			return find_fetch_type("u16");
-		case 32:
-			return find_fetch_type("u32");
-		case 64:
-			return find_fetch_type("u64");
-		default:
-			goto fail;
-		}
-	}
-
-	for (i = 0; i < ARRAY_SIZE(fetch_type_table); i++)
-		if (strcmp(type, fetch_type_table[i].name) == 0)
-			return &fetch_type_table[i];
-fail:
-	return NULL;
-}
-
-/* Special function : only accept unsigned long */
-static __kprobes void fetch_stack_address(struct pt_regs *regs,
-					  void *dummy, void *dest)
-{
-	*(unsigned long *)dest = kernel_stack_pointer(regs);
-}
-
-static fetch_func_t get_fetch_size_function(const struct fetch_type *type,
-					    fetch_func_t orig_fn)
-{
-	int i;
-
-	if (type != &fetch_type_table[FETCH_TYPE_STRING])
-		return NULL;	/* Only string type needs size function */
-	for (i = 0; i < FETCH_MTD_END; i++)
-		if (type->fetch[i] == orig_fn)
-			return fetch_type_table[FETCH_TYPE_STRSIZE].fetch[i];
-
-	WARN_ON(1);	/* This should not happen */
-	return NULL;
-}
+#define KPROBE_EVENT_SYSTEM "kprobes"
 
 /**
  * Kprobe event core functions
  */
 
-struct probe_arg {
-	struct fetch_param	fetch;
-	struct fetch_param	fetch_size;
-	unsigned int		offset;	/* Offset from argument entry */
-	const char		*name;	/* Name of this argument */
-	const char		*comm;	/* Command of this argument */
-	const struct fetch_type	*type;	/* Type of this argument */
-};
-
-/* Flags for trace_probe */
-#define TP_FLAG_TRACE	1
-#define TP_FLAG_PROFILE	2
-#define TP_FLAG_REGISTERED 4
-
 struct trace_probe {
 	struct list_head	list;
 	struct kretprobe	rp;	/* Use rp.kp for kprobe use */
@@ -631,18 +99,6 @@ static int kprobe_dispatcher(struct kprobe *kp, struct pt_regs *regs);
 static int kretprobe_dispatcher(struct kretprobe_instance *ri,
 				struct pt_regs *regs);
 
-/* Check the name is good for event/group/fields */
-static int is_good_name(const char *name)
-{
-	if (!isalpha(*name) && *name != '_')
-		return 0;
-	while (*++name != '\0') {
-		if (!isalpha(*name) && !isdigit(*name) && *name != '_')
-			return 0;
-	}
-	return 1;
-}
-
 /*
  * Allocate new trace_probe and initialize it (including kprobes).
  */
@@ -651,7 +107,7 @@ static struct trace_probe *alloc_trace_probe(const char *group,
 					     void *addr,
 					     const char *symbol,
 					     unsigned long offs,
-					     int nargs, int is_return)
+					     int nargs, bool is_return)
 {
 	struct trace_probe *tp;
 	int ret = -ENOMEM;
@@ -702,34 +158,12 @@ error:
 	return ERR_PTR(ret);
 }
 
-static void update_probe_arg(struct probe_arg *arg)
-{
-	if (CHECK_FETCH_FUNCS(bitfield, arg->fetch.fn))
-		update_bitfield_fetch_param(arg->fetch.data);
-	else if (CHECK_FETCH_FUNCS(deref, arg->fetch.fn))
-		update_deref_fetch_param(arg->fetch.data);
-	else if (CHECK_FETCH_FUNCS(symbol, arg->fetch.fn))
-		update_symbol_cache(arg->fetch.data);
-}
-
-static void free_probe_arg(struct probe_arg *arg)
-{
-	if (CHECK_FETCH_FUNCS(bitfield, arg->fetch.fn))
-		free_bitfield_fetch_param(arg->fetch.data);
-	else if (CHECK_FETCH_FUNCS(deref, arg->fetch.fn))
-		free_deref_fetch_param(arg->fetch.data);
-	else if (CHECK_FETCH_FUNCS(symbol, arg->fetch.fn))
-		free_symbol_cache(arg->fetch.data);
-	kfree(arg->name);
-	kfree(arg->comm);
-}
-
 static void free_trace_probe(struct trace_probe *tp)
 {
 	int i;
 
 	for (i = 0; i < tp->nr_args; i++)
-		free_probe_arg(&tp->args[i]);
+		traceprobe_free_probe_arg(&tp->args[i]);
 
 	kfree(tp->call.class->system);
 	kfree(tp->call.name);
@@ -787,7 +221,7 @@ static int __register_trace_probe(struct trace_probe *tp)
 		return -EINVAL;
 
 	for (i = 0; i < tp->nr_args; i++)
-		update_probe_arg(&tp->args[i]);
+		traceprobe_update_arg(&tp->args[i]);
 
 	/* Set/clear disabled flag according to tp->flag */
 	if (trace_probe_is_enabled(tp))
@@ -919,227 +353,6 @@ static struct notifier_block trace_probe_module_nb = {
 	.priority = 1	/* Invoked after kprobe module callback */
 };
 
-/* Split symbol and offset. */
-static int split_symbol_offset(char *symbol, unsigned long *offset)
-{
-	char *tmp;
-	int ret;
-
-	if (!offset)
-		return -EINVAL;
-
-	tmp = strchr(symbol, '+');
-	if (tmp) {
-		/* skip sign because strict_strtol doesn't accept '+' */
-		ret = strict_strtoul(tmp + 1, 0, offset);
-		if (ret)
-			return ret;
-		*tmp = '\0';
-	} else
-		*offset = 0;
-	return 0;
-}
-
-#define PARAM_MAX_ARGS 16
-#define PARAM_MAX_STACK (THREAD_SIZE / sizeof(unsigned long))
-
-static int parse_probe_vars(char *arg, const struct fetch_type *t,
-			    struct fetch_param *f, int is_return)
-{
-	int ret = 0;
-	unsigned long param;
-
-	if (strcmp(arg, "retval") == 0) {
-		if (is_return)
-			f->fn = t->fetch[FETCH_MTD_retval];
-		else
-			ret = -EINVAL;
-	} else if (strncmp(arg, "stack", 5) == 0) {
-		if (arg[5] == '\0') {
-			if (strcmp(t->name, DEFAULT_FETCH_TYPE_STR) == 0)
-				f->fn = fetch_stack_address;
-			else
-				ret = -EINVAL;
-		} else if (isdigit(arg[5])) {
-			ret = strict_strtoul(arg + 5, 10, &param);
-			if (ret || param > PARAM_MAX_STACK)
-				ret = -EINVAL;
-			else {
-				f->fn = t->fetch[FETCH_MTD_stack];
-				f->data = (void *)param;
-			}
-		} else
-			ret = -EINVAL;
-	} else
-		ret = -EINVAL;
-	return ret;
-}
-
-/* Recursive argument parser */
-static int __parse_probe_arg(char *arg, const struct fetch_type *t,
-			     struct fetch_param *f, int is_return)
-{
-	int ret = 0;
-	unsigned long param;
-	long offset;
-	char *tmp;
-
-	switch (arg[0]) {
-	case '$':
-		ret = parse_probe_vars(arg + 1, t, f, is_return);
-		break;
-	case '%':	/* named register */
-		ret = regs_query_register_offset(arg + 1);
-		if (ret >= 0) {
-			f->fn = t->fetch[FETCH_MTD_reg];
-			f->data = (void *)(unsigned long)ret;
-			ret = 0;
-		}
-		break;
-	case '@':	/* memory or symbol */
-		if (isdigit(arg[1])) {
-			ret = strict_strtoul(arg + 1, 0, &param);
-			if (ret)
-				break;
-			f->fn = t->fetch[FETCH_MTD_memory];
-			f->data = (void *)param;
-		} else {
-			ret = split_symbol_offset(arg + 1, &offset);
-			if (ret)
-				break;
-			f->data = alloc_symbol_cache(arg + 1, offset);
-			if (f->data)
-				f->fn = t->fetch[FETCH_MTD_symbol];
-		}
-		break;
-	case '+':	/* deref memory */
-		arg++;	/* Skip '+', because strict_strtol() rejects it. */
-	case '-':
-		tmp = strchr(arg, '(');
-		if (!tmp)
-			break;
-		*tmp = '\0';
-		ret = strict_strtol(arg, 0, &offset);
-		if (ret)
-			break;
-		arg = tmp + 1;
-		tmp = strrchr(arg, ')');
-		if (tmp) {
-			struct deref_fetch_param *dprm;
-			const struct fetch_type *t2 = find_fetch_type(NULL);
-			*tmp = '\0';
-			dprm = kzalloc(sizeof(struct deref_fetch_param),
-				       GFP_KERNEL);
-			if (!dprm)
-				return -ENOMEM;
-			dprm->offset = offset;
-			ret = __parse_probe_arg(arg, t2, &dprm->orig,
-						is_return);
-			if (ret)
-				kfree(dprm);
-			else {
-				f->fn = t->fetch[FETCH_MTD_deref];
-				f->data = (void *)dprm;
-			}
-		}
-		break;
-	}
-	if (!ret && !f->fn) {	/* Parsed, but do not find fetch method */
-		pr_info("%s type has no corresponding fetch method.\n",
-			t->name);
-		ret = -EINVAL;
-	}
-	return ret;
-}
-
-#define BYTES_TO_BITS(nb)	((BITS_PER_LONG * (nb)) / sizeof(long))
-
-/* Bitfield type needs to be parsed into a fetch function */
-static int __parse_bitfield_probe_arg(const char *bf,
-				      const struct fetch_type *t,
-				      struct fetch_param *f)
-{
-	struct bitfield_fetch_param *bprm;
-	unsigned long bw, bo;
-	char *tail;
-
-	if (*bf != 'b')
-		return 0;
-
-	bprm = kzalloc(sizeof(*bprm), GFP_KERNEL);
-	if (!bprm)
-		return -ENOMEM;
-	bprm->orig = *f;
-	f->fn = t->fetch[FETCH_MTD_bitfield];
-	f->data = (void *)bprm;
-
-	bw = simple_strtoul(bf + 1, &tail, 0);	/* Use simple one */
-	if (bw == 0 || *tail != '@')
-		return -EINVAL;
-
-	bf = tail + 1;
-	bo = simple_strtoul(bf, &tail, 0);
-	if (tail == bf || *tail != '/')
-		return -EINVAL;
-
-	bprm->hi_shift = BYTES_TO_BITS(t->size) - (bw + bo);
-	bprm->low_shift = bprm->hi_shift + bo;
-	return (BYTES_TO_BITS(t->size) < (bw + bo)) ? -EINVAL : 0;
-}
-
-/* String length checking wrapper */
-static int parse_probe_arg(char *arg, struct trace_probe *tp,
-			   struct probe_arg *parg, int is_return)
-{
-	const char *t;
-	int ret;
-
-	if (strlen(arg) > MAX_ARGSTR_LEN) {
-		pr_info("Argument is too long.: %s\n",  arg);
-		return -ENOSPC;
-	}
-	parg->comm = kstrdup(arg, GFP_KERNEL);
-	if (!parg->comm) {
-		pr_info("Failed to allocate memory for command '%s'.\n", arg);
-		return -ENOMEM;
-	}
-	t = strchr(parg->comm, ':');
-	if (t) {
-		arg[t - parg->comm] = '\0';
-		t++;
-	}
-	parg->type = find_fetch_type(t);
-	if (!parg->type) {
-		pr_info("Unsupported type: %s\n", t);
-		return -EINVAL;
-	}
-	parg->offset = tp->size;
-	tp->size += parg->type->size;
-	ret = __parse_probe_arg(arg, parg->type, &parg->fetch, is_return);
-	if (ret >= 0 && t != NULL)
-		ret = __parse_bitfield_probe_arg(t, parg->type, &parg->fetch);
-	if (ret >= 0) {
-		parg->fetch_size.fn = get_fetch_size_function(parg->type,
-							      parg->fetch.fn);
-		parg->fetch_size.data = parg->fetch.data;
-	}
-	return ret;
-}
-
-/* Return 1 if name is reserved or already used by another argument */
-static int conflict_field_name(const char *name,
-			       struct probe_arg *args, int narg)
-{
-	int i;
-	for (i = 0; i < ARRAY_SIZE(reserved_field_names); i++)
-		if (strcmp(reserved_field_names[i], name) == 0)
-			return 1;
-	for (i = 0; i < narg; i++)
-		if (strcmp(args[i].name, name) == 0)
-			return 1;
-	return 0;
-}
-
 static int create_trace_probe(int argc, char **argv)
 {
 	/*
@@ -1162,7 +375,7 @@ static int create_trace_probe(int argc, char **argv)
 	 */
 	struct trace_probe *tp;
 	int i, ret = 0;
-	int is_return = 0, is_delete = 0;
+	bool is_return = false, is_delete = false;
 	char *symbol = NULL, *event = NULL, *group = NULL;
 	char *arg;
 	unsigned long offset = 0;
@@ -1171,11 +384,11 @@ static int create_trace_probe(int argc, char **argv)
 
 	/* argc must be >= 1 */
 	if (argv[0][0] == 'p')
-		is_return = 0;
+		is_return = false;
 	else if (argv[0][0] == 'r')
-		is_return = 1;
+		is_return = true;
 	else if (argv[0][0] == '-')
-		is_delete = 1;
+		is_delete = true;
 	else {
 		pr_info("Probe definition must be started with 'p', 'r' or"
 			" '-'.\n");
@@ -1240,7 +453,7 @@ static int create_trace_probe(int argc, char **argv)
 		/* a symbol specified */
 		symbol = argv[1];
 		/* TODO: support .init module functions */
-		ret = split_symbol_offset(symbol, &offset);
+		ret = traceprobe_split_symbol_offset(symbol, &offset);
 		if (ret) {
 			pr_info("Failed to parse symbol.\n");
 			return ret;
@@ -1302,7 +515,8 @@ static int create_trace_probe(int argc, char **argv)
 			goto error;
 		}
 
-		if (conflict_field_name(tp->args[i].name, tp->args, i)) {
+		if (traceprobe_conflict_field_name(tp->args[i].name,
+							tp->args, i)) {
 			pr_info("Argument[%d] name '%s' conflicts with "
 				"another field.\n", i, argv[i]);
 			ret = -EINVAL;
@@ -1310,7 +524,8 @@ static int create_trace_probe(int argc, char **argv)
 		}
 
 		/* Parse fetch argument */
-		ret = parse_probe_arg(arg, tp, &tp->args[i], is_return);
+		ret = traceprobe_parse_probe_arg(arg, &tp->size, &tp->args[i],
+						is_return, true);
 		if (ret) {
 			pr_info("Parse error at argument[%d]. (%d)\n", i, ret);
 			goto error;
@@ -1412,70 +627,11 @@ static int probes_open(struct inode *inode, struct file *file)
 	return seq_open(file, &probes_seq_op);
 }
 
-static int command_trace_probe(const char *buf)
-{
-	char **argv;
-	int argc = 0, ret = 0;
-
-	argv = argv_split(GFP_KERNEL, buf, &argc);
-	if (!argv)
-		return -ENOMEM;
-
-	if (argc)
-		ret = create_trace_probe(argc, argv);
-
-	argv_free(argv);
-	return ret;
-}
-
-#define WRITE_BUFSIZE 4096
-
 static ssize_t probes_write(struct file *file, const char __user *buffer,
 			    size_t count, loff_t *ppos)
 {
-	char *kbuf, *tmp;
-	int ret;
-	size_t done;
-	size_t size;
-
-	kbuf = kmalloc(WRITE_BUFSIZE, GFP_KERNEL);
-	if (!kbuf)
-		return -ENOMEM;
-
-	ret = done = 0;
-	while (done < count) {
-		size = count - done;
-		if (size >= WRITE_BUFSIZE)
-			size = WRITE_BUFSIZE - 1;
-		if (copy_from_user(kbuf, buffer + done, size)) {
-			ret = -EFAULT;
-			goto out;
-		}
-		kbuf[size] = '\0';
-		tmp = strchr(kbuf, '\n');
-		if (tmp) {
-			*tmp = '\0';
-			size = tmp - kbuf + 1;
-		} else if (done + size < count) {
-			pr_warning("Line length is too long: "
-				   "Should be less than %d.", WRITE_BUFSIZE);
-			ret = -EINVAL;
-			goto out;
-		}
-		done += size;
-		/* Remove comments */
-		tmp = strchr(kbuf, '#');
-		if (tmp)
-			*tmp = '\0';
-
-		ret = command_trace_probe(kbuf);
-		if (ret)
-			goto out;
-	}
-	ret = done;
-out:
-	kfree(kbuf);
-	return ret;
+	return traceprobe_probes_write(file, buffer, count, ppos,
+			create_trace_probe);
 }
 
 static const struct file_operations kprobe_events_ops = {
@@ -1711,16 +867,6 @@ partial:
 	return TRACE_TYPE_PARTIAL_LINE;
 }
 
-#undef DEFINE_FIELD
-#define DEFINE_FIELD(type, item, name, is_signed)			\
-	do {								\
-		ret = trace_define_field(event_call, #type, name,	\
-					 offsetof(typeof(field), item),	\
-					 sizeof(field.item), is_signed, \
-					 FILTER_OTHER);			\
-		if (ret)						\
-			return ret;					\
-	} while (0)
 
 static int kprobe_event_define_fields(struct ftrace_event_call *event_call)
 {
@@ -2051,8 +1197,9 @@ static __init int kprobe_trace_self_tests_init(void)
 
 	pr_info("Testing kprobe tracing: ");
 
-	ret = command_trace_probe("p:testprobe kprobe_trace_selftest_target "
-				  "$stack $stack0 +0($stack)");
+	ret = traceprobe_command("p:testprobe kprobe_trace_selftest_target "
+				  "$stack $stack0 +0($stack)",
+				  create_trace_probe);
 	if (WARN_ON_ONCE(ret)) {
 		pr_warning("error on probing function entry.\n");
 		warn++;
@@ -2066,8 +1213,8 @@ static __init int kprobe_trace_self_tests_init(void)
 			enable_trace_probe(tp, TP_FLAG_TRACE);
 	}
 
-	ret = command_trace_probe("r:testprobe2 kprobe_trace_selftest_target "
-				  "$retval");
+	ret = traceprobe_command("r:testprobe2 kprobe_trace_selftest_target "
+				  "$retval", create_trace_probe);
 	if (WARN_ON_ONCE(ret)) {
 		pr_warning("error on probing function return.\n");
 		warn++;
@@ -2101,13 +1248,13 @@ static __init int kprobe_trace_self_tests_init(void)
 	} else
 		disable_trace_probe(tp, TP_FLAG_TRACE);
 
-	ret = command_trace_probe("-:testprobe");
+	ret = traceprobe_command("-:testprobe", create_trace_probe);
 	if (WARN_ON_ONCE(ret)) {
 		pr_warning("error on deleting a probe.\n");
 		warn++;
 	}
 
-	ret = command_trace_probe("-:testprobe2");
+	ret = traceprobe_command("-:testprobe2", create_trace_probe);
 	if (WARN_ON_ONCE(ret)) {
 		pr_warning("error on deleting a probe.\n");
 		warn++;
diff --git a/kernel/trace/trace_probe.c b/kernel/trace/trace_probe.c
new file mode 100644
index 000000000000..daa9980153af
--- /dev/null
+++ b/kernel/trace/trace_probe.c
@@ -0,0 +1,839 @@
+/*
+ * Common code for probe-based Dynamic events.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * This code was copied from kernel/trace/trace_kprobe.c written by
+ * Masami Hiramatsu <masami.hiramatsu.pt@hitachi.com>
+ *
+ * Updates to make this generic:
+ * Copyright (C) IBM Corporation, 2010-2011
+ * Author:     Srikar Dronamraju
+ */
+
+#include "trace_probe.h"
+
+const char *reserved_field_names[] = {
+	"common_type",
+	"common_flags",
+	"common_preempt_count",
+	"common_pid",
+	"common_tgid",
+	FIELD_STRING_IP,
+	FIELD_STRING_RETIP,
+	FIELD_STRING_FUNC,
+};
+
+/* Printing function type */
+#define PRINT_TYPE_FUNC_NAME(type)	print_type_##type
+#define PRINT_TYPE_FMT_NAME(type)	print_type_format_##type
+
+/* Printing  in basic type function template */
+#define DEFINE_BASIC_PRINT_TYPE_FUNC(type, fmt, cast)			\
+static __kprobes int PRINT_TYPE_FUNC_NAME(type)(struct trace_seq *s,	\
+						const char *name,	\
+						void *data, void *ent)\
+{									\
+	return trace_seq_printf(s, " %s=" fmt, name, (cast)*(type *)data);\
+}									\
+static const char PRINT_TYPE_FMT_NAME(type)[] = fmt;
+
+DEFINE_BASIC_PRINT_TYPE_FUNC(u8, "%x", unsigned int)
+DEFINE_BASIC_PRINT_TYPE_FUNC(u16, "%x", unsigned int)
+DEFINE_BASIC_PRINT_TYPE_FUNC(u32, "%lx", unsigned long)
+DEFINE_BASIC_PRINT_TYPE_FUNC(u64, "%llx", unsigned long long)
+DEFINE_BASIC_PRINT_TYPE_FUNC(s8, "%d", int)
+DEFINE_BASIC_PRINT_TYPE_FUNC(s16, "%d", int)
+DEFINE_BASIC_PRINT_TYPE_FUNC(s32, "%ld", long)
+DEFINE_BASIC_PRINT_TYPE_FUNC(s64, "%lld", long long)
+
+static inline void *get_rloc_data(u32 *dl)
+{
+	return (u8 *)dl + get_rloc_offs(*dl);
+}
+
+/* For data_loc conversion */
+static inline void *get_loc_data(u32 *dl, void *ent)
+{
+	return (u8 *)ent + get_rloc_offs(*dl);
+}
+
+/* For defining macros, define string/string_size types */
+typedef u32 string;
+typedef u32 string_size;
+
+/* Print type function for string type */
+static __kprobes int PRINT_TYPE_FUNC_NAME(string)(struct trace_seq *s,
+						  const char *name,
+						  void *data, void *ent)
+{
+	int len = *(u32 *)data >> 16;
+
+	if (!len)
+		return trace_seq_printf(s, " %s=(fault)", name);
+	else
+		return trace_seq_printf(s, " %s=\"%s\"", name,
+					(const char *)get_loc_data(data, ent));
+}
+
+static const char PRINT_TYPE_FMT_NAME(string)[] = "\\\"%s\\\"";
+
+#define FETCH_FUNC_NAME(method, type)	fetch_##method##_##type
+/*
+ * Define macro for basic types - we don't need to define s* types, because
+ * we have to care only about bitwidth at recording time.
+ */
+#define DEFINE_BASIC_FETCH_FUNCS(method) \
+DEFINE_FETCH_##method(u8)		\
+DEFINE_FETCH_##method(u16)		\
+DEFINE_FETCH_##method(u32)		\
+DEFINE_FETCH_##method(u64)
+
+#define CHECK_FETCH_FUNCS(method, fn)			\
+	(((FETCH_FUNC_NAME(method, u8) == fn) ||	\
+	  (FETCH_FUNC_NAME(method, u16) == fn) ||	\
+	  (FETCH_FUNC_NAME(method, u32) == fn) ||	\
+	  (FETCH_FUNC_NAME(method, u64) == fn) ||	\
+	  (FETCH_FUNC_NAME(method, string) == fn) ||	\
+	  (FETCH_FUNC_NAME(method, string_size) == fn)) \
+	 && (fn != NULL))
+
+/* Data fetch function templates */
+#define DEFINE_FETCH_reg(type)						\
+static __kprobes void FETCH_FUNC_NAME(reg, type)(struct pt_regs *regs,	\
+					void *offset, void *dest)	\
+{									\
+	*(type *)dest = (type)regs_get_register(regs,			\
+				(unsigned int)((unsigned long)offset));	\
+}
+DEFINE_BASIC_FETCH_FUNCS(reg)
+/* No string on the register */
+#define fetch_reg_string	NULL
+#define fetch_reg_string_size	NULL
+
+#define DEFINE_FETCH_stack(type)					\
+static __kprobes void FETCH_FUNC_NAME(stack, type)(struct pt_regs *regs,\
+					  void *offset, void *dest)	\
+{									\
+	*(type *)dest = (type)regs_get_kernel_stack_nth(regs,		\
+				(unsigned int)((unsigned long)offset));	\
+}
+DEFINE_BASIC_FETCH_FUNCS(stack)
+/* No string on the stack entry */
+#define fetch_stack_string	NULL
+#define fetch_stack_string_size	NULL
+
+#define DEFINE_FETCH_retval(type)					\
+static __kprobes void FETCH_FUNC_NAME(retval, type)(struct pt_regs *regs,\
+					  void *dummy, void *dest)	\
+{									\
+	*(type *)dest = (type)regs_return_value(regs);			\
+}
+DEFINE_BASIC_FETCH_FUNCS(retval)
+/* No string on the retval */
+#define fetch_retval_string		NULL
+#define fetch_retval_string_size	NULL
+
+#define DEFINE_FETCH_memory(type)					\
+static __kprobes void FETCH_FUNC_NAME(memory, type)(struct pt_regs *regs,\
+					  void *addr, void *dest)	\
+{									\
+	type retval;							\
+	if (probe_kernel_address(addr, retval))				\
+		*(type *)dest = 0;					\
+	else								\
+		*(type *)dest = retval;					\
+}
+DEFINE_BASIC_FETCH_FUNCS(memory)
+/*
+ * Fetch a null-terminated string. Caller MUST set *(u32 *)dest with max
+ * length and relative data location.
+ */
+static __kprobes void FETCH_FUNC_NAME(memory, string)(struct pt_regs *regs,
+						      void *addr, void *dest)
+{
+	long ret;
+	int maxlen = get_rloc_len(*(u32 *)dest);
+	u8 *dst = get_rloc_data(dest);
+	u8 *src = addr;
+	mm_segment_t old_fs = get_fs();
+
+	if (!maxlen)
+		return;
+
+	/*
+	 * Try to get string again, since the string can be changed while
+	 * probing.
+	 */
+	set_fs(KERNEL_DS);
+	pagefault_disable();
+
+	do
+		ret = __copy_from_user_inatomic(dst++, src++, 1);
+	while (dst[-1] && ret == 0 && src - (u8 *)addr < maxlen);
+
+	dst[-1] = '\0';
+	pagefault_enable();
+	set_fs(old_fs);
+
+	if (ret < 0) {	/* Failed to fetch string */
+		((u8 *)get_rloc_data(dest))[0] = '\0';
+		*(u32 *)dest = make_data_rloc(0, get_rloc_offs(*(u32 *)dest));
+	} else {
+		*(u32 *)dest = make_data_rloc(src - (u8 *)addr,
+					      get_rloc_offs(*(u32 *)dest));
+	}
+}
+
+/* Return the length of string -- including null terminal byte */
+static __kprobes void FETCH_FUNC_NAME(memory, string_size)(struct pt_regs *regs,
+							void *addr, void *dest)
+{
+	mm_segment_t old_fs;
+	int ret, len = 0;
+	u8 c;
+
+	old_fs = get_fs();
+	set_fs(KERNEL_DS);
+	pagefault_disable();
+
+	do {
+		ret = __copy_from_user_inatomic(&c, (u8 *)addr + len, 1);
+		len++;
+	} while (c && ret == 0 && len < MAX_STRING_SIZE);
+
+	pagefault_enable();
+	set_fs(old_fs);
+
+	if (ret < 0)	/* Failed to check the length */
+		*(u32 *)dest = 0;
+	else
+		*(u32 *)dest = len;
+}
+
+/* Memory fetching by symbol */
+struct symbol_cache {
+	char		*symbol;
+	long		offset;
+	unsigned long	addr;
+};
+
+static unsigned long update_symbol_cache(struct symbol_cache *sc)
+{
+	sc->addr = (unsigned long)kallsyms_lookup_name(sc->symbol);
+
+	if (sc->addr)
+		sc->addr += sc->offset;
+
+	return sc->addr;
+}
+
+static void free_symbol_cache(struct symbol_cache *sc)
+{
+	kfree(sc->symbol);
+	kfree(sc);
+}
+
+static struct symbol_cache *alloc_symbol_cache(const char *sym, long offset)
+{
+	struct symbol_cache *sc;
+
+	if (!sym || strlen(sym) == 0)
+		return NULL;
+
+	sc = kzalloc(sizeof(struct symbol_cache), GFP_KERNEL);
+	if (!sc)
+		return NULL;
+
+	sc->symbol = kstrdup(sym, GFP_KERNEL);
+	if (!sc->symbol) {
+		kfree(sc);
+		return NULL;
+	}
+	sc->offset = offset;
+	update_symbol_cache(sc);
+
+	return sc;
+}
+
+#define DEFINE_FETCH_symbol(type)					\
+static __kprobes void FETCH_FUNC_NAME(symbol, type)(struct pt_regs *regs,\
+					  void *data, void *dest)	\
+{									\
+	struct symbol_cache *sc = data;					\
+	if (sc->addr)							\
+		fetch_memory_##type(regs, (void *)sc->addr, dest);	\
+	else								\
+		*(type *)dest = 0;					\
+}
+DEFINE_BASIC_FETCH_FUNCS(symbol)
+DEFINE_FETCH_symbol(string)
+DEFINE_FETCH_symbol(string_size)
+
+/* Dereference memory access function */
+struct deref_fetch_param {
+	struct fetch_param	orig;
+	long			offset;
+};
+
+#define DEFINE_FETCH_deref(type)					\
+static __kprobes void FETCH_FUNC_NAME(deref, type)(struct pt_regs *regs,\
+					    void *data, void *dest)	\
+{									\
+	struct deref_fetch_param *dprm = data;				\
+	unsigned long addr;						\
+	call_fetch(&dprm->orig, regs, &addr);				\
+	if (addr) {							\
+		addr += dprm->offset;					\
+		fetch_memory_##type(regs, (void *)addr, dest);		\
+	} else								\
+		*(type *)dest = 0;					\
+}
+DEFINE_BASIC_FETCH_FUNCS(deref)
+DEFINE_FETCH_deref(string)
+DEFINE_FETCH_deref(string_size)
+
+static __kprobes void update_deref_fetch_param(struct deref_fetch_param *data)
+{
+	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
+		update_deref_fetch_param(data->orig.data);
+	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
+		update_symbol_cache(data->orig.data);
+}
+
+static __kprobes void free_deref_fetch_param(struct deref_fetch_param *data)
+{
+	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
+		free_deref_fetch_param(data->orig.data);
+	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
+		free_symbol_cache(data->orig.data);
+	kfree(data);
+}
+
+/* Bitfield fetch function */
+struct bitfield_fetch_param {
+	struct fetch_param	orig;
+	unsigned char		hi_shift;
+	unsigned char		low_shift;
+};
+
+#define DEFINE_FETCH_bitfield(type)					\
+static __kprobes void FETCH_FUNC_NAME(bitfield, type)(struct pt_regs *regs,\
+					    void *data, void *dest)	\
+{									\
+	struct bitfield_fetch_param *bprm = data;			\
+	type buf = 0;							\
+	call_fetch(&bprm->orig, regs, &buf);				\
+	if (buf) {							\
+		buf <<= bprm->hi_shift;					\
+		buf >>= bprm->low_shift;				\
+	}								\
+	*(type *)dest = buf;						\
+}
+
+DEFINE_BASIC_FETCH_FUNCS(bitfield)
+#define fetch_bitfield_string		NULL
+#define fetch_bitfield_string_size	NULL
+
+static __kprobes void
+update_bitfield_fetch_param(struct bitfield_fetch_param *data)
+{
+	/*
+	 * Don't check the bitfield itself, because this must be the
+	 * last fetch function.
+	 */
+	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
+		update_deref_fetch_param(data->orig.data);
+	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
+		update_symbol_cache(data->orig.data);
+}
+
+static __kprobes void
+free_bitfield_fetch_param(struct bitfield_fetch_param *data)
+{
+	/*
+	 * Don't check the bitfield itself, because this must be the
+	 * last fetch function.
+	 */
+	if (CHECK_FETCH_FUNCS(deref, data->orig.fn))
+		free_deref_fetch_param(data->orig.data);
+	else if (CHECK_FETCH_FUNCS(symbol, data->orig.fn))
+		free_symbol_cache(data->orig.data);
+
+	kfree(data);
+}
+
+/* Default (unsigned long) fetch type */
+#define __DEFAULT_FETCH_TYPE(t) u##t
+#define _DEFAULT_FETCH_TYPE(t) __DEFAULT_FETCH_TYPE(t)
+#define DEFAULT_FETCH_TYPE _DEFAULT_FETCH_TYPE(BITS_PER_LONG)
+#define DEFAULT_FETCH_TYPE_STR __stringify(DEFAULT_FETCH_TYPE)
+
+#define ASSIGN_FETCH_FUNC(method, type)	\
+	[FETCH_MTD_##method] = FETCH_FUNC_NAME(method, type)
+
+#define __ASSIGN_FETCH_TYPE(_name, ptype, ftype, _size, sign, _fmttype)	\
+	{.name = _name,				\
+	 .size = _size,					\
+	 .is_signed = sign,				\
+	 .print = PRINT_TYPE_FUNC_NAME(ptype),		\
+	 .fmt = PRINT_TYPE_FMT_NAME(ptype),		\
+	 .fmttype = _fmttype,				\
+	 .fetch = {					\
+ASSIGN_FETCH_FUNC(reg, ftype),				\
+ASSIGN_FETCH_FUNC(stack, ftype),			\
+ASSIGN_FETCH_FUNC(retval, ftype),			\
+ASSIGN_FETCH_FUNC(memory, ftype),			\
+ASSIGN_FETCH_FUNC(symbol, ftype),			\
+ASSIGN_FETCH_FUNC(deref, ftype),			\
+ASSIGN_FETCH_FUNC(bitfield, ftype),			\
+	  }						\
+	}
+
+#define ASSIGN_FETCH_TYPE(ptype, ftype, sign)			\
+	__ASSIGN_FETCH_TYPE(#ptype, ptype, ftype, sizeof(ftype), sign, #ptype)
+
+#define FETCH_TYPE_STRING	0
+#define FETCH_TYPE_STRSIZE	1
+
+/* Fetch type information table */
+static const struct fetch_type fetch_type_table[] = {
+	/* Special types */
+	[FETCH_TYPE_STRING] = __ASSIGN_FETCH_TYPE("string", string, string,
+					sizeof(u32), 1, "__data_loc char[]"),
+	[FETCH_TYPE_STRSIZE] = __ASSIGN_FETCH_TYPE("string_size", u32,
+					string_size, sizeof(u32), 0, "u32"),
+	/* Basic types */
+	ASSIGN_FETCH_TYPE(u8,  u8,  0),
+	ASSIGN_FETCH_TYPE(u16, u16, 0),
+	ASSIGN_FETCH_TYPE(u32, u32, 0),
+	ASSIGN_FETCH_TYPE(u64, u64, 0),
+	ASSIGN_FETCH_TYPE(s8,  u8,  1),
+	ASSIGN_FETCH_TYPE(s16, u16, 1),
+	ASSIGN_FETCH_TYPE(s32, u32, 1),
+	ASSIGN_FETCH_TYPE(s64, u64, 1),
+};
+
+static const struct fetch_type *find_fetch_type(const char *type)
+{
+	int i;
+
+	if (!type)
+		type = DEFAULT_FETCH_TYPE_STR;
+
+	/* Special case: bitfield */
+	if (*type == 'b') {
+		unsigned long bs;
+
+		type = strchr(type, '/');
+		if (!type)
+			goto fail;
+
+		type++;
+		if (strict_strtoul(type, 0, &bs))
+			goto fail;
+
+		switch (bs) {
+		case 8:
+			return find_fetch_type("u8");
+		case 16:
+			return find_fetch_type("u16");
+		case 32:
+			return find_fetch_type("u32");
+		case 64:
+			return find_fetch_type("u64");
+		default:
+			goto fail;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(fetch_type_table); i++)
+		if (strcmp(type, fetch_type_table[i].name) == 0)
+			return &fetch_type_table[i];
+
+fail:
+	return NULL;
+}
+
+/* Special function : only accept unsigned long */
+static __kprobes void fetch_stack_address(struct pt_regs *regs,
+					void *dummy, void *dest)
+{
+	*(unsigned long *)dest = kernel_stack_pointer(regs);
+}
+
+static fetch_func_t get_fetch_size_function(const struct fetch_type *type,
+					fetch_func_t orig_fn)
+{
+	int i;
+
+	if (type != &fetch_type_table[FETCH_TYPE_STRING])
+		return NULL;	/* Only string type needs size function */
+
+	for (i = 0; i < FETCH_MTD_END; i++)
+		if (type->fetch[i] == orig_fn)
+			return fetch_type_table[FETCH_TYPE_STRSIZE].fetch[i];
+
+	WARN_ON(1);	/* This should not happen */
+
+	return NULL;
+}
+
+/* Split symbol and offset. */
+int traceprobe_split_symbol_offset(char *symbol, unsigned long *offset)
+{
+	char *tmp;
+	int ret;
+
+	if (!offset)
+		return -EINVAL;
+
+	tmp = strchr(symbol, '+');
+	if (tmp) {
+		/* skip sign because strict_strtol doesn't accept '+' */
+		ret = strict_strtoul(tmp + 1, 0, offset);
+		if (ret)
+			return ret;
+
+		*tmp = '\0';
+	} else
+		*offset = 0;
+
+	return 0;
+}
+
+#define PARAM_MAX_STACK (THREAD_SIZE / sizeof(unsigned long))
+
+static int parse_probe_vars(char *arg, const struct fetch_type *t,
+			    struct fetch_param *f, bool is_return)
+{
+	int ret = 0;
+	unsigned long param;
+
+	if (strcmp(arg, "retval") == 0) {
+		if (is_return)
+			f->fn = t->fetch[FETCH_MTD_retval];
+		else
+			ret = -EINVAL;
+	} else if (strncmp(arg, "stack", 5) == 0) {
+		if (arg[5] == '\0') {
+			if (strcmp(t->name, DEFAULT_FETCH_TYPE_STR) == 0)
+				f->fn = fetch_stack_address;
+			else
+				ret = -EINVAL;
+		} else if (isdigit(arg[5])) {
+			ret = strict_strtoul(arg + 5, 10, &param);
+			if (ret || param > PARAM_MAX_STACK)
+				ret = -EINVAL;
+			else {
+				f->fn = t->fetch[FETCH_MTD_stack];
+				f->data = (void *)param;
+			}
+		} else
+			ret = -EINVAL;
+	} else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+/* Recursive argument parser */
+static int parse_probe_arg(char *arg, const struct fetch_type *t,
+		     struct fetch_param *f, bool is_return, bool is_kprobe)
+{
+	unsigned long param;
+	long offset;
+	char *tmp;
+	int ret;
+
+	ret = 0;
+
+	/* Until uprobe_events supports only reg arguments */
+	if (!is_kprobe && arg[0] != '%')
+		return -EINVAL;
+
+	switch (arg[0]) {
+	case '$':
+		ret = parse_probe_vars(arg + 1, t, f, is_return);
+		break;
+
+	case '%':	/* named register */
+		ret = regs_query_register_offset(arg + 1);
+		if (ret >= 0) {
+			f->fn = t->fetch[FETCH_MTD_reg];
+			f->data = (void *)(unsigned long)ret;
+			ret = 0;
+		}
+		break;
+
+	case '@':	/* memory or symbol */
+		if (isdigit(arg[1])) {
+			ret = strict_strtoul(arg + 1, 0, &param);
+			if (ret)
+				break;
+
+			f->fn = t->fetch[FETCH_MTD_memory];
+			f->data = (void *)param;
+		} else {
+			ret = traceprobe_split_symbol_offset(arg + 1, &offset);
+			if (ret)
+				break;
+
+			f->data = alloc_symbol_cache(arg + 1, offset);
+			if (f->data)
+				f->fn = t->fetch[FETCH_MTD_symbol];
+		}
+		break;
+
+	case '+':	/* deref memory */
+		arg++;	/* Skip '+', because strict_strtol() rejects it. */
+	case '-':
+		tmp = strchr(arg, '(');
+		if (!tmp)
+			break;
+
+		*tmp = '\0';
+		ret = strict_strtol(arg, 0, &offset);
+
+		if (ret)
+			break;
+
+		arg = tmp + 1;
+		tmp = strrchr(arg, ')');
+
+		if (tmp) {
+			struct deref_fetch_param	*dprm;
+			const struct fetch_type		*t2;
+
+			t2 = find_fetch_type(NULL);
+			*tmp = '\0';
+			dprm = kzalloc(sizeof(struct deref_fetch_param), GFP_KERNEL);
+
+			if (!dprm)
+				return -ENOMEM;
+
+			dprm->offset = offset;
+			ret = parse_probe_arg(arg, t2, &dprm->orig, is_return,
+							is_kprobe);
+			if (ret)
+				kfree(dprm);
+			else {
+				f->fn = t->fetch[FETCH_MTD_deref];
+				f->data = (void *)dprm;
+			}
+		}
+		break;
+	}
+	if (!ret && !f->fn) {	/* Parsed, but do not find fetch method */
+		pr_info("%s type has no corresponding fetch method.\n", t->name);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+#define BYTES_TO_BITS(nb)	((BITS_PER_LONG * (nb)) / sizeof(long))
+
+/* Bitfield type needs to be parsed into a fetch function */
+static int __parse_bitfield_probe_arg(const char *bf,
+				      const struct fetch_type *t,
+				      struct fetch_param *f)
+{
+	struct bitfield_fetch_param *bprm;
+	unsigned long bw, bo;
+	char *tail;
+
+	if (*bf != 'b')
+		return 0;
+
+	bprm = kzalloc(sizeof(*bprm), GFP_KERNEL);
+	if (!bprm)
+		return -ENOMEM;
+
+	bprm->orig = *f;
+	f->fn = t->fetch[FETCH_MTD_bitfield];
+	f->data = (void *)bprm;
+	bw = simple_strtoul(bf + 1, &tail, 0);	/* Use simple one */
+
+	if (bw == 0 || *tail != '@')
+		return -EINVAL;
+
+	bf = tail + 1;
+	bo = simple_strtoul(bf, &tail, 0);
+
+	if (tail == bf || *tail != '/')
+		return -EINVAL;
+
+	bprm->hi_shift = BYTES_TO_BITS(t->size) - (bw + bo);
+	bprm->low_shift = bprm->hi_shift + bo;
+
+	return (BYTES_TO_BITS(t->size) < (bw + bo)) ? -EINVAL : 0;
+}
+
+/* String length checking wrapper */
+int traceprobe_parse_probe_arg(char *arg, ssize_t *size,
+		struct probe_arg *parg, bool is_return, bool is_kprobe)
+{
+	const char *t;
+	int ret;
+
+	if (strlen(arg) > MAX_ARGSTR_LEN) {
+		pr_info("Argument is too long.: %s\n",  arg);
+		return -ENOSPC;
+	}
+	parg->comm = kstrdup(arg, GFP_KERNEL);
+	if (!parg->comm) {
+		pr_info("Failed to allocate memory for command '%s'.\n", arg);
+		return -ENOMEM;
+	}
+	t = strchr(parg->comm, ':');
+	if (t) {
+		arg[t - parg->comm] = '\0';
+		t++;
+	}
+	parg->type = find_fetch_type(t);
+	if (!parg->type) {
+		pr_info("Unsupported type: %s\n", t);
+		return -EINVAL;
+	}
+	parg->offset = *size;
+	*size += parg->type->size;
+	ret = parse_probe_arg(arg, parg->type, &parg->fetch, is_return, is_kprobe);
+
+	if (ret >= 0 && t != NULL)
+		ret = __parse_bitfield_probe_arg(t, parg->type, &parg->fetch);
+
+	if (ret >= 0) {
+		parg->fetch_size.fn = get_fetch_size_function(parg->type,
+							      parg->fetch.fn);
+		parg->fetch_size.data = parg->fetch.data;
+	}
+
+	return ret;
+}
+
+/* Return 1 if name is reserved or already used by another argument */
+int traceprobe_conflict_field_name(const char *name,
+			       struct probe_arg *args, int narg)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(reserved_field_names); i++)
+		if (strcmp(reserved_field_names[i], name) == 0)
+			return 1;
+
+	for (i = 0; i < narg; i++)
+		if (strcmp(args[i].name, name) == 0)
+			return 1;
+
+	return 0;
+}
+
+void traceprobe_update_arg(struct probe_arg *arg)
+{
+	if (CHECK_FETCH_FUNCS(bitfield, arg->fetch.fn))
+		update_bitfield_fetch_param(arg->fetch.data);
+	else if (CHECK_FETCH_FUNCS(deref, arg->fetch.fn))
+		update_deref_fetch_param(arg->fetch.data);
+	else if (CHECK_FETCH_FUNCS(symbol, arg->fetch.fn))
+		update_symbol_cache(arg->fetch.data);
+}
+
+void traceprobe_free_probe_arg(struct probe_arg *arg)
+{
+	if (CHECK_FETCH_FUNCS(bitfield, arg->fetch.fn))
+		free_bitfield_fetch_param(arg->fetch.data);
+	else if (CHECK_FETCH_FUNCS(deref, arg->fetch.fn))
+		free_deref_fetch_param(arg->fetch.data);
+	else if (CHECK_FETCH_FUNCS(symbol, arg->fetch.fn))
+		free_symbol_cache(arg->fetch.data);
+
+	kfree(arg->name);
+	kfree(arg->comm);
+}
+
+int traceprobe_command(const char *buf, int (*createfn)(int, char **))
+{
+	char **argv;
+	int argc, ret;
+
+	argc = 0;
+	ret = 0;
+	argv = argv_split(GFP_KERNEL, buf, &argc);
+	if (!argv)
+		return -ENOMEM;
+
+	if (argc)
+		ret = createfn(argc, argv);
+
+	argv_free(argv);
+
+	return ret;
+}
+
+#define WRITE_BUFSIZE  4096
+
+ssize_t traceprobe_probes_write(struct file *file, const char __user *buffer,
+				size_t count, loff_t *ppos,
+				int (*createfn)(int, char **))
+{
+	char *kbuf, *tmp;
+	int ret = 0;
+	size_t done = 0;
+	size_t size;
+
+	kbuf = kmalloc(WRITE_BUFSIZE, GFP_KERNEL);
+	if (!kbuf)
+		return -ENOMEM;
+
+	while (done < count) {
+		size = count - done;
+
+		if (size >= WRITE_BUFSIZE)
+			size = WRITE_BUFSIZE - 1;
+
+		if (copy_from_user(kbuf, buffer + done, size)) {
+			ret = -EFAULT;
+			goto out;
+		}
+		kbuf[size] = '\0';
+		tmp = strchr(kbuf, '\n');
+
+		if (tmp) {
+			*tmp = '\0';
+			size = tmp - kbuf + 1;
+		} else if (done + size < count) {
+			pr_warning("Line length is too long: "
+				   "Should be less than %d.", WRITE_BUFSIZE);
+			ret = -EINVAL;
+			goto out;
+		}
+		done += size;
+		/* Remove comments */
+		tmp = strchr(kbuf, '#');
+
+		if (tmp)
+			*tmp = '\0';
+
+		ret = traceprobe_command(kbuf, createfn);
+		if (ret)
+			goto out;
+	}
+	ret = done;
+
+out:
+	kfree(kbuf);
+
+	return ret;
+}
diff --git a/kernel/trace/trace_probe.h b/kernel/trace/trace_probe.h
new file mode 100644
index 000000000000..933708677814
--- /dev/null
+++ b/kernel/trace/trace_probe.h
@@ -0,0 +1,161 @@
+/*
+ * Common header file for probe-based Dynamic events.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * This code was copied from kernel/trace/trace_kprobe.h written by
+ * Masami Hiramatsu <masami.hiramatsu.pt@hitachi.com>
+ *
+ * Updates to make this generic:
+ * Copyright (C) IBM Corporation, 2010-2011
+ * Author:     Srikar Dronamraju
+ */
+
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/smp.h>
+#include <linux/debugfs.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <linux/ctype.h>
+#include <linux/ptrace.h>
+#include <linux/perf_event.h>
+#include <linux/kprobes.h>
+#include <linux/stringify.h>
+#include <linux/limits.h>
+#include <linux/uaccess.h>
+#include <asm/bitsperlong.h>
+
+#include "trace.h"
+#include "trace_output.h"
+
+#define MAX_TRACE_ARGS		128
+#define MAX_ARGSTR_LEN		63
+#define MAX_EVENT_NAME_LEN	64
+#define MAX_STRING_SIZE		PATH_MAX
+
+/* Reserved field names */
+#define FIELD_STRING_IP		"__probe_ip"
+#define FIELD_STRING_RETIP	"__probe_ret_ip"
+#define FIELD_STRING_FUNC	"__probe_func"
+
+#undef DEFINE_FIELD
+#define DEFINE_FIELD(type, item, name, is_signed)			\
+	do {								\
+		ret = trace_define_field(event_call, #type, name,	\
+					 offsetof(typeof(field), item),	\
+					 sizeof(field.item), is_signed, \
+					 FILTER_OTHER);			\
+		if (ret)						\
+			return ret;					\
+	} while (0)
+
+
+/* Flags for trace_probe */
+#define TP_FLAG_TRACE		1
+#define TP_FLAG_PROFILE		2
+#define TP_FLAG_REGISTERED	4
+#define TP_FLAG_UPROBE		8
+
+
+/* data_rloc: data relative location, compatible with u32 */
+#define make_data_rloc(len, roffs)	\
+	(((u32)(len) << 16) | ((u32)(roffs) & 0xffff))
+#define get_rloc_len(dl)		((u32)(dl) >> 16)
+#define get_rloc_offs(dl)		((u32)(dl) & 0xffff)
+
+/*
+ * Convert data_rloc to data_loc:
+ *  data_rloc stores the offset from data_rloc itself, but data_loc
+ *  stores the offset from event entry.
+ */
+#define convert_rloc_to_loc(dl, offs)	((u32)(dl) + (offs))
+
+/* Data fetch function type */
+typedef	void (*fetch_func_t)(struct pt_regs *, void *, void *);
+/* Printing function type */
+typedef int (*print_type_func_t)(struct trace_seq *, const char *, void *, void *);
+
+/* Fetch types */
+enum {
+	FETCH_MTD_reg = 0,
+	FETCH_MTD_stack,
+	FETCH_MTD_retval,
+	FETCH_MTD_memory,
+	FETCH_MTD_symbol,
+	FETCH_MTD_deref,
+	FETCH_MTD_bitfield,
+	FETCH_MTD_END,
+};
+
+/* Fetch type information table */
+struct fetch_type {
+	const char		*name;		/* Name of type */
+	size_t			size;		/* Byte size of type */
+	int			is_signed;	/* Signed flag */
+	print_type_func_t	print;		/* Print functions */
+	const char		*fmt;		/* Fromat string */
+	const char		*fmttype;	/* Name in format file */
+	/* Fetch functions */
+	fetch_func_t		fetch[FETCH_MTD_END];
+};
+
+struct fetch_param {
+	fetch_func_t		fn;
+	void 			*data;
+};
+
+struct probe_arg {
+	struct fetch_param	fetch;
+	struct fetch_param	fetch_size;
+	unsigned int		offset;	/* Offset from argument entry */
+	const char		*name;	/* Name of this argument */
+	const char		*comm;	/* Command of this argument */
+	const struct fetch_type	*type;	/* Type of this argument */
+};
+
+static inline __kprobes void call_fetch(struct fetch_param *fprm,
+				 struct pt_regs *regs, void *dest)
+{
+	return fprm->fn(regs, fprm->data, dest);
+}
+
+/* Check the name is good for event/group/fields */
+static inline int is_good_name(const char *name)
+{
+	if (!isalpha(*name) && *name != '_')
+		return 0;
+	while (*++name != '\0') {
+		if (!isalpha(*name) && !isdigit(*name) && *name != '_')
+			return 0;
+	}
+	return 1;
+}
+
+extern int traceprobe_parse_probe_arg(char *arg, ssize_t *size,
+		   struct probe_arg *parg, bool is_return, bool is_kprobe);
+
+extern int traceprobe_conflict_field_name(const char *name,
+			       struct probe_arg *args, int narg);
+
+extern void traceprobe_update_arg(struct probe_arg *arg);
+extern void traceprobe_free_probe_arg(struct probe_arg *arg);
+
+extern int traceprobe_split_symbol_offset(char *symbol, unsigned long *offset);
+
+extern ssize_t traceprobe_probes_write(struct file *file,
+		const char __user *buffer, size_t count, loff_t *ppos,
+		int (*createfn)(int, char**));
+
+extern int traceprobe_command(const char *buf, int (*createfn)(int, char**));
diff --git a/kernel/trace/trace_uprobe.c b/kernel/trace/trace_uprobe.c
new file mode 100644
index 000000000000..2b36ac68549e
--- /dev/null
+++ b/kernel/trace/trace_uprobe.c
@@ -0,0 +1,788 @@
+/*
+ * uprobes-based tracing events
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * Copyright (C) IBM Corporation, 2010-2012
+ * Author:	Srikar Dronamraju <srikar@linux.vnet.ibm.com>
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/uprobes.h>
+#include <linux/namei.h>
+
+#include "trace_probe.h"
+
+#define UPROBE_EVENT_SYSTEM	"uprobes"
+
+/*
+ * uprobe event core functions
+ */
+struct trace_uprobe;
+struct uprobe_trace_consumer {
+	struct uprobe_consumer		cons;
+	struct trace_uprobe		*tu;
+};
+
+struct trace_uprobe {
+	struct list_head		list;
+	struct ftrace_event_class	class;
+	struct ftrace_event_call	call;
+	struct uprobe_trace_consumer	*consumer;
+	struct inode			*inode;
+	char				*filename;
+	unsigned long			offset;
+	unsigned long			nhit;
+	unsigned int			flags;	/* For TP_FLAG_* */
+	ssize_t				size;	/* trace entry size */
+	unsigned int			nr_args;
+	struct probe_arg		args[];
+};
+
+#define SIZEOF_TRACE_UPROBE(n)			\
+	(offsetof(struct trace_uprobe, args) +	\
+	(sizeof(struct probe_arg) * (n)))
+
+static int register_uprobe_event(struct trace_uprobe *tu);
+static void unregister_uprobe_event(struct trace_uprobe *tu);
+
+static DEFINE_MUTEX(uprobe_lock);
+static LIST_HEAD(uprobe_list);
+
+static int uprobe_dispatcher(struct uprobe_consumer *con, struct pt_regs *regs);
+
+/*
+ * Allocate new trace_uprobe and initialize it (including uprobes).
+ */
+static struct trace_uprobe *
+alloc_trace_uprobe(const char *group, const char *event, int nargs)
+{
+	struct trace_uprobe *tu;
+
+	if (!event || !is_good_name(event))
+		return ERR_PTR(-EINVAL);
+
+	if (!group || !is_good_name(group))
+		return ERR_PTR(-EINVAL);
+
+	tu = kzalloc(SIZEOF_TRACE_UPROBE(nargs), GFP_KERNEL);
+	if (!tu)
+		return ERR_PTR(-ENOMEM);
+
+	tu->call.class = &tu->class;
+	tu->call.name = kstrdup(event, GFP_KERNEL);
+	if (!tu->call.name)
+		goto error;
+
+	tu->class.system = kstrdup(group, GFP_KERNEL);
+	if (!tu->class.system)
+		goto error;
+
+	INIT_LIST_HEAD(&tu->list);
+	return tu;
+
+error:
+	kfree(tu->call.name);
+	kfree(tu);
+
+	return ERR_PTR(-ENOMEM);
+}
+
+static void free_trace_uprobe(struct trace_uprobe *tu)
+{
+	int i;
+
+	for (i = 0; i < tu->nr_args; i++)
+		traceprobe_free_probe_arg(&tu->args[i]);
+
+	iput(tu->inode);
+	kfree(tu->call.class->system);
+	kfree(tu->call.name);
+	kfree(tu->filename);
+	kfree(tu);
+}
+
+static struct trace_uprobe *find_probe_event(const char *event, const char *group)
+{
+	struct trace_uprobe *tu;
+
+	list_for_each_entry(tu, &uprobe_list, list)
+		if (strcmp(tu->call.name, event) == 0 &&
+		    strcmp(tu->call.class->system, group) == 0)
+			return tu;
+
+	return NULL;
+}
+
+/* Unregister a trace_uprobe and probe_event: call with locking uprobe_lock */
+static void unregister_trace_uprobe(struct trace_uprobe *tu)
+{
+	list_del(&tu->list);
+	unregister_uprobe_event(tu);
+	free_trace_uprobe(tu);
+}
+
+/* Register a trace_uprobe and probe_event */
+static int register_trace_uprobe(struct trace_uprobe *tu)
+{
+	struct trace_uprobe *old_tp;
+	int ret;
+
+	mutex_lock(&uprobe_lock);
+
+	/* register as an event */
+	old_tp = find_probe_event(tu->call.name, tu->call.class->system);
+	if (old_tp)
+		/* delete old event */
+		unregister_trace_uprobe(old_tp);
+
+	ret = register_uprobe_event(tu);
+	if (ret) {
+		pr_warning("Failed to register probe event(%d)\n", ret);
+		goto end;
+	}
+
+	list_add_tail(&tu->list, &uprobe_list);
+
+end:
+	mutex_unlock(&uprobe_lock);
+
+	return ret;
+}
+
+/*
+ * Argument syntax:
+ *  - Add uprobe: p[:[GRP/]EVENT] PATH:SYMBOL[+offs] [FETCHARGS]
+ *
+ *  - Remove uprobe: -:[GRP/]EVENT
+ */
+static int create_trace_uprobe(int argc, char **argv)
+{
+	struct trace_uprobe *tu;
+	struct inode *inode;
+	char *arg, *event, *group, *filename;
+	char buf[MAX_EVENT_NAME_LEN];
+	struct path path;
+	unsigned long offset;
+	bool is_delete;
+	int i, ret;
+
+	inode = NULL;
+	ret = 0;
+	is_delete = false;
+	event = NULL;
+	group = NULL;
+
+	/* argc must be >= 1 */
+	if (argv[0][0] == '-')
+		is_delete = true;
+	else if (argv[0][0] != 'p') {
+		pr_info("Probe definition must be started with 'p', 'r' or" " '-'.\n");
+		return -EINVAL;
+	}
+
+	if (argv[0][1] == ':') {
+		event = &argv[0][2];
+		arg = strchr(event, '/');
+
+		if (arg) {
+			group = event;
+			event = arg + 1;
+			event[-1] = '\0';
+
+			if (strlen(group) == 0) {
+				pr_info("Group name is not specified\n");
+				return -EINVAL;
+			}
+		}
+		if (strlen(event) == 0) {
+			pr_info("Event name is not specified\n");
+			return -EINVAL;
+		}
+	}
+	if (!group)
+		group = UPROBE_EVENT_SYSTEM;
+
+	if (is_delete) {
+		if (!event) {
+			pr_info("Delete command needs an event name.\n");
+			return -EINVAL;
+		}
+		mutex_lock(&uprobe_lock);
+		tu = find_probe_event(event, group);
+
+		if (!tu) {
+			mutex_unlock(&uprobe_lock);
+			pr_info("Event %s/%s doesn't exist.\n", group, event);
+			return -ENOENT;
+		}
+		/* delete an event */
+		unregister_trace_uprobe(tu);
+		mutex_unlock(&uprobe_lock);
+		return 0;
+	}
+
+	if (argc < 2) {
+		pr_info("Probe point is not specified.\n");
+		return -EINVAL;
+	}
+	if (isdigit(argv[1][0])) {
+		pr_info("probe point must be have a filename.\n");
+		return -EINVAL;
+	}
+	arg = strchr(argv[1], ':');
+	if (!arg)
+		goto fail_address_parse;
+
+	*arg++ = '\0';
+	filename = argv[1];
+	ret = kern_path(filename, LOOKUP_FOLLOW, &path);
+	if (ret)
+		goto fail_address_parse;
+
+	ret = strict_strtoul(arg, 0, &offset);
+	if (ret)
+		goto fail_address_parse;
+
+	inode = igrab(path.dentry->d_inode);
+
+	argc -= 2;
+	argv += 2;
+
+	/* setup a probe */
+	if (!event) {
+		char *tail = strrchr(filename, '/');
+		char *ptr;
+
+		ptr = kstrdup((tail ? tail + 1 : filename), GFP_KERNEL);
+		if (!ptr) {
+			ret = -ENOMEM;
+			goto fail_address_parse;
+		}
+
+		tail = ptr;
+		ptr = strpbrk(tail, ".-_");
+		if (ptr)
+			*ptr = '\0';
+
+		snprintf(buf, MAX_EVENT_NAME_LEN, "%c_%s_0x%lx", 'p', tail, offset);
+		event = buf;
+		kfree(tail);
+	}
+
+	tu = alloc_trace_uprobe(group, event, argc);
+	if (IS_ERR(tu)) {
+		pr_info("Failed to allocate trace_uprobe.(%d)\n", (int)PTR_ERR(tu));
+		ret = PTR_ERR(tu);
+		goto fail_address_parse;
+	}
+	tu->offset = offset;
+	tu->inode = inode;
+	tu->filename = kstrdup(filename, GFP_KERNEL);
+
+	if (!tu->filename) {
+		pr_info("Failed to allocate filename.\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	/* parse arguments */
+	ret = 0;
+	for (i = 0; i < argc && i < MAX_TRACE_ARGS; i++) {
+		/* Increment count for freeing args in error case */
+		tu->nr_args++;
+
+		/* Parse argument name */
+		arg = strchr(argv[i], '=');
+		if (arg) {
+			*arg++ = '\0';
+			tu->args[i].name = kstrdup(argv[i], GFP_KERNEL);
+		} else {
+			arg = argv[i];
+			/* If argument name is omitted, set "argN" */
+			snprintf(buf, MAX_EVENT_NAME_LEN, "arg%d", i + 1);
+			tu->args[i].name = kstrdup(buf, GFP_KERNEL);
+		}
+
+		if (!tu->args[i].name) {
+			pr_info("Failed to allocate argument[%d] name.\n", i);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		if (!is_good_name(tu->args[i].name)) {
+			pr_info("Invalid argument[%d] name: %s\n", i, tu->args[i].name);
+			ret = -EINVAL;
+			goto error;
+		}
+
+		if (traceprobe_conflict_field_name(tu->args[i].name, tu->args, i)) {
+			pr_info("Argument[%d] name '%s' conflicts with "
+				"another field.\n", i, argv[i]);
+			ret = -EINVAL;
+			goto error;
+		}
+
+		/* Parse fetch argument */
+		ret = traceprobe_parse_probe_arg(arg, &tu->size, &tu->args[i], false, false);
+		if (ret) {
+			pr_info("Parse error at argument[%d]. (%d)\n", i, ret);
+			goto error;
+		}
+	}
+
+	ret = register_trace_uprobe(tu);
+	if (ret)
+		goto error;
+	return 0;
+
+error:
+	free_trace_uprobe(tu);
+	return ret;
+
+fail_address_parse:
+	if (inode)
+		iput(inode);
+
+	pr_info("Failed to parse address.\n");
+
+	return ret;
+}
+
+static void cleanup_all_probes(void)
+{
+	struct trace_uprobe *tu;
+
+	mutex_lock(&uprobe_lock);
+	while (!list_empty(&uprobe_list)) {
+		tu = list_entry(uprobe_list.next, struct trace_uprobe, list);
+		unregister_trace_uprobe(tu);
+	}
+	mutex_unlock(&uprobe_lock);
+}
+
+/* Probes listing interfaces */
+static void *probes_seq_start(struct seq_file *m, loff_t *pos)
+{
+	mutex_lock(&uprobe_lock);
+	return seq_list_start(&uprobe_list, *pos);
+}
+
+static void *probes_seq_next(struct seq_file *m, void *v, loff_t *pos)
+{
+	return seq_list_next(v, &uprobe_list, pos);
+}
+
+static void probes_seq_stop(struct seq_file *m, void *v)
+{
+	mutex_unlock(&uprobe_lock);
+}
+
+static int probes_seq_show(struct seq_file *m, void *v)
+{
+	struct trace_uprobe *tu = v;
+	int i;
+
+	seq_printf(m, "p:%s/%s", tu->call.class->system, tu->call.name);
+	seq_printf(m, " %s:0x%p", tu->filename, (void *)tu->offset);
+
+	for (i = 0; i < tu->nr_args; i++)
+		seq_printf(m, " %s=%s", tu->args[i].name, tu->args[i].comm);
+
+	seq_printf(m, "\n");
+	return 0;
+}
+
+static const struct seq_operations probes_seq_op = {
+	.start	= probes_seq_start,
+	.next	= probes_seq_next,
+	.stop	= probes_seq_stop,
+	.show	= probes_seq_show
+};
+
+static int probes_open(struct inode *inode, struct file *file)
+{
+	if ((file->f_mode & FMODE_WRITE) && (file->f_flags & O_TRUNC))
+		cleanup_all_probes();
+
+	return seq_open(file, &probes_seq_op);
+}
+
+static ssize_t probes_write(struct file *file, const char __user *buffer,
+			    size_t count, loff_t *ppos)
+{
+	return traceprobe_probes_write(file, buffer, count, ppos, create_trace_uprobe);
+}
+
+static const struct file_operations uprobe_events_ops = {
+	.owner		= THIS_MODULE,
+	.open		= probes_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+	.write		= probes_write,
+};
+
+/* Probes profiling interfaces */
+static int probes_profile_seq_show(struct seq_file *m, void *v)
+{
+	struct trace_uprobe *tu = v;
+
+	seq_printf(m, "  %s %-44s %15lu\n", tu->filename, tu->call.name, tu->nhit);
+	return 0;
+}
+
+static const struct seq_operations profile_seq_op = {
+	.start	= probes_seq_start,
+	.next	= probes_seq_next,
+	.stop	= probes_seq_stop,
+	.show	= probes_profile_seq_show
+};
+
+static int profile_open(struct inode *inode, struct file *file)
+{
+	return seq_open(file, &profile_seq_op);
+}
+
+static const struct file_operations uprobe_profile_ops = {
+	.owner		= THIS_MODULE,
+	.open		= profile_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
+
+/* uprobe handler */
+static void uprobe_trace_func(struct trace_uprobe *tu, struct pt_regs *regs)
+{
+	struct uprobe_trace_entry_head *entry;
+	struct ring_buffer_event *event;
+	struct ring_buffer *buffer;
+	u8 *data;
+	int size, i, pc;
+	unsigned long irq_flags;
+	struct ftrace_event_call *call = &tu->call;
+
+	tu->nhit++;
+
+	local_save_flags(irq_flags);
+	pc = preempt_count();
+
+	size = sizeof(*entry) + tu->size;
+
+	event = trace_current_buffer_lock_reserve(&buffer, call->event.type,
+						  size, irq_flags, pc);
+	if (!event)
+		return;
+
+	entry = ring_buffer_event_data(event);
+	entry->ip = uprobe_get_swbp_addr(task_pt_regs(current));
+	data = (u8 *)&entry[1];
+	for (i = 0; i < tu->nr_args; i++)
+		call_fetch(&tu->args[i].fetch, regs, data + tu->args[i].offset);
+
+	if (!filter_current_check_discard(buffer, call, entry, event))
+		trace_buffer_unlock_commit(buffer, event, irq_flags, pc);
+}
+
+/* Event entry printers */
+static enum print_line_t
+print_uprobe_event(struct trace_iterator *iter, int flags, struct trace_event *event)
+{
+	struct uprobe_trace_entry_head *field;
+	struct trace_seq *s = &iter->seq;
+	struct trace_uprobe *tu;
+	u8 *data;
+	int i;
+
+	field = (struct uprobe_trace_entry_head *)iter->ent;
+	tu = container_of(event, struct trace_uprobe, call.event);
+
+	if (!trace_seq_printf(s, "%s: (", tu->call.name))
+		goto partial;
+
+	if (!seq_print_ip_sym(s, field->ip, flags | TRACE_ITER_SYM_OFFSET))
+		goto partial;
+
+	if (!trace_seq_puts(s, ")"))
+		goto partial;
+
+	data = (u8 *)&field[1];
+	for (i = 0; i < tu->nr_args; i++) {
+		if (!tu->args[i].type->print(s, tu->args[i].name,
+					     data + tu->args[i].offset, field))
+			goto partial;
+	}
+
+	if (trace_seq_puts(s, "\n"))
+		return TRACE_TYPE_HANDLED;
+
+partial:
+	return TRACE_TYPE_PARTIAL_LINE;
+}
+
+static int probe_event_enable(struct trace_uprobe *tu, int flag)
+{
+	struct uprobe_trace_consumer *utc;
+	int ret = 0;
+
+	if (!tu->inode || tu->consumer)
+		return -EINTR;
+
+	utc = kzalloc(sizeof(struct uprobe_trace_consumer), GFP_KERNEL);
+	if (!utc)
+		return -EINTR;
+
+	utc->cons.handler = uprobe_dispatcher;
+	utc->cons.filter = NULL;
+	ret = uprobe_register(tu->inode, tu->offset, &utc->cons);
+	if (ret) {
+		kfree(utc);
+		return ret;
+	}
+
+	tu->flags |= flag;
+	utc->tu = tu;
+	tu->consumer = utc;
+
+	return 0;
+}
+
+static void probe_event_disable(struct trace_uprobe *tu, int flag)
+{
+	if (!tu->inode || !tu->consumer)
+		return;
+
+	uprobe_unregister(tu->inode, tu->offset, &tu->consumer->cons);
+	tu->flags &= ~flag;
+	kfree(tu->consumer);
+	tu->consumer = NULL;
+}
+
+static int uprobe_event_define_fields(struct ftrace_event_call *event_call)
+{
+	int ret, i;
+	struct uprobe_trace_entry_head field;
+	struct trace_uprobe *tu = (struct trace_uprobe *)event_call->data;
+
+	DEFINE_FIELD(unsigned long, ip, FIELD_STRING_IP, 0);
+	/* Set argument names as fields */
+	for (i = 0; i < tu->nr_args; i++) {
+		ret = trace_define_field(event_call, tu->args[i].type->fmttype,
+					 tu->args[i].name,
+					 sizeof(field) + tu->args[i].offset,
+					 tu->args[i].type->size,
+					 tu->args[i].type->is_signed,
+					 FILTER_OTHER);
+
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+#define LEN_OR_ZERO		(len ? len - pos : 0)
+static int __set_print_fmt(struct trace_uprobe *tu, char *buf, int len)
+{
+	const char *fmt, *arg;
+	int i;
+	int pos = 0;
+
+	fmt = "(%lx)";
+	arg = "REC->" FIELD_STRING_IP;
+
+	/* When len=0, we just calculate the needed length */
+
+	pos += snprintf(buf + pos, LEN_OR_ZERO, "\"%s", fmt);
+
+	for (i = 0; i < tu->nr_args; i++) {
+		pos += snprintf(buf + pos, LEN_OR_ZERO, " %s=%s",
+				tu->args[i].name, tu->args[i].type->fmt);
+	}
+
+	pos += snprintf(buf + pos, LEN_OR_ZERO, "\", %s", arg);
+
+	for (i = 0; i < tu->nr_args; i++) {
+		pos += snprintf(buf + pos, LEN_OR_ZERO, ", REC->%s",
+				tu->args[i].name);
+	}
+
+	return pos;	/* return the length of print_fmt */
+}
+#undef LEN_OR_ZERO
+
+static int set_print_fmt(struct trace_uprobe *tu)
+{
+	char *print_fmt;
+	int len;
+
+	/* First: called with 0 length to calculate the needed length */
+	len = __set_print_fmt(tu, NULL, 0);
+	print_fmt = kmalloc(len + 1, GFP_KERNEL);
+	if (!print_fmt)
+		return -ENOMEM;
+
+	/* Second: actually write the @print_fmt */
+	__set_print_fmt(tu, print_fmt, len + 1);
+	tu->call.print_fmt = print_fmt;
+
+	return 0;
+}
+
+#ifdef CONFIG_PERF_EVENTS
+/* uprobe profile handler */
+static void uprobe_perf_func(struct trace_uprobe *tu, struct pt_regs *regs)
+{
+	struct ftrace_event_call *call = &tu->call;
+	struct uprobe_trace_entry_head *entry;
+	struct hlist_head *head;
+	u8 *data;
+	int size, __size, i;
+	int rctx;
+
+	__size = sizeof(*entry) + tu->size;
+	size = ALIGN(__size + sizeof(u32), sizeof(u64));
+	size -= sizeof(u32);
+	if (WARN_ONCE(size > PERF_MAX_TRACE_SIZE, "profile buffer not large enough"))
+		return;
+
+	preempt_disable();
+
+	entry = perf_trace_buf_prepare(size, call->event.type, regs, &rctx);
+	if (!entry)
+		goto out;
+
+	entry->ip = uprobe_get_swbp_addr(task_pt_regs(current));
+	data = (u8 *)&entry[1];
+	for (i = 0; i < tu->nr_args; i++)
+		call_fetch(&tu->args[i].fetch, regs, data + tu->args[i].offset);
+
+	head = this_cpu_ptr(call->perf_events);
+	perf_trace_buf_submit(entry, size, rctx, entry->ip, 1, regs, head);
+
+ out:
+	preempt_enable();
+}
+#endif	/* CONFIG_PERF_EVENTS */
+
+static
+int trace_uprobe_register(struct ftrace_event_call *event, enum trace_reg type, void *data)
+{
+	struct trace_uprobe *tu = (struct trace_uprobe *)event->data;
+
+	switch (type) {
+	case TRACE_REG_REGISTER:
+		return probe_event_enable(tu, TP_FLAG_TRACE);
+
+	case TRACE_REG_UNREGISTER:
+		probe_event_disable(tu, TP_FLAG_TRACE);
+		return 0;
+
+#ifdef CONFIG_PERF_EVENTS
+	case TRACE_REG_PERF_REGISTER:
+		return probe_event_enable(tu, TP_FLAG_PROFILE);
+
+	case TRACE_REG_PERF_UNREGISTER:
+		probe_event_disable(tu, TP_FLAG_PROFILE);
+		return 0;
+#endif
+	default:
+		return 0;
+	}
+	return 0;
+}
+
+static int uprobe_dispatcher(struct uprobe_consumer *con, struct pt_regs *regs)
+{
+	struct uprobe_trace_consumer *utc;
+	struct trace_uprobe *tu;
+
+	utc = container_of(con, struct uprobe_trace_consumer, cons);
+	tu = utc->tu;
+	if (!tu || tu->consumer != utc)
+		return 0;
+
+	if (tu->flags & TP_FLAG_TRACE)
+		uprobe_trace_func(tu, regs);
+
+#ifdef CONFIG_PERF_EVENTS
+	if (tu->flags & TP_FLAG_PROFILE)
+		uprobe_perf_func(tu, regs);
+#endif
+	return 0;
+}
+
+static struct trace_event_functions uprobe_funcs = {
+	.trace		= print_uprobe_event
+};
+
+static int register_uprobe_event(struct trace_uprobe *tu)
+{
+	struct ftrace_event_call *call = &tu->call;
+	int ret;
+
+	/* Initialize ftrace_event_call */
+	INIT_LIST_HEAD(&call->class->fields);
+	call->event.funcs = &uprobe_funcs;
+	call->class->define_fields = uprobe_event_define_fields;
+
+	if (set_print_fmt(tu) < 0)
+		return -ENOMEM;
+
+	ret = register_ftrace_event(&call->event);
+	if (!ret) {
+		kfree(call->print_fmt);
+		return -ENODEV;
+	}
+	call->flags = 0;
+	call->class->reg = trace_uprobe_register;
+	call->data = tu;
+	ret = trace_add_event_call(call);
+
+	if (ret) {
+		pr_info("Failed to register uprobe event: %s\n", call->name);
+		kfree(call->print_fmt);
+		unregister_ftrace_event(&call->event);
+	}
+
+	return ret;
+}
+
+static void unregister_uprobe_event(struct trace_uprobe *tu)
+{
+	/* tu->event is unregistered in trace_remove_event_call() */
+	trace_remove_event_call(&tu->call);
+	kfree(tu->call.print_fmt);
+	tu->call.print_fmt = NULL;
+}
+
+/* Make a trace interface for controling probe points */
+static __init int init_uprobe_trace(void)
+{
+	struct dentry *d_tracer;
+
+	d_tracer = tracing_init_dentry();
+	if (!d_tracer)
+		return 0;
+
+	trace_create_file("uprobe_events", 0644, d_tracer,
+				    NULL, &uprobe_events_ops);
+	/* Profile interface */
+	trace_create_file("uprobe_profile", 0444, d_tracer,
+				    NULL, &uprobe_profile_ops);
+	return 0;
+}
+
+fs_initcall(init_uprobe_trace);
diff --git a/mm/memory.c b/mm/memory.c
index 1e77da6d82c1..e40f6759ba98 100644
--- a/mm/memory.c
+++ b/mm/memory.c
@@ -1307,6 +1307,9 @@ static void unmap_single_vma(struct mmu_gather *tlb,
 	if (end <= vma->vm_start)
 		return;
 
+	if (vma->vm_file)
+		uprobe_munmap(vma, start, end);
+
 	if (unlikely(is_pfn_mapping(vma)))
 		untrack_pfn_vma(vma, 0, 0);
 
diff --git a/mm/mmap.c b/mm/mmap.c
index 69a1889f3790..e8dcfc7de866 100644
--- a/mm/mmap.c
+++ b/mm/mmap.c
@@ -30,6 +30,7 @@
 #include <linux/perf_event.h>
 #include <linux/audit.h>
 #include <linux/khugepaged.h>
+#include <linux/uprobes.h>
 
 #include <asm/uaccess.h>
 #include <asm/cacheflush.h>
@@ -546,8 +547,15 @@ again:			remove_next = 1 + (end > next->vm_end);
 
 	if (file) {
 		mapping = file->f_mapping;
-		if (!(vma->vm_flags & VM_NONLINEAR))
+		if (!(vma->vm_flags & VM_NONLINEAR)) {
 			root = &mapping->i_mmap;
+			uprobe_munmap(vma, vma->vm_start, vma->vm_end);
+
+			if (adjust_next)
+				uprobe_munmap(next, next->vm_start,
+							next->vm_end);
+		}
+
 		mutex_lock(&mapping->i_mmap_mutex);
 		if (insert) {
 			/*
@@ -617,8 +625,16 @@ again:			remove_next = 1 + (end > next->vm_end);
 	if (mapping)
 		mutex_unlock(&mapping->i_mmap_mutex);
 
+	if (root) {
+		uprobe_mmap(vma);
+
+		if (adjust_next)
+			uprobe_mmap(next);
+	}
+
 	if (remove_next) {
 		if (file) {
+			uprobe_munmap(next, next->vm_start, next->vm_end);
 			fput(file);
 			if (next->vm_flags & VM_EXECUTABLE)
 				removed_exe_file_vma(mm);
@@ -638,6 +654,8 @@ again:			remove_next = 1 + (end > next->vm_end);
 			goto again;
 		}
 	}
+	if (insert && file)
+		uprobe_mmap(insert);
 
 	validate_mm(mm);
 
@@ -1371,6 +1389,11 @@ out:
 			mm->locked_vm += (len >> PAGE_SHIFT);
 	} else if ((flags & MAP_POPULATE) && !(flags & MAP_NONBLOCK))
 		make_pages_present(addr, addr + len);
+
+	if (file && uprobe_mmap(vma))
+		/* matching probes but cannot insert */
+		goto unmap_and_free_vma;
+
 	return addr;
 
 unmap_and_free_vma:
@@ -2358,6 +2381,10 @@ int insert_vm_struct(struct mm_struct * mm, struct vm_area_struct * vma)
 	if ((vma->vm_flags & VM_ACCOUNT) &&
 	     security_vm_enough_memory_mm(mm, vma_pages(vma)))
 		return -ENOMEM;
+
+	if (vma->vm_file && uprobe_mmap(vma))
+		return -EINVAL;
+
 	vma_link(mm, vma, prev, rb_link, rb_parent);
 	return 0;
 }
@@ -2427,6 +2454,10 @@ struct vm_area_struct *copy_vma(struct vm_area_struct **vmap,
 			new_vma->vm_pgoff = pgoff;
 			if (new_vma->vm_file) {
 				get_file(new_vma->vm_file);
+
+				if (uprobe_mmap(new_vma))
+					goto out_free_mempol;
+
 				if (vma->vm_flags & VM_EXECUTABLE)
 					added_exe_file_vma(mm);
 			}
diff --git a/mm/page_alloc.c b/mm/page_alloc.c
index 9f389e50ed18..1851df600438 100644
--- a/mm/page_alloc.c
+++ b/mm/page_alloc.c
@@ -5242,9 +5242,10 @@ void *__init alloc_large_system_hash(const char *tablename,
 				     int flags,
 				     unsigned int *_hash_shift,
 				     unsigned int *_hash_mask,
-				     unsigned long limit)
+				     unsigned long low_limit,
+				     unsigned long high_limit)
 {
-	unsigned long long max = limit;
+	unsigned long long max = high_limit;
 	unsigned long log2qty, size;
 	void *table = NULL;
 
@@ -5282,6 +5283,8 @@ void *__init alloc_large_system_hash(const char *tablename,
 	}
 	max = min(max, 0x80000000ULL);
 
+	if (numentries < low_limit)
+		numentries = low_limit;
 	if (numentries > max)
 		numentries = max;
 
diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index 6fb68a9743af..46e7f86acfc9 100644
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -210,7 +210,7 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock)
 		}
 
 		if (sk->sk_state == BT_CONNECTED || !newsock ||
-						bt_sk(parent)->defer_setup) {
+		    test_bit(BT_DEFER_SETUP, &bt_sk(parent)->flags)) {
 			bt_accept_unlink(sk);
 			if (newsock)
 				sock_graft(sk, newsock);
@@ -410,8 +410,8 @@ static inline unsigned int bt_accept_poll(struct sock *parent)
 	list_for_each_safe(p, n, &bt_sk(parent)->accept_q) {
 		sk = (struct sock *) list_entry(p, struct bt_sock, accept_q);
 		if (sk->sk_state == BT_CONNECTED ||
-					(bt_sk(parent)->defer_setup &&
-						sk->sk_state == BT_CONNECT2))
+		    (test_bit(BT_SK_DEFER_SETUP, &bt_sk(parent)->flags) &&
+		     sk->sk_state == BT_CONNECT2))
 			return POLLIN | POLLRDNORM;
 	}
 
@@ -450,7 +450,7 @@ unsigned int bt_sock_poll(struct file *file, struct socket *sock, poll_table *wa
 			sk->sk_state == BT_CONFIG)
 		return mask;
 
-	if (!bt_sk(sk)->suspended && sock_writeable(sk))
+	if (!test_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags) && sock_writeable(sk))
 		mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
 	else
 		set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags);
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index 88884d1d95fd..031d7d656754 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -340,7 +340,7 @@ static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
 	}
 
 	/* Strip 802.1p header */
-	if (ntohs(s->eh.h_proto) == 0x8100) {
+	if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
 		if (!skb_pull(skb, 4))
 			goto badframe;
 		s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 5238b6b3ea6a..3f18a6ed9731 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -223,36 +223,6 @@ void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __u8 rand[8],
 }
 EXPORT_SYMBOL(hci_le_start_enc);
 
-void hci_le_ltk_reply(struct hci_conn *conn, u8 ltk[16])
-{
-	struct hci_dev *hdev = conn->hdev;
-	struct hci_cp_le_ltk_reply cp;
-
-	BT_DBG("%p", conn);
-
-	memset(&cp, 0, sizeof(cp));
-
-	cp.handle = cpu_to_le16(conn->handle);
-	memcpy(cp.ltk, ltk, sizeof(ltk));
-
-	hci_send_cmd(hdev, HCI_OP_LE_LTK_REPLY, sizeof(cp), &cp);
-}
-EXPORT_SYMBOL(hci_le_ltk_reply);
-
-void hci_le_ltk_neg_reply(struct hci_conn *conn)
-{
-	struct hci_dev *hdev = conn->hdev;
-	struct hci_cp_le_ltk_neg_reply cp;
-
-	BT_DBG("%p", conn);
-
-	memset(&cp, 0, sizeof(cp));
-
-	cp.handle = cpu_to_le16(conn->handle);
-
-	hci_send_cmd(hdev, HCI_OP_LE_LTK_NEG_REPLY, sizeof(cp), &cp);
-}
-
 /* Device _must_ be locked */
 void hci_sco_setup(struct hci_conn *conn, __u8 status)
 {
@@ -513,7 +483,8 @@ EXPORT_SYMBOL(hci_get_route);
 
 /* Create SCO, ACL or LE connection.
  * Device _must_ be locked */
-struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst, __u8 sec_level, __u8 auth_type)
+struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
+			     __u8 dst_type, __u8 sec_level, __u8 auth_type)
 {
 	struct hci_conn *acl;
 	struct hci_conn *sco;
@@ -522,23 +493,18 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst, __u8
 	BT_DBG("%s dst %s", hdev->name, batostr(dst));
 
 	if (type == LE_LINK) {
-		struct adv_entry *entry;
-
 		le = hci_conn_hash_lookup_ba(hdev, LE_LINK, dst);
-		if (le)
-			return ERR_PTR(-EBUSY);
-
-		entry = hci_find_adv_entry(hdev, dst);
-		if (!entry)
-			return ERR_PTR(-EHOSTUNREACH);
+		if (!le) {
+			le = hci_conn_add(hdev, LE_LINK, dst);
+			if (!le)
+				return ERR_PTR(-ENOMEM);
 
-		le = hci_conn_add(hdev, LE_LINK, dst);
-		if (!le)
-			return ERR_PTR(-ENOMEM);
-
-		le->dst_type = entry->bdaddr_type;
+			le->dst_type = bdaddr_to_le(dst_type);
+			hci_le_connect(le);
+		}
 
-		hci_le_connect(le);
+		le->pending_sec_level = sec_level;
+		le->auth_type = auth_type;
 
 		hci_conn_hold(le);
 
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index d6dc44cd15b0..411ace8e647b 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -83,6 +83,7 @@ void hci_req_complete(struct hci_dev *hdev, __u16 cmd, int result)
 	 */
 	if (test_bit(HCI_INIT, &hdev->flags) && hdev->init_last_cmd != cmd) {
 		struct hci_command_hdr *sent = (void *) hdev->sent_cmd->data;
+		u16 opcode = __le16_to_cpu(sent->opcode);
 		struct sk_buff *skb;
 
 		/* Some CSR based controllers generate a spontaneous
@@ -92,7 +93,7 @@ void hci_req_complete(struct hci_dev *hdev, __u16 cmd, int result)
 		 * command.
 		 */
 
-		if (cmd != HCI_OP_RESET || sent->opcode == HCI_OP_RESET)
+		if (cmd != HCI_OP_RESET || opcode == HCI_OP_RESET)
 			return;
 
 		skb = skb_clone(hdev->sent_cmd, GFP_ATOMIC);
@@ -251,6 +252,9 @@ static void amp_init(struct hci_dev *hdev)
 
 	/* Read Local Version */
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL);
+
+	/* Read Local AMP Info */
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
 }
 
 static void hci_init_req(struct hci_dev *hdev, unsigned long opt)
@@ -384,7 +388,6 @@ void hci_discovery_set_state(struct hci_dev *hdev, int state)
 	case DISCOVERY_STOPPED:
 		if (hdev->discovery.state != DISCOVERY_STARTING)
 			mgmt_discovering(hdev, 0);
-		hdev->discovery.type = 0;
 		break;
 	case DISCOVERY_STARTING:
 		break;
@@ -1089,32 +1092,6 @@ static const struct rfkill_ops hci_rfkill_ops = {
 	.set_block = hci_rfkill_set_block,
 };
 
-/* Alloc HCI device */
-struct hci_dev *hci_alloc_dev(void)
-{
-	struct hci_dev *hdev;
-
-	hdev = kzalloc(sizeof(struct hci_dev), GFP_KERNEL);
-	if (!hdev)
-		return NULL;
-
-	hci_init_sysfs(hdev);
-	skb_queue_head_init(&hdev->driver_init);
-
-	return hdev;
-}
-EXPORT_SYMBOL(hci_alloc_dev);
-
-/* Free HCI device */
-void hci_free_dev(struct hci_dev *hdev)
-{
-	skb_queue_purge(&hdev->driver_init);
-
-	/* will free via device release */
-	put_device(&hdev->dev);
-}
-EXPORT_SYMBOL(hci_free_dev);
-
 static void hci_power_on(struct work_struct *work)
 {
 	struct hci_dev *hdev = container_of(work, struct hci_dev, power_on);
@@ -1336,7 +1313,7 @@ int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key,
 }
 
 int hci_add_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 addr_type, u8 type,
-		int new_key, u8 authenticated, u8 tk[16], u8 enc_size, u16
+		int new_key, u8 authenticated, u8 tk[16], u8 enc_size, __le16
 		ediv, u8 rand[8])
 {
 	struct smp_ltk *key, *old_key;
@@ -1544,75 +1521,6 @@ int hci_blacklist_del(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
 	return mgmt_device_unblocked(hdev, bdaddr, type);
 }
 
-static void hci_clear_adv_cache(struct work_struct *work)
-{
-	struct hci_dev *hdev = container_of(work, struct hci_dev,
-					    adv_work.work);
-
-	hci_dev_lock(hdev);
-
-	hci_adv_entries_clear(hdev);
-
-	hci_dev_unlock(hdev);
-}
-
-int hci_adv_entries_clear(struct hci_dev *hdev)
-{
-	struct adv_entry *entry, *tmp;
-
-	list_for_each_entry_safe(entry, tmp, &hdev->adv_entries, list) {
-		list_del(&entry->list);
-		kfree(entry);
-	}
-
-	BT_DBG("%s adv cache cleared", hdev->name);
-
-	return 0;
-}
-
-struct adv_entry *hci_find_adv_entry(struct hci_dev *hdev, bdaddr_t *bdaddr)
-{
-	struct adv_entry *entry;
-
-	list_for_each_entry(entry, &hdev->adv_entries, list)
-		if (bacmp(bdaddr, &entry->bdaddr) == 0)
-			return entry;
-
-	return NULL;
-}
-
-static inline int is_connectable_adv(u8 evt_type)
-{
-	if (evt_type == ADV_IND || evt_type == ADV_DIRECT_IND)
-		return 1;
-
-	return 0;
-}
-
-int hci_add_adv_entry(struct hci_dev *hdev,
-					struct hci_ev_le_advertising_info *ev) { struct adv_entry *entry; if (!is_connectable_adv(ev->evt_type))
-		return -EINVAL;
-
-	/* Only new entries should be added to adv_entries. So, if
-	 * bdaddr was found, don't add it. */
-	if (hci_find_adv_entry(hdev, &ev->bdaddr))
-		return 0;
-
-	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
-	if (!entry)
-		return -ENOMEM;
-
-	bacpy(&entry->bdaddr, &ev->bdaddr);
-	entry->bdaddr_type = ev->bdaddr_type;
-
-	list_add(&entry->list, &hdev->adv_entries);
-
-	BT_DBG("%s adv entry added: address %s type %u", hdev->name,
-				batostr(&entry->bdaddr), entry->bdaddr_type);
-
-	return 0;
-}
-
 static void le_scan_param_req(struct hci_dev *hdev, unsigned long opt)
 {
 	struct le_scan_params *param =  (struct le_scan_params *) opt;
@@ -1670,6 +1578,24 @@ static int hci_do_le_scan(struct hci_dev *hdev, u8 type, u16 interval,
 	return 0;
 }
 
+int hci_cancel_le_scan(struct hci_dev *hdev)
+{
+	BT_DBG("%s", hdev->name);
+
+	if (!test_bit(HCI_LE_SCAN, &hdev->dev_flags))
+		return -EALREADY;
+
+	if (cancel_delayed_work(&hdev->le_scan_disable)) {
+		struct hci_cp_le_set_scan_enable cp;
+
+		/* Send HCI command to disable LE Scan */
+		memset(&cp, 0, sizeof(cp));
+		hci_send_cmd(hdev, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp);
+	}
+
+	return 0;
+}
+
 static void le_scan_disable_work(struct work_struct *work)
 {
 	struct hci_dev *hdev = container_of(work, struct hci_dev,
@@ -1714,95 +1640,103 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 	return 0;
 }
 
-/* Register HCI device */
-int hci_register_dev(struct hci_dev *hdev)
+/* Alloc HCI device */
+struct hci_dev *hci_alloc_dev(void)
 {
-	struct list_head *head = &hci_dev_list, *p;
-	int i, id, error;
-
-	BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus);
-
-	if (!hdev->open || !hdev->close)
-		return -EINVAL;
-
-	/* Do not allow HCI_AMP devices to register at index 0,
-	 * so the index can be used as the AMP controller ID.
-	 */
-	id = (hdev->dev_type == HCI_BREDR) ? 0 : 1;
-
-	write_lock(&hci_dev_list_lock);
-
-	/* Find first available device id */
-	list_for_each(p, &hci_dev_list) {
-		if (list_entry(p, struct hci_dev, list)->id != id)
-			break;
-		head = p; id++;
-	}
-
-	sprintf(hdev->name, "hci%d", id);
-	hdev->id = id;
-	list_add_tail(&hdev->list, head);
+	struct hci_dev *hdev;
 
-	mutex_init(&hdev->lock);
+	hdev = kzalloc(sizeof(struct hci_dev), GFP_KERNEL);
+	if (!hdev)
+		return NULL;
 
-	hdev->flags = 0;
-	hdev->dev_flags = 0;
 	hdev->pkt_type  = (HCI_DM1 | HCI_DH1 | HCI_HV1);
 	hdev->esco_type = (ESCO_HV1);
 	hdev->link_mode = (HCI_LM_ACCEPT);
 	hdev->io_capability = 0x03; /* No Input No Output */
 
-	hdev->idle_timeout = 0;
 	hdev->sniff_max_interval = 800;
 	hdev->sniff_min_interval = 80;
 
+	mutex_init(&hdev->lock);
+	mutex_init(&hdev->req_lock);
+
+	INIT_LIST_HEAD(&hdev->mgmt_pending);
+	INIT_LIST_HEAD(&hdev->blacklist);
+	INIT_LIST_HEAD(&hdev->uuids);
+	INIT_LIST_HEAD(&hdev->link_keys);
+	INIT_LIST_HEAD(&hdev->long_term_keys);
+	INIT_LIST_HEAD(&hdev->remote_oob_data);
+
 	INIT_WORK(&hdev->rx_work, hci_rx_work);
 	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
 	INIT_WORK(&hdev->tx_work, hci_tx_work);
+	INIT_WORK(&hdev->power_on, hci_power_on);
+	INIT_WORK(&hdev->le_scan, le_scan_work);
 
+	INIT_DELAYED_WORK(&hdev->power_off, hci_power_off);
+	INIT_DELAYED_WORK(&hdev->discov_off, hci_discov_off);
+	INIT_DELAYED_WORK(&hdev->le_scan_disable, le_scan_disable_work);
 
+	skb_queue_head_init(&hdev->driver_init);
 	skb_queue_head_init(&hdev->rx_q);
 	skb_queue_head_init(&hdev->cmd_q);
 	skb_queue_head_init(&hdev->raw_q);
 
-	setup_timer(&hdev->cmd_timer, hci_cmd_timer, (unsigned long) hdev);
-
-	for (i = 0; i < NUM_REASSEMBLY; i++)
-		hdev->reassembly[i] = NULL;
-
 	init_waitqueue_head(&hdev->req_wait_q);
-	mutex_init(&hdev->req_lock);
 
-	discovery_init(hdev);
+	setup_timer(&hdev->cmd_timer, hci_cmd_timer, (unsigned long) hdev);
 
+	hci_init_sysfs(hdev);
+	discovery_init(hdev);
 	hci_conn_hash_init(hdev);
 
-	INIT_LIST_HEAD(&hdev->mgmt_pending);
-
-	INIT_LIST_HEAD(&hdev->blacklist);
+	return hdev;
+}
+EXPORT_SYMBOL(hci_alloc_dev);
 
-	INIT_LIST_HEAD(&hdev->uuids);
+/* Free HCI device */
+void hci_free_dev(struct hci_dev *hdev)
+{
+	skb_queue_purge(&hdev->driver_init);
 
-	INIT_LIST_HEAD(&hdev->link_keys);
-	INIT_LIST_HEAD(&hdev->long_term_keys);
+	/* will free via device release */
+	put_device(&hdev->dev);
+}
+EXPORT_SYMBOL(hci_free_dev);
 
-	INIT_LIST_HEAD(&hdev->remote_oob_data);
+/* Register HCI device */
+int hci_register_dev(struct hci_dev *hdev)
+{
+	struct list_head *head, *p;
+	int id, error;
 
-	INIT_LIST_HEAD(&hdev->adv_entries);
+	if (!hdev->open || !hdev->close)
+		return -EINVAL;
 
-	INIT_DELAYED_WORK(&hdev->adv_work, hci_clear_adv_cache);
-	INIT_WORK(&hdev->power_on, hci_power_on);
-	INIT_DELAYED_WORK(&hdev->power_off, hci_power_off);
+	write_lock(&hci_dev_list_lock);
 
-	INIT_DELAYED_WORK(&hdev->discov_off, hci_discov_off);
+	/* Do not allow HCI_AMP devices to register at index 0,
+	 * so the index can be used as the AMP controller ID.
+	 */
+	id = (hdev->dev_type == HCI_BREDR) ? 0 : 1;
+	head = &hci_dev_list;
 
-	memset(&hdev->stat, 0, sizeof(struct hci_dev_stats));
+	/* Find first available device id */
+	list_for_each(p, &hci_dev_list) {
+		int nid = list_entry(p, struct hci_dev, list)->id;
+		if (nid > id)
+			break;
+		if (nid == id)
+			id++;
+		head = p;
+	}
 
-	atomic_set(&hdev->promisc, 0);
+	sprintf(hdev->name, "hci%d", id);
+	hdev->id = id;
 
-	INIT_WORK(&hdev->le_scan, le_scan_work);
+	BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus);
 
-	INIT_DELAYED_WORK(&hdev->le_scan_disable, le_scan_disable_work);
+	list_add(&hdev->list, head);
 
 	write_unlock(&hci_dev_list_lock);
 
@@ -1884,8 +1818,6 @@ void hci_unregister_dev(struct hci_dev *hdev)
 
 	hci_del_sysfs(hdev);
 
-	cancel_delayed_work_sync(&hdev->adv_work);
-
 	destroy_workqueue(hdev->workqueue);
 
 	hci_dev_lock(hdev);
@@ -1894,7 +1826,6 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_link_keys_clear(hdev);
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
-	hci_adv_entries_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2231,6 +2162,12 @@ static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
 	struct hci_dev *hdev = conn->hdev;
 	struct sk_buff *list;
 
+	skb->len = skb_headlen(skb);
+	skb->data_len = 0;
+
+	bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
+	hci_add_acl_hdr(skb, conn->handle, flags);
+
 	list = skb_shinfo(skb)->frag_list;
 	if (!list) {
 		/* Non fragmented */
@@ -2274,8 +2211,6 @@ void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags)
 	BT_DBG("%s chan %p flags 0x%x", hdev->name, chan, flags);
 
 	skb->dev = (void *) hdev;
-	bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
-	hci_add_acl_hdr(skb, conn->handle, flags);
 
 	hci_queue_acl(conn, &chan->data_q, skb, flags);
 
@@ -2313,7 +2248,7 @@ static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int
 {
 	struct hci_conn_hash *h = &hdev->conn_hash;
 	struct hci_conn *conn = NULL, *c;
-	int num = 0, min = ~0;
+	unsigned int num = 0, min = ~0;
 
 	/* We don't have to lock device here. Connections are always
 	 * added and removed with TX task disabled. */
@@ -2394,7 +2329,7 @@ static inline struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type,
 {
 	struct hci_conn_hash *h = &hdev->conn_hash;
 	struct hci_chan *chan = NULL;
-	int num = 0, min = ~0, cur_prio = 0;
+	unsigned int num = 0, min = ~0, cur_prio = 0;
 	struct hci_conn *conn;
 	int cnt, q, conn_num = 0;
 
@@ -2945,7 +2880,19 @@ int hci_cancel_inquiry(struct hci_dev *hdev)
 	BT_DBG("%s", hdev->name);
 
 	if (!test_bit(HCI_INQUIRY, &hdev->flags))
-		return -EPERM;
+		return -EALREADY;
 
 	return hci_send_cmd(hdev, HCI_OP_INQUIRY_CANCEL, 0, NULL);
 }
+
+u8 bdaddr_to_le(u8 bdaddr_type)
+{
+	switch (bdaddr_type) {
+	case BDADDR_LE_PUBLIC:
+		return ADDR_LE_DEV_PUBLIC;
+
+	default:
+		/* Fallback to LE Random address type */
+		return ADDR_LE_DEV_RANDOM;
+	}
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 1266f78fa8e3..4eefb7f65cf6 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -69,6 +69,18 @@ static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb)
 	hci_conn_check_pending(hdev);
 }
 
+static void hci_cc_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	__u8 status = *((__u8 *) skb->data);
+
+	BT_DBG("%s status 0x%x", hdev->name, status);
+
+	if (status)
+		return;
+
+	set_bit(HCI_PERIODIC_INQ, &hdev->dev_flags);
+}
+
 static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	__u8 status = *((__u8 *) skb->data);
@@ -78,6 +90,8 @@ static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb)
 	if (status)
 		return;
 
+	clear_bit(HCI_PERIODIC_INQ, &hdev->dev_flags);
+
 	hci_conn_check_pending(hdev);
 }
 
@@ -192,7 +206,8 @@ static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb)
 	hci_req_complete(hdev, HCI_OP_RESET, status);
 
 	/* Reset all non-persistent flags */
-	hdev->dev_flags &= ~(BIT(HCI_LE_SCAN) | BIT(HCI_PENDING_CLASS));
+	hdev->dev_flags &= ~(BIT(HCI_LE_SCAN) | BIT(HCI_PENDING_CLASS) |
+			     BIT(HCI_PERIODIC_INQ));
 
 	hdev->discovery.state = DISCOVERY_STOPPED;
 }
@@ -505,7 +520,7 @@ static void hci_setup_event_mask(struct hci_dev *hdev)
 	events[5] |= 0x10; /* Synchronous Connection Changed */
 
 	if (hdev->features[3] & LMP_RSSI_INQ)
-		events[4] |= 0x04; /* Inquiry Result with RSSI */
+		events[4] |= 0x02; /* Inquiry Result with RSSI */
 
 	if (hdev->features[5] & LMP_SNIFF_SUBR)
 		events[5] |= 0x20; /* Sniff Subrating */
@@ -615,6 +630,7 @@ done:
 
 static void hci_setup_link_policy(struct hci_dev *hdev)
 {
+	struct hci_cp_write_def_link_policy cp;
 	u16 link_policy = 0;
 
 	if (hdev->features[0] & LMP_RSWITCH)
@@ -626,9 +642,8 @@ static void hci_setup_link_policy(struct hci_dev *hdev)
 	if (hdev->features[1] & LMP_PARK)
 		link_policy |= HCI_LP_PARK;
 
-	link_policy = cpu_to_le16(link_policy);
-	hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, sizeof(link_policy),
-		     &link_policy);
+	cp.policy = cpu_to_le16(link_policy);
+	hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, sizeof(cp), &cp);
 }
 
 static void hci_cc_read_local_commands(struct hci_dev *hdev, struct sk_buff *skb)
@@ -710,7 +725,7 @@ static void hci_set_le_support(struct hci_dev *hdev)
 
 	memset(&cp, 0, sizeof(cp));
 
-	if (enable_le && test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) {
+	if (test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) {
 		cp.le = 1;
 		cp.simul = !!(hdev->features[6] & LMP_SIMUL_LE_BR);
 	}
@@ -887,11 +902,14 @@ static void hci_cc_write_inquiry_mode(struct hci_dev *hdev,
 static void hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev,
 							struct sk_buff *skb)
 {
-	__u8 status = *((__u8 *) skb->data);
+	struct hci_rp_read_inq_rsp_tx_power *rp = (void *) skb->data;
 
-	BT_DBG("%s status 0x%x", hdev->name, status);
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (!rp->status)
+		hdev->inq_tx_power = rp->tx_power;
 
-	hci_req_complete(hdev, HCI_OP_READ_INQ_RSP_TX_POWER, status);
+	hci_req_complete(hdev, HCI_OP_READ_INQ_RSP_TX_POWER, rp->status);
 }
 
 static void hci_cc_set_event_flt(struct hci_dev *hdev, struct sk_buff *skb)
@@ -1082,23 +1100,23 @@ static void hci_cc_le_set_scan_enable(struct hci_dev *hdev,
 
 		set_bit(HCI_LE_SCAN, &hdev->dev_flags);
 
-		cancel_delayed_work_sync(&hdev->adv_work);
-
 		hci_dev_lock(hdev);
-		hci_adv_entries_clear(hdev);
 		hci_discovery_set_state(hdev, DISCOVERY_FINDING);
 		hci_dev_unlock(hdev);
 		break;
 
 	case LE_SCANNING_DISABLED:
-		if (status)
+		if (status) {
+			hci_dev_lock(hdev);
+			mgmt_stop_discovery_failed(hdev, status);
+			hci_dev_unlock(hdev);
 			return;
+		}
 
 		clear_bit(HCI_LE_SCAN, &hdev->dev_flags);
 
-		schedule_delayed_work(&hdev->adv_work, ADV_CLEAR_TIMEOUT);
-
-		if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED) {
+		if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED &&
+		    hdev->discovery.state == DISCOVERY_FINDING) {
 			mgmt_interleaved_discovery(hdev);
 		} else {
 			hci_dev_lock(hdev);
@@ -1625,6 +1643,8 @@ static void hci_cs_le_create_conn(struct hci_dev *hdev, __u8 status)
 	if (status) {
 		if (conn && conn->state == BT_CONNECT) {
 			conn->state = BT_CLOSED;
+			mgmt_connect_failed(hdev, &cp->peer_addr, conn->type,
+					    conn->dst_type, status);
 			hci_proto_connect_cfm(conn, status);
 			hci_conn_del(conn);
 		}
@@ -1699,6 +1719,9 @@ static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *
 	if (!num_rsp)
 		return;
 
+	if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags))
+		return;
+
 	hci_dev_lock(hdev);
 
 	for (; num_rsp; num_rsp--, info++) {
@@ -2040,7 +2063,7 @@ static inline void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *
 		clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags);
 
 		if (ev->status && conn->state == BT_CONNECTED) {
-			hci_acl_disconn(conn, 0x13);
+			hci_acl_disconn(conn, HCI_ERROR_AUTH_FAILURE);
 			hci_conn_put(conn);
 			goto unlock;
 		}
@@ -2154,6 +2177,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
 		hci_cc_inquiry_cancel(hdev, skb);
 		break;
 
+	case HCI_OP_PERIODIC_INQ:
+		hci_cc_periodic_inq(hdev, skb);
+		break;
+
 	case HCI_OP_EXIT_PERIODIC_INQ:
 		hci_cc_exit_periodic_inq(hdev, skb);
 		break;
@@ -2806,6 +2833,9 @@ static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct
 	if (!num_rsp)
 		return;
 
+	if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags))
+		return;
+
 	hci_dev_lock(hdev);
 
 	if ((skb->len - 1) / num_rsp != sizeof(struct inquiry_info_with_rssi)) {
@@ -2971,12 +3001,16 @@ static inline void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct
 	struct inquiry_data data;
 	struct extended_inquiry_info *info = (void *) (skb->data + 1);
 	int num_rsp = *((__u8 *) skb->data);
+	size_t eir_len;
 
 	BT_DBG("%s num_rsp %d", hdev->name, num_rsp);
 
 	if (!num_rsp)
 		return;
 
+	if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags))
+		return;
+
 	hci_dev_lock(hdev);
 
 	for (; num_rsp; num_rsp--, info++) {
@@ -3000,9 +3034,10 @@ static inline void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct
 
 		name_known = hci_inquiry_cache_update(hdev, &data, name_known,
 						      &ssp);
+		eir_len = eir_get_length(info->data, sizeof(info->data));
 		mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00,
 				  info->dev_class, info->rssi, !name_known,
-				  ssp, info->data, sizeof(info->data));
+				  ssp, info->data, eir_len);
 	}
 
 	hci_dev_unlock(hdev);
@@ -3322,8 +3357,6 @@ static inline void hci_le_adv_report_evt(struct hci_dev *hdev,
 	while (num_reports--) {
 		struct hci_ev_le_advertising_info *ev = ptr;
 
-		hci_add_adv_entry(hdev, ev);
-
 		rssi = ev->data[ev->length];
 		mgmt_device_found(hdev, &ev->bdaddr, LE_LINK, ev->bdaddr_type,
 				  NULL, rssi, 0, 1, ev->data, ev->length);
@@ -3343,7 +3376,7 @@ static inline void hci_le_ltk_request_evt(struct hci_dev *hdev,
 	struct hci_conn *conn;
 	struct smp_ltk *ltk;
 
-	BT_DBG("%s handle %d", hdev->name, cpu_to_le16(ev->handle));
+	BT_DBG("%s handle %d", hdev->name, __le16_to_cpu(ev->handle));
 
 	hci_dev_lock(hdev);
 
diff --git a/net/bluetooth/hci_sysfs.c b/net/bluetooth/hci_sysfs.c
index bc154298979a..937f3187eafa 100644
--- a/net/bluetooth/hci_sysfs.c
+++ b/net/bluetooth/hci_sysfs.c
@@ -444,8 +444,8 @@ static const struct file_operations blacklist_fops = {
 
 static void print_bt_uuid(struct seq_file *f, u8 *uuid)
 {
-	u32 data0, data4;
-	u16 data1, data2, data3, data5;
+	__be32 data0, data4;
+	__be16 data1, data2, data3, data5;
 
 	memcpy(&data0, &uuid[0], 4);
 	memcpy(&data1, &uuid[4], 2);
@@ -533,7 +533,6 @@ int hci_add_sysfs(struct hci_dev *hdev)
 
 	BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus);
 
-	dev->parent = hdev->parent;
 	dev_set_name(dev, "%s", hdev->name);
 
 	err = device_add(dev);
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 6f9c25b633a6..24f144b72a96 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -4,6 +4,7 @@
    Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
    Copyright (C) 2010 Google Inc.
    Copyright (C) 2011 ProFUSION Embedded Systems
+   Copyright (c) 2012 Code Aurora Forum.  All rights reserved.
 
    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
 
@@ -70,7 +71,7 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
 								void *data);
 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
 static void l2cap_send_disconn_req(struct l2cap_conn *conn,
-				struct l2cap_chan *chan, int err);
+				   struct l2cap_chan *chan, int err);
 
 /* ---- L2CAP channels ---- */
 
@@ -97,13 +98,15 @@ static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16
 }
 
 /* Find channel with given SCID.
- * Returns locked socket */
+ * Returns locked channel. */
 static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
 {
 	struct l2cap_chan *c;
 
 	mutex_lock(&conn->chan_lock);
 	c = __l2cap_get_chan_by_scid(conn, cid);
+	if (c)
+		l2cap_chan_lock(c);
 	mutex_unlock(&conn->chan_lock);
 
 	return c;
@@ -120,17 +123,6 @@ static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8
 	return NULL;
 }
 
-static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
-{
-	struct l2cap_chan *c;
-
-	mutex_lock(&conn->chan_lock);
-	c = __l2cap_get_chan_by_ident(conn, ident);
-	mutex_unlock(&conn->chan_lock);
-
-	return c;
-}
-
 static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
 {
 	struct l2cap_chan *c;
@@ -232,6 +224,124 @@ static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
 	release_sock(sk);
 }
 
+/* ---- L2CAP sequence number lists ---- */
+
+/* For ERTM, ordered lists of sequence numbers must be tracked for
+ * SREJ requests that are received and for frames that are to be
+ * retransmitted. These seq_list functions implement a singly-linked
+ * list in an array, where membership in the list can also be checked
+ * in constant time. Items can also be added to the tail of the list
+ * and removed from the head in constant time, without further memory
+ * allocs or frees.
+ */
+
+static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size)
+{
+	size_t alloc_size, i;
+
+	/* Allocated size is a power of 2 to map sequence numbers
+	 * (which may be up to 14 bits) in to a smaller array that is
+	 * sized for the negotiated ERTM transmit windows.
+	 */
+	alloc_size = roundup_pow_of_two(size);
+
+	seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL);
+	if (!seq_list->list)
+		return -ENOMEM;
+
+	seq_list->mask = alloc_size - 1;
+	seq_list->head = L2CAP_SEQ_LIST_CLEAR;
+	seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
+	for (i = 0; i < alloc_size; i++)
+		seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
+
+	return 0;
+}
+
+static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list)
+{
+	kfree(seq_list->list);
+}
+
+static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list,
+					   u16 seq)
+{
+	/* Constant-time check for list membership */
+	return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR;
+}
+
+static u16 l2cap_seq_list_remove(struct l2cap_seq_list *seq_list, u16 seq)
+{
+	u16 mask = seq_list->mask;
+
+	if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) {
+		/* In case someone tries to pop the head of an empty list */
+		return L2CAP_SEQ_LIST_CLEAR;
+	} else if (seq_list->head == seq) {
+		/* Head can be removed in constant time */
+		seq_list->head = seq_list->list[seq & mask];
+		seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
+
+		if (seq_list->head == L2CAP_SEQ_LIST_TAIL) {
+			seq_list->head = L2CAP_SEQ_LIST_CLEAR;
+			seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
+		}
+	} else {
+		/* Walk the list to find the sequence number */
+		u16 prev = seq_list->head;
+		while (seq_list->list[prev & mask] != seq) {
+			prev = seq_list->list[prev & mask];
+			if (prev == L2CAP_SEQ_LIST_TAIL)
+				return L2CAP_SEQ_LIST_CLEAR;
+		}
+
+		/* Unlink the number from the list and clear it */
+		seq_list->list[prev & mask] = seq_list->list[seq & mask];
+		seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
+		if (seq_list->tail == seq)
+			seq_list->tail = prev;
+	}
+	return seq;
+}
+
+static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list)
+{
+	/* Remove the head in constant time */
+	return l2cap_seq_list_remove(seq_list, seq_list->head);
+}
+
+static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list)
+{
+	u16 i;
+
+	if (seq_list->head == L2CAP_SEQ_LIST_CLEAR)
+		return;
+
+	for (i = 0; i <= seq_list->mask; i++)
+		seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
+
+	seq_list->head = L2CAP_SEQ_LIST_CLEAR;
+	seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
+}
+
+static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
+{
+	u16 mask = seq_list->mask;
+
+	/* All appends happen in constant time */
+
+	if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR)
+		return;
+
+	if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR)
+		seq_list->head = seq;
+	else
+		seq_list->list[seq_list->tail & mask] = seq;
+
+	seq_list->tail = seq;
+	seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL;
+}
+
 static void l2cap_chan_timeout(struct work_struct *work)
 {
 	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
@@ -262,7 +372,7 @@ static void l2cap_chan_timeout(struct work_struct *work)
 	l2cap_chan_put(chan);
 }
 
-struct l2cap_chan *l2cap_chan_create(struct sock *sk)
+struct l2cap_chan *l2cap_chan_create(void)
 {
 	struct l2cap_chan *chan;
 
@@ -272,8 +382,6 @@ struct l2cap_chan *l2cap_chan_create(struct sock *sk)
 
 	mutex_init(&chan->lock);
 
-	chan->sk = sk;
-
 	write_lock(&chan_list_lock);
 	list_add(&chan->global_l, &chan_list);
 	write_unlock(&chan_list_lock);
@@ -284,7 +392,7 @@ struct l2cap_chan *l2cap_chan_create(struct sock *sk)
 
 	atomic_set(&chan->refcnt, 1);
 
-	BT_DBG("sk %p chan %p", sk, chan);
+	BT_DBG("chan %p", chan);
 
 	return chan;
 }
@@ -298,10 +406,21 @@ void l2cap_chan_destroy(struct l2cap_chan *chan)
 	l2cap_chan_put(chan);
 }
 
-void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void l2cap_chan_set_defaults(struct l2cap_chan *chan)
+{
+	chan->fcs  = L2CAP_FCS_CRC16;
+	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
+	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
+	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
+	chan->sec_level = BT_SECURITY_LOW;
+
+	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
+}
+
+static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
-			chan->psm, chan->dcid);
+	       __le16_to_cpu(chan->psm), chan->dcid);
 
 	conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
 
@@ -347,7 +466,7 @@ void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 	list_add(&chan->list, &conn->chan_l);
 }
 
-void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+static void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	mutex_lock(&conn->chan_lock);
 	__l2cap_chan_add(conn, chan);
@@ -405,6 +524,8 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 
 		skb_queue_purge(&chan->srej_q);
 
+		l2cap_seq_list_free(&chan->srej_list);
+		l2cap_seq_list_free(&chan->retrans_list);
 		list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
 			list_del(&l->list);
 			kfree(l);
@@ -453,7 +574,6 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 	case BT_CONFIG:
 		if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
 					conn->hcon->type == ACL_LINK) {
-			__clear_chan_timer(chan);
 			__set_chan_timer(chan, sk->sk_sndtimeo);
 			l2cap_send_disconn_req(conn, chan, reason);
 		} else
@@ -466,7 +586,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 			struct l2cap_conn_rsp rsp;
 			__u16 result;
 
-			if (bt_sk(sk)->defer_setup)
+			if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))
 				result = L2CAP_CR_SEC_BLOCK;
 			else
 				result = L2CAP_CR_BAD_PSM;
@@ -599,6 +719,117 @@ static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
 	hci_send_acl(chan->conn->hchan, skb, flags);
 }
 
+static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control)
+{
+	control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
+	control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT;
+
+	if (enh & L2CAP_CTRL_FRAME_TYPE) {
+		/* S-Frame */
+		control->sframe = 1;
+		control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT;
+		control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
+
+		control->sar = 0;
+		control->txseq = 0;
+	} else {
+		/* I-Frame */
+		control->sframe = 0;
+		control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
+		control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
+
+		control->poll = 0;
+		control->super = 0;
+	}
+}
+
+static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control)
+{
+	control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT;
+	control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT;
+
+	if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) {
+		/* S-Frame */
+		control->sframe = 1;
+		control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT;
+		control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT;
+
+		control->sar = 0;
+		control->txseq = 0;
+	} else {
+		/* I-Frame */
+		control->sframe = 0;
+		control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
+		control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT;
+
+		control->poll = 0;
+		control->super = 0;
+	}
+}
+
+static inline void __unpack_control(struct l2cap_chan *chan,
+				    struct sk_buff *skb)
+{
+	if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
+		__unpack_extended_control(get_unaligned_le32(skb->data),
+					  &bt_cb(skb)->control);
+	} else {
+		__unpack_enhanced_control(get_unaligned_le16(skb->data),
+					  &bt_cb(skb)->control);
+	}
+}
+
+static u32 __pack_extended_control(struct l2cap_ctrl *control)
+{
+	u32 packed;
+
+	packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT;
+	packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT;
+
+	if (control->sframe) {
+		packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT;
+		packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT;
+		packed |= L2CAP_EXT_CTRL_FRAME_TYPE;
+	} else {
+		packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT;
+		packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT;
+	}
+
+	return packed;
+}
+
+static u16 __pack_enhanced_control(struct l2cap_ctrl *control)
+{
+	u16 packed;
+
+	packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT;
+	packed |= control->final << L2CAP_CTRL_FINAL_SHIFT;
+
+	if (control->sframe) {
+		packed |= control->poll << L2CAP_CTRL_POLL_SHIFT;
+		packed |= control->super << L2CAP_CTRL_SUPER_SHIFT;
+		packed |= L2CAP_CTRL_FRAME_TYPE;
+	} else {
+		packed |= control->sar << L2CAP_CTRL_SAR_SHIFT;
+		packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT;
+	}
+
+	return packed;
+}
+
+static inline void __pack_control(struct l2cap_chan *chan,
+				  struct l2cap_ctrl *control,
+				  struct sk_buff *skb)
+{
+	if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
+		put_unaligned_le32(__pack_extended_control(control),
+				   skb->data + L2CAP_HDR_SIZE);
+	} else {
+		put_unaligned_le16(__pack_enhanced_control(control),
+				   skb->data + L2CAP_HDR_SIZE);
+	}
+}
+
 static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control)
 {
 	struct sk_buff *skb;
@@ -681,10 +912,38 @@ static void l2cap_send_conn_req(struct l2cap_chan *chan)
 	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
 }
 
+static void l2cap_chan_ready(struct l2cap_chan *chan)
+{
+	struct sock *sk = chan->sk;
+	struct sock *parent;
+
+	lock_sock(sk);
+
+	parent = bt_sk(sk)->parent;
+
+	BT_DBG("sk %p, parent %p", sk, parent);
+
+	chan->conf_state = 0;
+	__clear_chan_timer(chan);
+
+	__l2cap_state_change(chan, BT_CONNECTED);
+	sk->sk_state_change(sk);
+
+	if (parent)
+		parent->sk_data_ready(parent, 0);
+
+	release_sock(sk);
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 
+	if (conn->hcon->type == LE_LINK) {
+		l2cap_chan_ready(chan);
+		return;
+	}
+
 	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
 		if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
 			return;
@@ -791,7 +1050,8 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 
 			if (l2cap_chan_check_security(chan)) {
 				lock_sock(sk);
-				if (bt_sk(sk)->defer_setup) {
+				if (test_bit(BT_SK_DEFER_SETUP,
+					     &bt_sk(sk)->flags)) {
 					struct sock *parent = bt_sk(sk)->parent;
 					rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 					rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
@@ -830,10 +1090,12 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 	mutex_unlock(&conn->chan_lock);
 }
 
-/* Find socket with cid and source bdaddr.
+/* Find socket with cid and source/destination bdaddr.
  * Returns closest match, locked.
  */
-static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src)
+static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid,
+						    bdaddr_t *src,
+						    bdaddr_t *dst)
 {
 	struct l2cap_chan *c, *c1 = NULL;
 
@@ -846,14 +1108,22 @@ static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdadd
 			continue;
 
 		if (c->scid == cid) {
+			int src_match, dst_match;
+			int src_any, dst_any;
+
 			/* Exact match. */
-			if (!bacmp(&bt_sk(sk)->src, src)) {
+			src_match = !bacmp(&bt_sk(sk)->src, src);
+			dst_match = !bacmp(&bt_sk(sk)->dst, dst);
+			if (src_match && dst_match) {
 				read_unlock(&chan_list_lock);
 				return c;
 			}
 
 			/* Closest match */
-			if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
+			src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
+			dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
+			if ((src_match && dst_any) || (src_any && dst_match) ||
+			    (src_any && dst_any))
 				c1 = c;
 		}
 	}
@@ -872,7 +1142,7 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
 	/* Check if we have socket listening on cid */
 	pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
-							conn->src);
+					  conn->src, conn->dst);
 	if (!pchan)
 		return;
 
@@ -910,29 +1180,6 @@ clean:
 	release_sock(parent);
 }
 
-static void l2cap_chan_ready(struct l2cap_chan *chan)
-{
-	struct sock *sk = chan->sk;
-	struct sock *parent;
-
-	lock_sock(sk);
-
-	parent = bt_sk(sk)->parent;
-
-	BT_DBG("sk %p, parent %p", sk, parent);
-
-	chan->conf_state = 0;
-	__clear_chan_timer(chan);
-
-	__l2cap_state_change(chan, BT_CONNECTED);
-	sk->sk_state_change(sk);
-
-	if (parent)
-		parent->sk_data_ready(parent, 0);
-
-	release_sock(sk);
-}
-
 static void l2cap_conn_ready(struct l2cap_conn *conn)
 {
 	struct l2cap_chan *chan;
@@ -1016,6 +1263,7 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
 
 	/* Kill channels */
 	list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
+		l2cap_chan_hold(chan);
 		l2cap_chan_lock(chan);
 
 		l2cap_chan_del(chan, err);
@@ -1023,6 +1271,7 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
 		l2cap_chan_unlock(chan);
 
 		chan->ops->close(chan->data);
+		l2cap_chan_put(chan);
 	}
 
 	mutex_unlock(&conn->chan_lock);
@@ -1100,10 +1349,12 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 
 /* ---- Socket interface ---- */
 
-/* Find socket with psm and source bdaddr.
+/* Find socket with psm and source / destination bdaddr.
  * Returns closest match.
  */
-static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src)
+static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
+						   bdaddr_t *src,
+						   bdaddr_t *dst)
 {
 	struct l2cap_chan *c, *c1 = NULL;
 
@@ -1116,14 +1367,22 @@ static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr
 			continue;
 
 		if (c->psm == psm) {
+			int src_match, dst_match;
+			int src_any, dst_any;
+
 			/* Exact match. */
-			if (!bacmp(&bt_sk(sk)->src, src)) {
+			src_match = !bacmp(&bt_sk(sk)->src, src);
+			dst_match = !bacmp(&bt_sk(sk)->dst, dst);
+			if (src_match && dst_match) {
 				read_unlock(&chan_list_lock);
 				return c;
 			}
 
 			/* Closest match */
-			if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
+			src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
+			dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
+			if ((src_match && dst_any) || (src_any && dst_match) ||
+			    (src_any && dst_any))
 				c1 = c;
 		}
 	}
@@ -1133,7 +1392,8 @@ static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr
 	return c1;
 }
 
-int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *dst)
+int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
+		       bdaddr_t *dst, u8 dst_type)
 {
 	struct sock *sk = chan->sk;
 	bdaddr_t *src = &bt_sk(sk)->src;
@@ -1143,8 +1403,8 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
 	__u8 auth_type;
 	int err;
 
-	BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
-							chan->psm);
+	BT_DBG("%s -> %s (type %u) psm 0x%2.2x", batostr(src), batostr(dst),
+	       dst_type, __le16_to_cpu(chan->psm));
 
 	hdev = hci_get_route(dst, src);
 	if (!hdev)
@@ -1218,11 +1478,11 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
 	auth_type = l2cap_get_auth_type(chan);
 
 	if (chan->dcid == L2CAP_CID_LE_DATA)
-		hcon = hci_connect(hdev, LE_LINK, dst,
-					chan->sec_level, auth_type);
+		hcon = hci_connect(hdev, LE_LINK, dst, dst_type,
+				   chan->sec_level, auth_type);
 	else
-		hcon = hci_connect(hdev, ACL_LINK, dst,
-					chan->sec_level, auth_type);
+		hcon = hci_connect(hdev, ACL_LINK, dst, dst_type,
+				   chan->sec_level, auth_type);
 
 	if (IS_ERR(hcon)) {
 		err = PTR_ERR(hcon);
@@ -1236,6 +1496,18 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
 		goto done;
 	}
 
+	if (hcon->type == LE_LINK) {
+		err = 0;
+
+		if (!list_empty(&conn->chan_l)) {
+			err = -EBUSY;
+			hci_conn_put(hcon);
+		}
+
+		if (err)
+			goto done;
+	}
+
 	/* Update source addr of the socket */
 	bacpy(src, conn->src);
 
@@ -1346,7 +1618,7 @@ static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
 
 	while ((skb = skb_peek(&chan->tx_q)) &&
 			chan->unacked_frames) {
-		if (bt_cb(skb)->tx_seq == chan->expected_ack_seq)
+		if (bt_cb(skb)->control.txseq == chan->expected_ack_seq)
 			break;
 
 		skb = skb_dequeue(&chan->tx_q);
@@ -1368,6 +1640,7 @@ static void l2cap_streaming_send(struct l2cap_chan *chan)
 	while ((skb = skb_dequeue(&chan->tx_q))) {
 		control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
 		control |= __set_txseq(chan, chan->next_tx_seq);
+		control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
 		__put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
 
 		if (chan->fcs == L2CAP_FCS_CRC16) {
@@ -1393,21 +1666,21 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq)
 	if (!skb)
 		return;
 
-	while (bt_cb(skb)->tx_seq != tx_seq) {
+	while (bt_cb(skb)->control.txseq != tx_seq) {
 		if (skb_queue_is_last(&chan->tx_q, skb))
 			return;
 
 		skb = skb_queue_next(&chan->tx_q, skb);
 	}
 
-	if (chan->remote_max_tx &&
-			bt_cb(skb)->retries == chan->remote_max_tx) {
+	if (bt_cb(skb)->control.retries == chan->remote_max_tx &&
+	    chan->remote_max_tx) {
 		l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
 		return;
 	}
 
 	tx_skb = skb_clone(skb, GFP_ATOMIC);
-	bt_cb(skb)->retries++;
+	bt_cb(skb)->control.retries++;
 
 	control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
 	control &= __get_sar_mask(chan);
@@ -1440,17 +1713,20 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
 	if (chan->state != BT_CONNECTED)
 		return -ENOTCONN;
 
+	if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
+		return 0;
+
 	while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
 
-		if (chan->remote_max_tx &&
-				bt_cb(skb)->retries == chan->remote_max_tx) {
+		if (bt_cb(skb)->control.retries == chan->remote_max_tx &&
+		    chan->remote_max_tx) {
 			l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
 			break;
 		}
 
 		tx_skb = skb_clone(skb, GFP_ATOMIC);
 
-		bt_cb(skb)->retries++;
+		bt_cb(skb)->control.retries++;
 
 		control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
 		control &= __get_sar_mask(chan);
@@ -1460,6 +1736,7 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
 
 		control |= __set_reqseq(chan, chan->buffer_seq);
 		control |= __set_txseq(chan, chan->next_tx_seq);
+		control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
 
 		__put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
 
@@ -1474,11 +1751,11 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
 
 		__set_retrans_timer(chan);
 
-		bt_cb(skb)->tx_seq = chan->next_tx_seq;
+		bt_cb(skb)->control.txseq = chan->next_tx_seq;
 
 		chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
 
-		if (bt_cb(skb)->retries == 1) {
+		if (bt_cb(skb)->control.retries == 1) {
 			chan->unacked_frames++;
 
 			if (!nsent++)
@@ -1554,7 +1831,7 @@ static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct sk_buff **frag;
-	int err, sent = 0;
+	int sent = 0;
 
 	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
 		return -EFAULT;
@@ -1565,14 +1842,17 @@ static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
 	/* Continuation fragments (no L2CAP header) */
 	frag = &skb_shinfo(skb)->frag_list;
 	while (len) {
+		struct sk_buff *tmp;
+
 		count = min_t(unsigned int, conn->mtu, len);
 
-		*frag = chan->ops->alloc_skb(chan, count,
-					     msg->msg_flags & MSG_DONTWAIT,
-					     &err);
+		tmp = chan->ops->alloc_skb(chan, count,
+					   msg->msg_flags & MSG_DONTWAIT);
+		if (IS_ERR(tmp))
+			return PTR_ERR(tmp);
+
+		*frag = tmp;
 
-		if (!*frag)
-			return err;
 		if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
 			return -EFAULT;
 
@@ -1581,6 +1861,9 @@ static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
 		sent += count;
 		len  -= count;
 
+		skb->len += (*frag)->len;
+		skb->data_len += (*frag)->len;
+
 		frag = &(*frag)->next;
 	}
 
@@ -1601,18 +1884,17 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
 	count = min_t(unsigned int, (conn->mtu - hlen), len);
 
 	skb = chan->ops->alloc_skb(chan, count + hlen,
-				   msg->msg_flags & MSG_DONTWAIT, &err);
-
-	if (!skb)
-		return ERR_PTR(err);
+				   msg->msg_flags & MSG_DONTWAIT);
+	if (IS_ERR(skb))
+		return skb;
 
 	skb->priority = priority;
 
 	/* Create L2CAP header */
 	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 	lh->cid = cpu_to_le16(chan->dcid);
-	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
-	put_unaligned_le16(chan->psm, skb_put(skb, 2));
+	lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
+	put_unaligned(chan->psm, skb_put(skb, L2CAP_PSMLEN_SIZE));
 
 	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
 	if (unlikely(err < 0)) {
@@ -1628,25 +1910,24 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct sk_buff *skb;
-	int err, count, hlen = L2CAP_HDR_SIZE;
+	int err, count;
 	struct l2cap_hdr *lh;
 
 	BT_DBG("chan %p len %d", chan, (int)len);
 
-	count = min_t(unsigned int, (conn->mtu - hlen), len);
+	count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
 
-	skb = chan->ops->alloc_skb(chan, count + hlen,
-				   msg->msg_flags & MSG_DONTWAIT, &err);
-
-	if (!skb)
-		return ERR_PTR(err);
+	skb = chan->ops->alloc_skb(chan, count + L2CAP_HDR_SIZE,
+				   msg->msg_flags & MSG_DONTWAIT);
+	if (IS_ERR(skb))
+		return skb;
 
 	skb->priority = priority;
 
 	/* Create L2CAP header */
 	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 	lh->cid = cpu_to_le16(chan->dcid);
-	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+	lh->len = cpu_to_le16(len);
 
 	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
 	if (unlikely(err < 0)) {
@@ -1658,7 +1939,7 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
 
 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
 						struct msghdr *msg, size_t len,
-						u32 control, u16 sdulen)
+						u16 sdulen)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct sk_buff *skb;
@@ -1684,17 +1965,16 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
 	count = min_t(unsigned int, (conn->mtu - hlen), len);
 
 	skb = chan->ops->alloc_skb(chan, count + hlen,
-					msg->msg_flags & MSG_DONTWAIT, &err);
-
-	if (!skb)
-		return ERR_PTR(err);
+				   msg->msg_flags & MSG_DONTWAIT);
+	if (IS_ERR(skb))
+		return skb;
 
 	/* Create L2CAP header */
 	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 	lh->cid = cpu_to_le16(chan->dcid);
 	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
 
-	__put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
+	__put_control(chan, 0, skb_put(skb, __ctrl_size(chan)));
 
 	if (sdulen)
 		put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
@@ -1708,61 +1988,82 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
 	if (chan->fcs == L2CAP_FCS_CRC16)
 		put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE));
 
-	bt_cb(skb)->retries = 0;
+	bt_cb(skb)->control.retries = 0;
 	return skb;
 }
 
-static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
+static int l2cap_segment_sdu(struct l2cap_chan *chan,
+			     struct sk_buff_head *seg_queue,
+			     struct msghdr *msg, size_t len)
 {
 	struct sk_buff *skb;
-	struct sk_buff_head sar_queue;
-	u32 control;
-	size_t size = 0;
+	u16 sdu_len;
+	size_t pdu_len;
+	int err = 0;
+	u8 sar;
 
-	skb_queue_head_init(&sar_queue);
-	control = __set_ctrl_sar(chan, L2CAP_SAR_START);
-	skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len);
-	if (IS_ERR(skb))
-		return PTR_ERR(skb);
+	BT_DBG("chan %p, msg %p, len %d", chan, msg, (int)len);
 
-	__skb_queue_tail(&sar_queue, skb);
-	len -= chan->remote_mps;
-	size += chan->remote_mps;
+	/* It is critical that ERTM PDUs fit in a single HCI fragment,
+	 * so fragmented skbs are not used.  The HCI layer's handling
+	 * of fragmented skbs is not compatible with ERTM's queueing.
+	 */
 
-	while (len > 0) {
-		size_t buflen;
+	/* PDU size is derived from the HCI MTU */
+	pdu_len = chan->conn->mtu;
 
-		if (len > chan->remote_mps) {
-			control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE);
-			buflen = chan->remote_mps;
-		} else {
-			control = __set_ctrl_sar(chan, L2CAP_SAR_END);
-			buflen = len;
-		}
+	pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
+
+	/* Adjust for largest possible L2CAP overhead. */
+	pdu_len -= L2CAP_EXT_HDR_SIZE + L2CAP_FCS_SIZE;
+
+	/* Remote device may have requested smaller PDUs */
+	pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
+
+	if (len <= pdu_len) {
+		sar = L2CAP_SAR_UNSEGMENTED;
+		sdu_len = 0;
+		pdu_len = len;
+	} else {
+		sar = L2CAP_SAR_START;
+		sdu_len = len;
+		pdu_len -= L2CAP_SDULEN_SIZE;
+	}
+
+	while (len > 0) {
+		skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
 
-		skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0);
 		if (IS_ERR(skb)) {
-			skb_queue_purge(&sar_queue);
+			__skb_queue_purge(seg_queue);
 			return PTR_ERR(skb);
 		}
 
-		__skb_queue_tail(&sar_queue, skb);
-		len -= buflen;
-		size += buflen;
+		bt_cb(skb)->control.sar = sar;
+		__skb_queue_tail(seg_queue, skb);
+
+		len -= pdu_len;
+		if (sdu_len) {
+			sdu_len = 0;
+			pdu_len += L2CAP_SDULEN_SIZE;
+		}
+
+		if (len <= pdu_len) {
+			sar = L2CAP_SAR_END;
+			pdu_len = len;
+		} else {
+			sar = L2CAP_SAR_CONTINUE;
+		}
 	}
-	skb_queue_splice_tail(&sar_queue, &chan->tx_q);
-	if (chan->tx_send_head == NULL)
-		chan->tx_send_head = sar_queue.next;
 
-	return size;
+	return err;
 }
 
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 								u32 priority)
 {
 	struct sk_buff *skb;
-	u32 control;
 	int err;
+	struct sk_buff_head seg_queue;
 
 	/* Connectionless channel */
 	if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
@@ -1791,42 +2092,47 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 
 	case L2CAP_MODE_ERTM:
 	case L2CAP_MODE_STREAMING:
-		/* Entire SDU fits into one PDU */
-		if (len <= chan->remote_mps) {
-			control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED);
-			skb = l2cap_create_iframe_pdu(chan, msg, len, control,
-									0);
-			if (IS_ERR(skb))
-				return PTR_ERR(skb);
+		/* Check outgoing MTU */
+		if (len > chan->omtu) {
+			err = -EMSGSIZE;
+			break;
+		}
 
-			__skb_queue_tail(&chan->tx_q, skb);
+		__skb_queue_head_init(&seg_queue);
 
-			if (chan->tx_send_head == NULL)
-				chan->tx_send_head = skb;
+		/* Do segmentation before calling in to the state machine,
+		 * since it's possible to block while waiting for memory
+		 * allocation.
+		 */
+		err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
 
-		} else {
-			/* Segment SDU into multiples PDUs */
-			err = l2cap_sar_segment_sdu(chan, msg, len);
-			if (err < 0)
-				return err;
+		/* The channel could have been closed while segmenting,
+		 * check that it is still connected.
+		 */
+		if (chan->state != BT_CONNECTED) {
+			__skb_queue_purge(&seg_queue);
+			err = -ENOTCONN;
 		}
 
-		if (chan->mode == L2CAP_MODE_STREAMING) {
-			l2cap_streaming_send(chan);
-			err = len;
+		if (err)
 			break;
-		}
 
-		if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
-				test_bit(CONN_WAIT_F, &chan->conn_state)) {
-			err = len;
-			break;
-		}
+		if (chan->mode == L2CAP_MODE_ERTM && chan->tx_send_head == NULL)
+			chan->tx_send_head = seg_queue.next;
+		skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
+
+		if (chan->mode == L2CAP_MODE_ERTM)
+			err = l2cap_ertm_send(chan);
+		else
+			l2cap_streaming_send(chan);
 
-		err = l2cap_ertm_send(chan);
 		if (err >= 0)
 			err = len;
 
+		/* If the skbs were not queued for sending, they'll still be in
+		 * seg_queue and need to be purged.
+		 */
+		__skb_queue_purge(&seg_queue);
 		break;
 
 	default:
@@ -2040,13 +2346,29 @@ static void l2cap_ack_timeout(struct work_struct *work)
 	l2cap_chan_put(chan);
 }
 
-static inline void l2cap_ertm_init(struct l2cap_chan *chan)
+static inline int l2cap_ertm_init(struct l2cap_chan *chan)
 {
+	int err;
+
+	chan->next_tx_seq = 0;
+	chan->expected_tx_seq = 0;
 	chan->expected_ack_seq = 0;
 	chan->unacked_frames = 0;
 	chan->buffer_seq = 0;
 	chan->num_acked = 0;
 	chan->frames_sent = 0;
+	chan->last_acked_seq = 0;
+	chan->sdu = NULL;
+	chan->sdu_last_frag = NULL;
+	chan->sdu_len = 0;
+
+	skb_queue_head_init(&chan->tx_q);
+
+	if (chan->mode != L2CAP_MODE_ERTM)
+		return 0;
+
+	chan->rx_state = L2CAP_RX_STATE_RECV;
+	chan->tx_state = L2CAP_TX_STATE_XMIT;
 
 	INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
 	INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
@@ -2055,6 +2377,11 @@ static inline void l2cap_ertm_init(struct l2cap_chan *chan)
 	skb_queue_head_init(&chan->srej_q);
 
 	INIT_LIST_HEAD(&chan->srej_l);
+	err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
+	if (err < 0)
+		return err;
+
+	return l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
 }
 
 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
@@ -2378,9 +2705,9 @@ done:
 			chan->remote_mps = size;
 
 			rfc.retrans_timeout =
-				le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
+				__constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
 			rfc.monitor_timeout =
-				le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
+				__constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
 
 			set_bit(CONF_MODE_DONE, &chan->conf_state);
 
@@ -2644,10 +2971,10 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 	u16 dcid = 0, scid = __le16_to_cpu(req->scid);
 	__le16 psm = req->psm;
 
-	BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
+	BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
 
 	/* Check if we have socket listening on psm */
-	pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
+	pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src, conn->dst);
 	if (!pchan) {
 		result = L2CAP_CR_BAD_PSM;
 		goto sendresp;
@@ -2706,7 +3033,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
 	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
 		if (l2cap_chan_check_security(chan)) {
-			if (bt_sk(sk)->defer_setup) {
+			if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) {
 				__l2cap_state_change(chan, BT_CONNECT2);
 				result = L2CAP_CR_PEND;
 				status = L2CAP_CS_AUTHOR_PEND;
@@ -2848,7 +3175,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 	u16 dcid, flags;
 	u8 rsp[64];
 	struct l2cap_chan *chan;
-	int len;
+	int len, err = 0;
 
 	dcid  = __le16_to_cpu(req->dcid);
 	flags = __le16_to_cpu(req->flags);
@@ -2859,8 +3186,6 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 	if (!chan)
 		return -ENOENT;
 
-	l2cap_chan_lock(chan);
-
 	if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
 		struct l2cap_cmd_rej_cid rej;
 
@@ -2915,13 +3240,15 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
 		l2cap_state_change(chan, BT_CONNECTED);
 
-		chan->next_tx_seq = 0;
-		chan->expected_tx_seq = 0;
-		skb_queue_head_init(&chan->tx_q);
-		if (chan->mode == L2CAP_MODE_ERTM)
-			l2cap_ertm_init(chan);
+		if (chan->mode == L2CAP_MODE_ERTM ||
+		    chan->mode == L2CAP_MODE_STREAMING)
+			err = l2cap_ertm_init(chan);
+
+		if (err < 0)
+			l2cap_send_disconn_req(chan->conn, chan, -err);
+		else
+			l2cap_chan_ready(chan);
 
-		l2cap_chan_ready(chan);
 		goto unlock;
 	}
 
@@ -2949,7 +3276,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
 unlock:
 	l2cap_chan_unlock(chan);
-	return 0;
+	return err;
 }
 
 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
@@ -2957,21 +3284,20 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 	struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
 	u16 scid, flags, result;
 	struct l2cap_chan *chan;
-	int len = cmd->len - sizeof(*rsp);
+	int len = le16_to_cpu(cmd->len) - sizeof(*rsp);
+	int err = 0;
 
 	scid   = __le16_to_cpu(rsp->scid);
 	flags  = __le16_to_cpu(rsp->flags);
 	result = __le16_to_cpu(rsp->result);
 
-	BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
-			scid, flags, result);
+	BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
+	       result, len);
 
 	chan = l2cap_get_chan_by_scid(conn, scid);
 	if (!chan)
 		return 0;
 
-	l2cap_chan_lock(chan);
-
 	switch (result) {
 	case L2CAP_CONF_SUCCESS:
 		l2cap_conf_rfc_get(chan, rsp->data, len);
@@ -3045,18 +3371,19 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 		set_default_fcs(chan);
 
 		l2cap_state_change(chan, BT_CONNECTED);
-		chan->next_tx_seq = 0;
-		chan->expected_tx_seq = 0;
-		skb_queue_head_init(&chan->tx_q);
-		if (chan->mode ==  L2CAP_MODE_ERTM)
-			l2cap_ertm_init(chan);
+		if (chan->mode == L2CAP_MODE_ERTM ||
+		    chan->mode == L2CAP_MODE_STREAMING)
+			err = l2cap_ertm_init(chan);
 
-		l2cap_chan_ready(chan);
+		if (err < 0)
+			l2cap_send_disconn_req(chan->conn, chan, -err);
+		else
+			l2cap_chan_ready(chan);
 	}
 
 done:
 	l2cap_chan_unlock(chan);
-	return 0;
+	return err;
 }
 
 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
@@ -3092,11 +3419,13 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
 	sk->sk_shutdown = SHUTDOWN_MASK;
 	release_sock(sk);
 
+	l2cap_chan_hold(chan);
 	l2cap_chan_del(chan, ECONNRESET);
 
 	l2cap_chan_unlock(chan);
 
 	chan->ops->close(chan->data);
+	l2cap_chan_put(chan);
 
 	mutex_unlock(&conn->chan_lock);
 
@@ -3124,11 +3453,13 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
 
 	l2cap_chan_lock(chan);
 
+	l2cap_chan_hold(chan);
 	l2cap_chan_del(chan, 0);
 
 	l2cap_chan_unlock(chan);
 
 	chan->ops->close(chan->data);
+	l2cap_chan_put(chan);
 
 	mutex_unlock(&conn->chan_lock);
 
@@ -3265,8 +3596,8 @@ static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
 	/* Placeholder: Always reject */
 	rsp.dcid = 0;
 	rsp.scid = cpu_to_le16(scid);
-	rsp.result = L2CAP_CR_NO_MEM;
-	rsp.status = L2CAP_CS_NO_INFO;
+	rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
+	rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
 
 	l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
 		       sizeof(rsp), &rsp);
@@ -3665,19 +3996,19 @@ static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb,
 	struct sk_buff *next_skb;
 	int tx_seq_offset, next_tx_seq_offset;
 
-	bt_cb(skb)->tx_seq = tx_seq;
-	bt_cb(skb)->sar = sar;
+	bt_cb(skb)->control.txseq = tx_seq;
+	bt_cb(skb)->control.sar = sar;
 
 	next_skb = skb_peek(&chan->srej_q);
 
 	tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
 
 	while (next_skb) {
-		if (bt_cb(next_skb)->tx_seq == tx_seq)
+		if (bt_cb(next_skb)->control.txseq == tx_seq)
 			return -EINVAL;
 
 		next_tx_seq_offset = __seq_offset(chan,
-				bt_cb(next_skb)->tx_seq, chan->buffer_seq);
+			bt_cb(next_skb)->control.txseq, chan->buffer_seq);
 
 		if (next_tx_seq_offset > tx_seq_offset) {
 			__skb_queue_before(&chan->srej_q, next_skb, skb);
@@ -3800,6 +4131,7 @@ static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
 	BT_DBG("chan %p, Enter local busy", chan);
 
 	set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
+	l2cap_seq_list_clear(&chan->srej_list);
 
 	__set_ack_timer(chan);
 }
@@ -3848,11 +4180,11 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
 			!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
 		int err;
 
-		if (bt_cb(skb)->tx_seq != tx_seq)
+		if (bt_cb(skb)->control.txseq != tx_seq)
 			break;
 
 		skb = skb_dequeue(&chan->srej_q);
-		control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
+		control = __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
 		err = l2cap_reassemble_sdu(chan, skb, control);
 
 		if (err < 0) {
@@ -3892,6 +4224,7 @@ static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
 	while (tx_seq != chan->expected_tx_seq) {
 		control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
 		control |= __set_reqseq(chan, chan->expected_tx_seq);
+		l2cap_seq_list_append(&chan->srej_list, chan->expected_tx_seq);
 		l2cap_send_sframe(chan, control);
 
 		new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
@@ -4022,8 +4355,8 @@ expected:
 	chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
 
 	if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
-		bt_cb(skb)->tx_seq = tx_seq;
-		bt_cb(skb)->sar = sar;
+		bt_cb(skb)->control.txseq = tx_seq;
+		bt_cb(skb)->control.sar = sar;
 		__skb_queue_tail(&chan->srej_q, skb);
 		return 0;
 	}
@@ -4220,6 +4553,8 @@ static int l2cap_ertm_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
 	u16 req_seq;
 	int len, next_tx_seq_offset, req_seq_offset;
 
+	__unpack_control(chan, skb);
+
 	control = __get_control(chan, skb->data);
 	skb_pull(skb, __ctrl_size(chan));
 	len = skb->len;
@@ -4295,8 +4630,6 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
 		return 0;
 	}
 
-	l2cap_chan_lock(chan);
-
 	BT_DBG("chan %p, len %d", chan, skb->len);
 
 	if (chan->state != BT_CONNECTED)
@@ -4375,7 +4708,7 @@ static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, str
 {
 	struct l2cap_chan *chan;
 
-	chan = l2cap_global_chan_by_psm(0, psm, conn->src);
+	chan = l2cap_global_chan_by_psm(0, psm, conn->src, conn->dst);
 	if (!chan)
 		goto drop;
 
@@ -4396,11 +4729,12 @@ drop:
 	return 0;
 }
 
-static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
+static inline int l2cap_att_channel(struct l2cap_conn *conn, u16 cid,
+				    struct sk_buff *skb)
 {
 	struct l2cap_chan *chan;
 
-	chan = l2cap_global_chan_by_scid(0, cid, conn->src);
+	chan = l2cap_global_chan_by_scid(0, cid, conn->src, conn->dst);
 	if (!chan)
 		goto drop;
 
@@ -4445,7 +4779,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
 		break;
 
 	case L2CAP_CID_CONN_LESS:
-		psm = get_unaligned_le16(skb->data);
+		psm = get_unaligned((__le16 *) skb->data);
 		skb_pull(skb, 2);
 		l2cap_conless_channel(conn, psm, skb);
 		break;
@@ -4540,7 +4874,6 @@ static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
 
 	if (encrypt == 0x00) {
 		if (chan->sec_level == BT_SECURITY_MEDIUM) {
-			__clear_chan_timer(chan);
 			__set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
 		} else if (chan->sec_level == BT_SECURITY_HIGH)
 			l2cap_chan_close(chan, ECONNREFUSED);
@@ -4561,7 +4894,8 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 	BT_DBG("conn %p", conn);
 
 	if (hcon->type == LE_LINK) {
-		smp_distribute_keys(conn, 0);
+		if (!status && encrypt)
+			smp_distribute_keys(conn, 0);
 		cancel_delayed_work(&conn->security_timer);
 	}
 
@@ -4591,7 +4925,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 						chan->state == BT_CONFIG)) {
 			struct sock *sk = chan->sk;
 
-			bt_sk(sk)->suspended = false;
+			clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
 			sk->sk_state_change(sk);
 
 			l2cap_check_encryption(chan, encrypt);
@@ -4603,7 +4937,6 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 			if (!status) {
 				l2cap_send_conn_req(chan);
 			} else {
-				__clear_chan_timer(chan);
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
 		} else if (chan->state == BT_CONNECT2) {
@@ -4614,7 +4947,8 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 			lock_sock(sk);
 
 			if (!status) {
-				if (bt_sk(sk)->defer_setup) {
+				if (test_bit(BT_SK_DEFER_SETUP,
+					     &bt_sk(sk)->flags)) {
 					struct sock *parent = bt_sk(sk)->parent;
 					res = L2CAP_CR_PEND;
 					stat = L2CAP_CS_AUTHOR_PEND;
@@ -4664,8 +4998,6 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
 	if (!(flags & ACL_CONT)) {
 		struct l2cap_hdr *hdr;
-		struct l2cap_chan *chan;
-		u16 cid;
 		int len;
 
 		if (conn->rx_len) {
@@ -4685,7 +5017,6 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
 		hdr = (struct l2cap_hdr *) skb->data;
 		len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
-		cid = __le16_to_cpu(hdr->cid);
 
 		if (len == skb->len) {
 			/* Complete frame received */
@@ -4702,23 +5033,6 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 			goto drop;
 		}
 
-		chan = l2cap_get_chan_by_scid(conn, cid);
-
-		if (chan && chan->sk) {
-			struct sock *sk = chan->sk;
-			lock_sock(sk);
-
-			if (chan->imtu < len - L2CAP_HDR_SIZE) {
-				BT_ERR("Frame exceeding recv MTU (len %d, "
-							"MTU %d)", len,
-							chan->imtu);
-				release_sock(sk);
-				l2cap_conn_unreliable(conn, ECOMM);
-				goto drop;
-			}
-			release_sock(sk);
-		}
-
 		/* Allocate skb for the complete frame (with header) */
 		conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
 		if (!conn->rx_skb)
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 04e7c172d49c..3bb1611b9d48 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -124,7 +124,7 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al
 		return -EINVAL;
 
 	err = l2cap_chan_connect(chan, la.l2_psm, __le16_to_cpu(la.l2_cid),
-				&la.l2_bdaddr);
+				 &la.l2_bdaddr, la.l2_bdaddr_type);
 	if (err)
 		return err;
 
@@ -148,12 +148,16 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)
 
 	lock_sock(sk);
 
-	if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
-			|| sk->sk_state != BT_BOUND) {
+	if (sk->sk_state != BT_BOUND) {
 		err = -EBADFD;
 		goto done;
 	}
 
+	if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) {
+		err = -EINVAL;
+		goto done;
+	}
+
 	switch (chan->mode) {
 	case L2CAP_MODE_BASIC:
 		break;
@@ -320,8 +324,8 @@ static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __us
 
 	case L2CAP_CONNINFO:
 		if (sk->sk_state != BT_CONNECTED &&
-					!(sk->sk_state == BT_CONNECT2 &&
-						bt_sk(sk)->defer_setup)) {
+		    !(sk->sk_state == BT_CONNECT2 &&
+		      test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))) {
 			err = -ENOTCONN;
 			break;
 		}
@@ -375,7 +379,10 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
 		}
 
 		memset(&sec, 0, sizeof(sec));
-		sec.level = chan->sec_level;
+		if (chan->conn)
+			sec.level = chan->conn->hcon->sec_level;
+		else
+			sec.level = chan->sec_level;
 
 		if (sk->sk_state == BT_CONNECTED)
 			sec.key_size = chan->conn->hcon->enc_key_size;
@@ -392,7 +399,8 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
 			break;
 		}
 
-		if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
+		if (put_user(test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags),
+			     (u32 __user *) optval))
 			err = -EFAULT;
 
 		break;
@@ -594,10 +602,10 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
 
 		/* or for ACL link */
 		} else if ((sk->sk_state == BT_CONNECT2 &&
-			   bt_sk(sk)->defer_setup) ||
+			   test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) ||
 			   sk->sk_state == BT_CONNECTED) {
 			if (!l2cap_chan_check_security(chan))
-				bt_sk(sk)->suspended = true;
+				set_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
 			else
 				sk->sk_state_change(sk);
 		} else {
@@ -616,7 +624,10 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
 			break;
 		}
 
-		bt_sk(sk)->defer_setup = opt;
+		if (opt)
+			set_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags);
+		else
+			clear_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags);
 		break;
 
 	case BT_FLUSHABLE:
@@ -716,16 +727,13 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
 	if (msg->msg_flags & MSG_OOB)
 		return -EOPNOTSUPP;
 
-	lock_sock(sk);
-
-	if (sk->sk_state != BT_CONNECTED) {
-		release_sock(sk);
+	if (sk->sk_state != BT_CONNECTED)
 		return -ENOTCONN;
-	}
 
+	l2cap_chan_lock(chan);
 	err = l2cap_chan_send(chan, msg, len, sk->sk_priority);
+	l2cap_chan_unlock(chan);
 
-	release_sock(sk);
 	return err;
 }
 
@@ -737,7 +745,8 @@ static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct ms
 
 	lock_sock(sk);
 
-	if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) {
+	if (sk->sk_state == BT_CONNECT2 && test_bit(BT_SK_DEFER_SETUP,
+						    &bt_sk(sk)->flags)) {
 		sk->sk_state = BT_CONFIG;
 		pi->chan->state = BT_CONFIG;
 
@@ -931,12 +940,19 @@ static void l2cap_sock_state_change_cb(void *data, int state)
 }
 
 static struct sk_buff *l2cap_sock_alloc_skb_cb(struct l2cap_chan *chan,
-					       unsigned long len, int nb,
-					       int *err)
+					       unsigned long len, int nb)
 {
-	struct sock *sk = chan->sk;
+	struct sk_buff *skb;
+	int err;
+
+	l2cap_chan_unlock(chan);
+	skb = bt_skb_send_alloc(chan->sk, len, nb, &err);
+	l2cap_chan_lock(chan);
+
+	if (!skb)
+		return ERR_PTR(err);
 
-	return bt_skb_send_alloc(sk, len, nb, err);
+	return skb;
 }
 
 static struct l2cap_ops l2cap_chan_ops = {
@@ -952,6 +968,7 @@ static void l2cap_sock_destruct(struct sock *sk)
 {
 	BT_DBG("sk %p", sk);
 
+	l2cap_chan_put(l2cap_pi(sk)->chan);
 	if (l2cap_pi(sk)->rx_busy_skb) {
 		kfree_skb(l2cap_pi(sk)->rx_busy_skb);
 		l2cap_pi(sk)->rx_busy_skb = NULL;
@@ -972,7 +989,7 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 		struct l2cap_chan *pchan = l2cap_pi(parent)->chan;
 
 		sk->sk_type = parent->sk_type;
-		bt_sk(sk)->defer_setup = bt_sk(parent)->defer_setup;
+		bt_sk(sk)->flags = bt_sk(parent)->flags;
 
 		chan->chan_type = pchan->chan_type;
 		chan->imtu = pchan->imtu;
@@ -1010,13 +1027,8 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 		} else {
 			chan->mode = L2CAP_MODE_BASIC;
 		}
-		chan->max_tx = L2CAP_DEFAULT_MAX_TX;
-		chan->fcs  = L2CAP_FCS_CRC16;
-		chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
-		chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
-		chan->sec_level = BT_SECURITY_LOW;
-		chan->flags = 0;
-		set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
+
+		l2cap_chan_set_defaults(chan);
 	}
 
 	/* Default config options */
@@ -1052,12 +1064,16 @@ static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int p
 	sk->sk_protocol = proto;
 	sk->sk_state = BT_OPEN;
 
-	chan = l2cap_chan_create(sk);
+	chan = l2cap_chan_create();
 	if (!chan) {
 		l2cap_sock_kill(sk);
 		return NULL;
 	}
 
+	l2cap_chan_hold(chan);
+
+	chan->sk = sk;
+
 	l2cap_pi(sk)->chan = chan;
 
 	return sk;
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 4bb03b111122..25d220776079 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -35,10 +35,9 @@
 #include <net/bluetooth/smp.h>
 
 bool enable_hs;
-bool enable_le;
 
 #define MGMT_VERSION	1
-#define MGMT_REVISION	0
+#define MGMT_REVISION	1
 
 static const u16 mgmt_commands[] = {
 	MGMT_OP_READ_INDEX_LIST,
@@ -78,6 +77,7 @@ static const u16 mgmt_commands[] = {
 	MGMT_OP_CONFIRM_NAME,
 	MGMT_OP_BLOCK_DEVICE,
 	MGMT_OP_UNBLOCK_DEVICE,
+	MGMT_OP_SET_DEVICE_ID,
 };
 
 static const u16 mgmt_events[] = {
@@ -224,7 +224,7 @@ static int cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
 
 	ev = (void *) skb_put(skb, sizeof(*ev));
 	ev->status = status;
-	put_unaligned_le16(cmd, &ev->opcode);
+	ev->opcode = cpu_to_le16(cmd);
 
 	err = sock_queue_rcv_skb(sk, skb);
 	if (err < 0)
@@ -254,7 +254,7 @@ static int cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
 	hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
 
 	ev = (void *) skb_put(skb, sizeof(*ev) + rp_len);
-	put_unaligned_le16(cmd, &ev->opcode);
+	ev->opcode = cpu_to_le16(cmd);
 	ev->status = status;
 
 	if (rp)
@@ -275,7 +275,7 @@ static int read_version(struct sock *sk, struct hci_dev *hdev, void *data,
 	BT_DBG("sock %p", sk);
 
 	rp.version = MGMT_VERSION;
-	put_unaligned_le16(MGMT_REVISION, &rp.revision);
+	rp.revision = __constant_cpu_to_le16(MGMT_REVISION);
 
 	return cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_VERSION, 0, &rp,
 			    sizeof(rp));
@@ -285,9 +285,9 @@ static int read_commands(struct sock *sk, struct hci_dev *hdev, void *data,
 			 u16 data_len)
 {
 	struct mgmt_rp_read_commands *rp;
-	u16 num_commands = ARRAY_SIZE(mgmt_commands);
-	u16 num_events = ARRAY_SIZE(mgmt_events);
-	u16 *opcode;
+	const u16 num_commands = ARRAY_SIZE(mgmt_commands);
+	const u16 num_events = ARRAY_SIZE(mgmt_events);
+	__le16 *opcode;
 	size_t rp_size;
 	int i, err;
 
@@ -299,8 +299,8 @@ static int read_commands(struct sock *sk, struct hci_dev *hdev, void *data,
 	if (!rp)
 		return -ENOMEM;
 
-	put_unaligned_le16(num_commands, &rp->num_commands);
-	put_unaligned_le16(num_events, &rp->num_events);
+	rp->num_commands = __constant_cpu_to_le16(num_commands);
+	rp->num_events = __constant_cpu_to_le16(num_events);
 
 	for (i = 0, opcode = rp->opcodes; i < num_commands; i++, opcode++)
 		put_unaligned_le16(mgmt_commands[i], opcode);
@@ -341,14 +341,14 @@ static int read_index_list(struct sock *sk, struct hci_dev *hdev, void *data,
 		return -ENOMEM;
 	}
 
-	put_unaligned_le16(count, &rp->num_controllers);
+	rp->num_controllers = cpu_to_le16(count);
 
 	i = 0;
 	list_for_each_entry(d, &hci_dev_list, list) {
 		if (test_bit(HCI_SETUP, &d->dev_flags))
 			continue;
 
-		put_unaligned_le16(d->id, &rp->index[i++]);
+		rp->index[i++] = cpu_to_le16(d->id);
 		BT_DBG("Added hci%u", d->id);
 	}
 
@@ -383,10 +383,8 @@ static u32 get_supported_settings(struct hci_dev *hdev)
 	if (enable_hs)
 		settings |= MGMT_SETTING_HS;
 
-	if (enable_le) {
-		if (hdev->features[4] & LMP_LE)
-			settings |= MGMT_SETTING_LE;
-	}
+	if (hdev->features[4] & LMP_LE)
+		settings |= MGMT_SETTING_LE;
 
 	return settings;
 }
@@ -442,9 +440,7 @@ static u16 get_uuid16(u8 *uuid128)
 			return 0;
 	}
 
-	memcpy(&val, &uuid128[12], 4);
-
-	val = le32_to_cpu(val);
+	val = get_unaligned_le32(&uuid128[12]);
 	if (val > 0xffff)
 		return 0;
 
@@ -479,6 +475,28 @@ static void create_eir(struct hci_dev *hdev, u8 *data)
 		ptr += (name_len + 2);
 	}
 
+	if (hdev->inq_tx_power) {
+		ptr[0] = 2;
+		ptr[1] = EIR_TX_POWER;
+		ptr[2] = (u8) hdev->inq_tx_power;
+
+		eir_len += 3;
+		ptr += 3;
+	}
+
+	if (hdev->devid_source > 0) {
+		ptr[0] = 9;
+		ptr[1] = EIR_DEVICE_ID;
+
+		put_unaligned_le16(hdev->devid_source, ptr + 2);
+		put_unaligned_le16(hdev->devid_vendor, ptr + 4);
+		put_unaligned_le16(hdev->devid_product, ptr + 6);
+		put_unaligned_le16(hdev->devid_version, ptr + 8);
+
+		eir_len += 10;
+		ptr += 10;
+	}
+
 	memset(uuid16_list, 0, sizeof(uuid16_list));
 
 	/* Group all UUID16 types */
@@ -642,8 +660,7 @@ static int read_controller_info(struct sock *sk, struct hci_dev *hdev,
 	bacpy(&rp.bdaddr, &hdev->bdaddr);
 
 	rp.version = hdev->hci_ver;
-
-	put_unaligned_le16(hdev->manufacturer, &rp.manufacturer);
+	rp.manufacturer = cpu_to_le16(hdev->manufacturer);
 
 	rp.supported_settings = cpu_to_le32(get_supported_settings(hdev));
 	rp.current_settings = cpu_to_le32(get_current_settings(hdev));
@@ -840,7 +857,7 @@ static int set_discoverable(struct sock *sk, struct hci_dev *hdev, void *data,
 
 	BT_DBG("request for %s", hdev->name);
 
-	timeout = get_unaligned_le16(&cp->timeout);
+	timeout = __le16_to_cpu(cp->timeout);
 	if (!cp->val && timeout > 0)
 		return cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE,
 				  MGMT_STATUS_INVALID_PARAMS);
@@ -1122,8 +1139,8 @@ static int set_ssp(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 	}
 
 	if (mgmt_pending_find(MGMT_OP_SET_SSP, hdev)) {
-	     err = cmd_status(sk, hdev->id, MGMT_OP_SET_SSP,
-			      MGMT_STATUS_BUSY);
+		err = cmd_status(sk, hdev->id, MGMT_OP_SET_SSP,
+				 MGMT_STATUS_BUSY);
 		goto failed;
 	}
 
@@ -1179,7 +1196,7 @@ static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 
 	hci_dev_lock(hdev);
 
-	if (!enable_le || !(hdev->features[4] & LMP_LE)) {
+	if (!(hdev->features[4] & LMP_LE)) {
 		err = cmd_status(sk, hdev->id, MGMT_OP_SET_LE,
 				 MGMT_STATUS_NOT_SUPPORTED);
 		goto unlock;
@@ -1227,10 +1244,8 @@ static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 
 	err = hci_send_cmd(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(hci_cp),
 			   &hci_cp);
-	if (err < 0) {
+	if (err < 0)
 		mgmt_pending_remove(cmd);
-		goto unlock;
-	}
 
 unlock:
 	hci_dev_unlock(hdev);
@@ -1280,10 +1295,8 @@ static int add_uuid(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 	}
 
 	cmd = mgmt_pending_add(sk, MGMT_OP_ADD_UUID, hdev, data, len);
-	if (!cmd) {
+	if (!cmd)
 		err = -ENOMEM;
-		goto failed;
-	}
 
 failed:
 	hci_dev_unlock(hdev);
@@ -1368,10 +1381,8 @@ update_class:
 	}
 
 	cmd = mgmt_pending_add(sk, MGMT_OP_REMOVE_UUID, hdev, data, len);
-	if (!cmd) {
+	if (!cmd)
 		err = -ENOMEM;
-		goto unlock;
-	}
 
 unlock:
 	hci_dev_unlock(hdev);
@@ -1422,10 +1433,8 @@ static int set_dev_class(struct sock *sk, struct hci_dev *hdev, void *data,
 	}
 
 	cmd = mgmt_pending_add(sk, MGMT_OP_SET_DEV_CLASS, hdev, data, len);
-	if (!cmd) {
+	if (!cmd)
 		err = -ENOMEM;
-		goto unlock;
-	}
 
 unlock:
 	hci_dev_unlock(hdev);
@@ -1439,7 +1448,7 @@ static int load_link_keys(struct sock *sk, struct hci_dev *hdev, void *data,
 	u16 key_count, expected_len;
 	int i;
 
-	key_count = get_unaligned_le16(&cp->key_count);
+	key_count = __le16_to_cpu(cp->key_count);
 
 	expected_len = sizeof(*cp) + key_count *
 					sizeof(struct mgmt_link_key_info);
@@ -1512,7 +1521,7 @@ static int unpair_device(struct sock *sk, struct hci_dev *hdev, void *data,
 		goto unlock;
 	}
 
-	if (cp->addr.type == MGMT_ADDR_BREDR)
+	if (cp->addr.type == BDADDR_BREDR)
 		err = hci_remove_link_key(hdev, &cp->addr.bdaddr);
 	else
 		err = hci_remove_ltk(hdev, &cp->addr.bdaddr);
@@ -1524,7 +1533,7 @@ static int unpair_device(struct sock *sk, struct hci_dev *hdev, void *data,
 	}
 
 	if (cp->disconnect) {
-		if (cp->addr.type == MGMT_ADDR_BREDR)
+		if (cp->addr.type == BDADDR_BREDR)
 			conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
 							&cp->addr.bdaddr);
 		else
@@ -1548,7 +1557,7 @@ static int unpair_device(struct sock *sk, struct hci_dev *hdev, void *data,
 		goto unlock;
 	}
 
-	put_unaligned_le16(conn->handle, &dc.handle);
+	dc.handle = cpu_to_le16(conn->handle);
 	dc.reason = 0x13; /* Remote User Terminated Connection */
 	err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc);
 	if (err < 0)
@@ -1584,7 +1593,7 @@ static int disconnect(struct sock *sk, struct hci_dev *hdev, void *data,
 		goto failed;
 	}
 
-	if (cp->addr.type == MGMT_ADDR_BREDR)
+	if (cp->addr.type == BDADDR_BREDR)
 		conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->addr.bdaddr);
 	else
 		conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->addr.bdaddr);
@@ -1601,7 +1610,7 @@ static int disconnect(struct sock *sk, struct hci_dev *hdev, void *data,
 		goto failed;
 	}
 
-	put_unaligned_le16(conn->handle, &dc.handle);
+	dc.handle = cpu_to_le16(conn->handle);
 	dc.reason = 0x13; /* Remote User Terminated Connection */
 
 	err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc);
@@ -1613,22 +1622,22 @@ failed:
 	return err;
 }
 
-static u8 link_to_mgmt(u8 link_type, u8 addr_type)
+static u8 link_to_bdaddr(u8 link_type, u8 addr_type)
 {
 	switch (link_type) {
 	case LE_LINK:
 		switch (addr_type) {
 		case ADDR_LE_DEV_PUBLIC:
-			return MGMT_ADDR_LE_PUBLIC;
-		case ADDR_LE_DEV_RANDOM:
-			return MGMT_ADDR_LE_RANDOM;
+			return BDADDR_LE_PUBLIC;
+
 		default:
-			return MGMT_ADDR_INVALID;
+			/* Fallback to LE Random address type */
+			return BDADDR_LE_RANDOM;
 		}
-	case ACL_LINK:
-		return MGMT_ADDR_BREDR;
+
 	default:
-		return MGMT_ADDR_INVALID;
+		/* Fallback to BR/EDR type */
+		return BDADDR_BREDR;
 	}
 }
 
@@ -1669,13 +1678,13 @@ static int get_connections(struct sock *sk, struct hci_dev *hdev, void *data,
 		if (!test_bit(HCI_CONN_MGMT_CONNECTED, &c->flags))
 			continue;
 		bacpy(&rp->addr[i].bdaddr, &c->dst);
-		rp->addr[i].type = link_to_mgmt(c->type, c->dst_type);
-		if (rp->addr[i].type == MGMT_ADDR_INVALID)
+		rp->addr[i].type = link_to_bdaddr(c->type, c->dst_type);
+		if (c->type == SCO_LINK || c->type == ESCO_LINK)
 			continue;
 		i++;
 	}
 
-	put_unaligned_le16(i, &rp->conn_count);
+	rp->conn_count = cpu_to_le16(i);
 
 	/* Recalculate length in case of filtered SCO connections, etc */
 	rp_len = sizeof(*rp) + (i * sizeof(struct mgmt_addr_info));
@@ -1836,7 +1845,7 @@ static void pairing_complete(struct pending_cmd *cmd, u8 status)
 	struct hci_conn *conn = cmd->user_data;
 
 	bacpy(&rp.addr.bdaddr, &conn->dst);
-	rp.addr.type = link_to_mgmt(conn->type, conn->dst_type);
+	rp.addr.type = link_to_bdaddr(conn->type, conn->dst_type);
 
 	cmd_complete(cmd->sk, cmd->index, MGMT_OP_PAIR_DEVICE, status,
 		     &rp, sizeof(rp));
@@ -1890,12 +1899,12 @@ static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data,
 	else
 		auth_type = HCI_AT_DEDICATED_BONDING_MITM;
 
-	if (cp->addr.type == MGMT_ADDR_BREDR)
-		conn = hci_connect(hdev, ACL_LINK, &cp->addr.bdaddr, sec_level,
-				   auth_type);
+	if (cp->addr.type == BDADDR_BREDR)
+		conn = hci_connect(hdev, ACL_LINK, &cp->addr.bdaddr,
+				   cp->addr.type, sec_level, auth_type);
 	else
-		conn = hci_connect(hdev, LE_LINK, &cp->addr.bdaddr, sec_level,
-				   auth_type);
+		conn = hci_connect(hdev, LE_LINK, &cp->addr.bdaddr,
+				   cp->addr.type, sec_level, auth_type);
 
 	memset(&rp, 0, sizeof(rp));
 	bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr);
@@ -1923,7 +1932,7 @@ static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data,
 	}
 
 	/* For LE, just connecting isn't a proof that the pairing finished */
-	if (cp->addr.type == MGMT_ADDR_BREDR)
+	if (cp->addr.type == BDADDR_BREDR)
 		conn->connect_cfm_cb = pairing_complete_cb;
 
 	conn->security_cfm_cb = pairing_complete_cb;
@@ -2000,7 +2009,7 @@ static int user_pairing_resp(struct sock *sk, struct hci_dev *hdev,
 		goto done;
 	}
 
-	if (type == MGMT_ADDR_BREDR)
+	if (type == BDADDR_BREDR)
 		conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, bdaddr);
 	else
 		conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, bdaddr);
@@ -2011,7 +2020,7 @@ static int user_pairing_resp(struct sock *sk, struct hci_dev *hdev,
 		goto done;
 	}
 
-	if (type == MGMT_ADDR_LE_PUBLIC || type == MGMT_ADDR_LE_RANDOM) {
+	if (type == BDADDR_LE_PUBLIC || type == BDADDR_LE_RANDOM) {
 		/* Continue with pairing via SMP */
 		err = smp_user_confirm_reply(conn, mgmt_op, passkey);
 
@@ -2295,6 +2304,12 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
 		goto failed;
 	}
 
+	if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) {
+		err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+				 MGMT_STATUS_BUSY);
+		goto failed;
+	}
+
 	if (hdev->discovery.state != DISCOVERY_STOPPED) {
 		err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
 				 MGMT_STATUS_BUSY);
@@ -2381,27 +2396,39 @@ static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
 		goto unlock;
 	}
 
-	if (hdev->discovery.state == DISCOVERY_FINDING) {
-		err = hci_cancel_inquiry(hdev);
-		if (err < 0)
-			mgmt_pending_remove(cmd);
+	switch (hdev->discovery.state) {
+	case DISCOVERY_FINDING:
+		if (test_bit(HCI_INQUIRY, &hdev->flags))
+			err = hci_cancel_inquiry(hdev);
 		else
-			hci_discovery_set_state(hdev, DISCOVERY_STOPPING);
-		goto unlock;
-	}
+			err = hci_cancel_le_scan(hdev);
 
-	e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, NAME_PENDING);
-	if (!e) {
-		mgmt_pending_remove(cmd);
-		err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, 0,
-				   &mgmt_cp->type, sizeof(mgmt_cp->type));
-		hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
-		goto unlock;
+		break;
+
+	case DISCOVERY_RESOLVING:
+		e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY,
+							NAME_PENDING);
+		if (!e) {
+			mgmt_pending_remove(cmd);
+			err = cmd_complete(sk, hdev->id,
+					   MGMT_OP_STOP_DISCOVERY, 0,
+					   &mgmt_cp->type,
+					   sizeof(mgmt_cp->type));
+			hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
+			goto unlock;
+		}
+
+		bacpy(&cp.bdaddr, &e->data.bdaddr);
+		err = hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ_CANCEL,
+				   sizeof(cp), &cp);
+
+		break;
+
+	default:
+		BT_DBG("unknown discovery state %u", hdev->discovery.state);
+		err = -EFAULT;
 	}
 
-	bacpy(&cp.bdaddr, &e->data.bdaddr);
-	err = hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ_CANCEL, sizeof(cp),
-			   &cp);
 	if (err < 0)
 		mgmt_pending_remove(cmd);
 	else
@@ -2501,6 +2528,37 @@ static int unblock_device(struct sock *sk, struct hci_dev *hdev, void *data,
 	return err;
 }
 
+static int set_device_id(struct sock *sk, struct hci_dev *hdev, void *data,
+			 u16 len)
+{
+	struct mgmt_cp_set_device_id *cp = data;
+	int err;
+	__u16 source;
+
+	BT_DBG("%s", hdev->name);
+
+	source = __le16_to_cpu(cp->source);
+
+	if (source > 0x0002)
+		return cmd_status(sk, hdev->id, MGMT_OP_SET_DEVICE_ID,
+				  MGMT_STATUS_INVALID_PARAMS);
+
+	hci_dev_lock(hdev);
+
+	hdev->devid_source = source;
+	hdev->devid_vendor = __le16_to_cpu(cp->vendor);
+	hdev->devid_product = __le16_to_cpu(cp->product);
+	hdev->devid_version = __le16_to_cpu(cp->version);
+
+	err = cmd_complete(sk, hdev->id, MGMT_OP_SET_DEVICE_ID, 0, NULL, 0);
+
+	update_eir(hdev);
+
+	hci_dev_unlock(hdev);
+
+	return err;
+}
+
 static int set_fast_connectable(struct sock *sk, struct hci_dev *hdev,
 				void *data, u16 len)
 {
@@ -2565,7 +2623,7 @@ static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev,
 	u16 key_count, expected_len;
 	int i;
 
-	key_count = get_unaligned_le16(&cp->key_count);
+	key_count = __le16_to_cpu(cp->key_count);
 
 	expected_len = sizeof(*cp) + key_count *
 					sizeof(struct mgmt_ltk_info);
@@ -2591,7 +2649,8 @@ static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev,
 		else
 			type = HCI_SMP_LTK_SLAVE;
 
-		hci_add_ltk(hdev, &key->addr.bdaddr, key->addr.type,
+		hci_add_ltk(hdev, &key->addr.bdaddr,
+			    bdaddr_to_le(key->addr.type),
 			    type, 0, key->authenticated, key->val,
 			    key->enc_size, key->ediv, key->rand);
 	}
@@ -2601,7 +2660,7 @@ static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev,
 	return 0;
 }
 
-struct mgmt_handler {
+static const struct mgmt_handler {
 	int (*func) (struct sock *sk, struct hci_dev *hdev, void *data,
 		     u16 data_len);
 	bool var_len;
@@ -2647,6 +2706,7 @@ struct mgmt_handler {
 	{ confirm_name,           false, MGMT_CONFIRM_NAME_SIZE },
 	{ block_device,           false, MGMT_BLOCK_DEVICE_SIZE },
 	{ unblock_device,         false, MGMT_UNBLOCK_DEVICE_SIZE },
+	{ set_device_id,          false, MGMT_SET_DEVICE_ID_SIZE },
 };
 
 
@@ -2657,7 +2717,7 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 	struct mgmt_hdr *hdr;
 	u16 opcode, index, len;
 	struct hci_dev *hdev = NULL;
-	struct mgmt_handler *handler;
+	const struct mgmt_handler *handler;
 	int err;
 
 	BT_DBG("got %zu bytes", msglen);
@@ -2675,9 +2735,9 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 	}
 
 	hdr = buf;
-	opcode = get_unaligned_le16(&hdr->opcode);
-	index = get_unaligned_le16(&hdr->index);
-	len = get_unaligned_le16(&hdr->len);
+	opcode = __le16_to_cpu(hdr->opcode);
+	index = __le16_to_cpu(hdr->index);
+	len = __le16_to_cpu(hdr->len);
 
 	if (len != msglen - sizeof(*hdr)) {
 		err = -EINVAL;
@@ -2884,7 +2944,8 @@ int mgmt_write_scan_failed(struct hci_dev *hdev, u8 scan, u8 status)
 	return 0;
 }
 
-int mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key, bool persistent)
+int mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key,
+		      bool persistent)
 {
 	struct mgmt_ev_new_link_key ev;
 
@@ -2892,7 +2953,7 @@ int mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key, bool persisten
 
 	ev.store_hint = persistent;
 	bacpy(&ev.key.addr.bdaddr, &key->bdaddr);
-	ev.key.addr.type = MGMT_ADDR_BREDR;
+	ev.key.addr.type = BDADDR_BREDR;
 	ev.key.type = key->type;
 	memcpy(ev.key.val, key->val, 16);
 	ev.key.pin_len = key->pin_len;
@@ -2908,7 +2969,7 @@ int mgmt_new_ltk(struct hci_dev *hdev, struct smp_ltk *key, u8 persistent)
 
 	ev.store_hint = persistent;
 	bacpy(&ev.key.addr.bdaddr, &key->bdaddr);
-	ev.key.addr.type = key->bdaddr_type;
+	ev.key.addr.type = link_to_bdaddr(LE_LINK, key->bdaddr_type);
 	ev.key.authenticated = key->authenticated;
 	ev.key.enc_size = key->enc_size;
 	ev.key.ediv = key->ediv;
@@ -2932,7 +2993,7 @@ int mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 	u16 eir_len = 0;
 
 	bacpy(&ev->addr.bdaddr, bdaddr);
-	ev->addr.type = link_to_mgmt(link_type, addr_type);
+	ev->addr.type = link_to_bdaddr(link_type, addr_type);
 
 	ev->flags = __cpu_to_le32(flags);
 
@@ -2944,7 +3005,7 @@ int mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 		eir_len = eir_append_data(ev->eir, eir_len,
 					  EIR_CLASS_OF_DEV, dev_class, 3);
 
-	put_unaligned_le16(eir_len, &ev->eir_len);
+	ev->eir_len = cpu_to_le16(eir_len);
 
 	return mgmt_event(MGMT_EV_DEVICE_CONNECTED, hdev, buf,
 			  sizeof(*ev) + eir_len, NULL);
@@ -2995,13 +3056,13 @@ int mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr,
 	mgmt_pending_foreach(MGMT_OP_DISCONNECT, hdev, disconnect_rsp, &sk);
 
 	bacpy(&ev.bdaddr, bdaddr);
-	ev.type = link_to_mgmt(link_type, addr_type);
+	ev.type = link_to_bdaddr(link_type, addr_type);
 
 	err = mgmt_event(MGMT_EV_DEVICE_DISCONNECTED, hdev, &ev, sizeof(ev),
 			 sk);
 
 	if (sk)
-	  sock_put(sk);
+		sock_put(sk);
 
 	mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp,
 			     hdev);
@@ -3021,7 +3082,7 @@ int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
 		return -ENOENT;
 
 	bacpy(&rp.addr.bdaddr, bdaddr);
-	rp.addr.type = link_to_mgmt(link_type, addr_type);
+	rp.addr.type = link_to_bdaddr(link_type, addr_type);
 
 	err = cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT,
 			   mgmt_status(status), &rp, sizeof(rp));
@@ -3039,7 +3100,7 @@ int mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 	struct mgmt_ev_connect_failed ev;
 
 	bacpy(&ev.addr.bdaddr, bdaddr);
-	ev.addr.type = link_to_mgmt(link_type, addr_type);
+	ev.addr.type = link_to_bdaddr(link_type, addr_type);
 	ev.status = mgmt_status(status);
 
 	return mgmt_event(MGMT_EV_CONNECT_FAILED, hdev, &ev, sizeof(ev), NULL);
@@ -3050,7 +3111,7 @@ int mgmt_pin_code_request(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 secure)
 	struct mgmt_ev_pin_code_request ev;
 
 	bacpy(&ev.addr.bdaddr, bdaddr);
-	ev.addr.type = MGMT_ADDR_BREDR;
+	ev.addr.type = BDADDR_BREDR;
 	ev.secure = secure;
 
 	return mgmt_event(MGMT_EV_PIN_CODE_REQUEST, hdev, &ev, sizeof(ev),
@@ -3069,7 +3130,7 @@ int mgmt_pin_code_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
 		return -ENOENT;
 
 	bacpy(&rp.addr.bdaddr, bdaddr);
-	rp.addr.type = MGMT_ADDR_BREDR;
+	rp.addr.type = BDADDR_BREDR;
 
 	err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_REPLY,
 			   mgmt_status(status), &rp, sizeof(rp));
@@ -3091,7 +3152,7 @@ int mgmt_pin_code_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
 		return -ENOENT;
 
 	bacpy(&rp.addr.bdaddr, bdaddr);
-	rp.addr.type = MGMT_ADDR_BREDR;
+	rp.addr.type = BDADDR_BREDR;
 
 	err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_NEG_REPLY,
 			   mgmt_status(status), &rp, sizeof(rp));
@@ -3110,9 +3171,9 @@ int mgmt_user_confirm_request(struct hci_dev *hdev, bdaddr_t *bdaddr,
 	BT_DBG("%s", hdev->name);
 
 	bacpy(&ev.addr.bdaddr, bdaddr);
-	ev.addr.type = link_to_mgmt(link_type, addr_type);
+	ev.addr.type = link_to_bdaddr(link_type, addr_type);
 	ev.confirm_hint = confirm_hint;
-	put_unaligned_le32(value, &ev.value);
+	ev.value = value;
 
 	return mgmt_event(MGMT_EV_USER_CONFIRM_REQUEST, hdev, &ev, sizeof(ev),
 			  NULL);
@@ -3126,7 +3187,7 @@ int mgmt_user_passkey_request(struct hci_dev *hdev, bdaddr_t *bdaddr,
 	BT_DBG("%s", hdev->name);
 
 	bacpy(&ev.addr.bdaddr, bdaddr);
-	ev.addr.type = link_to_mgmt(link_type, addr_type);
+	ev.addr.type = link_to_bdaddr(link_type, addr_type);
 
 	return mgmt_event(MGMT_EV_USER_PASSKEY_REQUEST, hdev, &ev, sizeof(ev),
 			  NULL);
@@ -3145,7 +3206,7 @@ static int user_pairing_resp_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
 		return -ENOENT;
 
 	bacpy(&rp.addr.bdaddr, bdaddr);
-	rp.addr.type = link_to_mgmt(link_type, addr_type);
+	rp.addr.type = link_to_bdaddr(link_type, addr_type);
 	err = cmd_complete(cmd->sk, hdev->id, opcode, mgmt_status(status),
 			   &rp, sizeof(rp));
 
@@ -3188,7 +3249,7 @@ int mgmt_auth_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 	struct mgmt_ev_auth_failed ev;
 
 	bacpy(&ev.addr.bdaddr, bdaddr);
-	ev.addr.type = link_to_mgmt(link_type, addr_type);
+	ev.addr.type = link_to_bdaddr(link_type, addr_type);
 	ev.status = mgmt_status(status);
 
 	return mgmt_event(MGMT_EV_AUTH_FAILED, hdev, &ev, sizeof(ev), NULL);
@@ -3413,10 +3474,10 @@ int mgmt_le_enable_complete(struct hci_dev *hdev, u8 enable, u8 status)
 
 		if (enable && test_and_clear_bit(HCI_LE_ENABLED,
 						 &hdev->dev_flags))
-		  err = new_settings(hdev, NULL);
+			err = new_settings(hdev, NULL);
 
-		mgmt_pending_foreach(MGMT_OP_SET_LE, hdev,
-				     cmd_status_rsp, &mgmt_err);
+		mgmt_pending_foreach(MGMT_OP_SET_LE, hdev, cmd_status_rsp,
+				     &mgmt_err);
 
 		return err;
 	}
@@ -3455,7 +3516,7 @@ int mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 	memset(buf, 0, sizeof(buf));
 
 	bacpy(&ev->addr.bdaddr, bdaddr);
-	ev->addr.type = link_to_mgmt(link_type, addr_type);
+	ev->addr.type = link_to_bdaddr(link_type, addr_type);
 	ev->rssi = rssi;
 	if (cfm_name)
 		ev->flags[0] |= MGMT_DEV_FOUND_CONFIRM_NAME;
@@ -3469,7 +3530,7 @@ int mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 		eir_len = eir_append_data(ev->eir, eir_len, EIR_CLASS_OF_DEV,
 					  dev_class, 3);
 
-	put_unaligned_le16(eir_len, &ev->eir_len);
+	ev->eir_len = cpu_to_le16(eir_len);
 
 	ev_size = sizeof(*ev) + eir_len;
 
@@ -3488,13 +3549,13 @@ int mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 	memset(buf, 0, sizeof(buf));
 
 	bacpy(&ev->addr.bdaddr, bdaddr);
-	ev->addr.type = link_to_mgmt(link_type, addr_type);
+	ev->addr.type = link_to_bdaddr(link_type, addr_type);
 	ev->rssi = rssi;
 
 	eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, name,
 				  name_len);
 
-	put_unaligned_le16(eir_len, &ev->eir_len);
+	ev->eir_len = cpu_to_le16(eir_len);
 
 	return mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev,
 			  sizeof(*ev) + eir_len, NULL);
@@ -3594,6 +3655,3 @@ int mgmt_device_unblocked(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
 
 module_param(enable_hs, bool, 0644);
 MODULE_PARM_DESC(enable_hs, "Enable High Speed support");
-
-module_param(enable_le, bool, 0644);
-MODULE_PARM_DESC(enable_le, "Enable Low Energy support");
diff --git a/net/bluetooth/rfcomm/sock.c b/net/bluetooth/rfcomm/sock.c
index a55a43e9f70e..e8707debb864 100644
--- a/net/bluetooth/rfcomm/sock.c
+++ b/net/bluetooth/rfcomm/sock.c
@@ -260,7 +260,8 @@ static void rfcomm_sock_init(struct sock *sk, struct sock *parent)
 
 	if (parent) {
 		sk->sk_type = parent->sk_type;
-		pi->dlc->defer_setup = bt_sk(parent)->defer_setup;
+		pi->dlc->defer_setup = test_bit(BT_SK_DEFER_SETUP,
+						&bt_sk(parent)->flags);
 
 		pi->sec_level = rfcomm_pi(parent)->sec_level;
 		pi->role_switch = rfcomm_pi(parent)->role_switch;
@@ -731,7 +732,11 @@ static int rfcomm_sock_setsockopt(struct socket *sock, int level, int optname, c
 			break;
 		}
 
-		bt_sk(sk)->defer_setup = opt;
+		if (opt)
+			set_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags);
+		else
+			clear_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags);
+
 		break;
 
 	default:
@@ -849,7 +854,8 @@ static int rfcomm_sock_getsockopt(struct socket *sock, int level, int optname, c
 			break;
 		}
 
-		if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
+		if (put_user(test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags),
+			     (u32 __user *) optval))
 			err = -EFAULT;
 
 		break;
@@ -972,7 +978,7 @@ int rfcomm_connect_ind(struct rfcomm_session *s, u8 channel, struct rfcomm_dlc *
 done:
 	bh_unlock_sock(parent);
 
-	if (bt_sk(parent)->defer_setup)
+	if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(parent)->flags))
 		parent->sk_state_change(parent);
 
 	return result;
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index f6ab12907963..cbdd313659a7 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -61,8 +61,6 @@ static struct bt_sock_list sco_sk_list = {
 static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent);
 static void sco_chan_del(struct sock *sk, int err);
 
-static int  sco_conn_del(struct hci_conn *conn, int err);
-
 static void sco_sock_close(struct sock *sk);
 static void sco_sock_kill(struct sock *sk);
 
@@ -95,12 +93,12 @@ static void sco_sock_clear_timer(struct sock *sk)
 }
 
 /* ---- SCO connections ---- */
-static struct sco_conn *sco_conn_add(struct hci_conn *hcon, __u8 status)
+static struct sco_conn *sco_conn_add(struct hci_conn *hcon)
 {
 	struct hci_dev *hdev = hcon->hdev;
 	struct sco_conn *conn = hcon->sco_data;
 
-	if (conn || status)
+	if (conn)
 		return conn;
 
 	conn = kzalloc(sizeof(struct sco_conn), GFP_ATOMIC);
@@ -195,13 +193,14 @@ static int sco_connect(struct sock *sk)
 	else
 		type = SCO_LINK;
 
-	hcon = hci_connect(hdev, type, dst, BT_SECURITY_LOW, HCI_AT_NO_BONDING);
+	hcon = hci_connect(hdev, type, dst, BDADDR_BREDR, BT_SECURITY_LOW,
+			   HCI_AT_NO_BONDING);
 	if (IS_ERR(hcon)) {
 		err = PTR_ERR(hcon);
 		goto done;
 	}
 
-	conn = sco_conn_add(hcon, 0);
+	conn = sco_conn_add(hcon);
 	if (!conn) {
 		hci_conn_put(hcon);
 		err = -ENOMEM;
@@ -233,7 +232,7 @@ static inline int sco_send_frame(struct sock *sk, struct msghdr *msg, int len)
 {
 	struct sco_conn *conn = sco_pi(sk)->conn;
 	struct sk_buff *skb;
-	int err, count;
+	int err;
 
 	/* Check outgoing MTU */
 	if (len > conn->mtu)
@@ -241,20 +240,18 @@ static inline int sco_send_frame(struct sock *sk, struct msghdr *msg, int len)
 
 	BT_DBG("sk %p len %d", sk, len);
 
-	count = min_t(unsigned int, conn->mtu, len);
-	skb = bt_skb_send_alloc(sk, count,
-			msg->msg_flags & MSG_DONTWAIT, &err);
+	skb = bt_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err);
 	if (!skb)
 		return err;
 
-	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
+	if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) {
 		kfree_skb(skb);
 		return -EFAULT;
 	}
 
 	hci_send_sco(conn->hcon, skb);
 
-	return count;
+	return len;
 }
 
 static inline void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb)
@@ -277,17 +274,20 @@ drop:
 }
 
 /* -------- Socket interface ---------- */
-static struct sock *__sco_get_sock_by_addr(bdaddr_t *ba)
+static struct sock *__sco_get_sock_listen_by_addr(bdaddr_t *ba)
 {
-	struct sock *sk;
 	struct hlist_node *node;
+	struct sock *sk;
+
+	sk_for_each(sk, node, &sco_sk_list.head) {
+		if (sk->sk_state != BT_LISTEN)
+			continue;
 
-	sk_for_each(sk, node, &sco_sk_list.head)
 		if (!bacmp(&bt_sk(sk)->src, ba))
-			goto found;
-	sk = NULL;
-found:
-	return sk;
+			return sk;
+	}
+
+	return NULL;
 }
 
 /* Find socket listening on source bdaddr.
@@ -466,7 +466,6 @@ static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_le
 {
 	struct sockaddr_sco *sa = (struct sockaddr_sco *) addr;
 	struct sock *sk = sock->sk;
-	bdaddr_t *src = &sa->sco_bdaddr;
 	int err = 0;
 
 	BT_DBG("sk %p %s", sk, batostr(&sa->sco_bdaddr));
@@ -481,17 +480,14 @@ static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_le
 		goto done;
 	}
 
-	write_lock(&sco_sk_list.lock);
-
-	if (bacmp(src, BDADDR_ANY) && __sco_get_sock_by_addr(src)) {
-		err = -EADDRINUSE;
-	} else {
-		/* Save source address */
-		bacpy(&bt_sk(sk)->src, &sa->sco_bdaddr);
-		sk->sk_state = BT_BOUND;
+	if (sk->sk_type != SOCK_SEQPACKET) {
+		err = -EINVAL;
+		goto done;
 	}
 
-	write_unlock(&sco_sk_list.lock);
+	bacpy(&bt_sk(sk)->src, &sa->sco_bdaddr);
+
+	sk->sk_state = BT_BOUND;
 
 done:
 	release_sock(sk);
@@ -537,21 +533,38 @@ done:
 static int sco_sock_listen(struct socket *sock, int backlog)
 {
 	struct sock *sk = sock->sk;
+	bdaddr_t *src = &bt_sk(sk)->src;
 	int err = 0;
 
 	BT_DBG("sk %p backlog %d", sk, backlog);
 
 	lock_sock(sk);
 
-	if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
+	if (sk->sk_state != BT_BOUND) {
 		err = -EBADFD;
 		goto done;
 	}
 
+	if (sk->sk_type != SOCK_SEQPACKET) {
+		err = -EINVAL;
+		goto done;
+	}
+
+	write_lock(&sco_sk_list.lock);
+
+	if (__sco_get_sock_listen_by_addr(src)) {
+		err = -EADDRINUSE;
+		goto unlock;
+	}
+
 	sk->sk_max_ack_backlog = backlog;
 	sk->sk_ack_backlog = 0;
+
 	sk->sk_state = BT_LISTEN;
 
+unlock:
+	write_unlock(&sco_sk_list.lock);
+
 done:
 	release_sock(sk);
 	return err;
@@ -923,7 +936,7 @@ int sco_connect_cfm(struct hci_conn *hcon, __u8 status)
 	if (!status) {
 		struct sco_conn *conn;
 
-		conn = sco_conn_add(hcon, status);
+		conn = sco_conn_add(hcon);
 		if (conn)
 			sco_conn_ready(conn);
 	} else
diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index deb119875fd9..6fc7c4708f3e 100644
--- a/net/bluetooth/smp.c
+++ b/net/bluetooth/smp.c
@@ -956,7 +956,7 @@ int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 			    HCI_SMP_LTK_SLAVE, 1, authenticated,
 			    enc.ltk, smp->enc_key_size, ediv, ident.rand);
 
-		ident.ediv = cpu_to_le16(ediv);
+		ident.ediv = ediv;
 
 		smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident);
 
diff --git a/net/ipv4/fib_semantics.c b/net/ipv4/fib_semantics.c
index a8bdf7405433..e5b7182fa099 100644
--- a/net/ipv4/fib_semantics.c
+++ b/net/ipv4/fib_semantics.c
@@ -145,6 +145,12 @@ static void free_fib_info_rcu(struct rcu_head *head)
 {
 	struct fib_info *fi = container_of(head, struct fib_info, rcu);
 
+	change_nexthops(fi) {
+		if (nexthop_nh->nh_dev)
+			dev_put(nexthop_nh->nh_dev);
+	} endfor_nexthops(fi);
+
+	release_net(fi->fib_net);
 	if (fi->fib_metrics != (u32 *) dst_default_metrics)
 		kfree(fi->fib_metrics);
 	kfree(fi);
@@ -156,13 +162,7 @@ void free_fib_info(struct fib_info *fi)
 		pr_warn("Freeing alive fib_info %p\n", fi);
 		return;
 	}
-	change_nexthops(fi) {
-		if (nexthop_nh->nh_dev)
-			dev_put(nexthop_nh->nh_dev);
-		nexthop_nh->nh_dev = NULL;
-	} endfor_nexthops(fi);
 	fib_info_cnt--;
-	release_net(fi->fib_net);
 	call_rcu(&fi->rcu, free_fib_info_rcu);
 }
 
diff --git a/net/ipv4/route.c b/net/ipv4/route.c
index ffcb3b016843..98b30d08efe9 100644
--- a/net/ipv4/route.c
+++ b/net/ipv4/route.c
@@ -3452,6 +3452,7 @@ int __init ip_rt_init(void)
 					0,
 					&rt_hash_log,
 					&rt_hash_mask,
+					0,
 					rhash_entries ? 0 : 512 * 1024);
 	memset(rt_hash_table, 0, (rt_hash_mask + 1) * sizeof(struct rt_hash_bucket));
 	rt_hash_lock_init();
diff --git a/net/ipv4/tcp.c b/net/ipv4/tcp.c
index bb485fcb077e..3ba605f60e4e 100644
--- a/net/ipv4/tcp.c
+++ b/net/ipv4/tcp.c
@@ -3514,6 +3514,7 @@ void __init tcp_init(void)
 					0,
 					NULL,
 					&tcp_hashinfo.ehash_mask,
+					0,
 					thash_entries ? 0 : 512 * 1024);
 	for (i = 0; i <= tcp_hashinfo.ehash_mask; i++) {
 		INIT_HLIST_NULLS_HEAD(&tcp_hashinfo.ehash[i].chain, i);
@@ -3530,6 +3531,7 @@ void __init tcp_init(void)
 					0,
 					&tcp_hashinfo.bhash_size,
 					NULL,
+					0,
 					64 * 1024);
 	tcp_hashinfo.bhash_size = 1U << tcp_hashinfo.bhash_size;
 	for (i = 0; i < tcp_hashinfo.bhash_size; i++) {
diff --git a/net/ipv4/tcp_input.c b/net/ipv4/tcp_input.c
index cfa2aa128342..b224eb8bce8b 100644
--- a/net/ipv4/tcp_input.c
+++ b/net/ipv4/tcp_input.c
@@ -4555,6 +4555,11 @@ static bool tcp_try_coalesce(struct sock *sk,
 
 	if (tcp_hdr(from)->fin)
 		return false;
+
+	/* Its possible this segment overlaps with prior segment in queue */
+	if (TCP_SKB_CB(from)->seq != TCP_SKB_CB(to)->end_seq)
+		return false;
+
 	if (!skb_try_coalesce(to, from, fragstolen, &delta))
 		return false;
 
diff --git a/net/ipv4/udp.c b/net/ipv4/udp.c
index 609397ee78fb..eaca73644e79 100644
--- a/net/ipv4/udp.c
+++ b/net/ipv4/udp.c
@@ -2192,26 +2192,16 @@ void __init udp_table_init(struct udp_table *table, const char *name)
 {
 	unsigned int i;
 
-	if (!CONFIG_BASE_SMALL)
-		table->hash = alloc_large_system_hash(name,
-			2 * sizeof(struct udp_hslot),
-			uhash_entries,
-			21, /* one slot per 2 MB */
-			0,
-			&table->log,
-			&table->mask,
-			64 * 1024);
-	/*
-	 * Make sure hash table has the minimum size
-	 */
-	if (CONFIG_BASE_SMALL || table->mask < UDP_HTABLE_SIZE_MIN - 1) {
-		table->hash = kmalloc(UDP_HTABLE_SIZE_MIN *
-				      2 * sizeof(struct udp_hslot), GFP_KERNEL);
-		if (!table->hash)
-			panic(name);
-		table->log = ilog2(UDP_HTABLE_SIZE_MIN);
-		table->mask = UDP_HTABLE_SIZE_MIN - 1;
-	}
+	table->hash = alloc_large_system_hash(name,
+					      2 * sizeof(struct udp_hslot),
+					      uhash_entries,
+					      21, /* one slot per 2 MB */
+					      0,
+					      &table->log,
+					      &table->mask,
+					      UDP_HTABLE_SIZE_MIN,
+					      64 * 1024);
+
 	table->hash2 = table->hash + (table->mask + 1);
 	for (i = 0; i <= table->mask; i++) {
 		INIT_HLIST_NULLS_HEAD(&table->hash[i].head, i);
diff --git a/net/mac80211/agg-tx.c b/net/mac80211/agg-tx.c
index 5b7053c58732..7cf07158805c 100644
--- a/net/mac80211/agg-tx.c
+++ b/net/mac80211/agg-tx.c
@@ -421,16 +421,22 @@ static void sta_tx_agg_session_timer_expired(unsigned long data)
 	struct tid_ampdu_tx *tid_tx;
 	unsigned long timeout;
 
-	tid_tx = rcu_dereference_protected_tid_tx(sta, *ptid);
-	if (!tid_tx)
+	rcu_read_lock();
+	tid_tx = rcu_dereference(sta->ampdu_mlme.tid_tx[*ptid]);
+	if (!tid_tx || test_bit(HT_AGG_STATE_STOPPING, &tid_tx->state)) {
+		rcu_read_unlock();
 		return;
+	}
 
 	timeout = tid_tx->last_tx + TU_TO_JIFFIES(tid_tx->timeout);
 	if (time_is_after_jiffies(timeout)) {
 		mod_timer(&tid_tx->session_timer, timeout);
+		rcu_read_unlock();
 		return;
 	}
 
+	rcu_read_unlock();
+
 #ifdef CONFIG_MAC80211_HT_DEBUG
 	printk(KERN_DEBUG "tx session timer expired on tid %d\n", (u16)*ptid);
 #endif
diff --git a/net/mac80211/debugfs_netdev.c b/net/mac80211/debugfs_netdev.c
index ea0122dbd2b3..7ed433c66d68 100644
--- a/net/mac80211/debugfs_netdev.c
+++ b/net/mac80211/debugfs_netdev.c
@@ -509,6 +509,7 @@ IEEE80211_IF_FILE(dot11MeshHWMPRannInterval,
 		u.mesh.mshcfg.dot11MeshHWMPRannInterval, DEC);
 IEEE80211_IF_FILE(dot11MeshForwarding, u.mesh.mshcfg.dot11MeshForwarding, DEC);
 IEEE80211_IF_FILE(rssi_threshold, u.mesh.mshcfg.rssi_threshold, DEC);
+IEEE80211_IF_FILE(ht_opmode, u.mesh.mshcfg.ht_opmode, DEC);
 #endif
 
 #define DEBUGFS_ADD_MODE(name, mode) \
@@ -608,6 +609,7 @@ static void add_mesh_config(struct ieee80211_sub_if_data *sdata)
 	MESHPARAMS_ADD(dot11MeshHWMPRannInterval);
 	MESHPARAMS_ADD(dot11MeshGateAnnouncementProtocol);
 	MESHPARAMS_ADD(rssi_threshold);
+	MESHPARAMS_ADD(ht_opmode);
 #undef MESHPARAMS_ADD
 }
 #endif
diff --git a/net/mac80211/ibss.c b/net/mac80211/ibss.c
index 3ad33a824624..33d9d0c3e3d0 100644
--- a/net/mac80211/ibss.c
+++ b/net/mac80211/ibss.c
@@ -163,6 +163,11 @@ static void __ieee80211_sta_join_ibss(struct ieee80211_sub_if_data *sdata,
 				   sizeof(struct ieee80211_ht_operation));
 		pos = ieee80211_ie_build_ht_cap(pos, &sband->ht_cap,
 						sband->ht_cap.cap);
+		/*
+		 * Note: According to 802.11n-2009 9.13.3.1, HT Protection
+		 * field and RIFS Mode are reserved in IBSS mode, therefore
+		 * keep them at 0
+		 */
 		pos = ieee80211_ie_build_ht_oper(pos, &sband->ht_cap,
 						 chan, channel_type, 0);
 	}
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index 856237c5c1f8..d4c19a7773db 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -206,8 +206,10 @@ static void ieee80211_set_default_queues(struct ieee80211_sub_if_data *sdata)
 	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 		if (local->hw.flags & IEEE80211_HW_QUEUE_CONTROL)
 			sdata->vif.hw_queue[i] = IEEE80211_INVAL_HW_QUEUE;
-		else
+		else if (local->hw.queues >= IEEE80211_NUM_ACS)
 			sdata->vif.hw_queue[i] = i;
+		else
+			sdata->vif.hw_queue[i] = 0;
 	}
 	sdata->vif.cab_queue = IEEE80211_INVAL_HW_QUEUE;
 }
diff --git a/net/mac80211/main.c b/net/mac80211/main.c
index b70f7f09da61..f5548e953259 100644
--- a/net/mac80211/main.c
+++ b/net/mac80211/main.c
@@ -596,6 +596,9 @@ struct ieee80211_hw *ieee80211_alloc_hw(size_t priv_data_len,
 	local->hw.offchannel_tx_hw_queue = IEEE80211_INVAL_HW_QUEUE;
 	local->hw.conf.long_frame_max_tx_count = wiphy->retry_long;
 	local->hw.conf.short_frame_max_tx_count = wiphy->retry_short;
+	local->hw.radiotap_mcs_details = IEEE80211_RADIOTAP_MCS_HAVE_MCS |
+					 IEEE80211_RADIOTAP_MCS_HAVE_GI |
+					 IEEE80211_RADIOTAP_MCS_HAVE_BW;
 	local->user_power_level = -1;
 	wiphy->ht_capa_mod_mask = &mac80211_ht_capa_mod_mask;
 
diff --git a/net/mac80211/mesh.c b/net/mac80211/mesh.c
index 0675a2fec6a6..2913113c5833 100644
--- a/net/mac80211/mesh.c
+++ b/net/mac80211/mesh.c
@@ -109,8 +109,10 @@ bool mesh_matches_local(struct ieee80211_sub_if_data *sdata,
 
 	/* Disallow HT40+/- mismatch */
 	if (ie->ht_operation &&
-	    local->_oper_channel_type > NL80211_CHAN_HT20 &&
-	    sta_channel_type > NL80211_CHAN_HT20 &&
+	    (local->_oper_channel_type == NL80211_CHAN_HT40MINUS ||
+	    local->_oper_channel_type == NL80211_CHAN_HT40PLUS) &&
+	    (sta_channel_type == NL80211_CHAN_HT40MINUS ||
+	     sta_channel_type == NL80211_CHAN_HT40PLUS) &&
 	    local->_oper_channel_type != sta_channel_type)
 		goto mismatch;
 
diff --git a/net/mac80211/mesh_hwmp.c b/net/mac80211/mesh_hwmp.c
index 27e0c2f06795..9b59658e8650 100644
--- a/net/mac80211/mesh_hwmp.c
+++ b/net/mac80211/mesh_hwmp.c
@@ -603,7 +603,10 @@ static void hwmp_preq_frame_process(struct ieee80211_sub_if_data *sdata,
 				hopcount, ttl, cpu_to_le32(lifetime),
 				cpu_to_le32(metric), cpu_to_le32(preq_id),
 				sdata);
-		ifmsh->mshstats.fwded_mcast++;
+		if (!is_multicast_ether_addr(da))
+			ifmsh->mshstats.fwded_unicast++;
+		else
+			ifmsh->mshstats.fwded_mcast++;
 		ifmsh->mshstats.fwded_frames++;
 	}
 }
diff --git a/net/mac80211/mesh_plink.c b/net/mac80211/mesh_plink.c
index 8cc8461b48a0..60ef235c9d9b 100644
--- a/net/mac80211/mesh_plink.c
+++ b/net/mac80211/mesh_plink.c
@@ -105,15 +105,15 @@ static struct sta_info *mesh_plink_alloc(struct ieee80211_sub_if_data *sdata,
 	return sta;
 }
 
-/** mesh_set_ht_prot_mode - set correct HT protection mode
+/*
+ * mesh_set_ht_prot_mode - set correct HT protection mode
  *
- * Section 9.23.3.5 of IEEE 80211s standard describes the protection rules for
- * HT mesh STA in a MBSS. Three HT protection modes are supported for now,
- * non-HT mixed mode, 20MHz-protection and no-protection mode. non-HT mixed
- * mode is selected if any non-HT peers are present in our MBSS.
- * 20MHz-protection mode is selected if all peers in our 20/40MHz MBSS support
- * HT and atleast one HT20 peer is present. Otherwise no-protection mode is
- * selected.
+ * Section 9.23.3.5 of IEEE 80211-2012 describes the protection rules for HT
+ * mesh STA in a MBSS. Three HT protection modes are supported for now, non-HT
+ * mixed mode, 20MHz-protection and no-protection mode. non-HT mixed mode is
+ * selected if any non-HT peers are present in our MBSS.  20MHz-protection mode
+ * is selected if all peers in our 20/40MHz MBSS support HT and atleast one
+ * HT20 peer is present. Otherwise no-protection mode is selected.
  */
 static u32 mesh_set_ht_prot_mode(struct ieee80211_sub_if_data *sdata)
 {
@@ -128,21 +128,22 @@ static u32 mesh_set_ht_prot_mode(struct ieee80211_sub_if_data *sdata)
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(sta, &local->sta_list, list) {
-		if (sdata == sta->sdata &&
-		    sta->plink_state == NL80211_PLINK_ESTAB) {
-			switch (sta->ch_type) {
-			case NL80211_CHAN_NO_HT:
-				mpl_dbg("mesh_plink %pM: nonHT sta (%pM) is present",
-					sdata->vif.addr, sta->sta.addr);
-				non_ht_sta = true;
-				goto out;
-			case NL80211_CHAN_HT20:
-				mpl_dbg("mesh_plink %pM: HT20 sta (%pM) is present",
-					sdata->vif.addr, sta->sta.addr);
-				ht20_sta = true;
-			default:
-				break;
-			}
+		if (sdata != sta->sdata ||
+		    sta->plink_state != NL80211_PLINK_ESTAB)
+			continue;
+
+		switch (sta->ch_type) {
+		case NL80211_CHAN_NO_HT:
+			mpl_dbg("mesh_plink %pM: nonHT sta (%pM) is present",
+				sdata->vif.addr, sta->sta.addr);
+			non_ht_sta = true;
+			goto out;
+		case NL80211_CHAN_HT20:
+			mpl_dbg("mesh_plink %pM: HT20 sta (%pM) is present",
+				sdata->vif.addr, sta->sta.addr);
+			ht20_sta = true;
+		default:
+			break;
 		}
 	}
 out:
@@ -346,6 +347,15 @@ static struct sta_info *mesh_peer_init(struct ieee80211_sub_if_data *sdata,
 
 	sta = sta_info_get(sdata, addr);
 	if (!sta) {
+		/* Userspace handles peer allocation when security is enabled */
+		if (sdata->u.mesh.security & IEEE80211_MESH_SEC_AUTHED) {
+			cfg80211_notify_new_peer_candidate(sdata->dev, addr,
+							   elems->ie_start,
+							   elems->total_len,
+							   GFP_ATOMIC);
+			return NULL;
+		}
+
 		sta = mesh_plink_alloc(sdata, addr);
 		if (!sta)
 			return NULL;
@@ -387,15 +397,6 @@ void mesh_neighbour_update(struct ieee80211_sub_if_data *sdata,
 {
 	struct sta_info *sta;
 
-	/* Userspace handles peer allocation when security is enabled */
-	if (sdata->u.mesh.security & IEEE80211_MESH_SEC_AUTHED) {
-		cfg80211_notify_new_peer_candidate(sdata->dev, hw_addr,
-						   elems->ie_start,
-						   elems->total_len,
-						   GFP_KERNEL);
-		return;
-	}
-
 	rcu_read_lock();
 	sta = mesh_peer_init(sdata, hw_addr, elems);
 	if (!sta)
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 8257a09eeed4..7bcecf73aafb 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -204,14 +204,14 @@ ieee80211_add_rx_radiotap_header(struct ieee80211_local *local,
 
 	if (status->flag & RX_FLAG_HT) {
 		rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_MCS);
-		*pos++ = IEEE80211_RADIOTAP_MCS_HAVE_MCS |
-			 IEEE80211_RADIOTAP_MCS_HAVE_GI |
-			 IEEE80211_RADIOTAP_MCS_HAVE_BW;
+		*pos++ = local->hw.radiotap_mcs_details;
 		*pos = 0;
 		if (status->flag & RX_FLAG_SHORT_GI)
 			*pos |= IEEE80211_RADIOTAP_MCS_SGI;
 		if (status->flag & RX_FLAG_40MHZ)
 			*pos |= IEEE80211_RADIOTAP_MCS_BW_40;
+		if (status->flag & RX_FLAG_HT_GF)
+			*pos |= IEEE80211_RADIOTAP_MCS_FMT_GF;
 		pos++;
 		*pos++ = status->rate_idx;
 	}
diff --git a/net/mac80211/wep.c b/net/mac80211/wep.c
index 7aa31bbfaa3b..c04d401dae92 100644
--- a/net/mac80211/wep.c
+++ b/net/mac80211/wep.c
@@ -92,6 +92,7 @@ static u8 *ieee80211_wep_add_iv(struct ieee80211_local *local,
 				int keylen, int keyidx)
 {
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	unsigned int hdrlen;
 	u8 *newhdr;
 
@@ -104,6 +105,13 @@ static u8 *ieee80211_wep_add_iv(struct ieee80211_local *local,
 	hdrlen = ieee80211_hdrlen(hdr->frame_control);
 	newhdr = skb_push(skb, WEP_IV_LEN);
 	memmove(newhdr, newhdr + WEP_IV_LEN, hdrlen);
+
+	/* the HW only needs room for the IV, but not the actual IV */
+	if (info->control.hw_key &&
+	    (info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE))
+		return newhdr + hdrlen;
+
+	skb_set_network_header(skb, skb_network_offset(skb) + WEP_IV_LEN);
 	ieee80211_wep_get_iv(local, keylen, keyidx, newhdr + hdrlen);
 	return newhdr + hdrlen;
 }
@@ -313,14 +321,15 @@ ieee80211_crypto_wep_decrypt(struct ieee80211_rx_data *rx)
 static int wep_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
 {
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_key_conf *hw_key = info->control.hw_key;
 
-	if (!info->control.hw_key) {
+	if (!hw_key) {
 		if (ieee80211_wep_encrypt(tx->local, skb, tx->key->conf.key,
 					  tx->key->conf.keylen,
 					  tx->key->conf.keyidx))
 			return -1;
-	} else if (info->control.hw_key->flags &
-			IEEE80211_KEY_FLAG_GENERATE_IV) {
+	} else if ((hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV) ||
+		   (hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE)) {
 		if (!ieee80211_wep_add_iv(tx->local, skb,
 					  tx->key->conf.keylen,
 					  tx->key->conf.keyidx))
diff --git a/net/mac80211/wpa.c b/net/mac80211/wpa.c
index 0ae23c60968c..bdb53aba888e 100644
--- a/net/mac80211/wpa.c
+++ b/net/mac80211/wpa.c
@@ -183,7 +183,8 @@ static int tkip_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
 	u8 *pos;
 
 	if (info->control.hw_key &&
-	    !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV)) {
+	    !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV) &&
+	    !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE)) {
 		/* hwaccel - with no need for software-generated IV */
 		return 0;
 	}
@@ -202,8 +203,14 @@ static int tkip_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
 
 	pos = skb_push(skb, TKIP_IV_LEN);
 	memmove(pos, pos + TKIP_IV_LEN, hdrlen);
+	skb_set_network_header(skb, skb_network_offset(skb) + TKIP_IV_LEN);
 	pos += hdrlen;
 
+	/* the HW only needs room for the IV, but not the actual IV */
+	if (info->control.hw_key &&
+	    (info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE))
+		return 0;
+
 	/* Increase IV for the frame */
 	spin_lock_irqsave(&key->u.tkip.txlock, flags);
 	key->u.tkip.tx.iv16++;
@@ -422,6 +429,7 @@ static int ccmp_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
 
 	pos = skb_push(skb, CCMP_HDR_LEN);
 	memmove(pos, pos + CCMP_HDR_LEN, hdrlen);
+	skb_set_network_header(skb, skb_network_offset(skb) + CCMP_HDR_LEN);
 
 	/* the HW only needs room for the IV, but not the actual IV */
 	if (info->control.hw_key &&
diff --git a/net/nfc/core.c b/net/nfc/core.c
index 3192c3f589ee..9f6ce011d35d 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -97,7 +97,7 @@ int nfc_dev_down(struct nfc_dev *dev)
 		goto error;
 	}
 
-	if (dev->polling || dev->activated_target_idx != NFC_TARGET_IDX_NONE) {
+	if (dev->polling || dev->active_target) {
 		rc = -EBUSY;
 		goto error;
 	}
@@ -183,11 +183,27 @@ error:
 	return rc;
 }
 
+static struct nfc_target *nfc_find_target(struct nfc_dev *dev, u32 target_idx)
+{
+	int i;
+
+	if (dev->n_targets == 0)
+		return NULL;
+
+	for (i = 0; i < dev->n_targets ; i++) {
+		if (dev->targets[i].idx == target_idx)
+			return &dev->targets[i];
+	}
+
+	return NULL;
+}
+
 int nfc_dep_link_up(struct nfc_dev *dev, int target_index, u8 comm_mode)
 {
 	int rc = 0;
 	u8 *gb;
 	size_t gb_len;
+	struct nfc_target *target;
 
 	pr_debug("dev_name=%s comm %d\n", dev_name(&dev->dev), comm_mode);
 
@@ -212,9 +228,15 @@ int nfc_dep_link_up(struct nfc_dev *dev, int target_index, u8 comm_mode)
 		goto error;
 	}
 
-	rc = dev->ops->dep_link_up(dev, target_index, comm_mode, gb, gb_len);
+	target = nfc_find_target(dev, target_index);
+	if (target == NULL) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
+	rc = dev->ops->dep_link_up(dev, target, comm_mode, gb, gb_len);
 	if (!rc)
-		dev->activated_target_idx = target_index;
+		dev->active_target = target;
 
 error:
 	device_unlock(&dev->dev);
@@ -250,7 +272,7 @@ int nfc_dep_link_down(struct nfc_dev *dev)
 	rc = dev->ops->dep_link_down(dev);
 	if (!rc) {
 		dev->dep_link_up = false;
-		dev->activated_target_idx = NFC_TARGET_IDX_NONE;
+		dev->active_target = NULL;
 		nfc_llcp_mac_is_down(dev);
 		nfc_genl_dep_link_down_event(dev);
 	}
@@ -282,6 +304,7 @@ EXPORT_SYMBOL(nfc_dep_link_is_up);
 int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol)
 {
 	int rc;
+	struct nfc_target *target;
 
 	pr_debug("dev_name=%s target_idx=%u protocol=%u\n",
 		 dev_name(&dev->dev), target_idx, protocol);
@@ -293,9 +316,20 @@ int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol)
 		goto error;
 	}
 
-	rc = dev->ops->activate_target(dev, target_idx, protocol);
+	if (dev->active_target) {
+		rc = -EBUSY;
+		goto error;
+	}
+
+	target = nfc_find_target(dev, target_idx);
+	if (target == NULL) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
+	rc = dev->ops->activate_target(dev, target, protocol);
 	if (!rc) {
-		dev->activated_target_idx = target_idx;
+		dev->active_target = target;
 
 		if (dev->ops->check_presence)
 			mod_timer(&dev->check_pres_timer, jiffies +
@@ -327,11 +361,21 @@ int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx)
 		goto error;
 	}
 
+	if (dev->active_target == NULL) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
+	if (dev->active_target->idx != target_idx) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
 	if (dev->ops->check_presence)
 		del_timer_sync(&dev->check_pres_timer);
 
-	dev->ops->deactivate_target(dev, target_idx);
-	dev->activated_target_idx = NFC_TARGET_IDX_NONE;
+	dev->ops->deactivate_target(dev, dev->active_target);
+	dev->active_target = NULL;
 
 error:
 	device_unlock(&dev->dev);
@@ -365,13 +409,13 @@ int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
 		goto error;
 	}
 
-	if (dev->activated_target_idx == NFC_TARGET_IDX_NONE) {
+	if (dev->active_target == NULL) {
 		rc = -ENOTCONN;
 		kfree_skb(skb);
 		goto error;
 	}
 
-	if (target_idx != dev->activated_target_idx) {
+	if (dev->active_target->idx != target_idx) {
 		rc = -EADDRNOTAVAIL;
 		kfree_skb(skb);
 		goto error;
@@ -380,7 +424,8 @@ int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
 	if (dev->ops->check_presence)
 		del_timer_sync(&dev->check_pres_timer);
 
-	rc = dev->ops->data_exchange(dev, target_idx, skb, cb, cb_context);
+	rc = dev->ops->data_exchange(dev, dev->active_target, skb, cb,
+				     cb_context);
 
 	if (!rc && dev->ops->check_presence)
 		mod_timer(&dev->check_pres_timer, jiffies +
@@ -456,6 +501,9 @@ EXPORT_SYMBOL(nfc_alloc_recv_skb);
  * The device driver must call this function when one or many nfc targets
  * are found. After calling this function, the device driver must stop
  * polling for targets.
+ * IMPORTANT: this function must not be called from an atomic context.
+ * In addition, it must also not be called from a context that would prevent
+ * the NFC Core to call other nfc ops entry point concurrently.
  */
 int nfc_targets_found(struct nfc_dev *dev,
 		      struct nfc_target *targets, int n_targets)
@@ -469,7 +517,7 @@ int nfc_targets_found(struct nfc_dev *dev,
 	for (i = 0; i < n_targets; i++)
 		targets[i].idx = dev->target_next_idx++;
 
-	spin_lock_bh(&dev->targets_lock);
+	device_lock(&dev->dev);
 
 	dev->targets_generation++;
 
@@ -479,12 +527,12 @@ int nfc_targets_found(struct nfc_dev *dev,
 
 	if (!dev->targets) {
 		dev->n_targets = 0;
-		spin_unlock_bh(&dev->targets_lock);
+		device_unlock(&dev->dev);
 		return -ENOMEM;
 	}
 
 	dev->n_targets = n_targets;
-	spin_unlock_bh(&dev->targets_lock);
+	device_unlock(&dev->dev);
 
 	nfc_genl_targets_found(dev);
 
@@ -492,6 +540,18 @@ int nfc_targets_found(struct nfc_dev *dev,
 }
 EXPORT_SYMBOL(nfc_targets_found);
 
+/**
+ * nfc_target_lost - inform that an activated target went out of field
+ *
+ * @dev: The nfc device that had the activated target in field
+ * @target_idx: the nfc index of the target
+ *
+ * The device driver must call this function when the activated target
+ * goes out of the field.
+ * IMPORTANT: this function must not be called from an atomic context.
+ * In addition, it must also not be called from a context that would prevent
+ * the NFC Core to call other nfc ops entry point concurrently.
+ */
 int nfc_target_lost(struct nfc_dev *dev, u32 target_idx)
 {
 	struct nfc_target *tg;
@@ -499,7 +559,7 @@ int nfc_target_lost(struct nfc_dev *dev, u32 target_idx)
 
 	pr_debug("dev_name %s n_target %d\n", dev_name(&dev->dev), target_idx);
 
-	spin_lock_bh(&dev->targets_lock);
+	device_lock(&dev->dev);
 
 	for (i = 0; i < dev->n_targets; i++) {
 		tg = &dev->targets[i];
@@ -508,13 +568,13 @@ int nfc_target_lost(struct nfc_dev *dev, u32 target_idx)
 	}
 
 	if (i == dev->n_targets) {
-		spin_unlock_bh(&dev->targets_lock);
+		device_unlock(&dev->dev);
 		return -EINVAL;
 	}
 
 	dev->targets_generation++;
 	dev->n_targets--;
-	dev->activated_target_idx = NFC_TARGET_IDX_NONE;
+	dev->active_target = NULL;
 
 	if (dev->n_targets) {
 		memcpy(&dev->targets[i], &dev->targets[i + 1],
@@ -524,7 +584,7 @@ int nfc_target_lost(struct nfc_dev *dev, u32 target_idx)
 		dev->targets = NULL;
 	}
 
-	spin_unlock_bh(&dev->targets_lock);
+	device_unlock(&dev->dev);
 
 	nfc_genl_target_lost(dev, target_idx);
 
@@ -556,15 +616,16 @@ static void nfc_check_pres_work(struct work_struct *work)
 
 	device_lock(&dev->dev);
 
-	if (dev->activated_target_idx != NFC_TARGET_IDX_NONE &&
-	    timer_pending(&dev->check_pres_timer) == 0) {
-		rc = dev->ops->check_presence(dev, dev->activated_target_idx);
+	if (dev->active_target && timer_pending(&dev->check_pres_timer) == 0) {
+		rc = dev->ops->check_presence(dev, dev->active_target);
 		if (!rc) {
 			mod_timer(&dev->check_pres_timer, jiffies +
 				  msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
 		} else {
-			nfc_target_lost(dev, dev->activated_target_idx);
-			dev->activated_target_idx = NFC_TARGET_IDX_NONE;
+			u32 active_target_idx = dev->active_target->idx;
+			device_unlock(&dev->dev);
+			nfc_target_lost(dev, active_target_idx);
+			return;
 		}
 	}
 
@@ -637,14 +698,12 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
 	dev->tx_headroom = tx_headroom;
 	dev->tx_tailroom = tx_tailroom;
 
-	spin_lock_init(&dev->targets_lock);
 	nfc_genl_data_init(&dev->genl_data);
 
+
 	/* first generation must not be 0 */
 	dev->targets_generation = 1;
 
-	dev->activated_target_idx = NFC_TARGET_IDX_NONE;
-
 	if (ops->check_presence) {
 		char name[32];
 		init_timer(&dev->check_pres_timer);
@@ -662,7 +721,6 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
 		}
 	}
 
-
 	return dev;
 }
 EXPORT_SYMBOL(nfc_allocate_device);
diff --git a/net/nfc/hci/Kconfig b/net/nfc/hci/Kconfig
index 17213a6362b4..fd67f51d18e9 100644
--- a/net/nfc/hci/Kconfig
+++ b/net/nfc/hci/Kconfig
@@ -9,6 +9,7 @@ config NFC_HCI
 
 config NFC_SHDLC
 	depends on NFC_HCI
+	select CRC_CCITT
 	bool "SHDLC link layer for HCI based NFC drivers"
 	default n
 	---help---
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
index 86fd00d5a099..e1a640d2b588 100644
--- a/net/nfc/hci/core.c
+++ b/net/nfc/hci/core.c
@@ -235,13 +235,6 @@ static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
 	targets->hci_reader_gate = gate;
 
 	r = nfc_targets_found(hdev->ndev, targets, 1);
-	if (r < 0)
-		goto exit;
-
-	kfree(hdev->targets);
-	hdev->targets = targets;
-	targets = NULL;
-	hdev->target_count = 1;
 
 exit:
 	kfree(targets);
@@ -258,11 +251,6 @@ void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
 
 	switch (event) {
 	case NFC_HCI_EVT_TARGET_DISCOVERED:
-		if (hdev->poll_started == false) {
-			r = -EPROTO;
-			goto exit;
-		}
-
 		if (skb->len < 1) {	/* no status data? */
 			r = -EPROTO;
 			goto exit;
@@ -496,74 +484,42 @@ static int hci_dev_down(struct nfc_dev *nfc_dev)
 static int hci_start_poll(struct nfc_dev *nfc_dev, u32 protocols)
 {
 	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
-	int r;
 
 	if (hdev->ops->start_poll)
-		r = hdev->ops->start_poll(hdev, protocols);
+		return hdev->ops->start_poll(hdev, protocols);
 	else
-		r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+		return nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
 				       NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
-	if (r == 0)
-		hdev->poll_started = true;
-
-	return r;
 }
 
 static void hci_stop_poll(struct nfc_dev *nfc_dev)
 {
 	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 
-	if (hdev->poll_started) {
-		nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
-				   NFC_HCI_EVT_END_OPERATION, NULL, 0);
-		hdev->poll_started = false;
-	}
-}
-
-static struct nfc_target *hci_find_target(struct nfc_hci_dev *hdev,
-					  u32 target_idx)
-{
-	int i;
-	if (hdev->poll_started == false || hdev->targets == NULL)
-		return NULL;
-
-	for (i = 0; i < hdev->target_count; i++) {
-		if (hdev->targets[i].idx == target_idx)
-			return &hdev->targets[i];
-	}
-
-	return NULL;
+	nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			   NFC_HCI_EVT_END_OPERATION, NULL, 0);
 }
 
-static int hci_activate_target(struct nfc_dev *nfc_dev, u32 target_idx,
-			       u32 protocol)
+static int hci_activate_target(struct nfc_dev *nfc_dev,
+			       struct nfc_target *target, u32 protocol)
 {
-	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
-
-	if (hci_find_target(hdev, target_idx) == NULL)
-		return -ENOMEDIUM;
-
 	return 0;
 }
 
-static void hci_deactivate_target(struct nfc_dev *nfc_dev, u32 target_idx)
+static void hci_deactivate_target(struct nfc_dev *nfc_dev,
+				  struct nfc_target *target)
 {
 }
 
-static int hci_data_exchange(struct nfc_dev *nfc_dev, u32 target_idx,
+static int hci_data_exchange(struct nfc_dev *nfc_dev, struct nfc_target *target,
 			     struct sk_buff *skb, data_exchange_cb_t cb,
 			     void *cb_context)
 {
 	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 	int r;
-	struct nfc_target *target;
 	struct sk_buff *res_skb = NULL;
 
-	pr_debug("target_idx=%d\n", target_idx);
-
-	target = hci_find_target(hdev, target_idx);
-	if (target == NULL)
-		return -ENOMEDIUM;
+	pr_debug("target_idx=%d\n", target->idx);
 
 	switch (target->hci_reader_gate) {
 	case NFC_HCI_RF_READER_A_GATE:
@@ -605,7 +561,18 @@ static int hci_data_exchange(struct nfc_dev *nfc_dev, u32 target_idx,
 	return 0;
 }
 
-struct nfc_ops hci_nfc_ops = {
+static int hci_check_presence(struct nfc_dev *nfc_dev,
+			      struct nfc_target *target)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->check_presence)
+		return hdev->ops->check_presence(hdev, target);
+
+	return 0;
+}
+
+static struct nfc_ops hci_nfc_ops = {
 	.dev_up = hci_dev_up,
 	.dev_down = hci_dev_down,
 	.start_poll = hci_start_poll,
@@ -613,6 +580,7 @@ struct nfc_ops hci_nfc_ops = {
 	.activate_target = hci_activate_target,
 	.deactivate_target = hci_deactivate_target,
 	.data_exchange = hci_data_exchange,
+	.check_presence = hci_check_presence,
 };
 
 struct nfc_hci_dev *nfc_hci_allocate_device(struct nfc_hci_ops *ops,
diff --git a/net/nfc/hci/shdlc.c b/net/nfc/hci/shdlc.c
index 923bdf7c26d6..5665dc6d893a 100644
--- a/net/nfc/hci/shdlc.c
+++ b/net/nfc/hci/shdlc.c
@@ -816,6 +816,17 @@ static int nfc_shdlc_data_exchange(struct nfc_hci_dev *hdev,
 	return -EPERM;
 }
 
+static int nfc_shdlc_check_presence(struct nfc_hci_dev *hdev,
+				    struct nfc_target *target)
+{
+	struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+
+	if (shdlc->ops->check_presence)
+		return shdlc->ops->check_presence(shdlc, target);
+
+	return 0;
+}
+
 static struct nfc_hci_ops shdlc_ops = {
 	.open = nfc_shdlc_open,
 	.close = nfc_shdlc_close,
@@ -825,6 +836,7 @@ static struct nfc_hci_ops shdlc_ops = {
 	.target_from_gate = nfc_shdlc_target_from_gate,
 	.complete_target_discovered = nfc_shdlc_complete_target_discovered,
 	.data_exchange = nfc_shdlc_data_exchange,
+	.check_presence = nfc_shdlc_check_presence,
 };
 
 struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops,
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 11a3b7d98dc5..bf8ae4f0b90c 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -488,7 +488,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 
 		memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
 
-		skb_queue_head(&sock->tx_queue, pdu);
+		skb_queue_tail(&sock->tx_queue, pdu);
 
 		lock_sock(sk);
 
@@ -502,7 +502,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 
 	kfree(msg_data);
 
-	return 0;
+	return len;
 }
 
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 92988aa620dc..42994fac26d6 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -448,6 +448,8 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 {
 	struct nfc_llcp_sock *sock, *llcp_sock, *n;
 
+	pr_debug("ssap dsap %d %d\n", ssap, dsap);
+
 	if (ssap == 0 && dsap == 0)
 		return NULL;
 
@@ -783,6 +785,7 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
 {
 	struct nfc_llcp_sock *llcp_sock;
+	struct sock *sk;
 	u8 dsap, ssap;
 
 	dsap = nfc_llcp_dsap(skb);
@@ -801,10 +804,14 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
 	}
 
 	llcp_sock->dsap = ssap;
+	sk = &llcp_sock->sk;
 
 	nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
 			   skb->len - LLCP_HEADER_SIZE);
 
+	sk->sk_state = LLCP_CONNECTED;
+	sk->sk_state_change(sk);
+
 	nfc_llcp_sock_put(llcp_sock);
 }
 
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index c13e02ebdef9..3f339b19d140 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -27,6 +27,42 @@
 #include "../nfc.h"
 #include "llcp.h"
 
+static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
+{
+	DECLARE_WAITQUEUE(wait, current);
+	int err = 0;
+
+	pr_debug("sk %p", sk);
+
+	add_wait_queue(sk_sleep(sk), &wait);
+	set_current_state(TASK_INTERRUPTIBLE);
+
+	while (sk->sk_state != state) {
+		if (!timeo) {
+			err = -EINPROGRESS;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			err = sock_intr_errno(timeo);
+			break;
+		}
+
+		release_sock(sk);
+		timeo = schedule_timeout(timeo);
+		lock_sock(sk);
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		err = sock_error(sk);
+		if (err)
+			break;
+	}
+
+	__set_current_state(TASK_RUNNING);
+	remove_wait_queue(sk_sleep(sk), &wait);
+	return err;
+}
+
 static struct proto llcp_sock_proto = {
 	.name     = "NFC_LLCP",
 	.owner    = THIS_MODULE,
@@ -304,11 +340,24 @@ static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
 		mask |= POLLERR;
 
 	if (!skb_queue_empty(&sk->sk_receive_queue))
-		mask |= POLLIN;
+		mask |= POLLIN | POLLRDNORM;
 
 	if (sk->sk_state == LLCP_CLOSED)
 		mask |= POLLHUP;
 
+	if (sk->sk_shutdown & RCV_SHUTDOWN)
+		mask |= POLLRDHUP | POLLIN | POLLRDNORM;
+
+	if (sk->sk_shutdown == SHUTDOWN_MASK)
+		mask |= POLLHUP;
+
+	if (sock_writeable(sk))
+		mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
+	else
+		set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags);
+
+	pr_debug("mask 0x%x\n", mask);
+
 	return mask;
 }
 
@@ -462,9 +511,13 @@ static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
 	if (ret)
 		goto put_dev;
 
-	sk->sk_state = LLCP_CONNECTED;
+	ret = sock_wait_state(sk, LLCP_CONNECTED,
+			      sock_sndtimeo(sk, flags & O_NONBLOCK));
+	if (ret)
+		goto put_dev;
 
 	release_sock(sk);
+
 	return 0;
 
 put_dev:
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index 8737c2089fdd..d560e6f13072 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -436,16 +436,16 @@ static void nci_stop_poll(struct nfc_dev *nfc_dev)
 		    msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
 }
 
-static int nci_activate_target(struct nfc_dev *nfc_dev, __u32 target_idx,
-			       __u32 protocol)
+static int nci_activate_target(struct nfc_dev *nfc_dev,
+			       struct nfc_target *target, __u32 protocol)
 {
 	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 	struct nci_rf_discover_select_param param;
-	struct nfc_target *target = NULL;
+	struct nfc_target *nci_target = NULL;
 	int i;
 	int rc = 0;
 
-	pr_debug("target_idx %d, protocol 0x%x\n", target_idx, protocol);
+	pr_debug("target_idx %d, protocol 0x%x\n", target->idx, protocol);
 
 	if ((atomic_read(&ndev->state) != NCI_W4_HOST_SELECT) &&
 	    (atomic_read(&ndev->state) != NCI_POLL_ACTIVE)) {
@@ -459,25 +459,25 @@ static int nci_activate_target(struct nfc_dev *nfc_dev, __u32 target_idx,
 	}
 
 	for (i = 0; i < ndev->n_targets; i++) {
-		if (ndev->targets[i].idx == target_idx) {
-			target = &ndev->targets[i];
+		if (ndev->targets[i].idx == target->idx) {
+			nci_target = &ndev->targets[i];
 			break;
 		}
 	}
 
-	if (!target) {
+	if (!nci_target) {
 		pr_err("unable to find the selected target\n");
 		return -EINVAL;
 	}
 
-	if (!(target->supported_protocols & (1 << protocol))) {
+	if (!(nci_target->supported_protocols & (1 << protocol))) {
 		pr_err("target does not support the requested protocol 0x%x\n",
 		       protocol);
 		return -EINVAL;
 	}
 
 	if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) {
-		param.rf_discovery_id = target->logical_idx;
+		param.rf_discovery_id = nci_target->logical_idx;
 
 		if (protocol == NFC_PROTO_JEWEL)
 			param.rf_protocol = NCI_RF_PROTOCOL_T1T;
@@ -501,11 +501,12 @@ static int nci_activate_target(struct nfc_dev *nfc_dev, __u32 target_idx,
 	return rc;
 }
 
-static void nci_deactivate_target(struct nfc_dev *nfc_dev, __u32 target_idx)
+static void nci_deactivate_target(struct nfc_dev *nfc_dev,
+				  struct nfc_target *target)
 {
 	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 
-	pr_debug("target_idx %d\n", target_idx);
+	pr_debug("target_idx %d\n", target->idx);
 
 	if (!ndev->target_active_prot) {
 		pr_err("unable to deactivate target, no active target\n");
@@ -520,14 +521,14 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev, __u32 target_idx)
 	}
 }
 
-static int nci_data_exchange(struct nfc_dev *nfc_dev, __u32 target_idx,
+static int nci_data_exchange(struct nfc_dev *nfc_dev, struct nfc_target *target,
 			     struct sk_buff *skb,
 			     data_exchange_cb_t cb, void *cb_context)
 {
 	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 	int rc;
 
-	pr_debug("target_idx %d, len %d\n", target_idx, skb->len);
+	pr_debug("target_idx %d, len %d\n", target->idx, skb->len);
 
 	if (!ndev->target_active_prot) {
 		pr_err("unable to exchange data, no active target\n");
diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c
index a0bc326308a5..76c48c5324f8 100644
--- a/net/nfc/nci/data.c
+++ b/net/nfc/nci/data.c
@@ -49,7 +49,7 @@ void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
 
 	if (cb) {
 		ndev->data_exchange_cb = NULL;
-		ndev->data_exchange_cb_context = 0;
+		ndev->data_exchange_cb_context = NULL;
 
 		/* forward skb to nfc core */
 		cb(cb_context, skb, err);
@@ -200,10 +200,10 @@ static void nci_add_rx_data_frag(struct nci_dev *ndev,
 			pr_err("error adding room for accumulated rx data\n");
 
 			kfree_skb(skb);
-			skb = 0;
+			skb = NULL;
 
 			kfree_skb(ndev->rx_data_reassembly);
-			ndev->rx_data_reassembly = 0;
+			ndev->rx_data_reassembly = NULL;
 
 			err = -ENOMEM;
 			goto exit;
@@ -216,7 +216,7 @@ static void nci_add_rx_data_frag(struct nci_dev *ndev,
 
 		/* third, free old reassembly */
 		kfree_skb(ndev->rx_data_reassembly);
-		ndev->rx_data_reassembly = 0;
+		ndev->rx_data_reassembly = NULL;
 	}
 
 	if (pbf == NCI_PBF_CONT) {
diff --git a/net/nfc/nci/lib.c b/net/nfc/nci/lib.c
index 6a63e5eb483d..6b7fd26c68d9 100644
--- a/net/nfc/nci/lib.c
+++ b/net/nfc/nci/lib.c
@@ -31,6 +31,7 @@
 #include <linux/errno.h>
 
 #include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
 
 /* NCI status codes to Unix errno mapping */
 int nci_to_errno(__u8 code)
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c
index 99e1632e6aac..cb2646179e5f 100644
--- a/net/nfc/nci/ntf.c
+++ b/net/nfc/nci/ntf.c
@@ -497,7 +497,7 @@ static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev,
 	/* drop partial rx data packet */
 	if (ndev->rx_data_reassembly) {
 		kfree_skb(ndev->rx_data_reassembly);
-		ndev->rx_data_reassembly = 0;
+		ndev->rx_data_reassembly = NULL;
 	}
 
 	/* complete the data exchange transaction, if exists */
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index f1829f6ae9c5..581d419083aa 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -33,7 +33,7 @@ static struct genl_multicast_group nfc_genl_event_mcgrp = {
 	.name = NFC_GENL_MCAST_EVENT_NAME,
 };
 
-struct genl_family nfc_genl_family = {
+static struct genl_family nfc_genl_family = {
 	.id = GENL_ID_GENERATE,
 	.hdrsize = 0,
 	.name = NFC_GENL_NAME,
@@ -128,7 +128,7 @@ static int nfc_genl_dump_targets(struct sk_buff *skb,
 		cb->args[1] = (long) dev;
 	}
 
-	spin_lock_bh(&dev->targets_lock);
+	device_lock(&dev->dev);
 
 	cb->seq = dev->targets_generation;
 
@@ -141,7 +141,7 @@ static int nfc_genl_dump_targets(struct sk_buff *skb,
 		i++;
 	}
 
-	spin_unlock_bh(&dev->targets_lock);
+	device_unlock(&dev->dev);
 
 	cb->args[0] = i;
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index 7d589a81942e..3dd4232ae664 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -84,7 +84,7 @@ static inline int nfc_llcp_set_remote_gb(struct nfc_dev *dev,
 	return 0;
 }
 
-static inline u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *gb_len)
+static inline u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *gb_len)
 {
 	*gb_len = 0;
 	return NULL;
diff --git a/net/wireless/chan.c b/net/wireless/chan.c
index 2fcfe0993ca2..884801ac4dd0 100644
--- a/net/wireless/chan.c
+++ b/net/wireless/chan.c
@@ -45,7 +45,7 @@ rdev_freq_to_chan(struct cfg80211_registered_device *rdev,
 	return chan;
 }
 
-int cfg80211_can_beacon_sec_chan(struct wiphy *wiphy,
+bool cfg80211_can_beacon_sec_chan(struct wiphy *wiphy,
 				  struct ieee80211_channel *chan,
 				  enum nl80211_channel_type channel_type)
 {
diff --git a/net/wireless/core.c b/net/wireless/core.c
index 39f2538a46fc..a87d43552974 100644
--- a/net/wireless/core.c
+++ b/net/wireless/core.c
@@ -664,7 +664,7 @@ void wiphy_unregister(struct wiphy *wiphy)
 		mutex_lock(&rdev->devlist_mtx);
 		__count = rdev->opencount;
 		mutex_unlock(&rdev->devlist_mtx);
-		__count == 0;}));
+		__count == 0; }));
 
 	mutex_lock(&rdev->devlist_mtx);
 	BUG_ON(!list_empty(&rdev->netdev_list));
@@ -776,7 +776,7 @@ static struct device_type wiphy_type = {
 	.name	= "wlan",
 };
 
-static int cfg80211_netdev_notifier_call(struct notifier_block * nb,
+static int cfg80211_netdev_notifier_call(struct notifier_block *nb,
 					 unsigned long state,
 					 void *ndev)
 {
diff --git a/net/wireless/core.h b/net/wireless/core.h
index 3ac2dd00d714..8523f3878677 100644
--- a/net/wireless/core.h
+++ b/net/wireless/core.h
@@ -445,8 +445,6 @@ int cfg80211_set_freq(struct cfg80211_registered_device *rdev,
 		      struct wireless_dev *wdev, int freq,
 		      enum nl80211_channel_type channel_type);
 
-u16 cfg80211_calculate_bitrate(struct rate_info *rate);
-
 int ieee80211_get_ratemask(struct ieee80211_supported_band *sband,
 			   const u8 *rates, unsigned int n_rates,
 			   u32 *mask);
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index b67b1114e25a..206465dc0cab 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -1179,6 +1179,27 @@ static bool nl80211_can_set_dev_channel(struct wireless_dev *wdev)
 		wdev->iftype == NL80211_IFTYPE_P2P_GO;
 }
 
+static bool nl80211_valid_channel_type(struct genl_info *info,
+				       enum nl80211_channel_type *channel_type)
+{
+	enum nl80211_channel_type tmp;
+
+	if (!info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE])
+		return false;
+
+	tmp = nla_get_u32(info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]);
+	if (tmp != NL80211_CHAN_NO_HT &&
+	    tmp != NL80211_CHAN_HT20 &&
+	    tmp != NL80211_CHAN_HT40PLUS &&
+	    tmp != NL80211_CHAN_HT40MINUS)
+		return false;
+
+	if (channel_type)
+		*channel_type = tmp;
+
+	return true;
+}
+
 static int __nl80211_set_channel(struct cfg80211_registered_device *rdev,
 				 struct wireless_dev *wdev,
 				 struct genl_info *info)
@@ -1193,15 +1214,9 @@ static int __nl80211_set_channel(struct cfg80211_registered_device *rdev,
 	if (!nl80211_can_set_dev_channel(wdev))
 		return -EOPNOTSUPP;
 
-	if (info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]) {
-		channel_type = nla_get_u32(info->attrs[
-				   NL80211_ATTR_WIPHY_CHANNEL_TYPE]);
-		if (channel_type != NL80211_CHAN_NO_HT &&
-		    channel_type != NL80211_CHAN_HT20 &&
-		    channel_type != NL80211_CHAN_HT40PLUS &&
-		    channel_type != NL80211_CHAN_HT40MINUS)
-			return -EINVAL;
-	}
+	if (info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE] &&
+	    !nl80211_valid_channel_type(info, &channel_type))
+		return -EINVAL;
 
 	freq = nla_get_u32(info->attrs[NL80211_ATTR_WIPHY_FREQ]);
 
@@ -2410,10 +2425,16 @@ static int parse_station_flags(struct genl_info *info,
 		return -EINVAL;
 	}
 
-	for (flag = 1; flag <= NL80211_STA_FLAG_MAX; flag++)
-		if (flags[flag])
+	for (flag = 1; flag <= NL80211_STA_FLAG_MAX; flag++) {
+		if (flags[flag]) {
 			params->sta_flags_set |= (1<<flag);
 
+			/* no longer support new API additions in old API */
+			if (flag > NL80211_STA_FLAG_MAX_OLD_API)
+				return -EINVAL;
+		}
+	}
+
 	return 0;
 }
 
@@ -4912,12 +4933,7 @@ static int nl80211_join_ibss(struct sk_buff *skb, struct genl_info *info)
 	if (info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]) {
 		enum nl80211_channel_type channel_type;
 
-		channel_type = nla_get_u32(
-				info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]);
-		if (channel_type != NL80211_CHAN_NO_HT &&
-		    channel_type != NL80211_CHAN_HT20 &&
-		    channel_type != NL80211_CHAN_HT40MINUS &&
-		    channel_type != NL80211_CHAN_HT40PLUS)
+		if (!nl80211_valid_channel_type(info, &channel_type))
 			return -EINVAL;
 
 		if (channel_type != NL80211_CHAN_NO_HT &&
@@ -5485,15 +5501,9 @@ static int nl80211_remain_on_channel(struct sk_buff *skb,
 	    !(rdev->wiphy.flags & WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL))
 		return -EOPNOTSUPP;
 
-	if (info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]) {
-		channel_type = nla_get_u32(
-			info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]);
-		if (channel_type != NL80211_CHAN_NO_HT &&
-		    channel_type != NL80211_CHAN_HT20 &&
-		    channel_type != NL80211_CHAN_HT40PLUS &&
-		    channel_type != NL80211_CHAN_HT40MINUS)
-			return -EINVAL;
-	}
+	if (info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE] &&
+	    !nl80211_valid_channel_type(info, &channel_type))
+		return -EINVAL;
 
 	freq = nla_get_u32(info->attrs[NL80211_ATTR_WIPHY_FREQ]);
 	chan = rdev_freq_to_chan(rdev, freq, channel_type);
@@ -5764,12 +5774,7 @@ static int nl80211_tx_mgmt(struct sk_buff *skb, struct genl_info *info)
 	}
 
 	if (info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]) {
-		channel_type = nla_get_u32(
-			info->attrs[NL80211_ATTR_WIPHY_CHANNEL_TYPE]);
-		if (channel_type != NL80211_CHAN_NO_HT &&
-		    channel_type != NL80211_CHAN_HT20 &&
-		    channel_type != NL80211_CHAN_HT40PLUS &&
-		    channel_type != NL80211_CHAN_HT40MINUS)
+		if (!nl80211_valid_channel_type(info, &channel_type))
 			return -EINVAL;
 		channel_type_valid = true;
 	}
diff --git a/net/wireless/util.c b/net/wireless/util.c
index 1cd255892a43..55d99466babb 100644
--- a/net/wireless/util.c
+++ b/net/wireless/util.c
@@ -879,7 +879,7 @@ u16 cfg80211_calculate_bitrate(struct rate_info *rate)
 		return rate->legacy;
 
 	/* the formula below does only work for MCS values smaller than 32 */
-	if (rate->mcs >= 32)
+	if (WARN_ON_ONCE(rate->mcs >= 32))
 		return 0;
 
 	modulation = rate->mcs & 7;
diff --git a/sound/firewire/cmp.c b/sound/firewire/cmp.c
index 76294f2ae47f..645cb0ba4293 100644
--- a/sound/firewire/cmp.c
+++ b/sound/firewire/cmp.c
@@ -84,7 +84,7 @@ static int pcr_modify(struct cmp_connection *c,
 	return 0;
 
 io_error:
-	cmp_error(c, "transaction failed: %s\n", rcode_string(rcode));
+	cmp_error(c, "transaction failed: %s\n", fw_rcode_string(rcode));
 	return -EIO;
 
 bus_reset:
diff --git a/sound/firewire/lib.c b/sound/firewire/lib.c
index 4750cea2210e..14eb41498372 100644
--- a/sound/firewire/lib.c
+++ b/sound/firewire/lib.c
@@ -14,32 +14,6 @@
 #define ERROR_RETRY_DELAY_MS	5
 
 /**
- * rcode_string - convert a firewire result code to a string
- * @rcode: the result
- */
-const char *rcode_string(unsigned int rcode)
-{
-	static const char *const names[] = {
-		[RCODE_COMPLETE]	= "complete",
-		[RCODE_CONFLICT_ERROR]	= "conflict error",
-		[RCODE_DATA_ERROR]	= "data error",
-		[RCODE_TYPE_ERROR]	= "type error",
-		[RCODE_ADDRESS_ERROR]	= "address error",
-		[RCODE_SEND_ERROR]	= "send error",
-		[RCODE_CANCELLED]	= "cancelled",
-		[RCODE_BUSY]		= "busy",
-		[RCODE_GENERATION]	= "generation",
-		[RCODE_NO_ACK]		= "no ack",
-	};
-
-	if (rcode < ARRAY_SIZE(names) && names[rcode])
-		return names[rcode];
-	else
-		return "unknown";
-}
-EXPORT_SYMBOL(rcode_string);
-
-/**
  * snd_fw_transaction - send a request and wait for its completion
  * @unit: the driver's unit on the target device
  * @tcode: the transaction code
@@ -71,7 +45,7 @@ int snd_fw_transaction(struct fw_unit *unit, int tcode,
 
 		if (rcode_is_permanent_error(rcode) || ++tries >= 3) {
 			dev_err(&unit->device, "transaction failed: %s\n",
-				rcode_string(rcode));
+				fw_rcode_string(rcode));
 			return -EIO;
 		}
 
diff --git a/sound/firewire/lib.h b/sound/firewire/lib.h
index 064f3fd9ab06..aef301476ea9 100644
--- a/sound/firewire/lib.h
+++ b/sound/firewire/lib.h
@@ -8,7 +8,6 @@ struct fw_unit;
 
 int snd_fw_transaction(struct fw_unit *unit, int tcode,
 		       u64 offset, void *buffer, size_t length);
-const char *rcode_string(unsigned int rcode);
 
 /* returns true if retrying the transaction would not make sense */
 static inline bool rcode_is_permanent_error(int rcode)
diff --git a/sound/i2c/other/tea575x-tuner.c b/sound/i2c/other/tea575x-tuner.c
index a63faec5e7fd..582aace20ea3 100644
--- a/sound/i2c/other/tea575x-tuner.c
+++ b/sound/i2c/other/tea575x-tuner.c
@@ -375,6 +375,9 @@ int snd_tea575x_init(struct snd_tea575x *tea)
 	tea->vd.v4l2_dev = tea->v4l2_dev;
 	tea->vd.ctrl_handler = &tea->ctrl_handler;
 	set_bit(V4L2_FL_USE_FH_PRIO, &tea->vd.flags);
+	/* disable hw_freq_seek if we can't use it */
+	if (tea->cannot_read_data)
+		v4l2_disable_ioctl(&tea->vd, VIDIOC_S_HW_FREQ_SEEK);
 
 	v4l2_ctrl_handler_init(&tea->ctrl_handler, 1);
 	v4l2_ctrl_new_std(&tea->ctrl_handler, &tea575x_ctrl_ops, V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
diff --git a/tools/perf/Documentation/perf-probe.txt b/tools/perf/Documentation/perf-probe.txt
index 2780d9ce48bf..b715cb71592b 100644
--- a/tools/perf/Documentation/perf-probe.txt
+++ b/tools/perf/Documentation/perf-probe.txt
@@ -77,7 +77,8 @@ OPTIONS
 
 -F::
 --funcs::
-	Show available functions in given module or kernel.
+	Show available functions in given module or kernel. With -x/--exec,
+	can also list functions in a user space executable / shared library.
 
 --filter=FILTER::
 	(Only for --vars and --funcs) Set filter. FILTER is a combination of glob
@@ -98,6 +99,15 @@ OPTIONS
 --max-probes::
 	Set the maximum number of probe points for an event. Default is 128.
 
+-x::
+--exec=PATH::
+	Specify path to the executable or shared library file for user
+	space tracing. Can also be used with --funcs option.
+
+In absence of -m/-x options, perf probe checks if the first argument after
+the options is an absolute path name. If its an absolute path, perf probe
+uses it as a target module/target user space binary to probe.
+
 PROBE SYNTAX
 ------------
 Probe points are defined by following syntax.
@@ -182,6 +192,13 @@ Delete all probes on schedule().
 
  ./perf probe --del='schedule*'
 
+Add probes at zfree() function on /bin/zsh
+
+ ./perf probe -x /bin/zsh zfree or ./perf probe /bin/zsh zfree
+
+Add probes at malloc() function on libc
+
+ ./perf probe -x /lib/libc.so.6 malloc or ./perf probe /lib/libc.so.6 malloc
 
 SEE ALSO
 --------
diff --git a/tools/perf/builtin-probe.c b/tools/perf/builtin-probe.c
index 4935c09dd5b5..e215ae61b2ae 100644
--- a/tools/perf/builtin-probe.c
+++ b/tools/perf/builtin-probe.c
@@ -54,6 +54,7 @@ static struct {
 	bool show_ext_vars;
 	bool show_funcs;
 	bool mod_events;
+	bool uprobes;
 	int nevents;
 	struct perf_probe_event events[MAX_PROBES];
 	struct strlist *dellist;
@@ -75,6 +76,8 @@ static int parse_probe_event(const char *str)
 		return -1;
 	}
 
+	pev->uprobes = params.uprobes;
+
 	/* Parse a perf-probe command into event */
 	ret = parse_perf_probe_command(str, pev);
 	pr_debug("%d arguments\n", pev->nargs);
@@ -82,21 +85,58 @@ static int parse_probe_event(const char *str)
 	return ret;
 }
 
+static int set_target(const char *ptr)
+{
+	int found = 0;
+	const char *buf;
+
+	/*
+	 * The first argument after options can be an absolute path
+	 * to an executable / library or kernel module.
+	 *
+	 * TODO: Support relative path, and $PATH, $LD_LIBRARY_PATH,
+	 * short module name.
+	 */
+	if (!params.target && ptr && *ptr == '/') {
+		params.target = ptr;
+		found = 1;
+		buf = ptr + (strlen(ptr) - 3);
+
+		if (strcmp(buf, ".ko"))
+			params.uprobes = true;
+
+	}
+
+	return found;
+}
+
 static int parse_probe_event_argv(int argc, const char **argv)
 {
-	int i, len, ret;
+	int i, len, ret, found_target;
 	char *buf;
 
+	found_target = set_target(argv[0]);
+	if (found_target && argc == 1)
+		return 0;
+
 	/* Bind up rest arguments */
 	len = 0;
-	for (i = 0; i < argc; i++)
+	for (i = 0; i < argc; i++) {
+		if (i == 0 && found_target)
+			continue;
+
 		len += strlen(argv[i]) + 1;
+	}
 	buf = zalloc(len + 1);
 	if (buf == NULL)
 		return -ENOMEM;
 	len = 0;
-	for (i = 0; i < argc; i++)
+	for (i = 0; i < argc; i++) {
+		if (i == 0 && found_target)
+			continue;
+
 		len += sprintf(&buf[len], "%s ", argv[i]);
+	}
 	params.mod_events = true;
 	ret = parse_probe_event(buf);
 	free(buf);
@@ -125,6 +165,28 @@ static int opt_del_probe_event(const struct option *opt __used,
 	return 0;
 }
 
+static int opt_set_target(const struct option *opt, const char *str,
+			int unset __used)
+{
+	int ret = -ENOENT;
+
+	if  (str && !params.target) {
+		if (!strcmp(opt->long_name, "exec"))
+			params.uprobes = true;
+#ifdef DWARF_SUPPORT
+		else if (!strcmp(opt->long_name, "module"))
+			params.uprobes = false;
+#endif
+		else
+			return ret;
+
+		params.target = str;
+		ret = 0;
+	}
+
+	return ret;
+}
+
 #ifdef DWARF_SUPPORT
 static int opt_show_lines(const struct option *opt __used,
 			  const char *str, int unset __used)
@@ -246,9 +308,9 @@ static const struct option options[] = {
 		   "file", "vmlinux pathname"),
 	OPT_STRING('s', "source", &symbol_conf.source_prefix,
 		   "directory", "path to kernel source"),
-	OPT_STRING('m', "module", &params.target,
-		   "modname|path",
-		   "target module name (for online) or path (for offline)"),
+	OPT_CALLBACK('m', "module", NULL, "modname|path",
+		"target module name (for online) or path (for offline)",
+		opt_set_target),
 #endif
 	OPT__DRY_RUN(&probe_event_dry_run),
 	OPT_INTEGER('\0', "max-probes", &params.max_probe_points,
@@ -260,6 +322,8 @@ static const struct option options[] = {
 		     "\t\t\t(default: \"" DEFAULT_VAR_FILTER "\" for --vars,\n"
 		     "\t\t\t \"" DEFAULT_FUNC_FILTER "\" for --funcs)",
 		     opt_set_filter),
+	OPT_CALLBACK('x', "exec", NULL, "executable|path",
+			"target executable name or path", opt_set_target),
 	OPT_END()
 };
 
@@ -310,6 +374,10 @@ int cmd_probe(int argc, const char **argv, const char *prefix __used)
 			pr_err("  Error: Don't use --list with --funcs.\n");
 			usage_with_options(probe_usage, options);
 		}
+		if (params.uprobes) {
+			pr_warning("  Error: Don't use --list with --exec.\n");
+			usage_with_options(probe_usage, options);
+		}
 		ret = show_perf_probe_events();
 		if (ret < 0)
 			pr_err("  Error: Failed to show event list. (%d)\n",
@@ -333,8 +401,8 @@ int cmd_probe(int argc, const char **argv, const char *prefix __used)
 		if (!params.filter)
 			params.filter = strfilter__new(DEFAULT_FUNC_FILTER,
 						       NULL);
-		ret = show_available_funcs(params.target,
-					   params.filter);
+		ret = show_available_funcs(params.target, params.filter,
+					params.uprobes);
 		strfilter__delete(params.filter);
 		if (ret < 0)
 			pr_err("  Error: Failed to show functions."
@@ -343,7 +411,7 @@ int cmd_probe(int argc, const char **argv, const char *prefix __used)
 	}
 
 #ifdef DWARF_SUPPORT
-	if (params.show_lines) {
+	if (params.show_lines && !params.uprobes) {
 		if (params.mod_events) {
 			pr_err("  Error: Don't use --line with"
 			       " --add/--del.\n");
diff --git a/tools/perf/util/probe-event.c b/tools/perf/util/probe-event.c
index 8a8ee64e72d1..59dccc98b554 100644
--- a/tools/perf/util/probe-event.c
+++ b/tools/perf/util/probe-event.c
@@ -44,6 +44,7 @@
 #include "trace-event.h"	/* For __unused */
 #include "probe-event.h"
 #include "probe-finder.h"
+#include "session.h"
 
 #define MAX_CMDLEN 256
 #define MAX_PROBE_ARGS 128
@@ -70,6 +71,8 @@ static int e_snprintf(char *str, size_t size, const char *format, ...)
 }
 
 static char *synthesize_perf_probe_point(struct perf_probe_point *pp);
+static int convert_name_to_addr(struct perf_probe_event *pev,
+				const char *exec);
 static struct machine machine;
 
 /* Initialize symbol maps and path of vmlinux/modules */
@@ -170,6 +173,34 @@ const char *kernel_get_module_path(const char *module)
 	return (dso) ? dso->long_name : NULL;
 }
 
+static int init_user_exec(void)
+{
+	int ret = 0;
+
+	symbol_conf.try_vmlinux_path = false;
+	symbol_conf.sort_by_name = true;
+	ret = symbol__init();
+
+	if (ret < 0)
+		pr_debug("Failed to init symbol map.\n");
+
+	return ret;
+}
+
+static int convert_to_perf_probe_point(struct probe_trace_point *tp,
+					struct perf_probe_point *pp)
+{
+	pp->function = strdup(tp->symbol);
+
+	if (pp->function == NULL)
+		return -ENOMEM;
+
+	pp->offset = tp->offset;
+	pp->retprobe = tp->retprobe;
+
+	return 0;
+}
+
 #ifdef DWARF_SUPPORT
 /* Open new debuginfo of given module */
 static struct debuginfo *open_debuginfo(const char *module)
@@ -224,10 +255,7 @@ static int kprobe_convert_to_perf_probe(struct probe_trace_point *tp,
 	if (ret <= 0) {
 		pr_debug("Failed to find corresponding probes from "
 			 "debuginfo. Use kprobe event information.\n");
-		pp->function = strdup(tp->symbol);
-		if (pp->function == NULL)
-			return -ENOMEM;
-		pp->offset = tp->offset;
+		return convert_to_perf_probe_point(tp, pp);
 	}
 	pp->retprobe = tp->retprobe;
 
@@ -275,9 +303,20 @@ static int try_to_find_probe_trace_events(struct perf_probe_event *pev,
 					  int max_tevs, const char *target)
 {
 	bool need_dwarf = perf_probe_event_need_dwarf(pev);
-	struct debuginfo *dinfo = open_debuginfo(target);
+	struct debuginfo *dinfo;
 	int ntevs, ret = 0;
 
+	if (pev->uprobes) {
+		if (need_dwarf) {
+			pr_warning("Debuginfo-analysis is not yet supported"
+					" with -x/--exec option.\n");
+			return -ENOSYS;
+		}
+		return convert_name_to_addr(pev, target);
+	}
+
+	dinfo = open_debuginfo(target);
+
 	if (!dinfo) {
 		if (need_dwarf) {
 			pr_warning("Failed to open debuginfo file.\n");
@@ -603,23 +642,22 @@ static int kprobe_convert_to_perf_probe(struct probe_trace_point *tp,
 		pr_err("Failed to find symbol %s in kernel.\n", tp->symbol);
 		return -ENOENT;
 	}
-	pp->function = strdup(tp->symbol);
-	if (pp->function == NULL)
-		return -ENOMEM;
-	pp->offset = tp->offset;
-	pp->retprobe = tp->retprobe;
 
-	return 0;
+	return convert_to_perf_probe_point(tp, pp);
 }
 
 static int try_to_find_probe_trace_events(struct perf_probe_event *pev,
 				struct probe_trace_event **tevs __unused,
-				int max_tevs __unused, const char *mod __unused)
+				int max_tevs __unused, const char *target)
 {
 	if (perf_probe_event_need_dwarf(pev)) {
 		pr_warning("Debuginfo-analysis is not supported.\n");
 		return -ENOSYS;
 	}
+
+	if (pev->uprobes)
+		return convert_name_to_addr(pev, target);
+
 	return 0;
 }
 
@@ -1341,11 +1379,18 @@ char *synthesize_probe_trace_command(struct probe_trace_event *tev)
 	if (buf == NULL)
 		return NULL;
 
-	len = e_snprintf(buf, MAX_CMDLEN, "%c:%s/%s %s%s%s+%lu",
-			 tp->retprobe ? 'r' : 'p',
-			 tev->group, tev->event,
-			 tp->module ?: "", tp->module ? ":" : "",
-			 tp->symbol, tp->offset);
+	if (tev->uprobes)
+		len = e_snprintf(buf, MAX_CMDLEN, "%c:%s/%s %s:%s",
+				 tp->retprobe ? 'r' : 'p',
+				 tev->group, tev->event,
+				 tp->module, tp->symbol);
+	else
+		len = e_snprintf(buf, MAX_CMDLEN, "%c:%s/%s %s%s%s+%lu",
+				 tp->retprobe ? 'r' : 'p',
+				 tev->group, tev->event,
+				 tp->module ?: "", tp->module ? ":" : "",
+				 tp->symbol, tp->offset);
+
 	if (len <= 0)
 		goto error;
 
@@ -1364,7 +1409,7 @@ error:
 }
 
 static int convert_to_perf_probe_event(struct probe_trace_event *tev,
-				       struct perf_probe_event *pev)
+			       struct perf_probe_event *pev, bool is_kprobe)
 {
 	char buf[64] = "";
 	int i, ret;
@@ -1376,7 +1421,11 @@ static int convert_to_perf_probe_event(struct probe_trace_event *tev,
 		return -ENOMEM;
 
 	/* Convert trace_point to probe_point */
-	ret = kprobe_convert_to_perf_probe(&tev->point, &pev->point);
+	if (is_kprobe)
+		ret = kprobe_convert_to_perf_probe(&tev->point, &pev->point);
+	else
+		ret = convert_to_perf_probe_point(&tev->point, &pev->point);
+
 	if (ret < 0)
 		return ret;
 
@@ -1472,7 +1521,26 @@ static void clear_probe_trace_event(struct probe_trace_event *tev)
 	memset(tev, 0, sizeof(*tev));
 }
 
-static int open_kprobe_events(bool readwrite)
+static void print_warn_msg(const char *file, bool is_kprobe)
+{
+
+	if (errno == ENOENT) {
+		const char *config;
+
+		if (!is_kprobe)
+			config = "CONFIG_UPROBE_EVENTS";
+		else
+			config = "CONFIG_KPROBE_EVENTS";
+
+		pr_warning("%s file does not exist - please rebuild kernel"
+				" with %s.\n", file, config);
+	} else
+		pr_warning("Failed to open %s file: %s\n", file,
+				strerror(errno));
+}
+
+static int open_probe_events(const char *trace_file, bool readwrite,
+				bool is_kprobe)
 {
 	char buf[PATH_MAX];
 	const char *__debugfs;
@@ -1484,27 +1552,31 @@ static int open_kprobe_events(bool readwrite)
 		return -ENOENT;
 	}
 
-	ret = e_snprintf(buf, PATH_MAX, "%stracing/kprobe_events", __debugfs);
+	ret = e_snprintf(buf, PATH_MAX, "%s/%s", __debugfs, trace_file);
 	if (ret >= 0) {
 		pr_debug("Opening %s write=%d\n", buf, readwrite);
 		if (readwrite && !probe_event_dry_run)
 			ret = open(buf, O_RDWR, O_APPEND);
 		else
 			ret = open(buf, O_RDONLY, 0);
-	}
 
-	if (ret < 0) {
-		if (errno == ENOENT)
-			pr_warning("kprobe_events file does not exist - please"
-				 " rebuild kernel with CONFIG_KPROBE_EVENT.\n");
-		else
-			pr_warning("Failed to open kprobe_events file: %s\n",
-				   strerror(errno));
+		if (ret < 0)
+			print_warn_msg(buf, is_kprobe);
 	}
 	return ret;
 }
 
-/* Get raw string list of current kprobe_events */
+static int open_kprobe_events(bool readwrite)
+{
+	return open_probe_events("tracing/kprobe_events", readwrite, true);
+}
+
+static int open_uprobe_events(bool readwrite)
+{
+	return open_probe_events("tracing/uprobe_events", readwrite, false);
+}
+
+/* Get raw string list of current kprobe_events  or uprobe_events */
 static struct strlist *get_probe_trace_command_rawlist(int fd)
 {
 	int ret, idx;
@@ -1569,36 +1641,26 @@ static int show_perf_probe_event(struct perf_probe_event *pev)
 	return ret;
 }
 
-/* List up current perf-probe events */
-int show_perf_probe_events(void)
+static int __show_perf_probe_events(int fd, bool is_kprobe)
 {
-	int fd, ret;
+	int ret = 0;
 	struct probe_trace_event tev;
 	struct perf_probe_event pev;
 	struct strlist *rawlist;
 	struct str_node *ent;
 
-	setup_pager();
-	ret = init_vmlinux();
-	if (ret < 0)
-		return ret;
-
 	memset(&tev, 0, sizeof(tev));
 	memset(&pev, 0, sizeof(pev));
 
-	fd = open_kprobe_events(false);
-	if (fd < 0)
-		return fd;
-
 	rawlist = get_probe_trace_command_rawlist(fd);
-	close(fd);
 	if (!rawlist)
 		return -ENOENT;
 
 	strlist__for_each(ent, rawlist) {
 		ret = parse_probe_trace_command(ent->s, &tev);
 		if (ret >= 0) {
-			ret = convert_to_perf_probe_event(&tev, &pev);
+			ret = convert_to_perf_probe_event(&tev, &pev,
+								is_kprobe);
 			if (ret >= 0)
 				ret = show_perf_probe_event(&pev);
 		}
@@ -1612,6 +1674,33 @@ int show_perf_probe_events(void)
 	return ret;
 }
 
+/* List up current perf-probe events */
+int show_perf_probe_events(void)
+{
+	int fd, ret;
+
+	setup_pager();
+	fd = open_kprobe_events(false);
+
+	if (fd < 0)
+		return fd;
+
+	ret = init_vmlinux();
+	if (ret < 0)
+		return ret;
+
+	ret = __show_perf_probe_events(fd, true);
+	close(fd);
+
+	fd = open_uprobe_events(false);
+	if (fd >= 0) {
+		ret = __show_perf_probe_events(fd, false);
+		close(fd);
+	}
+
+	return ret;
+}
+
 /* Get current perf-probe event names */
 static struct strlist *get_probe_trace_event_names(int fd, bool include_group)
 {
@@ -1717,7 +1806,11 @@ static int __add_probe_trace_events(struct perf_probe_event *pev,
 	const char *event, *group;
 	struct strlist *namelist;
 
-	fd = open_kprobe_events(true);
+	if (pev->uprobes)
+		fd = open_uprobe_events(true);
+	else
+		fd = open_kprobe_events(true);
+
 	if (fd < 0)
 		return fd;
 	/* Get current event names */
@@ -1829,6 +1922,8 @@ static int convert_to_probe_trace_events(struct perf_probe_event *pev,
 	tev->point.offset = pev->point.offset;
 	tev->point.retprobe = pev->point.retprobe;
 	tev->nargs = pev->nargs;
+	tev->uprobes = pev->uprobes;
+
 	if (tev->nargs) {
 		tev->args = zalloc(sizeof(struct probe_trace_arg)
 				   * tev->nargs);
@@ -1859,6 +1954,9 @@ static int convert_to_probe_trace_events(struct perf_probe_event *pev,
 		}
 	}
 
+	if (pev->uprobes)
+		return 1;
+
 	/* Currently just checking function name from symbol map */
 	sym = __find_kernel_function_by_name(tev->point.symbol, NULL);
 	if (!sym) {
@@ -1894,12 +1992,18 @@ int add_perf_probe_events(struct perf_probe_event *pevs, int npevs,
 	int i, j, ret;
 	struct __event_package *pkgs;
 
+	ret = 0;
 	pkgs = zalloc(sizeof(struct __event_package) * npevs);
+
 	if (pkgs == NULL)
 		return -ENOMEM;
 
-	/* Init vmlinux path */
-	ret = init_vmlinux();
+	if (!pevs->uprobes)
+		/* Init vmlinux path */
+		ret = init_vmlinux();
+	else
+		ret = init_user_exec();
+
 	if (ret < 0) {
 		free(pkgs);
 		return ret;
@@ -1971,23 +2075,15 @@ error:
 	return ret;
 }
 
-static int del_trace_probe_event(int fd, const char *group,
-				  const char *event, struct strlist *namelist)
+static int del_trace_probe_event(int fd, const char *buf,
+						  struct strlist *namelist)
 {
-	char buf[128];
 	struct str_node *ent, *n;
-	int found = 0, ret = 0;
-
-	ret = e_snprintf(buf, 128, "%s:%s", group, event);
-	if (ret < 0) {
-		pr_err("Failed to copy event.\n");
-		return ret;
-	}
+	int ret = -1;
 
 	if (strpbrk(buf, "*?")) { /* Glob-exp */
 		strlist__for_each_safe(ent, n, namelist)
 			if (strglobmatch(ent->s, buf)) {
-				found++;
 				ret = __del_trace_probe_event(fd, ent);
 				if (ret < 0)
 					break;
@@ -1996,40 +2092,43 @@ static int del_trace_probe_event(int fd, const char *group,
 	} else {
 		ent = strlist__find(namelist, buf);
 		if (ent) {
-			found++;
 			ret = __del_trace_probe_event(fd, ent);
 			if (ret >= 0)
 				strlist__remove(namelist, ent);
 		}
 	}
-	if (found == 0 && ret >= 0)
-		pr_info("Info: Event \"%s\" does not exist.\n", buf);
 
 	return ret;
 }
 
 int del_perf_probe_events(struct strlist *dellist)
 {
-	int fd, ret = 0;
+	int ret = -1, ufd = -1, kfd = -1;
+	char buf[128];
 	const char *group, *event;
 	char *p, *str;
 	struct str_node *ent;
-	struct strlist *namelist;
-
-	fd = open_kprobe_events(true);
-	if (fd < 0)
-		return fd;
+	struct strlist *namelist = NULL, *unamelist = NULL;
 
 	/* Get current event names */
-	namelist = get_probe_trace_event_names(fd, true);
-	if (namelist == NULL)
-		return -EINVAL;
+	kfd = open_kprobe_events(true);
+	if (kfd < 0)
+		return kfd;
+
+	namelist = get_probe_trace_event_names(kfd, true);
+	ufd = open_uprobe_events(true);
+
+	if (ufd >= 0)
+		unamelist = get_probe_trace_event_names(ufd, true);
+
+	if (namelist == NULL && unamelist == NULL)
+		goto error;
 
 	strlist__for_each(ent, dellist) {
 		str = strdup(ent->s);
 		if (str == NULL) {
 			ret = -ENOMEM;
-			break;
+			goto error;
 		}
 		pr_debug("Parsing: %s\n", str);
 		p = strchr(str, ':');
@@ -2041,17 +2140,46 @@ int del_perf_probe_events(struct strlist *dellist)
 			group = "*";
 			event = str;
 		}
+
+		ret = e_snprintf(buf, 128, "%s:%s", group, event);
+		if (ret < 0) {
+			pr_err("Failed to copy event.");
+			free(str);
+			goto error;
+		}
+
 		pr_debug("Group: %s, Event: %s\n", group, event);
-		ret = del_trace_probe_event(fd, group, event, namelist);
+
+		if (namelist)
+			ret = del_trace_probe_event(kfd, buf, namelist);
+
+		if (unamelist && ret != 0)
+			ret = del_trace_probe_event(ufd, buf, unamelist);
+
+		if (ret != 0)
+			pr_info("Info: Event \"%s\" does not exist.\n", buf);
+
 		free(str);
-		if (ret < 0)
-			break;
 	}
-	strlist__delete(namelist);
-	close(fd);
+
+error:
+	if (kfd >= 0) {
+		if (namelist)
+			strlist__delete(namelist);
+
+		close(kfd);
+	}
+
+	if (ufd >= 0) {
+		if (unamelist)
+			strlist__delete(unamelist);
+
+		close(ufd);
+	}
 
 	return ret;
 }
+
 /* TODO: don't use a global variable for filter ... */
 static struct strfilter *available_func_filter;
 
@@ -2068,30 +2196,152 @@ static int filter_available_functions(struct map *map __unused,
 	return 1;
 }
 
-int show_available_funcs(const char *target, struct strfilter *_filter)
+static int __show_available_funcs(struct map *map)
+{
+	if (map__load(map, filter_available_functions)) {
+		pr_err("Failed to load map.\n");
+		return -EINVAL;
+	}
+	if (!dso__sorted_by_name(map->dso, map->type))
+		dso__sort_by_name(map->dso, map->type);
+
+	dso__fprintf_symbols_by_name(map->dso, map->type, stdout);
+	return 0;
+}
+
+static int available_kernel_funcs(const char *module)
 {
 	struct map *map;
 	int ret;
 
-	setup_pager();
-
 	ret = init_vmlinux();
 	if (ret < 0)
 		return ret;
 
-	map = kernel_get_module_map(target);
+	map = kernel_get_module_map(module);
 	if (!map) {
-		pr_err("Failed to find %s map.\n", (target) ? : "kernel");
+		pr_err("Failed to find %s map.\n", (module) ? : "kernel");
 		return -EINVAL;
 	}
+	return __show_available_funcs(map);
+}
+
+static int available_user_funcs(const char *target)
+{
+	struct map *map;
+	int ret;
+
+	ret = init_user_exec();
+	if (ret < 0)
+		return ret;
+
+	map = dso__new_map(target);
+	ret = __show_available_funcs(map);
+	dso__delete(map->dso);
+	map__delete(map);
+	return ret;
+}
+
+int show_available_funcs(const char *target, struct strfilter *_filter,
+					bool user)
+{
+	setup_pager();
 	available_func_filter = _filter;
+
+	if (!user)
+		return available_kernel_funcs(target);
+
+	return available_user_funcs(target);
+}
+
+/*
+ * uprobe_events only accepts address:
+ * Convert function and any offset to address
+ */
+static int convert_name_to_addr(struct perf_probe_event *pev, const char *exec)
+{
+	struct perf_probe_point *pp = &pev->point;
+	struct symbol *sym;
+	struct map *map = NULL;
+	char *function = NULL, *name = NULL;
+	int ret = -EINVAL;
+	unsigned long long vaddr = 0;
+
+	if (!pp->function) {
+		pr_warning("No function specified for uprobes");
+		goto out;
+	}
+
+	function = strdup(pp->function);
+	if (!function) {
+		pr_warning("Failed to allocate memory by strdup.\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	name = realpath(exec, NULL);
+	if (!name) {
+		pr_warning("Cannot find realpath for %s.\n", exec);
+		goto out;
+	}
+	map = dso__new_map(name);
+	if (!map) {
+		pr_warning("Cannot find appropriate DSO for %s.\n", exec);
+		goto out;
+	}
+	available_func_filter = strfilter__new(function, NULL);
 	if (map__load(map, filter_available_functions)) {
 		pr_err("Failed to load map.\n");
-		return -EINVAL;
+		goto out;
 	}
-	if (!dso__sorted_by_name(map->dso, map->type))
-		dso__sort_by_name(map->dso, map->type);
 
-	dso__fprintf_symbols_by_name(map->dso, map->type, stdout);
-	return 0;
+	sym = map__find_symbol_by_name(map, function, NULL);
+	if (!sym) {
+		pr_warning("Cannot find %s in DSO %s\n", function, exec);
+		goto out;
+	}
+
+	if (map->start > sym->start)
+		vaddr = map->start;
+	vaddr += sym->start + pp->offset + map->pgoff;
+	pp->offset = 0;
+
+	if (!pev->event) {
+		pev->event = function;
+		function = NULL;
+	}
+	if (!pev->group) {
+		char *ptr1, *ptr2;
+
+		pev->group = zalloc(sizeof(char *) * 64);
+		ptr1 = strdup(basename(exec));
+		if (ptr1) {
+			ptr2 = strpbrk(ptr1, "-._");
+			if (ptr2)
+				*ptr2 = '\0';
+			e_snprintf(pev->group, 64, "%s_%s", PERFPROBE_GROUP,
+					ptr1);
+			free(ptr1);
+		}
+	}
+	free(pp->function);
+	pp->function = zalloc(sizeof(char *) * MAX_PROBE_ARGS);
+	if (!pp->function) {
+		ret = -ENOMEM;
+		pr_warning("Failed to allocate memory by zalloc.\n");
+		goto out;
+	}
+	e_snprintf(pp->function, MAX_PROBE_ARGS, "0x%llx", vaddr);
+	ret = 0;
+
+out:
+	if (map) {
+		dso__delete(map->dso);
+		map__delete(map);
+	}
+	if (function)
+		free(function);
+	if (name)
+		free(name);
+	return ret;
 }
diff --git a/tools/perf/util/probe-event.h b/tools/perf/util/probe-event.h
index a7dee835f49c..f9f3de8b4220 100644
--- a/tools/perf/util/probe-event.h
+++ b/tools/perf/util/probe-event.h
@@ -7,7 +7,7 @@
 
 extern bool probe_event_dry_run;
 
-/* kprobe-tracer tracing point */
+/* kprobe-tracer and uprobe-tracer tracing point */
 struct probe_trace_point {
 	char		*symbol;	/* Base symbol */
 	char		*module;	/* Module name */
@@ -21,7 +21,7 @@ struct probe_trace_arg_ref {
 	long				offset;	/* Offset value */
 };
 
-/* kprobe-tracer tracing argument */
+/* kprobe-tracer and uprobe-tracer tracing argument */
 struct probe_trace_arg {
 	char				*name;	/* Argument name */
 	char				*value;	/* Base value */
@@ -29,12 +29,13 @@ struct probe_trace_arg {
 	struct probe_trace_arg_ref	*ref;	/* Referencing offset */
 };
 
-/* kprobe-tracer tracing event (point + arg) */
+/* kprobe-tracer and uprobe-tracer tracing event (point + arg) */
 struct probe_trace_event {
 	char				*event;	/* Event name */
 	char				*group;	/* Group name */
 	struct probe_trace_point	point;	/* Trace point */
 	int				nargs;	/* Number of args */
+	bool				uprobes;	/* uprobes only */
 	struct probe_trace_arg		*args;	/* Arguments */
 };
 
@@ -70,6 +71,7 @@ struct perf_probe_event {
 	char			*group;	/* Group name */
 	struct perf_probe_point	point;	/* Probe point */
 	int			nargs;	/* Number of arguments */
+	bool			uprobes;
 	struct perf_probe_arg	*args;	/* Arguments */
 };
 
@@ -129,8 +131,8 @@ extern int show_line_range(struct line_range *lr, const char *module);
 extern int show_available_vars(struct perf_probe_event *pevs, int npevs,
 			       int max_probe_points, const char *module,
 			       struct strfilter *filter, bool externs);
-extern int show_available_funcs(const char *module, struct strfilter *filter);
-
+extern int show_available_funcs(const char *module, struct strfilter *filter,
+				bool user);
 
 /* Maximum index number of event-name postfix */
 #define MAX_EVENT_INDEX	1024
diff --git a/tools/perf/util/symbol.c b/tools/perf/util/symbol.c
index ab9867b2b433..e2ba8858f3e1 100644
--- a/tools/perf/util/symbol.c
+++ b/tools/perf/util/symbol.c
@@ -2783,3 +2783,11 @@ int machine__load_vmlinux_path(struct machine *machine, enum map_type type,
 
 	return ret;
 }
+
+struct map *dso__new_map(const char *name)
+{
+	struct dso *dso = dso__new(name);
+	struct map *map = map__new2(0, dso, MAP__FUNCTION);
+
+	return map;
+}
diff --git a/tools/perf/util/symbol.h b/tools/perf/util/symbol.h
index 1f003884f1ab..5649d63798cb 100644
--- a/tools/perf/util/symbol.h
+++ b/tools/perf/util/symbol.h
@@ -242,6 +242,7 @@ void dso__set_long_name(struct dso *dso, char *name);
 void dso__set_build_id(struct dso *dso, void *build_id);
 void dso__read_running_kernel_build_id(struct dso *dso,
 				       struct machine *machine);
+struct map *dso__new_map(const char *name);
 struct symbol *dso__find_symbol(struct dso *dso, enum map_type type,
 				u64 addr);
 struct symbol *dso__find_symbol_by_name(struct dso *dso, enum map_type type,